main/control/Position.cpp
// Calculation new position based on gyro measurements
Calculation new position based on gyro measurements
Calculation new position based on gyro measurements
void Position::apply_movements(array<float, 3> gyro, float elapsed_sec, PositionUpdate& out) {
    gyro = Vector3::multiply(gyro, (M_PI/180) * elapsed_sec);
    out.rotation_matrix = {
        1,
        -gyro[2],
        gyro[1],
        gyro[2],
        1,
        -gyro[0],
        -gyro[1],
        gyro[0],
        1
    };
    matrix = Matrix3::multiply(matrix, out.rotation_matrix);
}
void Position :: apply_movements ( array < float , 3 > gyro , float elapsed_sec , PositionUpdate & out ) { gyro = Vector3 :: multiply ( gyro , ( M_PI / 180 ) * elapsed_sec ) ; out . rotation_matrix = { 1 , - gyro [ 2 ] , gyro [ 1 ] , gyro [ 2 ] , 1 , - gyro [ 0 ] , - gyro [ 1 ] , gyro [ 0 ] , 1 } ; matrix = Matrix3 :: multiply ( matrix , out . rotation_matrix ) ; }
Calculation new position based on gyro measurements
Calculation new position based on gyro measurements
// Convert gyro to angles (in rad) // Infinitesimal rotation matrix // Apply