mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-05 06:54:49 +08:00
[rotorcraft] rename autopilot_detect_ground to autopilot_ground_detected
This commit is contained in:
@@ -49,7 +49,7 @@ bool_t kill_throttle;
|
|||||||
bool_t autopilot_rc;
|
bool_t autopilot_rc;
|
||||||
bool_t autopilot_power_switch;
|
bool_t autopilot_power_switch;
|
||||||
|
|
||||||
bool_t autopilot_detect_ground;
|
bool_t autopilot_ground_detected;
|
||||||
bool_t autopilot_detect_ground_once;
|
bool_t autopilot_detect_ground_once;
|
||||||
|
|
||||||
#define AUTOPILOT_IN_FLIGHT_TIME 20
|
#define AUTOPILOT_IN_FLIGHT_TIME 20
|
||||||
@@ -105,7 +105,7 @@ void autopilot_init(void) {
|
|||||||
autopilot_in_flight = FALSE;
|
autopilot_in_flight = FALSE;
|
||||||
autopilot_in_flight_counter = 0;
|
autopilot_in_flight_counter = 0;
|
||||||
autopilot_mode_auto2 = MODE_AUTO2;
|
autopilot_mode_auto2 = MODE_AUTO2;
|
||||||
autopilot_detect_ground = FALSE;
|
autopilot_ground_detected = FALSE;
|
||||||
autopilot_detect_ground_once = FALSE;
|
autopilot_detect_ground_once = FALSE;
|
||||||
autopilot_flight_time = 0;
|
autopilot_flight_time = 0;
|
||||||
autopilot_rc = TRUE;
|
autopilot_rc = TRUE;
|
||||||
@@ -131,9 +131,9 @@ void autopilot_periodic(void) {
|
|||||||
RunOnceEvery(NAV_PRESCALER, nav_periodic_task());
|
RunOnceEvery(NAV_PRESCALER, nav_periodic_task());
|
||||||
#if FAILSAFE_GROUND_DETECT
|
#if FAILSAFE_GROUND_DETECT
|
||||||
INFO("Using FAILSAFE_GROUND_DETECT")
|
INFO("Using FAILSAFE_GROUND_DETECT")
|
||||||
if (autopilot_mode == AP_MODE_FAILSAFE && autopilot_detect_ground) {
|
if (autopilot_mode == AP_MODE_FAILSAFE && autopilot_ground_detected) {
|
||||||
autopilot_set_mode(AP_MODE_KILL);
|
autopilot_set_mode(AP_MODE_KILL);
|
||||||
autopilot_detect_ground = FALSE;
|
autopilot_ground_detected = FALSE;
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|||||||
@@ -69,7 +69,7 @@ extern void autopilot_set_mode(uint8_t new_autopilot_mode);
|
|||||||
extern void autopilot_set_motors_on(bool_t motors_on);
|
extern void autopilot_set_motors_on(bool_t motors_on);
|
||||||
extern void autopilot_check_in_flight(bool_t motors_on);
|
extern void autopilot_check_in_flight(bool_t motors_on);
|
||||||
|
|
||||||
extern bool_t autopilot_detect_ground;
|
extern bool_t autopilot_ground_detected;
|
||||||
extern bool_t autopilot_detect_ground_once;
|
extern bool_t autopilot_detect_ground_once;
|
||||||
|
|
||||||
extern uint16_t autopilot_flight_time;
|
extern uint16_t autopilot_flight_time;
|
||||||
@@ -152,7 +152,7 @@ static inline void DetectGroundEvent(void) {
|
|||||||
struct NedCoor_f* accel = stateGetAccelNed_f();
|
struct NedCoor_f* accel = stateGetAccelNed_f();
|
||||||
if (accel->z < -THRESHOLD_GROUND_DETECT ||
|
if (accel->z < -THRESHOLD_GROUND_DETECT ||
|
||||||
accel->z > THRESHOLD_GROUND_DETECT) {
|
accel->z > THRESHOLD_GROUND_DETECT) {
|
||||||
autopilot_detect_ground = TRUE;
|
autopilot_ground_detected = TRUE;
|
||||||
autopilot_detect_ground_once = FALSE;
|
autopilot_detect_ground_once = FALSE;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -336,8 +336,8 @@ void navigation_update_wp_from_speed(uint8_t wp, struct Int16Vect3 speed_sp, int
|
|||||||
}
|
}
|
||||||
|
|
||||||
bool_t nav_detect_ground(void) {
|
bool_t nav_detect_ground(void) {
|
||||||
if (!autopilot_detect_ground) return FALSE;
|
if (!autopilot_ground_detected) return FALSE;
|
||||||
autopilot_detect_ground = FALSE;
|
autopilot_ground_detected = FALSE;
|
||||||
return TRUE;
|
return TRUE;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
Reference in New Issue
Block a user