Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-05 15:30:08 +08:00
Code Issues Actions 29 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
&& stabilization_cmd[COMMAND_THRUST] < INS_SONAR_THROTTLE_THRESHOLD
#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
&& 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
&& ins_update_on_agl
&& baro.status == BS_RUNNING) {