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

[ins sonar] fix test and add threshold

Browse Source
This commit is contained in:
Gautier Hattenberger
2012-07-11 11:43:19 +02:00
parent 3d6f17454d
commit c77307b8ed
1 changed files with 9 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/subsystems/ins_extended.c
+9 -1
View File
@@ -297,8 +297,16 @@ void ins_update_sonar() {
#ifdef INS_SONAR_THROTTLE_THRESHOLD #ifdef INS_SONAR_THROTTLE_THRESHOLD
&& stabilization_cmd[COMMAND_THRUST] < INS_SONAR_THROTTLE_THRESHOLD && stabilization_cmd[COMMAND_THRUST] < INS_SONAR_THROTTLE_THRESHOLD
#endif #endif
#ifdef INS_SONAR_STAB_THRESHOLD
&& stabilization_cmd[COMMAND_ROLL] < INS_SONAR_STAB_THRESHOLD
&& stabilization_cmd[COMMAND_ROLL] > -INS_SONAR_STAB_THRESHOLD
&& stabilization_cmd[COMMAND_PITCH] < INS_SONAR_STAB_THRESHOLD
&& stabilization_cmd[COMMAND_PITCH] > -INS_SONAR_STAB_THRESHOLD
&& stabilization_cmd[COMMAND_YAW] < INS_SONAR_STAB_THRESHOLD
&& stabilization_cmd[COMMAND_YAW] > -INS_SONAR_STAB_THRESHOLD
#endif
#ifdef INS_SONAR_BARO_THRESHOLD #ifdef INS_SONAR_BARO_THRESHOLD
&& ins_baro_alt < POS_BFP_OF_REAL(INS_SONAR_BARO_THRESHOLD) && ins_baro_alt > -POS_BFP_OF_REAL(INS_SONAR_BARO_THRESHOLD) /* z down */
#endif #endif
&& ins_update_on_agl && ins_update_on_agl
&& baro.status == BS_RUNNING) { && baro.status == BS_RUNNING) {