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

Added arming status msg to pprzlink

Browse Source 
Arm status msg in yaw arming, and require all sticks centered for arming
Arm status message for throttle arming
This commit is contained in:
kevindehecker
2018-01-07 13:37:35 +01:00
committed by Gautier Hattenberger
parent b18bcb2643
commit 2ebd36e08a
8 changed files with 119 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/autopilot.h
+1
View File
@@ -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
sw/airborne/firmwares/rotorcraft/autopilot_arming_throttle.h
+38 -7
View File
@@ -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;
}
}
sw/airborne/firmwares/rotorcraft/autopilot_arming_yaw.h
+48 -12
View File
@@ -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;
}
}
sw/airborne/firmwares/rotorcraft/autopilot_firmware.c
+6 -5
View File
@@ -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
sw/airborne/firmwares/rotorcraft/autopilot_generated.c
+2
View File
@@ -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 */
sw/airborne/firmwares/rotorcraft/autopilot_static.c
+2
View File
@@ -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 */
sw/airborne/firmwares/rotorcraft/autopilot_static.h
+21
View File
@@ -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
sw/ext/pprzlink
+1 -1
Submodule sw/ext/pprzlink updated: 25b7024ad4...7c7d0abe59