Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-31 20:38:27 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

Merge pull request #3026 from paparazzi/fix-state_speed

Browse Source 
[state] fix speed convert functions
This commit is contained in:
Hector Garcia de Marina
2023-04-27 07:11:32 +02:00
committed by GitHub
parent 5f4a4603f0 66363bf806
commit cd03b42651
1 changed files with 8 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/state.c
+8 -8
View File
@@ -733,10 +733,10 @@ void stateCalcSpeedEnu_i(void)
INT32_VECT3_ENU_OF_NED(state.enu_speed_i, state.ned_speed_i); INT32_VECT3_ENU_OF_NED(state.enu_speed_i, state.ned_speed_i);
} }
if (bit_is_set(state.speed_status, SPEED_ENU_F)) { if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
ENU_BFP_OF_REAL(state.enu_speed_i, state.enu_speed_f); SPEEDS_BFP_OF_REAL(state.enu_speed_i, state.enu_speed_f);
} else if (bit_is_set(state.speed_status, SPEED_NED_F)) { } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f); SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f);
SetBit(state.pos_status, SPEED_NED_I); SetBit(state.speed_status, SPEED_NED_I);
INT32_VECT3_ENU_OF_NED(state.enu_speed_i, state.ned_speed_i); INT32_VECT3_ENU_OF_NED(state.enu_speed_i, state.ned_speed_i);
} else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) { } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
enu_of_ecef_vect_i(&state.enu_speed_i, &state.ned_origin_i, &state.ecef_speed_i); enu_of_ecef_vect_i(&state.enu_speed_i, &state.ned_origin_i, &state.ecef_speed_i);
@@ -753,10 +753,10 @@ void stateCalcSpeedEnu_i(void)
INT32_VECT3_ENU_OF_NED(state.enu_speed_i, state.ned_speed_i); INT32_VECT3_ENU_OF_NED(state.enu_speed_i, state.ned_speed_i);
} }
if (bit_is_set(state.speed_status, SPEED_ENU_F)) { if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
ENU_BFP_OF_REAL(state.enu_speed_i, state.enu_speed_f); SPEEDS_BFP_OF_REAL(state.enu_speed_i, state.enu_speed_f);
} else if (bit_is_set(state.speed_status, SPEED_NED_F)) { } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f); SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f);
SetBit(state.pos_status, SPEED_NED_I); SetBit(state.speed_status, SPEED_NED_I);
INT32_VECT3_ENU_OF_NED(state.enu_speed_i, state.ned_speed_i); INT32_VECT3_ENU_OF_NED(state.enu_speed_i, state.ned_speed_i);
} else { /* could not get this representation, set errno */ } else { /* could not get this representation, set errno */
errno = 2; errno = 2;
@@ -936,10 +936,10 @@ void stateCalcSpeedEnu_f(void)
if (bit_is_set(state.speed_status, SPEED_NED_F)) { if (bit_is_set(state.speed_status, SPEED_NED_F)) {
VECT3_ENU_OF_NED(state.enu_speed_f, state.ned_speed_f); VECT3_ENU_OF_NED(state.enu_speed_f, state.ned_speed_f);
} else if (bit_is_set(state.speed_status, SPEED_ENU_I)) { } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
ENU_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i); SPEEDS_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i);
} else if (bit_is_set(state.speed_status, SPEED_NED_I)) { } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i); SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i);
SetBit(state.pos_status, SPEED_NED_F); SetBit(state.speed_status, SPEED_NED_F);
VECT3_ENU_OF_NED(state.enu_speed_f, state.ned_speed_f); VECT3_ENU_OF_NED(state.enu_speed_f, state.ned_speed_f);
} else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) { } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
enu_of_ecef_vect_f(&state.enu_speed_f, &state.ned_origin_f, &state.ecef_speed_f); enu_of_ecef_vect_f(&state.enu_speed_f, &state.ned_origin_f, &state.ecef_speed_f);
@@ -955,10 +955,10 @@ void stateCalcSpeedEnu_f(void)
if (bit_is_set(state.speed_status, SPEED_NED_F)) { if (bit_is_set(state.speed_status, SPEED_NED_F)) {
VECT3_ENU_OF_NED(state.enu_speed_f, state.ned_speed_f); VECT3_ENU_OF_NED(state.enu_speed_f, state.ned_speed_f);
} else if (bit_is_set(state.speed_status, SPEED_ENU_I)) { } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
ENU_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i); SPEEDS_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i);
} else if (bit_is_set(state.speed_status, SPEED_NED_I)) { } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i); SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i);
SetBit(state.pos_status, SPEED_NED_F); SetBit(state.speed_status, SPEED_NED_F);
VECT3_ENU_OF_NED(state.enu_speed_f, state.ned_speed_f); VECT3_ENU_OF_NED(state.enu_speed_f, state.ned_speed_f);
} else { /* could not get this representation, set errno */ } else { /* could not get this representation, set errno */
errno = 2; errno = 2;