Skip to content

Commit

Permalink
[check] Fix preflight check RC arming with unkill procedure
Browse files Browse the repository at this point in the history
  • Loading branch information
@fvantienen @dewagter
fvantienen authored and dewagter committed Nov 6, 2023
1 parent 148ccb9 commit 7276913
Show file tree
Hide file tree
Showing 5 changed files with 43 additions and 20 deletions.
17 changes: 17 additions & 0 deletions sw/airborne/autopilot.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -256,6 +256,23 @@ bool autopilot_set_motors_on(bool motors_on)
return true;
}

/** turn motors on/off during arming, not done automatically
* prevents takeoff with preflight checks
*/
bool autopilot_arming_motors_on(bool motors_on)
{
#if PREFLIGHT_CHECKS
// When we fail the preflight checks abort
if(motors_on && !preflight_check()) {
// Bypass the preflight checks even if they fail but still preform them
if(!preflight_bypass)
return false;
}
#endif
autopilot.motors_on = motors_on;
return true;
}

/** get motors status
*/
bool autopilot_get_motors_on(void)
Expand Down
6 changes: 6 additions & 0 deletions sw/airborne/autopilot.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -140,6 +140,12 @@ extern void autopilot_force_motors_on(bool motors_on);
*/
extern bool autopilot_set_motors_on(bool motors_on);

/**
* Start or stop the motors during arming
* May not happen when preflight checks are failing
*/
extern bool autopilot_arming_motors_on(bool motors_on);

/** Get motor status
*
* @return true if motors are running
Expand Down
10 changes: 5 additions & 5 deletions sw/airborne/firmwares/rotorcraft/autopilot_arming_switch.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -80,21 +80,21 @@ static inline void autopilot_arming_check_motors_on(void)
{
switch (autopilot_arming_state) {
case STATE_UNINIT:
autopilot_set_motors_on(false);
autopilot_arming_motors_on(false);
if (kill_switch_is_on()) {
autopilot_arming_state = STATE_STARTABLE;
} else {
autopilot_arming_state = STATE_WAITING;
}
break;
case STATE_WAITING:
autopilot_set_motors_on(false);
autopilot_arming_motors_on(false);
if (kill_switch_is_on()) {
autopilot_arming_state = STATE_STARTABLE;
}
break;
case STATE_STARTABLE:
autopilot_set_motors_on(false);
autopilot_arming_motors_on(false);
/* don't allow to start if in KILL mode or kill switch is on */
if (autopilot_get_mode() == AP_MODE_KILL || kill_switch_is_on()) {
break;
Expand All @@ -107,7 +107,7 @@ static inline void autopilot_arming_check_motors_on(void)
case STATE_MOTORS_ON:
if (kill_switch_is_on()) {
/* kill motors, go to startable state */
autopilot_set_motors_on(false);
autopilot_arming_motors_on(false);
autopilot_arming_state = STATE_STARTABLE;
/* if turned off in an AUTO mode, remember it so it can be turned on again in AUTO */
if (autopilot_get_mode() != MODE_MANUAL && autopilot_get_mode() != AP_MODE_KILL &&
Expand All @@ -117,7 +117,7 @@ static inline void autopilot_arming_check_motors_on(void)
autopilot_unarmed_in_auto = false;
}
} else {
autopilot_set_motors_on(true);
autopilot_arming_motors_on(true);
}
break;
default:
Expand Down
12 changes: 6 additions & 6 deletions sw/airborne/firmwares/rotorcraft/autopilot_arming_throttle.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -103,12 +103,12 @@ static inline void autopilot_arming_check_motors_on(void)

switch (autopilot_arming_state) {
case STATE_UNINIT:
autopilot_set_motors_on(false);
autopilot_arming_motors_on(false);
autopilot_arming_delay_counter = 0;
autopilot_arming_state = STATE_WAITING;
break;
case STATE_WAITING: // after startup wait until throttle is down before attempting to arm
autopilot_set_motors_on(false);
autopilot_arming_motors_on(false);
autopilot_arming_delay_counter = 0;
if (THROTTLE_STICK_DOWN()) {
autopilot_arming_state = STATE_MOTORS_OFF_READY;
Expand All @@ -117,14 +117,14 @@ static inline void autopilot_arming_check_motors_on(void)
}
break;
case STATE_MOTORS_OFF_READY:
autopilot_set_motors_on(false);
autopilot_arming_motors_on(false);
autopilot_arming_delay_counter = 0;
if (autopilot_arming_check_valid()) {
autopilot_arming_state = STATE_ARMING;
}
break;
case STATE_ARMING:
autopilot_set_motors_on(false);
autopilot_arming_motors_on(false);
autopilot_arming_delay_counter++;
if (!autopilot_arming_check_valid()) {
autopilot_arming_state = STATE_MOTORS_OFF_READY;
Expand All @@ -135,15 +135,15 @@ static inline void autopilot_arming_check_motors_on(void)
}
break;
case STATE_MOTORS_ON:
autopilot_set_motors_on(true);
autopilot_arming_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;
}
break;
case STATE_UNARMING:
autopilot_set_motors_on(true);
autopilot_arming_motors_on(true);
autopilot.arming_status = AP_ARMING_STATUS_DISARMING;
autopilot_arming_delay_counter--;
if (!THROTTLE_STICK_DOWN()) {
Expand Down
18 changes: 9 additions & 9 deletions sw/airborne/firmwares/rotorcraft/autopilot_arming_yaw.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -136,13 +136,13 @@ static inline void autopilot_arming_check_motors_on(void)
autopilot_check_motor_status = STATUS_MOTORS_ON;
} else {
autopilot_check_motor_status = STATUS_MOTORS_AUTOMATICALLY_OFF;
autopilot_set_motors_on(false);
autopilot_arming_motors_on(false);
}
break;
case STATUS_MOTORS_AUTOMATICALLY_OFF: // Motors were disarmed externally
//(possibly due to crash)
//wait extra delay before enabling the normal arming state machine
autopilot_set_motors_on(false);
autopilot_arming_motors_on(false);
autopilot_motors_on_counter = 0;
if (autopilot_arming_check_valid(YAW_MUST_BE_CENTERED)) {
autopilot_check_motor_status = STATUS_MOTORS_AUTOMATICALLY_OFF_SAFETY_WAIT;
Expand All @@ -157,15 +157,15 @@ static inline void autopilot_arming_check_motors_on(void)
}
break;
case STATUS_MOTORS_OFF:
autopilot_set_motors_on(false);
autopilot_arming_motors_on(false);
autopilot_motors_on_counter = 0;
autopilot.arming_status = AP_ARMING_STATUS_WAITING;
if (autopilot_arming_check_valid(YAW_MUST_BE_PUSHED)) { // stick pushed
autopilot_check_motor_status = STATUS_M_OFF_STICK_PUSHED;
}
break;
case STATUS_M_OFF_STICK_PUSHED:
autopilot_set_motors_on(false);
autopilot_arming_motors_on(false);
autopilot_motors_on_counter++;
if (autopilot_motors_on_counter >= MOTOR_ARMING_DELAY) {
autopilot_check_motor_status = STATUS_START_MOTORS;
Expand All @@ -176,7 +176,7 @@ static inline void autopilot_arming_check_motors_on(void)
}
break;
case STATUS_START_MOTORS:
autopilot_set_motors_on(true);
autopilot_arming_motors_on(true);
autopilot_motors_on_counter = MOTOR_ARMING_DELAY;
autopilot_set_in_flight(false); // stop fc from starting control (integration and yaw) till arm process is complete
if (YAW_STICK_CENTERED()) { // wait until stick released
Expand All @@ -185,14 +185,14 @@ static inline void autopilot_arming_check_motors_on(void)
break;
case STATUS_MOTORS_ON:
autopilot.arming_status = AP_ARMING_STATUS_ARMED;
autopilot_set_motors_on(true);
autopilot_arming_motors_on(true);
autopilot_motors_on_counter = MOTOR_ARMING_DELAY;
if (THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED()) { // stick pushed
autopilot_check_motor_status = STATUS_M_ON_STICK_PUSHED;
}
break;
case STATUS_M_ON_STICK_PUSHED:
autopilot_set_motors_on(true);
autopilot_arming_motors_on(true);
autopilot_motors_on_counter--;
if (autopilot_motors_on_counter == 0) {
autopilot_check_motor_status = STATUS_STOP_MOTORS;
Expand All @@ -203,7 +203,7 @@ static inline void autopilot_arming_check_motors_on(void)
}
break;
case STATUS_STOP_MOTORS:
autopilot_set_motors_on(false);
autopilot_arming_motors_on(false);
autopilot_motors_on_counter = 0;
if (autopilot_arming_check_valid(YAW_MUST_BE_CENTERED)) { // wait till release disarm stick before allowing to re-arm
autopilot_check_motor_status = STATUS_MOTORS_OFF;
Expand All @@ -215,7 +215,7 @@ static inline void autopilot_arming_check_motors_on(void)
} else {
autopilot.arming_status = AP_ARMING_STATUS_KILLED;
if (kill_switch_is_on()) {
autopilot_set_motors_on(false);
autopilot_arming_motors_on(false);
}
}
}
Expand Down

0 comments on commit 7276913

Please sign in to comment.