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

[autopilot] send AP status at every changes (#3308)

Browse Source 
- works for all firmwares implementing the autopilot_send_mode function
- avoid to wait the next telemetry message to have the notification
This commit is contained in:
Gautier Hattenberger
2024-06-15 11:29:50 +02:00
committed by GitHub
parent 3c85bdfaf3
commit 7adcd00fd7
3 changed files with 19 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/autopilot.c
+6 -1
View File
@@ -198,7 +198,12 @@ bool autopilot_set_mode(uint8_t new_autopilot_mode)
#else #else
autopilot_static_set_mode(new_autopilot_mode); autopilot_static_set_mode(new_autopilot_mode);
#endif #endif
return (autopilot.mode != mode); if (autopilot.mode != mode) {
autopilot_send_mode();
return true;
} else {
return false;
}
} }
/** AP mode setting handler /** AP mode setting handler
sw/airborne/firmwares/fixedwing/autopilot_static.c
+5 -13
View File
@@ -215,18 +215,15 @@ void navigation_task(void)
*/ */
void autopilot_failsafe_checks(void) void autopilot_failsafe_checks(void)
{ {
uint8_t mode_changed = false;
#if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1 #if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1
/* check normal mode from RC channel(s) /* check normal mode from RC channel(s)
*/ */
if (!RadioControlIsLost()) { if (!RadioControlIsLost()) {
bool pprz_mode_changed = pprz_mode_update(); bool pprz_mode_changed = pprz_mode_update();
mode_changed |= pprz_mode_changed;
#if defined RADIO_CALIB && defined RADIO_CONTROL_SETTINGS #if defined RADIO_CALIB && defined RADIO_CONTROL_SETTINGS
bool calib_mode_changed = RcSettingsModeUpdate(radio_control.values); bool calib_mode_changed = RcSettingsModeUpdate(radio_control.values);
rc_settings(calib_mode_changed || pprz_mode_changed); rc_settings(calib_mode_changed || pprz_mode_changed);
mode_changed |= calib_mode_changed; calib_mode_changed;
#endif #endif
} }
@@ -237,7 +234,7 @@ void autopilot_failsafe_checks(void)
if (RadioControlIsLost() && if (RadioControlIsLost() &&
(autopilot_get_mode() == AP_MODE_AUTO1 || (autopilot_get_mode() == AP_MODE_AUTO1 ||
autopilot_get_mode() == AP_MODE_MANUAL)) { autopilot_get_mode() == AP_MODE_MANUAL)) {
mode_changed |= autopilot_set_mode(RC_LOST_MODE); autopilot_set_mode(RC_LOST_MODE);
} }
/* Check RC kill switch /* Check RC kill switch
@@ -274,12 +271,12 @@ void autopilot_failsafe_checks(void)
if (autopilot_get_mode() == AP_MODE_AUTO2 || if (autopilot_get_mode() == AP_MODE_AUTO2 ||
autopilot_get_mode() == AP_MODE_HOME) { autopilot_get_mode() == AP_MODE_HOME) {
last_pprz_mode = autopilot_get_mode(); last_pprz_mode = autopilot_get_mode();
mode_changed |= autopilot_set_mode(AP_MODE_GPS_OUT_OF_ORDER); autopilot_set_mode(AP_MODE_GPS_OUT_OF_ORDER);
gps_lost = true; gps_lost = true;
} }
} else if (gps_lost) { /* GPS is ok */ } else if (gps_lost) { /* GPS is ok */
/** If aircraft was in failsafe mode, come back in previous mode */ /** If aircraft was in failsafe mode, come back in previous mode */
mode_changed |= autopilot_set_mode(last_pprz_mode); autopilot_set_mode(last_pprz_mode);
gps_lost = false; gps_lost = false;
} }
} }
@@ -291,14 +288,9 @@ void autopilot_failsafe_checks(void)
autopilot_get_mode() != AP_MODE_GPS_OUT_OF_ORDER && autopilot_get_mode() != AP_MODE_GPS_OUT_OF_ORDER &&
autopilot.launch) { autopilot.launch) {
if (too_far_from_home || datalink_lost() || higher_than_max_altitude()) { if (too_far_from_home || datalink_lost() || higher_than_max_altitude()) {
mode_changed |= autopilot_set_mode(AP_MODE_HOME); autopilot_set_mode(AP_MODE_HOME);
} }
} }
// send new mode if needed
if (mode_changed) {
autopilot_send_mode();
}
} }
sw/airborne/firmwares/rotorcraft/autopilot_firmware.c
+8 -1
View File
@@ -143,7 +143,7 @@ static void send_energy(struct transport_tx *trans, struct link_device *dev)
if(electrical.avg_cnt != 0) { if(electrical.avg_cnt != 0) {
avg_power = (float)electrical.avg_power / electrical.avg_cnt; avg_power = (float)electrical.avg_power / electrical.avg_cnt;
} }
pprz_msg_send_ENERGY(trans, dev, AC_ID, pprz_msg_send_ENERGY(trans, dev, AC_ID,
&throttle, &electrical.vsupply, &electrical.current, &power, &avg_power, &electrical.charge, &electrical.energy); &throttle, &electrical.vsupply, &electrical.current, &power, &avg_power, &electrical.charge, &electrical.energy);
} }
@@ -258,6 +258,13 @@ void autopilot_firmware_init(void)
#endif #endif
} }
void autopilot_send_mode(void)
{
#if DOWNLINK
send_status(&(DefaultChannel).trans_tx, &(DefaultDevice).device);
#endif
}
/** autopilot event function /** autopilot event function
* *
* used for automatic ground detection * used for automatic ground detection