diff --git a/sw/airborne/autopilot.h b/sw/airborne/autopilot.h index 3a051e3ea7..ae727ac93a 100644 --- a/sw/airborne/autopilot.h +++ b/sw/airborne/autopilot.h @@ -59,6 +59,7 @@ struct pprz_autopilot { uint8_t mode; ///< current autopilot mode uint8_t mode_auto2; ///< FIXME hide this in a private part ? uint16_t flight_time; ///< flight time in seconds + uint8_t arming_status; /// arming status bool motors_on; ///< motor status bool kill_throttle; ///< allow autopilot to use throttle bool in_flight; ///< in flight status diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_arming_throttle.h b/sw/airborne/firmwares/rotorcraft/autopilot_arming_throttle.h index 267710ec0f..2d3444c97e 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot_arming_throttle.h +++ b/sw/airborne/firmwares/rotorcraft/autopilot_arming_throttle.h @@ -30,6 +30,7 @@ #define AUTOPILOT_ARMING_THROTTLE_H #include "autopilot_rc_helpers.h" +#include "autopilot_firmware.h" #define AUTOPILOT_ARMING_DELAY 10 @@ -64,6 +65,31 @@ static inline void autopilot_arming_set(bool motors_on) } } +/** Checks all arm requirements and returns true if OK and false otherwise. + * Also sets the arming status to provide information to the user + * + * @return true if arming checks are all valid + */ +static inline bool autopilot_arming_check_valid(void) +{ + if (!PITCH_STICK_CENTERED()) { + autopilot.arming_status = AP_ARMING_STATUS_PITCH_NOT_CENTERED; + } else if (!ROLL_STICK_CENTERED()) { + autopilot.arming_status = AP_ARMING_STATUS_ROLL_NOT_CENTERED; + } else if (!YAW_STICK_CENTERED()) { + autopilot.arming_status = AP_ARMING_STATUS_YAW_NOT_CENTERED; + } else if (autopilot_get_mode() != MODE_MANUAL) { + autopilot.arming_status = AP_ARMING_STATUS_NOT_MODE_MANUAL; + } else if (autopilot_unarmed_in_auto) { + autopilot.arming_status = AP_ARMING_STATUS_UNARMED_IN_AUTO; + } else if (THROTTLE_STICK_DOWN()) { + autopilot.arming_status = AP_ARMING_STATUS_THROTTLE_DOWN; + } else { + return true; // all checks valid + } + return false; // one of the checks failed +} + /** * State machine to check if motors should be turned ON or OFF. * - automatically unkill when applying throttle @@ -87,35 +113,37 @@ static inline void autopilot_arming_check_motors_on(void) autopilot_arming_state = STATE_WAITING; } break; - case STATE_WAITING: + case STATE_WAITING: // after startup wait until throttle is down before attempting to arm autopilot.motors_on = false; autopilot_arming_delay_counter = 0; if (THROTTLE_STICK_DOWN()) { autopilot_arming_state = STATE_MOTORS_OFF_READY; + } else { + autopilot.arming_status = AP_ARMING_STATUS_THROTTLE_NOT_DOWN; } break; case STATE_MOTORS_OFF_READY: autopilot.motors_on = false; autopilot_arming_delay_counter = 0; - if (!THROTTLE_STICK_DOWN() && - rc_attitude_sticks_centered() && - (autopilot_get_mode() == MODE_MANUAL || autopilot_unarmed_in_auto)) { + if (autopilot_arming_check_valid()) { autopilot_arming_state = STATE_ARMING; } break; case STATE_ARMING: autopilot.motors_on = false; + autopilot.arming_status = AP_ARMING_STATUS_ARMING; autopilot_arming_delay_counter++; - if (THROTTLE_STICK_DOWN() || - !rc_attitude_sticks_centered() || - (autopilot_get_mode() != MODE_MANUAL && !autopilot_unarmed_in_auto)) { + if (!autopilot_arming_check_valid()) { autopilot_arming_state = STATE_MOTORS_OFF_READY; } else if (autopilot_arming_delay_counter >= AUTOPILOT_ARMING_DELAY) { autopilot_arming_state = STATE_MOTORS_ON; + } else { + autopilot.arming_status = AP_ARMING_STATUS_ARMING; } break; case STATE_MOTORS_ON: autopilot.motors_on = true; + autopilot.arming_status = AP_ARMING_STATUS_ARMED; autopilot_arming_delay_counter = AUTOPILOT_ARMING_DELAY; if (THROTTLE_STICK_DOWN()) { autopilot_arming_state = STATE_UNARMING; @@ -123,6 +151,7 @@ static inline void autopilot_arming_check_motors_on(void) break; case STATE_UNARMING: autopilot.motors_on = true; + autopilot.arming_status = AP_ARMING_STATUS_DISARMING; autopilot_arming_delay_counter--; if (!THROTTLE_STICK_DOWN()) { autopilot_arming_state = STATE_MOTORS_ON; @@ -138,6 +167,8 @@ static inline void autopilot_arming_check_motors_on(void) default: break; } + } else { + autopilot.arming_status = AP_ARMING_STATUS_KILLED; } } diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_arming_yaw.h b/sw/airborne/firmwares/rotorcraft/autopilot_arming_yaw.h index 0efb8e23a3..0217e3068a 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot_arming_yaw.h +++ b/sw/airborne/firmwares/rotorcraft/autopilot_arming_yaw.h @@ -30,6 +30,7 @@ #define AUTOPILOT_ARMING_YAW_H #include "autopilot_rc_helpers.h" +#include "autopilot_firmware.h" /** Delay until motors are armed/disarmed. * In number of rc frames recieved. @@ -74,6 +75,34 @@ static inline void autopilot_arming_set(bool motors_on) } } +#define YAW_MUST_BE_CENTERED true +#define YAW_MUST_BE_PUSHED false +/** Checks all arm requirements and returns true if OK and false otherwise. + * Also sets the arming status to provide information to the user + * + * @param[in] yaw_must_be_centered check is vallid of a yaw stick centered (true) or pushed (false) + * @return true if arming checks are all valid + */ +static inline bool autopilot_arming_check_valid(bool yaw_must_be_centered) +{ + if (!THROTTLE_STICK_DOWN()) { + autopilot.arming_status = AP_ARMING_STATUS_THROTTLE_NOT_DOWN; + } else if (!PITCH_STICK_CENTERED()) { + autopilot.arming_status = AP_ARMING_STATUS_PITCH_NOT_CENTERED; + } else if (!ROLL_STICK_CENTERED()) { + autopilot.arming_status = AP_ARMING_STATUS_ROLL_NOT_CENTERED; + } else { + if (yaw_must_be_centered && !YAW_STICK_CENTERED()) { + autopilot.arming_status = AP_ARMING_STATUS_YAW_NOT_CENTERED; + } else if (!yaw_must_be_centered && YAW_STICK_CENTERED()) { + autopilot.arming_status = AP_ARMING_STATUS_YAW_CENTERED; + } else { + return true; // all checks valid + } + } + return false; // one of the checks failed +} + /** * State machine to check if motors should be turned ON or OFF. * The motors start/stop when pushing the yaw stick without throttle until #MOTOR_ARMING_DELAY is reached. @@ -87,7 +116,7 @@ static inline void autopilot_arming_check_motors_on(void) switch (autopilot_check_motor_status) { case STATUS_INITIALISE_RC: // Wait until RC is initialised (it being centered is a good pointer to this) - if (THROTTLE_STICK_DOWN() && YAW_STICK_CENTERED() && PITCH_STICK_CENTERED() && ROLL_STICK_CENTERED()) { + if (autopilot_arming_check_valid(YAW_MUST_BE_CENTERED)) { autopilot_check_motor_status = STATUS_MOTORS_OFF; } break; @@ -96,20 +125,22 @@ static inline void autopilot_arming_check_motors_on(void) //wait extra delay before enabling the normal arming state machine autopilot.motors_on = false; autopilot_motors_on_counter = 0; - if (THROTTLE_STICK_DOWN() && YAW_STICK_CENTERED()) { // stick released + if (autopilot_arming_check_valid(YAW_MUST_BE_CENTERED)) { autopilot_check_motor_status = STATUS_MOTORS_AUTOMATICALLY_OFF_SAFETY_WAIT; } break; case STATUS_MOTORS_AUTOMATICALLY_OFF_SAFETY_WAIT: - autopilot_motors_on_counter++; - if (autopilot_motors_on_counter >= MOTOR_ARMING_DELAY) { - autopilot_check_motor_status = STATUS_MOTORS_OFF; - } + autopilot_motors_on_counter++; + if (autopilot_motors_on_counter >= MOTOR_ARMING_DELAY) { + autopilot_check_motor_status = STATUS_MOTORS_OFF; + } else { + autopilot.arming_status = AP_ARMING_STATUS_WAITING; + } break; case STATUS_MOTORS_OFF: autopilot.motors_on = false; autopilot_motors_on_counter = 0; - if (THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED()) { // stick pushed + if (autopilot_arming_check_valid(YAW_MUST_BE_PUSHED)) { // stick pushed autopilot_check_motor_status = STATUS_M_OFF_STICK_PUSHED; } break; @@ -118,18 +149,21 @@ static inline void autopilot_arming_check_motors_on(void) autopilot_motors_on_counter++; if (autopilot_motors_on_counter >= MOTOR_ARMING_DELAY) { autopilot_check_motor_status = STATUS_START_MOTORS; - } else if (!(THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED())) { // stick released too soon + } else if (autopilot_arming_check_valid(YAW_MUST_BE_PUSHED)) { // stick released too soon autopilot_check_motor_status = STATUS_MOTORS_OFF; + } else { + autopilot.arming_status = AP_ARMING_STATUS_WAITING; } break; case STATUS_START_MOTORS: autopilot.motors_on = true; autopilot_motors_on_counter = MOTOR_ARMING_DELAY; - if (!(THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED())) { // wait until stick released + if (autopilot_arming_check_valid(YAW_MUST_BE_CENTERED)) { // wait until stick released autopilot_check_motor_status = STATUS_MOTORS_ON; } break; case STATUS_MOTORS_ON: + autopilot.arming_status = AP_ARMING_STATUS_ARMED; autopilot.motors_on = true; autopilot_motors_on_counter = MOTOR_ARMING_DELAY; if (THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED()) { // stick pushed @@ -143,18 +177,20 @@ static inline void autopilot_arming_check_motors_on(void) autopilot_check_motor_status = STATUS_STOP_MOTORS; } else if (!(THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED())) { // stick released too soon autopilot_check_motor_status = STATUS_MOTORS_ON; + } else { + autopilot.arming_status = AP_ARMING_STATUS_DISARMING; } break; case STATUS_STOP_MOTORS: autopilot.motors_on = false; autopilot_motors_on_counter = 0; - if (!(THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED())) { // wait until stick released - autopilot_check_motor_status = STATUS_MOTORS_OFF; - } + autopilot_check_motor_status = STATUS_MOTORS_OFF; break; default: break; } + } else { + autopilot.arming_status = AP_ARMING_STATUS_KILLED; } } diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_firmware.c b/sw/airborne/firmwares/rotorcraft/autopilot_firmware.c index 5b1475b536..3713c6fe61 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot_firmware.c +++ b/sw/airborne/firmwares/rotorcraft/autopilot_firmware.c @@ -97,12 +97,13 @@ static void send_status(struct transport_tx *trans, struct link_device *dev) #endif uint8_t in_flight = autopilot.in_flight; uint8_t motors_on = autopilot.motors_on; + uint8_t arming_status = autopilot.arming_status; uint16_t time_sec = sys_time.nb_sec; pprz_msg_send_ROTORCRAFT_STATUS(trans, dev, AC_ID, &imu_nb_err, &_motor_nb_err, &radio_control.status, &radio_control.frame_rate, &fix, &autopilot.mode, &in_flight, &motors_on, - &guidance_h.mode, &guidance_v_mode, + &arming_status, &guidance_h.mode, &guidance_v_mode, &electrical.vsupply, &time_sec); } @@ -149,10 +150,10 @@ static void send_fp_min(struct transport_tx *trans, struct link_device *dev) uint16_t gspeed = stateGetHorizontalSpeedNorm_f() / 100; #endif pprz_msg_send_ROTORCRAFT_FP_MIN(trans, dev, AC_ID, - &(stateGetPositionEnu_i()->x), - &(stateGetPositionEnu_i()->y), - &(stateGetPositionEnu_i()->z), - &gspeed); + &(stateGetPositionEnu_i()->x), + &(stateGetPositionEnu_i()->y), + &(stateGetPositionEnu_i()->z), + &gspeed); } #ifdef RADIO_CONTROL diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_generated.c b/sw/airborne/firmwares/rotorcraft/autopilot_generated.c index 821d308110..228be90f9f 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot_generated.c +++ b/sw/airborne/firmwares/rotorcraft/autopilot_generated.c @@ -122,6 +122,8 @@ void autopilot_generated_on_rc_frame(void) if (ap_ahrs_is_aligned()) { autopilot_arming_check_motors_on(); autopilot.kill_throttle = ! autopilot.motors_on; + } else { + autopilot.arming_status = AP_ARMING_STATUS_AHRS_NOT_ALLIGNED; } /* if not in FAILSAFE or HOME mode, read RC and set commands accordingly */ diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_static.c b/sw/airborne/firmwares/rotorcraft/autopilot_static.c index 0d5464295b..d780ccaa33 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot_static.c +++ b/sw/airborne/firmwares/rotorcraft/autopilot_static.c @@ -376,6 +376,8 @@ void autopilot_static_on_rc_frame(void) if (ap_ahrs_is_aligned()) { autopilot_arming_check_motors_on(); autopilot.kill_throttle = ! autopilot.motors_on; + } else { + autopilot.arming_status = AP_ARMING_STATUS_AHRS_NOT_ALLIGNED; } /* if not in FAILSAFE or HOME mode, read RC and set commands accordingly */ diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_static.h b/sw/airborne/firmwares/rotorcraft/autopilot_static.h index a76bba2ea8..7191091c7e 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot_static.h +++ b/sw/airborne/firmwares/rotorcraft/autopilot_static.h @@ -53,6 +53,27 @@ #define AP_MODE_FLIP 18 #define AP_MODE_GUIDED 19 + +/* Arming status*/ +#define AP_ARMING_STATUS_NO_RC 0 +#define AP_ARMING_STATUS_WAITING 1 +#define AP_ARMING_STATUS_ARMING 2 +#define AP_ARMING_STATUS_ARMED 3 +#define AP_ARMING_STATUS_DISARMING 4 +#define AP_ARMING_STATUS_KILLED 5 +#define AP_ARMING_STATUS_YAW_CENTERED 6 +#define AP_ARMING_STATUS_THROTTLE_DOWN 7 +#define AP_ARMING_STATUS_NOT_MODE_MANUAL 8 +#define AP_ARMING_STATUS_UNARMED_IN_AUTO 9 +#define AP_ARMING_STATUS_THROTTLE_NOT_DOWN 10 +#define AP_ARMING_STATUS_STICKS_NOT_CENTERED 11 +#define AP_ARMING_STATUS_PITCH_NOT_CENTERED 12 +#define AP_ARMING_STATUS_ROLL_NOT_CENTERED 13 +#define AP_ARMING_STATUS_YAW_NOT_CENTERED 14 +#define AP_ARMING_STATUS_AHRS_NOT_ALLIGNED 15 +#define AP_ARMING_STATUS_OUT_OF_GEOFENCE 16 +#define AP_ARMING_STATUS_LOW_BATTERY 17 + /** Default RC mode. */ #ifndef MODE_MANUAL diff --git a/sw/ext/pprzlink b/sw/ext/pprzlink index 25b7024ad4..7c7d0abe59 160000 --- a/sw/ext/pprzlink +++ b/sw/ext/pprzlink @@ -1 +1 @@ -Subproject commit 25b7024ad46964b4c99c998b75eca6db34ceab5b +Subproject commit 7c7d0abe596a1fee86295612cc9b7f4594c7e09d