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

[rotorcraft] failsafe and ground_detect

Browse Source 
- always reset ground detection
- KILL if in FAILSAFE and not in_flight anymore
This commit is contained in:
Felix Ruess
2013-09-22 03:02:32 +02:00
parent 16d5bd4478
commit 0901318807
1 changed files with 22 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/autopilot.c
+22 -6
View File
@@ -129,13 +129,28 @@ void autopilot_init(void) {
void autopilot_periodic(void) { void autopilot_periodic(void) {
RunOnceEvery(NAV_PRESCALER, nav_periodic_task()); RunOnceEvery(NAV_PRESCALER, nav_periodic_task());
/* If in FAILSAFE mode and either already not in_flight anymore
* or just "detected" ground, go to KILL mode.
*/
if (autopilot_mode == AP_MODE_FAILSAFE) {
if (!autopilot_in_flight)
autopilot_set_mode(AP_MODE_KILL);
#if FAILSAFE_GROUND_DETECT #if FAILSAFE_GROUND_DETECT
INFO("Using FAILSAFE_GROUND_DETECT") INFO("Using FAILSAFE_GROUND_DETECT: KILL")
if (autopilot_mode == AP_MODE_FAILSAFE && autopilot_ground_detected) { if (autopilot_ground_detected)
autopilot_set_mode(AP_MODE_KILL); autopilot_set_mode(AP_MODE_KILL);
autopilot_ground_detected = FALSE;
}
#endif #endif
}
/* Reset ground detection _after_ running flight plan
*/
if (!autopilot_in_flight || autopilot_ground_detected) {
autopilot_ground_detected = FALSE;
autopilot_detect_ground_once = FALSE;
}
/* Set fixed "failsafe" commands from airframe file if in KILL mode. /* Set fixed "failsafe" commands from airframe file if in KILL mode.
* If in FAILSAFE mode, run normal loops with failsafe attitude and * If in FAILSAFE mode, run normal loops with failsafe attitude and
@@ -169,7 +184,6 @@ void autopilot_set_mode(uint8_t new_autopilot_mode) {
break; break;
#endif #endif
case AP_MODE_KILL: case AP_MODE_KILL:
autopilot_set_motors_on(FALSE);
autopilot_in_flight = FALSE; autopilot_in_flight = FALSE;
autopilot_in_flight_counter = 0; autopilot_in_flight_counter = 0;
guidance_h_mode_changed(GUIDANCE_H_MODE_KILL); guidance_h_mode_changed(GUIDANCE_H_MODE_KILL);
@@ -213,6 +227,8 @@ void autopilot_set_mode(uint8_t new_autopilot_mode) {
break; break;
#endif #endif
case AP_MODE_KILL: case AP_MODE_KILL:
autopilot_set_motors_on(FALSE);
stabilization_cmd[COMMAND_THRUST] = 0;
guidance_v_mode_changed(GUIDANCE_V_MODE_KILL); guidance_v_mode_changed(GUIDANCE_V_MODE_KILL);
break; break;
case AP_MODE_RC_DIRECT: case AP_MODE_RC_DIRECT: