diff --git a/conf/airframes/ENAC/hoops_gen_ap.xml b/conf/airframes/ENAC/hoops_gen_ap.xml
new file mode 100644
index 0000000000..3b2a094a6a
--- /dev/null
+++ b/conf/airframes/ENAC/hoops_gen_ap.xml
@@ -0,0 +1,287 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/autopilot/autopilot.dtd b/conf/autopilot/autopilot.dtd
index b0105aecba..44075da7c1 100644
--- a/conf/autopilot/autopilot.dtd
+++ b/conf/autopilot/autopilot.dtd
@@ -1,10 +1,10 @@
-
+
-
+
@@ -16,6 +16,10 @@
+
+
+
+
@@ -24,13 +28,15 @@ name CDATA #IMPLIED>
name CDATA #REQUIRED
freq CDATA #REQUIRED
gcs_mode CDATA #IMPLIED
-settings_mode CDATA #IMPLIED>
+settings_mode CDATA #IMPLIED
+settings_handler CDATA #IMPLIED>
+name CDATA #REQUIRED>
+value CDATA #IMPLIED
+cond CDATA #IMPLIED>
+
+
+
+
+
+
+
+
diff --git a/conf/autopilot/rotorcraft_autopilot.xml b/conf/autopilot/rotorcraft_autopilot.xml
index 719556fc01..fa231dd574 100644
--- a/conf/autopilot/rotorcraft_autopilot.xml
+++ b/conf/autopilot/rotorcraft_autopilot.xml
@@ -2,7 +2,31 @@
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -10,36 +34,40 @@
-
-
+
+
+
+
+
+
+
-
-
+
+
-
-
+
-
+
-
+
-
+
-
+
@@ -47,48 +75,66 @@
-
-
-
-
+
+
+
-
-
-
-
-
-
-
+
+
+
+
-
+
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
-
-
+
+
+
-
+
-
+
+
+
+
+
+
+
+
+
+
+
-
diff --git a/conf/firmwares/rotorcraft.makefile b/conf/firmwares/rotorcraft.makefile
index f1d4918dbb..bfdbf7d4d8 100644
--- a/conf/firmwares/rotorcraft.makefile
+++ b/conf/firmwares/rotorcraft.makefile
@@ -122,7 +122,14 @@ else
ifneq ($(TARGET), fbw)
$(TARGET).srcs += $(SRC_FIRMWARE)/main.c
$(TARGET).srcs += $(SRC_FIRMWARE)/autopilot.c
+$(TARGET).srcs += $(SRC_FIRMWARE)/autopilot_utils.c
$(TARGET).srcs += $(SRC_FIRMWARE)/autopilot_guided.c
+ifeq ($(USE_GENERATED_AUTOPILOT), TRUE)
+$(TARGET).srcs += $(SRC_FIRMWARE)/autopilot_generated.c
+$(TARGET).CFLAGS += -DUSE_GENERATED_AUTOPILOT=1
+else
+$(TARGET).srcs += $(SRC_FIRMWARE)/autopilot_static.c
+endif
else
$(TARGET).srcs += $(SRC_FIRMWARE)/main_fbw.c
endif # TARGET == fbw
diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.c b/sw/airborne/firmwares/rotorcraft/autopilot.c
index 6e844de5e4..a026d6f240 100644
--- a/sw/airborne/firmwares/rotorcraft/autopilot.c
+++ b/sw/airborne/firmwares/rotorcraft/autopilot.c
@@ -1,5 +1,6 @@
/*
* Copyright (C) 2008-2012 The Paparazzi Team
+ * Copyright (C) 2016 Gautier Hattenberger
*
* This file is part of paparazzi.
*
@@ -105,62 +106,6 @@ bool autopilot_detect_ground_once;
#define AUTOPILOT_IN_FLIGHT_MIN_THRUST 500
#endif
-#ifndef AUTOPILOT_DISABLE_AHRS_KILL
-static inline int ahrs_is_aligned(void)
-{
- return stateIsAttitudeValid();
-}
-#else
-PRINT_CONFIG_MSG("Using AUTOPILOT_DISABLE_AHRS_KILL")
-static inline int ahrs_is_aligned(void)
-{
- return true;
-}
-#endif
-
-/** Set descent speed in failsafe mode */
-#ifndef FAILSAFE_DESCENT_SPEED
-#define FAILSAFE_DESCENT_SPEED 1.5
-PRINT_CONFIG_VAR(FAILSAFE_DESCENT_SPEED)
-#endif
-
-/** Mode that is set when the plane is really too far from home */
-#ifndef FAILSAFE_MODE_TOO_FAR_FROM_HOME
-#define FAILSAFE_MODE_TOO_FAR_FROM_HOME AP_MODE_FAILSAFE
-#endif
-
-
-#if USE_KILL_SWITCH_FOR_MOTOR_ARMING
-#include "autopilot_arming_switch.h"
-PRINT_CONFIG_MSG("Using kill switch for motor arming")
-#elif USE_THROTTLE_FOR_MOTOR_ARMING
-#include "autopilot_arming_throttle.h"
-PRINT_CONFIG_MSG("Using throttle for motor arming")
-#else
-#include "autopilot_arming_yaw.h"
-PRINT_CONFIG_MSG("Using 2 sec yaw for motor arming")
-#endif
-
-#ifndef MODE_STARTUP
-#define MODE_STARTUP AP_MODE_KILL
-PRINT_CONFIG_MSG("Using default AP_MODE_KILL as MODE_STARTUP")
-#endif
-
-#ifndef UNLOCKED_HOME_MODE
-#if MODE_AUTO1 == AP_MODE_HOME
-#define UNLOCKED_HOME_MODE TRUE
-PRINT_CONFIG_MSG("Enabled UNLOCKED_HOME_MODE since MODE_AUTO1 is AP_MODE_HOME")
-#elif MODE_AUTO2 == AP_MODE_HOME
-#define UNLOCKED_HOME_MODE TRUE
-PRINT_CONFIG_MSG("Enabled UNLOCKED_HOME_MODE since MODE_AUTO2 is AP_MODE_HOME")
-#else
-#define UNLOCKED_HOME_MODE FALSE
-#endif
-#endif
-
-#if MODE_MANUAL == AP_MODE_NAV
-#error "MODE_MANUAL mustn't be AP_MODE_NAV"
-#endif
void send_autopilot_version(struct transport_tx *trans, struct link_device *dev)
{
@@ -304,8 +249,6 @@ static void send_rotorcraft_cmd(struct transport_tx *trans, struct link_device *
void autopilot_init(void)
{
- /* mode is finally set at end of init if MODE_STARTUP is not KILL */
- autopilot_mode = AP_MODE_KILL;
autopilot_motors_on = false;
kill_throttle = ! autopilot_motors_on;
autopilot_in_flight = false;
@@ -321,12 +264,11 @@ void autopilot_init(void)
gpio_clear(POWER_SWITCH_GPIO); // POWER OFF
#endif
- autopilot_arming_init();
-
+ // init GNC stack
+ // TODO this should be done in modules init
nav_init();
guidance_h_init();
guidance_v_init();
-
stabilization_init();
stabilization_none_init();
#if USE_STABILIZATION_RATE
@@ -334,9 +276,15 @@ void autopilot_init(void)
#endif
stabilization_attitude_init();
- /* set startup mode, propagates through to guidance h/v */
- autopilot_set_mode(MODE_STARTUP);
+ // call implementation init
+ // it will set startup mode
+#if USE_GENERATED_AUTOPILOT
+ autopilot_generated_init();
+#else
+ autopilot_static_init();
+#endif
+ // register messages
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AUTOPILOT_VERSION, send_autopilot_version);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ALIVE, send_alive);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_STATUS, send_status);
@@ -355,215 +303,65 @@ void autopilot_init(void)
#endif
}
-
-#define NAV_PRESCALER (PERIODIC_FREQUENCY / NAV_FREQ)
+/** AP periodic call
+ */
void autopilot_periodic(void)
{
-
- RunOnceEvery(NAV_PRESCALER, compute_dist2_to_home());
-
- if (autopilot_in_flight && autopilot_mode == AP_MODE_NAV) {
- if (too_far_from_home || datalink_lost() || higher_than_max_altitude()) {
- if (dist2_to_home > failsafe_mode_dist2) {
- autopilot_set_mode(FAILSAFE_MODE_TOO_FAR_FROM_HOME);
- } else {
- autopilot_set_mode(AP_MODE_HOME);
- }
- }
- }
-
- if (autopilot_mode == AP_MODE_HOME) {
- RunOnceEvery(NAV_PRESCALER, nav_home());
- } else {
- // otherwise always call nav_periodic_task so that carrot is always updated in GCS for other modes
- RunOnceEvery(NAV_PRESCALER, nav_periodic_task());
- }
-
-
- /* If in FAILSAFE mode and either already not in_flight anymore
- * or just "detected" ground, go to KILL mode.
- */
- if (autopilot_mode == AP_MODE_FAILSAFE) {
- if (!autopilot_in_flight) {
- autopilot_set_mode(AP_MODE_KILL);
- }
-
-#if FAILSAFE_GROUND_DETECT
- INFO("Using FAILSAFE_GROUND_DETECT: KILL")
- if (autopilot_ground_detected) {
- autopilot_set_mode(AP_MODE_KILL);
- }
+#if USE_GENERATED_AUTOPILOT
+ autopilot_generated_periodic();
+#else
+ autopilot_static_periodic();
#endif
- }
-
- /* Reset ground detection _after_ running flight plan
- */
- if (!autopilot_in_flight) {
- autopilot_ground_detected = false;
- autopilot_detect_ground_once = false;
- }
-
- /* Set fixed "failsafe" commands from airframe file if in KILL mode.
- * If in FAILSAFE mode, run normal loops with failsafe attitude and
- * downwards velocity setpoints.
- */
- if (autopilot_mode == AP_MODE_KILL) {
- SetCommands(commands_failsafe);
- } else {
- guidance_v_run(autopilot_in_flight);
- guidance_h_run(autopilot_in_flight);
- SetRotorcraftCommands(stabilization_cmd, autopilot_in_flight, autopilot_motors_on);
- }
+}
+/** set autopilot mode
+ */
+void autopilot_set_mode(uint8_t new_autopilot_mode)
+{
+#if USE_GENERATED_AUTOPILOT
+ autopilot_generated_set_mode(new_autopilot_mode);
+#else
+ autopilot_static_set_mode(new_autopilot_mode);
+#endif
}
/** AP mode setting handler
- *
- * Checks RC status before calling autopilot_set_mode function
*/
void autopilot_SetModeHandler(float mode)
{
- if (mode == AP_MODE_KILL || mode == AP_MODE_FAILSAFE || mode == AP_MODE_HOME) {
- // safety modes are always accessible via settings
- autopilot_set_mode(mode);
- } else {
- if (radio_control.status != RC_OK &&
- (mode == AP_MODE_NAV || mode == AP_MODE_GUIDED || mode == AP_MODE_FLIP || mode == AP_MODE_MODULE)) {
- // without RC, only nav-like modes are accessible
- autopilot_set_mode(mode);
- }
- }
- // with RC, other modes can only be changed from the RC
-}
-
-
-void autopilot_set_mode(uint8_t new_autopilot_mode)
-{
-
- /* force startup mode (default is kill) as long as AHRS is not aligned */
- if (!ahrs_is_aligned()) {
- new_autopilot_mode = MODE_STARTUP;
- }
-
- if (new_autopilot_mode != autopilot_mode) {
- /* horizontal mode */
- switch (new_autopilot_mode) {
- case AP_MODE_FAILSAFE:
-#ifndef KILL_AS_FAILSAFE
- stabilization_attitude_set_failsafe_setpoint();
- guidance_h_mode_changed(GUIDANCE_H_MODE_ATTITUDE);
- break;
-#endif
- case AP_MODE_KILL:
- autopilot_in_flight = false;
- autopilot_in_flight_counter = 0;
- guidance_h_mode_changed(GUIDANCE_H_MODE_KILL);
- break;
- case AP_MODE_RC_DIRECT:
- guidance_h_mode_changed(GUIDANCE_H_MODE_RC_DIRECT);
- break;
- case AP_MODE_RATE_RC_CLIMB:
- case AP_MODE_RATE_DIRECT:
- case AP_MODE_RATE_Z_HOLD:
-#if USE_STABILIZATION_RATE
- guidance_h_mode_changed(GUIDANCE_H_MODE_RATE);
+#if USE_GENERATED_AUTOPILOT
+ autopilot_generated_SetModeHandler(mode);
#else
- return;
+ autopilot_static_SetModeHandler(mode);
#endif
- break;
- case AP_MODE_ATTITUDE_RC_CLIMB:
- case AP_MODE_ATTITUDE_DIRECT:
- case AP_MODE_ATTITUDE_CLIMB:
- case AP_MODE_ATTITUDE_Z_HOLD:
- guidance_h_mode_changed(GUIDANCE_H_MODE_ATTITUDE);
- break;
- case AP_MODE_FORWARD:
- guidance_h_mode_changed(GUIDANCE_H_MODE_FORWARD);
- break;
- case AP_MODE_CARE_FREE_DIRECT:
- guidance_h_mode_changed(GUIDANCE_H_MODE_CARE_FREE);
- break;
- case AP_MODE_HOVER_DIRECT:
- case AP_MODE_HOVER_CLIMB:
- case AP_MODE_HOVER_Z_HOLD:
- guidance_h_mode_changed(GUIDANCE_H_MODE_HOVER);
- break;
- case AP_MODE_HOME:
- case AP_MODE_NAV:
- guidance_h_mode_changed(GUIDANCE_H_MODE_NAV);
- break;
- case AP_MODE_MODULE:
-#ifdef GUIDANCE_H_MODE_MODULE_SETTING
- guidance_h_mode_changed(GUIDANCE_H_MODE_MODULE_SETTING);
-#endif
- break;
- case AP_MODE_FLIP:
- guidance_h_mode_changed(GUIDANCE_H_MODE_FLIP);
- break;
- case AP_MODE_GUIDED:
- guidance_h_mode_changed(GUIDANCE_H_MODE_GUIDED);
- break;
- default:
- break;
- }
- /* vertical mode */
- switch (new_autopilot_mode) {
- case AP_MODE_FAILSAFE:
-#ifndef KILL_AS_FAILSAFE
- guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB);
- guidance_v_zd_sp = SPEED_BFP_OF_REAL(FAILSAFE_DESCENT_SPEED);
- break;
-#endif
- case AP_MODE_KILL:
- autopilot_set_motors_on(FALSE);
- stabilization_cmd[COMMAND_THRUST] = 0;
- guidance_v_mode_changed(GUIDANCE_V_MODE_KILL);
- break;
- case AP_MODE_RC_DIRECT:
- case AP_MODE_RATE_DIRECT:
- case AP_MODE_ATTITUDE_DIRECT:
- case AP_MODE_HOVER_DIRECT:
- case AP_MODE_CARE_FREE_DIRECT:
- case AP_MODE_FORWARD:
- guidance_v_mode_changed(GUIDANCE_V_MODE_RC_DIRECT);
- break;
- case AP_MODE_RATE_RC_CLIMB:
- case AP_MODE_ATTITUDE_RC_CLIMB:
- guidance_v_mode_changed(GUIDANCE_V_MODE_RC_CLIMB);
- break;
- case AP_MODE_ATTITUDE_CLIMB:
- case AP_MODE_HOVER_CLIMB:
- guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB);
- break;
- case AP_MODE_RATE_Z_HOLD:
- case AP_MODE_ATTITUDE_Z_HOLD:
- case AP_MODE_HOVER_Z_HOLD:
- guidance_v_mode_changed(GUIDANCE_V_MODE_HOVER);
- break;
- case AP_MODE_HOME:
- case AP_MODE_NAV:
- guidance_v_mode_changed(GUIDANCE_V_MODE_NAV);
- break;
- case AP_MODE_MODULE:
-#ifdef GUIDANCE_V_MODE_MODULE_SETTING
- guidance_v_mode_changed(GUIDANCE_V_MODE_MODULE_SETTING);
-#endif
- break;
- case AP_MODE_FLIP:
- guidance_v_mode_changed(GUIDANCE_V_MODE_FLIP);
- break;
- case AP_MODE_GUIDED:
- guidance_v_mode_changed(GUIDANCE_V_MODE_GUIDED);
- break;
- default:
- break;
- }
- //if switching to rate mode but rate mode is not defined, the function returned
- autopilot_mode = new_autopilot_mode;
- }
}
+/** RC frame handler
+ */
+void autopilot_on_rc_frame(void)
+{
+#if USE_GENERATED_AUTOPILOT
+ autopilot_generated_on_rc_frame();
+#else
+ autopilot_static_on_rc_frame();
+#endif
+}
+/** turn motors on/off, eventually depending of the current mode
+ * set kill_throttle accordingly
+ */
+void autopilot_set_motors_on(bool motors_on)
+{
+#if USE_GENERATED_AUTOPILOT
+ autopilot_generated_set_motors_on(motors_on);
+#else
+ autopilot_static_set_motors_on(motors_on);
+#endif
+ kill_throttle = ! autopilot_motors_on;
+}
+
+/** in flight check utility function
+ */
void autopilot_check_in_flight(bool motors_on)
{
if (autopilot_in_flight) {
@@ -598,153 +396,3 @@ void autopilot_check_in_flight(bool motors_on)
}
}
-
-void autopilot_set_motors_on(bool motors_on)
-{
- if (autopilot_mode != AP_MODE_KILL && ahrs_is_aligned() && motors_on) {
- autopilot_motors_on = true;
- } else {
- autopilot_motors_on = false;
- }
- kill_throttle = ! autopilot_motors_on;
- autopilot_arming_set(autopilot_motors_on);
-}
-
-#if defined RADIO_MODE_2x3
-
-#define THRESHOLD_1d3_PPRZ (MAX_PPRZ / 3)
-#define THRESHOLD_2d3_PPRZ ((MAX_PPRZ / 3) * 2)
-/** Get autopilot mode as set by a RADIO_MODE 3-way switch and a 2-way switch, which are mixed together
- * The 2 way switch negates the value, the 3 way switch changes in three steps from 0 - MAX_PPRZ.
- * E.g. SW_1 has two positions (On/Off), SW_Mode has three positions (M1/M2/M3)
- * 1 Mode value
- * Off M1 -9500
- * Off M2 -4800
- * Off M3 -1850
- * On M1 2100
- * On M2 4900
- * On M3 9600
- * This function filters out the effect of SW_1, such that a normal 3-way switch comes out.
-**/
-static uint8_t ap_mode_of_3x2way_switch(void)
-{
- int val = radio_control.values[RADIO_MODE];
- if (radio_control.values[RADIO_MODE] < 0) {
- val = MAX_PPRZ + val;
- }
- if (val < THRESHOLD_1d3_PPRZ) {
- return MODE_MANUAL;
- } else if (val < THRESHOLD_2d3_PPRZ) {
- return MODE_AUTO1;
- } else {
- return autopilot_mode_auto2;
- }
-}
-
-#else
-
-#define THRESHOLD_1_PPRZ (MIN_PPRZ / 2)
-#define THRESHOLD_2_PPRZ (MAX_PPRZ / 2)
-/** get autopilot mode as set by RADIO_MODE 3-way switch */
-static uint8_t ap_mode_of_3way_switch(void)
-{
- if (radio_control.values[RADIO_MODE] > THRESHOLD_2_PPRZ) {
- return autopilot_mode_auto2;
- } else if (radio_control.values[RADIO_MODE] > THRESHOLD_1_PPRZ) {
- return MODE_AUTO1;
- } else {
- return MODE_MANUAL;
- }
-}
-#endif
-
-/**
- * Get autopilot mode from two 2way switches.
- * RADIO_MODE switch just selectes between MANUAL and AUTO.
- * If not MANUAL, the RADIO_AUTO_MODE switch selects between AUTO1 and AUTO2.
- *
- * This is mainly a cludge for entry level radios with no three-way switch,
- * but two available two-way switches which can be used.
- */
-#if defined RADIO_AUTO_MODE || defined(__DOXYGEN__)
-static uint8_t ap_mode_of_two_switches(void)
-{
- if (radio_control.values[RADIO_MODE] < THRESHOLD_1_PPRZ) {
- /* RADIO_MODE in MANUAL position */
- return MODE_MANUAL;
- } else {
- /* RADIO_MODE not in MANUAL position.
- * Select AUTO mode bassed on RADIO_AUTO_MODE channel
- */
- if (radio_control.values[RADIO_AUTO_MODE] > THRESHOLD_2_PPRZ) {
- return autopilot_mode_auto2;
- } else {
- return MODE_AUTO1;
- }
- }
-}
-#endif
-
-void autopilot_on_rc_frame(void)
-{
-
- if (kill_switch_is_on()) {
- autopilot_set_mode(AP_MODE_KILL);
- } else {
-#ifdef RADIO_AUTO_MODE
- INFO("Using RADIO_AUTO_MODE to switch between AUTO1 and AUTO2.")
- uint8_t new_autopilot_mode = ap_mode_of_two_switches();
-#else
-#ifdef RADIO_MODE_2x3
- uint8_t new_autopilot_mode = ap_mode_of_3x2way_switch();
-#else
- uint8_t new_autopilot_mode = ap_mode_of_3way_switch();
-#endif
-#endif
-
- /* don't enter NAV mode if GPS is lost (this also prevents mode oscillations) */
- if (!(new_autopilot_mode == AP_MODE_NAV && GpsIsLost())) {
- /* always allow to switch to manual */
- if (new_autopilot_mode == MODE_MANUAL) {
- autopilot_set_mode(new_autopilot_mode);
- }
- /* if in HOME mode, don't allow switching to non-manual modes */
- else if ((autopilot_mode != AP_MODE_HOME)
-#if UNLOCKED_HOME_MODE
- /* Allowed to leave home mode when UNLOCKED_HOME_MODE */
- || !too_far_from_home
-#endif
- ) {
- autopilot_set_mode(new_autopilot_mode);
- }
- }
- }
-
- /* an arming sequence is used to start/stop motors.
- * only allow arming if ahrs is aligned
- */
- if (ahrs_is_aligned()) {
- autopilot_arming_check_motors_on();
- kill_throttle = ! autopilot_motors_on;
- }
-
- /* if not in FAILSAFE or HOME mode, read RC and set commands accordingly */
- if (autopilot_mode != AP_MODE_FAILSAFE && autopilot_mode != AP_MODE_HOME) {
-
- /* if there are some commands that should always be set from RC, do it */
-#ifdef SetAutoCommandsFromRC
- SetAutoCommandsFromRC(commands, radio_control.values);
-#endif
-
- /* if not in NAV_MODE set commands from the rc */
-#ifdef SetCommandsFromRC
- if (autopilot_mode != AP_MODE_NAV) {
- SetCommandsFromRC(commands, radio_control.values);
- }
-#endif
-
- guidance_v_read_rc();
- guidance_h_read_rc(autopilot_in_flight);
- }
-
-}
diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.h b/sw/airborne/firmwares/rotorcraft/autopilot.h
index 1632fecb0e..95643097fe 100644
--- a/sw/airborne/firmwares/rotorcraft/autopilot.h
+++ b/sw/airborne/firmwares/rotorcraft/autopilot.h
@@ -1,5 +1,6 @@
/*
* Copyright (C) 2008-2009 Antoine Drouin
+ * Copyright (C) 2016 Gautier Hattenberger
*
* This file is part of paparazzi.
*
@@ -22,7 +23,8 @@
/**
* @file firmwares/rotorcraft/autopilot.h
*
- * Autopilot modes.
+ * Core autopilot file.
+ * Using either static or generated autopilot logic.
*
*/
@@ -32,28 +34,19 @@
#include "std.h"
#include "generated/airframe.h"
#include "state.h"
-#include "autopilot_guided.h"
+#include "firmwares/rotorcraft/autopilot_utils.h"
-#define AP_MODE_KILL 0
-#define AP_MODE_FAILSAFE 1
-#define AP_MODE_HOME 2
-#define AP_MODE_RATE_DIRECT 3
-#define AP_MODE_ATTITUDE_DIRECT 4
-#define AP_MODE_RATE_RC_CLIMB 5
-#define AP_MODE_ATTITUDE_RC_CLIMB 6
-#define AP_MODE_ATTITUDE_CLIMB 7
-#define AP_MODE_RATE_Z_HOLD 8
-#define AP_MODE_ATTITUDE_Z_HOLD 9
-#define AP_MODE_HOVER_DIRECT 10
-#define AP_MODE_HOVER_CLIMB 11
-#define AP_MODE_HOVER_Z_HOLD 12
-#define AP_MODE_NAV 13
-#define AP_MODE_RC_DIRECT 14 // Safety Pilot Direct Commands for helicopter direct control
-#define AP_MODE_CARE_FREE_DIRECT 15
-#define AP_MODE_FORWARD 16
-#define AP_MODE_MODULE 17
-#define AP_MODE_FLIP 18
-#define AP_MODE_GUIDED 19
+// include static or generated autopilot
+// static version by default
+#ifndef USE_GENERATED_AUTOPILOT
+#define USE_GENERATED_AUTOPILOT FALSE
+#endif
+
+#if USE_GENERATED_AUTOPILOT
+#include "firmwares/rotorcraft/autopilot_generated.h"
+#else
+#include "firmwares/rotorcraft/autopilot_static.h"
+#endif
extern uint8_t autopilot_mode;
extern uint8_t autopilot_mode_auto2;
@@ -61,6 +54,7 @@ extern bool autopilot_motors_on;
extern bool autopilot_in_flight;
extern bool kill_throttle;
extern bool autopilot_rc;
+extern uint32_t autopilot_in_flight_counter;
extern bool autopilot_power_switch;
@@ -77,24 +71,11 @@ extern bool autopilot_detect_ground_once;
extern uint16_t autopilot_flight_time;
-/** Default RC mode.
- */
-#ifndef MODE_MANUAL
-#define MODE_MANUAL AP_MODE_ATTITUDE_DIRECT
-#endif
-#ifndef MODE_AUTO1
-#define MODE_AUTO1 AP_MODE_HOVER_Z_HOLD
-#endif
-#ifndef MODE_AUTO2
-#define MODE_AUTO2 AP_MODE_NAV
-#endif
-
-
#define autopilot_KillThrottle(_kill) { \
if (_kill) \
- autopilot_set_motors_on(FALSE); \
+ autopilot_set_motors_on(false); \
else \
- autopilot_set_motors_on(TRUE); \
+ autopilot_set_motors_on(true); \
}
#ifdef POWER_SWITCH_GPIO
@@ -110,39 +91,6 @@ extern uint16_t autopilot_flight_time;
}
#endif
-/** Set Rotorcraft commands.
- * Limit thrust and/or yaw depending of the in_flight
- * and motors_on flag status
- */
-#ifdef ROTORCRAFT_IS_HELI
-#define SetRotorcraftCommands(_cmd, _in_flight, _motor_on) { \
- commands[COMMAND_ROLL] = _cmd[COMMAND_ROLL]; \
- commands[COMMAND_PITCH] = _cmd[COMMAND_PITCH]; \
- commands[COMMAND_YAW] = _cmd[COMMAND_YAW]; \
- commands[COMMAND_THRUST] = _cmd[COMMAND_THRUST]; \
- }
-#else
-
-#ifndef ROTORCRAFT_COMMANDS_YAW_ALWAYS_ENABLED
-#define SetRotorcraftCommands(_cmd, _in_flight, _motor_on) { \
- if (!(_in_flight)) { _cmd[COMMAND_YAW] = 0; } \
- if (!(_motor_on)) { _cmd[COMMAND_THRUST] = 0; } \
- commands[COMMAND_ROLL] = _cmd[COMMAND_ROLL]; \
- commands[COMMAND_PITCH] = _cmd[COMMAND_PITCH]; \
- commands[COMMAND_YAW] = _cmd[COMMAND_YAW]; \
- commands[COMMAND_THRUST] = _cmd[COMMAND_THRUST]; \
- }
-#else
-#define SetRotorcraftCommands(_cmd, _in_flight, _motor_on) { \
- if (!(_motor_on)) { _cmd[COMMAND_THRUST] = 0; } \
- commands[COMMAND_ROLL] = _cmd[COMMAND_ROLL]; \
- commands[COMMAND_PITCH] = _cmd[COMMAND_PITCH]; \
- commands[COMMAND_YAW] = _cmd[COMMAND_YAW]; \
- commands[COMMAND_THRUST] = _cmd[COMMAND_THRUST]; \
- }
-#endif
-#endif
-
/** Z-acceleration threshold to detect ground in m/s^2 */
#ifndef THRESHOLD_GROUND_DETECT
#define THRESHOLD_GROUND_DETECT 25.0
@@ -151,7 +99,11 @@ extern uint16_t autopilot_flight_time;
*/
static inline void DetectGroundEvent(void)
{
- if (autopilot_mode == AP_MODE_FAILSAFE || autopilot_detect_ground_once) {
+ if (autopilot_detect_ground_once
+#ifdef AP_MODE_FAILSAFE
+ || autopilot_mode == AP_MODE_FAILSAFE
+#endif
+ ) {
struct NedCoor_f *accel = stateGetAccelNed_f();
if (accel->z < -THRESHOLD_GROUND_DETECT ||
accel->z > THRESHOLD_GROUND_DETECT) {
diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_generated.c b/sw/airborne/firmwares/rotorcraft/autopilot_generated.c
new file mode 100644
index 0000000000..853f779fcb
--- /dev/null
+++ b/sw/airborne/firmwares/rotorcraft/autopilot_generated.c
@@ -0,0 +1,153 @@
+/*
+ * Copyright (C) 2016 Gautier Hattenberger
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file firmwares/rotorcraft/autopilot_generated.c
+ *
+ * Generated autopilot implementation.
+ *
+ */
+#define AUTOPILOT_CORE_AP_C
+
+#include "firmwares/rotorcraft/autopilot_generated.h"
+#include "firmwares/rotorcraft/autopilot.h"
+
+#include "subsystems/radio_control.h"
+#include "subsystems/commands.h"
+#include "subsystems/actuators.h"
+#include "subsystems/settings.h"
+#include "subsystems/datalink/telemetry.h"
+
+#include "generated/settings.h"
+
+#if USE_KILL_SWITCH_FOR_MOTOR_ARMING
+#include "autopilot_arming_switch.h"
+PRINT_CONFIG_MSG("Using kill switch for motor arming")
+#elif USE_THROTTLE_FOR_MOTOR_ARMING
+#include "autopilot_arming_throttle.h"
+PRINT_CONFIG_MSG("Using throttle for motor arming")
+#else
+#include "autopilot_arming_yaw.h"
+PRINT_CONFIG_MSG("Using 2 sec yaw for motor arming")
+#endif
+
+/** Set descent speed in failsafe mode */
+#ifndef FAILSAFE_DESCENT_SPEED
+#define FAILSAFE_DESCENT_SPEED 1.5
+PRINT_CONFIG_VAR(FAILSAFE_DESCENT_SPEED)
+#endif
+
+
+void autopilot_generated_init(void)
+{
+ // call generated init
+ autopilot_core_ap_init();
+ // copy generated mode to public mode (set at startup)
+ autopilot_mode = autopilot_mode_ap;
+
+ // init arming
+ autopilot_arming_init();
+}
+
+
+void autopilot_generated_periodic(void)
+{
+
+ autopilot_core_ap_periodic_task();
+
+ // copy generated mode to public mode (may be changed on internal exceptions)
+ autopilot_mode = autopilot_mode_ap;
+
+ /* Reset ground detection _after_ running flight plan
+ */
+ if (!autopilot_in_flight) {
+ autopilot_ground_detected = false;
+ autopilot_detect_ground_once = false;
+ }
+
+}
+
+/** AP mode setting handler
+ *
+ * FIXME generated something else for this?
+ */
+void autopilot_generated_SetModeHandler(float mode)
+{
+ autopilot_generated_set_mode(mode);
+}
+
+
+void autopilot_generated_set_mode(uint8_t new_autopilot_mode)
+{
+ autopilot_core_ap_set_mode(new_autopilot_mode);
+ // copy generated mode to public mode
+ autopilot_mode = autopilot_mode_ap;
+}
+
+
+void autopilot_generated_set_motors_on(bool motors_on)
+{
+ if (ap_ahrs_is_aligned() && motors_on
+#ifdef AP_MODE_KILL
+ && autopilot_mode != AP_MODE_KILL
+#endif
+ ) {
+ autopilot_motors_on = true;
+ } else {
+ autopilot_motors_on = false;
+ }
+ autopilot_arming_set(autopilot_motors_on);
+}
+
+
+void autopilot_generated_on_rc_frame(void)
+{
+
+ // FIXME what to do here ?
+
+ /* an arming sequence is used to start/stop motors.
+ * only allow arming if ahrs is aligned
+ */
+ if (ap_ahrs_is_aligned()) {
+ autopilot_arming_check_motors_on();
+ kill_throttle = ! autopilot_motors_on;
+ }
+
+ /* if not in FAILSAFE or HOME mode, read RC and set commands accordingly */
+// if (autopilot_mode != AP_MODE_FAILSAFE && autopilot_mode != AP_MODE_HOME) {
+//
+// /* if there are some commands that should always be set from RC, do it */
+//#ifdef SetAutoCommandsFromRC
+// SetAutoCommandsFromRC(commands, radio_control.values);
+//#endif
+//
+// /* if not in NAV_MODE set commands from the rc */
+//#ifdef SetCommandsFromRC
+// if (autopilot_mode != AP_MODE_NAV) {
+// SetCommandsFromRC(commands, radio_control.values);
+// }
+//#endif
+//
+// guidance_v_read_rc();
+// guidance_h_read_rc(autopilot_in_flight);
+// }
+
+}
diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_generated.h b/sw/airborne/firmwares/rotorcraft/autopilot_generated.h
new file mode 100644
index 0000000000..4c864aa78c
--- /dev/null
+++ b/sw/airborne/firmwares/rotorcraft/autopilot_generated.h
@@ -0,0 +1,47 @@
+/*
+ * Copyright (C) 2008-2009 Antoine Drouin
+ * Copyright (C) 2016 Gautier Hattenberger
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file firmwares/rotorcraft/autopilot_generated.h
+ *
+ * Autopilot generated implementation
+ * Calls the code generated from autopilot XML file
+ *
+ */
+
+#ifndef AUTOPILOT_GENERATED_H
+#define AUTOPILOT_GENERATED_H
+
+#include "std.h"
+#include "generated/autopilot_core_ap.h"
+#include "generated/airframe.h"
+#include "state.h"
+
+extern void autopilot_generated_init(void);
+extern void autopilot_generated_periodic(void);
+extern void autopilot_generated_on_rc_frame(void);
+extern void autopilot_generated_set_mode(uint8_t new_autopilot_mode);
+extern void autopilot_generated_SetModeHandler(float new_autopilot_mode); // handler for dl_setting
+extern void autopilot_generated_set_motors_on(bool motors_on);
+
+#endif /* AUTOPILOT_GENERATED_H */
+
diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_rc_helpers.h b/sw/airborne/firmwares/rotorcraft/autopilot_rc_helpers.h
index bd9ca01467..3f7416026d 100644
--- a/sw/airborne/firmwares/rotorcraft/autopilot_rc_helpers.h
+++ b/sw/airborne/firmwares/rotorcraft/autopilot_rc_helpers.h
@@ -52,6 +52,33 @@
(radio_control.values[RADIO_ROLL] < AUTOPILOT_STICK_CENTER_THRESHOLD && \
radio_control.values[RADIO_ROLL] > -AUTOPILOT_STICK_CENTER_THRESHOLD)
+/** RC mode switch position helper
+ * switch positions threshold are evenly spaced
+ *
+ * @param[in] chan RC mode channel number
+ * @param[in] pos switch position to be tested
+ * @param[in] max maximum number of position of the switch
+ * @return true if current position is the same as requested (and RC status is OK)
+ */
+static inline bool rc_mode_switch(uint8_t chan, uint8_t pos, uint8_t max)
+{
+ if (radio_control.status != RC_OK) return false;
+ if (pos >= max) return false;
+ int32_t v = (int32_t)radio_control.values[chan] - MIN_PPRZ;
+ // round final value
+ int32_t p = (((((int32_t)max - 1) * 10 * v) / (MAX_PPRZ - MIN_PPRZ)) + 5) / 10;
+ Bound(p, 0, max - 1); // just in case
+ return pos == (uint8_t)p;
+}
+
+/** Convenience macro for 3way switch
+ */
+#ifdef RADIO_MODE
+#define RCMode0() rc_mode_switch(RADIO_MODE, 0, 3)
+#define RCMode1() rc_mode_switch(RADIO_MODE, 1, 3)
+#define RCMode2() rc_mode_switch(RADIO_MODE, 2, 3)
+#endif
+
static inline bool rc_attitude_sticks_centered(void)
{
return ROLL_STICK_CENTERED() && PITCH_STICK_CENTERED() && YAW_STICK_CENTERED();
diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_static.c b/sw/airborne/firmwares/rotorcraft/autopilot_static.c
new file mode 100644
index 0000000000..e3897befd4
--- /dev/null
+++ b/sw/airborne/firmwares/rotorcraft/autopilot_static.c
@@ -0,0 +1,405 @@
+/*
+ * Copyright (C) 2008-2012 The Paparazzi Team
+ * Copyright (C) 2016 Gautier Hattenberger
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file firmwares/rotorcraft/autopilot_static.c
+ *
+ * Static autopilot implementation.
+ *
+ */
+
+#include "firmwares/rotorcraft/autopilot.h"
+
+#include "subsystems/radio_control.h"
+#include "subsystems/commands.h"
+#include "subsystems/actuators.h"
+#include "subsystems/electrical.h"
+#include "subsystems/settings.h"
+#include "firmwares/rotorcraft/navigation.h"
+#include "firmwares/rotorcraft/guidance.h"
+
+#include "firmwares/rotorcraft/stabilization.h"
+#include "firmwares/rotorcraft/stabilization/stabilization_none.h"
+#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
+
+#if USE_STABILIZATION_RATE
+#include "firmwares/rotorcraft/stabilization/stabilization_rate.h"
+#endif
+
+#include "firmwares/rotorcraft/autopilot_rc_helpers.h"
+#include "firmwares/rotorcraft/autopilot_guided.h"
+
+#include "generated/settings.h"
+
+#if USE_GPS
+#include "subsystems/gps.h"
+#else
+#if NO_GPS_NEEDED_FOR_NAV
+#define GpsIsLost() FALSE
+#else
+#define GpsIsLost() TRUE
+#endif
+#endif
+
+#if USE_KILL_SWITCH_FOR_MOTOR_ARMING
+#include "autopilot_arming_switch.h"
+PRINT_CONFIG_MSG("Using kill switch for motor arming")
+#elif USE_THROTTLE_FOR_MOTOR_ARMING
+#include "autopilot_arming_throttle.h"
+PRINT_CONFIG_MSG("Using throttle for motor arming")
+#else
+#include "autopilot_arming_yaw.h"
+PRINT_CONFIG_MSG("Using 2 sec yaw for motor arming")
+#endif
+
+/* Geofence exceptions */
+#include "modules/nav/nav_geofence.h"
+
+/** Set descent speed in failsafe mode */
+#ifndef FAILSAFE_DESCENT_SPEED
+#define FAILSAFE_DESCENT_SPEED 1.5
+PRINT_CONFIG_VAR(FAILSAFE_DESCENT_SPEED)
+#endif
+
+/** Mode that is set when the plane is really too far from home */
+#ifndef FAILSAFE_MODE_TOO_FAR_FROM_HOME
+#define FAILSAFE_MODE_TOO_FAR_FROM_HOME AP_MODE_FAILSAFE
+#endif
+
+
+#ifndef MODE_STARTUP
+#define MODE_STARTUP AP_MODE_KILL
+PRINT_CONFIG_MSG("Using default AP_MODE_KILL as MODE_STARTUP")
+#endif
+
+#ifndef UNLOCKED_HOME_MODE
+#if MODE_AUTO1 == AP_MODE_HOME
+#define UNLOCKED_HOME_MODE TRUE
+PRINT_CONFIG_MSG("Enabled UNLOCKED_HOME_MODE since MODE_AUTO1 is AP_MODE_HOME")
+#elif MODE_AUTO2 == AP_MODE_HOME
+#define UNLOCKED_HOME_MODE TRUE
+PRINT_CONFIG_MSG("Enabled UNLOCKED_HOME_MODE since MODE_AUTO2 is AP_MODE_HOME")
+#else
+#define UNLOCKED_HOME_MODE FALSE
+#endif
+#endif
+
+#if MODE_MANUAL == AP_MODE_NAV
+#error "MODE_MANUAL mustn't be AP_MODE_NAV"
+#endif
+
+
+void autopilot_static_init(void)
+{
+ /* mode is finally set at end of init if MODE_STARTUP is not KILL */
+ autopilot_mode = AP_MODE_KILL; // really needed ?
+
+ /* set startup mode, propagates through to guidance h/v */
+ autopilot_static_set_mode(MODE_STARTUP);
+
+ /* init arming */
+ autopilot_arming_init();
+}
+
+
+#define NAV_PRESCALER (PERIODIC_FREQUENCY / NAV_FREQ)
+void autopilot_static_periodic(void)
+{
+
+ RunOnceEvery(NAV_PRESCALER, compute_dist2_to_home());
+
+ if (autopilot_in_flight && autopilot_mode == AP_MODE_NAV) {
+ if (too_far_from_home || datalink_lost() || higher_than_max_altitude()) {
+ if (dist2_to_home > failsafe_mode_dist2) {
+ autopilot_static_set_mode(FAILSAFE_MODE_TOO_FAR_FROM_HOME);
+ } else {
+ autopilot_static_set_mode(AP_MODE_HOME);
+ }
+ }
+ }
+
+ if (autopilot_mode == AP_MODE_HOME) {
+ RunOnceEvery(NAV_PRESCALER, nav_home());
+ } else {
+ // otherwise always call nav_periodic_task so that carrot is always updated in GCS for other modes
+ RunOnceEvery(NAV_PRESCALER, nav_periodic_task());
+ }
+
+
+ /* If in FAILSAFE mode and either already not in_flight anymore
+ * or just "detected" ground, go to KILL mode.
+ */
+ if (autopilot_mode == AP_MODE_FAILSAFE) {
+ if (!autopilot_in_flight) {
+ autopilot_static_set_mode(AP_MODE_KILL);
+ }
+
+#if FAILSAFE_GROUND_DETECT
+ INFO("Using FAILSAFE_GROUND_DETECT: KILL")
+ if (autopilot_ground_detected) {
+ autopilot_static_set_mode(AP_MODE_KILL);
+ }
+#endif
+ }
+
+ /* Reset ground detection _after_ running flight plan
+ */
+ if (!autopilot_in_flight) {
+ autopilot_ground_detected = false;
+ autopilot_detect_ground_once = false;
+ }
+
+ /* Set fixed "failsafe" commands from airframe file if in KILL mode.
+ * If in FAILSAFE mode, run normal loops with failsafe attitude and
+ * downwards velocity setpoints.
+ */
+ if (autopilot_mode == AP_MODE_KILL) {
+ SetCommands(commands_failsafe);
+ } else {
+ guidance_v_run(autopilot_in_flight);
+ guidance_h_run(autopilot_in_flight);
+ SetRotorcraftCommands(stabilization_cmd, autopilot_in_flight, autopilot_motors_on);
+ }
+
+}
+
+/** AP mode setting handler
+ *
+ * Checks RC status before calling autopilot_static_set_mode function
+ */
+void autopilot_static_SetModeHandler(float mode)
+{
+ if (mode == AP_MODE_KILL || mode == AP_MODE_FAILSAFE || mode == AP_MODE_HOME) {
+ // safety modes are always accessible via settings
+ autopilot_static_set_mode(mode);
+ } else {
+ if (radio_control.status != RC_OK &&
+ (mode == AP_MODE_NAV || mode == AP_MODE_GUIDED || mode == AP_MODE_FLIP || mode == AP_MODE_MODULE)) {
+ // without RC, only nav-like modes are accessible
+ autopilot_static_set_mode(mode);
+ }
+ }
+ // with RC, other modes can only be changed from the RC
+}
+
+
+void autopilot_static_set_mode(uint8_t new_autopilot_mode)
+{
+
+ /* force startup mode (default is kill) as long as AHRS is not aligned */
+ if (!ap_ahrs_is_aligned()) {
+ new_autopilot_mode = MODE_STARTUP;
+ }
+
+ if (new_autopilot_mode != autopilot_mode) {
+ /* horizontal mode */
+ switch (new_autopilot_mode) {
+ case AP_MODE_FAILSAFE:
+#ifndef KILL_AS_FAILSAFE
+ stabilization_attitude_set_failsafe_setpoint();
+ guidance_h_mode_changed(GUIDANCE_H_MODE_ATTITUDE);
+ break;
+#endif
+ case AP_MODE_KILL:
+ autopilot_in_flight = false;
+ autopilot_in_flight_counter = 0;
+ guidance_h_mode_changed(GUIDANCE_H_MODE_KILL);
+ break;
+ case AP_MODE_RC_DIRECT:
+ guidance_h_mode_changed(GUIDANCE_H_MODE_RC_DIRECT);
+ break;
+ case AP_MODE_RATE_RC_CLIMB:
+ case AP_MODE_RATE_DIRECT:
+ case AP_MODE_RATE_Z_HOLD:
+#if USE_STABILIZATION_RATE
+ guidance_h_mode_changed(GUIDANCE_H_MODE_RATE);
+#else
+ return;
+#endif
+ break;
+ case AP_MODE_ATTITUDE_RC_CLIMB:
+ case AP_MODE_ATTITUDE_DIRECT:
+ case AP_MODE_ATTITUDE_CLIMB:
+ case AP_MODE_ATTITUDE_Z_HOLD:
+ guidance_h_mode_changed(GUIDANCE_H_MODE_ATTITUDE);
+ break;
+ case AP_MODE_FORWARD:
+ guidance_h_mode_changed(GUIDANCE_H_MODE_FORWARD);
+ break;
+ case AP_MODE_CARE_FREE_DIRECT:
+ guidance_h_mode_changed(GUIDANCE_H_MODE_CARE_FREE);
+ break;
+ case AP_MODE_HOVER_DIRECT:
+ case AP_MODE_HOVER_CLIMB:
+ case AP_MODE_HOVER_Z_HOLD:
+ guidance_h_mode_changed(GUIDANCE_H_MODE_HOVER);
+ break;
+ case AP_MODE_HOME:
+ case AP_MODE_NAV:
+ guidance_h_mode_changed(GUIDANCE_H_MODE_NAV);
+ break;
+ case AP_MODE_MODULE:
+#ifdef GUIDANCE_H_MODE_MODULE_SETTING
+ guidance_h_mode_changed(GUIDANCE_H_MODE_MODULE_SETTING);
+#endif
+ break;
+ case AP_MODE_FLIP:
+ guidance_h_mode_changed(GUIDANCE_H_MODE_FLIP);
+ break;
+ case AP_MODE_GUIDED:
+ guidance_h_mode_changed(GUIDANCE_H_MODE_GUIDED);
+ break;
+ default:
+ break;
+ }
+ /* vertical mode */
+ switch (new_autopilot_mode) {
+ case AP_MODE_FAILSAFE:
+#ifndef KILL_AS_FAILSAFE
+ guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB);
+ guidance_v_zd_sp = SPEED_BFP_OF_REAL(FAILSAFE_DESCENT_SPEED);
+ break;
+#endif
+ case AP_MODE_KILL:
+ autopilot_set_motors_on(FALSE);
+ stabilization_cmd[COMMAND_THRUST] = 0;
+ guidance_v_mode_changed(GUIDANCE_V_MODE_KILL);
+ break;
+ case AP_MODE_RC_DIRECT:
+ case AP_MODE_RATE_DIRECT:
+ case AP_MODE_ATTITUDE_DIRECT:
+ case AP_MODE_HOVER_DIRECT:
+ case AP_MODE_CARE_FREE_DIRECT:
+ case AP_MODE_FORWARD:
+ guidance_v_mode_changed(GUIDANCE_V_MODE_RC_DIRECT);
+ break;
+ case AP_MODE_RATE_RC_CLIMB:
+ case AP_MODE_ATTITUDE_RC_CLIMB:
+ guidance_v_mode_changed(GUIDANCE_V_MODE_RC_CLIMB);
+ break;
+ case AP_MODE_ATTITUDE_CLIMB:
+ case AP_MODE_HOVER_CLIMB:
+ guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB);
+ break;
+ case AP_MODE_RATE_Z_HOLD:
+ case AP_MODE_ATTITUDE_Z_HOLD:
+ case AP_MODE_HOVER_Z_HOLD:
+ guidance_v_mode_changed(GUIDANCE_V_MODE_HOVER);
+ break;
+ case AP_MODE_HOME:
+ case AP_MODE_NAV:
+ guidance_v_mode_changed(GUIDANCE_V_MODE_NAV);
+ break;
+ case AP_MODE_MODULE:
+#ifdef GUIDANCE_V_MODE_MODULE_SETTING
+ guidance_v_mode_changed(GUIDANCE_V_MODE_MODULE_SETTING);
+#endif
+ break;
+ case AP_MODE_FLIP:
+ guidance_v_mode_changed(GUIDANCE_V_MODE_FLIP);
+ break;
+ case AP_MODE_GUIDED:
+ guidance_v_mode_changed(GUIDANCE_V_MODE_GUIDED);
+ break;
+ default:
+ break;
+ }
+ //if switching to rate mode but rate mode is not defined, the function returned
+ autopilot_mode = new_autopilot_mode;
+ }
+}
+
+
+void autopilot_static_set_motors_on(bool motors_on)
+{
+ if (autopilot_mode != AP_MODE_KILL && ap_ahrs_is_aligned() && motors_on) {
+ autopilot_motors_on = true;
+ } else {
+ autopilot_motors_on = false;
+ }
+ autopilot_arming_set(autopilot_motors_on);
+}
+
+void autopilot_static_on_rc_frame(void)
+{
+
+ if (kill_switch_is_on()) {
+ autopilot_static_set_mode(AP_MODE_KILL);
+ } else {
+#ifdef RADIO_AUTO_MODE
+ INFO("Using RADIO_AUTO_MODE to switch between AUTO1 and AUTO2.")
+ uint8_t new_autopilot_mode = ap_mode_of_two_switches();
+#else
+#ifdef RADIO_MODE_2x3
+ uint8_t new_autopilot_mode = ap_mode_of_3x2way_switch();
+#else
+ uint8_t new_autopilot_mode = ap_mode_of_3way_switch();
+#endif
+#endif
+
+ /* don't enter NAV mode if GPS is lost (this also prevents mode oscillations) */
+ if (!(new_autopilot_mode == AP_MODE_NAV && GpsIsLost())) {
+ /* always allow to switch to manual */
+ if (new_autopilot_mode == MODE_MANUAL) {
+ autopilot_static_set_mode(new_autopilot_mode);
+ }
+ /* if in HOME mode, don't allow switching to non-manual modes */
+ else if ((autopilot_mode != AP_MODE_HOME)
+#if UNLOCKED_HOME_MODE
+ /* Allowed to leave home mode when UNLOCKED_HOME_MODE */
+ || !too_far_from_home
+#endif
+ ) {
+ autopilot_static_set_mode(new_autopilot_mode);
+ }
+ }
+ }
+
+ /* an arming sequence is used to start/stop motors.
+ * only allow arming if ahrs is aligned
+ */
+ if (ap_ahrs_is_aligned()) {
+ autopilot_arming_check_motors_on();
+ kill_throttle = ! autopilot_motors_on;
+ }
+
+ /* if not in FAILSAFE or HOME mode, read RC and set commands accordingly */
+ if (autopilot_mode != AP_MODE_FAILSAFE && autopilot_mode != AP_MODE_HOME) {
+
+ /* if there are some commands that should always be set from RC, do it */
+#ifdef SetAutoCommandsFromRC
+ SetAutoCommandsFromRC(commands, radio_control.values);
+#endif
+
+ /* if not in NAV_MODE set commands from the rc */
+#ifdef SetCommandsFromRC
+ if (autopilot_mode != AP_MODE_NAV) {
+ SetCommandsFromRC(commands, radio_control.values);
+ }
+#endif
+
+ guidance_v_read_rc();
+ guidance_h_read_rc(autopilot_in_flight);
+ }
+
+}
+
diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_static.h b/sw/airborne/firmwares/rotorcraft/autopilot_static.h
new file mode 100644
index 0000000000..a76bba2ea8
--- /dev/null
+++ b/sw/airborne/firmwares/rotorcraft/autopilot_static.h
@@ -0,0 +1,77 @@
+/*
+ * Copyright (C) 2008-2009 Antoine Drouin
+ * Copyright (C) 2016 Gautier Hattenberger
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file firmwares/rotorcraft/autopilot_static.h
+ *
+ * Autopilot static implementation
+ *
+ */
+
+#ifndef AUTOPILOT_STATIC_H
+#define AUTOPILOT_STATIC_H
+
+/** Static autopilot modes
+ */
+#define AP_MODE_KILL 0
+#define AP_MODE_FAILSAFE 1
+#define AP_MODE_HOME 2
+#define AP_MODE_RATE_DIRECT 3
+#define AP_MODE_ATTITUDE_DIRECT 4
+#define AP_MODE_RATE_RC_CLIMB 5
+#define AP_MODE_ATTITUDE_RC_CLIMB 6
+#define AP_MODE_ATTITUDE_CLIMB 7
+#define AP_MODE_RATE_Z_HOLD 8
+#define AP_MODE_ATTITUDE_Z_HOLD 9
+#define AP_MODE_HOVER_DIRECT 10
+#define AP_MODE_HOVER_CLIMB 11
+#define AP_MODE_HOVER_Z_HOLD 12
+#define AP_MODE_NAV 13
+#define AP_MODE_RC_DIRECT 14 // Safety Pilot Direct Commands for helicopter direct control
+#define AP_MODE_CARE_FREE_DIRECT 15
+#define AP_MODE_FORWARD 16
+#define AP_MODE_MODULE 17
+#define AP_MODE_FLIP 18
+#define AP_MODE_GUIDED 19
+
+/** Default RC mode.
+ */
+#ifndef MODE_MANUAL
+#define MODE_MANUAL AP_MODE_ATTITUDE_DIRECT
+#endif
+#ifndef MODE_AUTO1
+#define MODE_AUTO1 AP_MODE_HOVER_Z_HOLD
+#endif
+#ifndef MODE_AUTO2
+#define MODE_AUTO2 AP_MODE_NAV
+#endif
+
+/** Specific function for static AP
+ */
+extern void autopilot_static_init(void);
+extern void autopilot_static_periodic(void);
+extern void autopilot_static_on_rc_frame(void);
+extern void autopilot_static_set_mode(uint8_t new_autopilot_mode);
+extern void autopilot_static_SetModeHandler(float new_autopilot_mode); // handler for dl_setting
+extern void autopilot_static_set_motors_on(bool motors_on);
+
+#endif /* AUTOPILOT_STATIC_H */
diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_utils.c b/sw/airborne/firmwares/rotorcraft/autopilot_utils.c
new file mode 100644
index 0000000000..a93cb178d0
--- /dev/null
+++ b/sw/airborne/firmwares/rotorcraft/autopilot_utils.c
@@ -0,0 +1,127 @@
+/*
+ * Copyright (C) 2016 Gautier Hattenberger
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file firmwares/rotorcraft/autopilot_utils.c
+ *
+ * Utility functions and includes for autopilots
+ *
+ */
+
+#include "firmwares/rotorcraft/autopilot_utils.h"
+#include "firmwares/rotorcraft/autopilot.h"
+#include "firmwares/rotorcraft/autopilot_rc_helpers.h"
+#include "state.h"
+#include "subsystems/radio_control.h"
+
+
+// Utility functions
+#ifndef AUTOPILOT_DISABLE_AHRS_KILL
+bool ap_ahrs_is_aligned(void)
+{
+ return stateIsAttitudeValid();
+}
+#else
+PRINT_CONFIG_MSG("Using AUTOPILOT_DISABLE_AHRS_KILL")
+bool ap_ahrs_is_aligned(void)
+{
+ return true;
+}
+#endif
+
+#if defined RADIO_MODE_2x3
+
+#define THRESHOLD_1d3_PPRZ (MAX_PPRZ / 3)
+#define THRESHOLD_2d3_PPRZ ((MAX_PPRZ / 3) * 2)
+/** Get autopilot mode as set by a RADIO_MODE 3-way switch and a 2-way switch, which are mixed together
+ * The 2 way switch negates the value, the 3 way switch changes in three steps from 0 - MAX_PPRZ.
+ * E.g. SW_1 has two positions (On/Off), SW_Mode has three positions (M1/M2/M3)
+ * 1 Mode value
+ * Off M1 -9500
+ * Off M2 -4800
+ * Off M3 -1850
+ * On M1 2100
+ * On M2 4900
+ * On M3 9600
+ * This function filters out the effect of SW_1, such that a normal 3-way switch comes out.
+**/
+uint8_t ap_mode_of_3x2way_switch(void)
+{
+ int val = radio_control.values[RADIO_MODE];
+ if (radio_control.values[RADIO_MODE] < 0) {
+ val = MAX_PPRZ + val;
+ }
+ if (val < THRESHOLD_1d3_PPRZ) {
+ return MODE_MANUAL;
+ } else if (val < THRESHOLD_2d3_PPRZ) {
+ return MODE_AUTO1;
+ } else {
+ return autopilot_mode_auto2;
+ }
+}
+
+#else
+
+#define THRESHOLD_1_PPRZ (MIN_PPRZ / 2)
+#define THRESHOLD_2_PPRZ (MAX_PPRZ / 2)
+
+/** get autopilot mode as set by RADIO_MODE 3-way switch */
+uint8_t ap_mode_of_3way_switch(void)
+{
+ if (radio_control.values[RADIO_MODE] > THRESHOLD_2_PPRZ) {
+ return autopilot_mode_auto2;
+ } else if (radio_control.values[RADIO_MODE] > THRESHOLD_1_PPRZ) {
+ return MODE_AUTO1;
+ } else {
+ return MODE_MANUAL;
+ }
+}
+
+#endif
+
+/**
+ * Get autopilot mode from two 2way switches.
+ * RADIO_MODE switch just selectes between MANUAL and AUTO.
+ * If not MANUAL, the RADIO_AUTO_MODE switch selects between AUTO1 and AUTO2.
+ *
+ * This is mainly a cludge for entry level radios with no three-way switch,
+ * but two available two-way switches which can be used.
+ */
+#if defined RADIO_AUTO_MODE || defined(__DOXYGEN__)
+uint8_t ap_mode_of_two_switches(void)
+{
+ if (radio_control.values[RADIO_MODE] < THRESHOLD_1_PPRZ) {
+ /* RADIO_MODE in MANUAL position */
+ return MODE_MANUAL;
+ } else {
+ /* RADIO_MODE not in MANUAL position.
+ * Select AUTO mode bassed on RADIO_AUTO_MODE channel
+ */
+ if (radio_control.values[RADIO_AUTO_MODE] > THRESHOLD_2_PPRZ) {
+ return autopilot_mode_auto2;
+ } else {
+ return MODE_AUTO1;
+ }
+ }
+}
+#endif
+
+
diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_utils.h b/sw/airborne/firmwares/rotorcraft/autopilot_utils.h
new file mode 100644
index 0000000000..97bbd9e6a5
--- /dev/null
+++ b/sw/airborne/firmwares/rotorcraft/autopilot_utils.h
@@ -0,0 +1,84 @@
+/*
+ * Copyright (C) 2016 Gautier Hattenberger
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file firmwares/rotorcraft/autopilot_utils.h
+ *
+ * Utility functions and includes for autopilots
+ *
+ */
+
+#ifndef AUTOPILOT_UTILS_H
+#define AUTOPILOT_UTILS_H
+
+#include "std.h"
+#include "subsystems/commands.h"
+
+/** Set descent speed in failsafe mode */
+#ifndef FAILSAFE_DESCENT_SPEED
+#define FAILSAFE_DESCENT_SPEED 1.5
+#endif
+
+extern bool ap_ahrs_is_aligned(void);
+#if defined RADIO_MODE_2x3
+extern uint8_t ap_mode_of_3x2way_switch(void);
+#else
+extern uint8_t ap_mode_of_3way_switch(void);
+#endif
+#if defined RADIO_AUTO_MODE || defined(__DOXYGEN__)
+extern uint8_t ap_mode_of_two_switches(void)
+#endif
+
+/** Set Rotorcraft commands.
+ * Limit thrust and/or yaw depending of the in_flight
+ * and motors_on flag status
+ */
+#ifdef ROTORCRAFT_IS_HELI
+#define SetRotorcraftCommands(_cmd, _in_flight, _motor_on) { \
+ commands[COMMAND_ROLL] = _cmd[COMMAND_ROLL]; \
+ commands[COMMAND_PITCH] = _cmd[COMMAND_PITCH]; \
+ commands[COMMAND_YAW] = _cmd[COMMAND_YAW]; \
+ commands[COMMAND_THRUST] = _cmd[COMMAND_THRUST]; \
+ }
+#else
+
+#ifndef ROTORCRAFT_COMMANDS_YAW_ALWAYS_ENABLED
+#define SetRotorcraftCommands(_cmd, _in_flight, _motor_on) { \
+ if (!(_in_flight)) { _cmd[COMMAND_YAW] = 0; } \
+ if (!(_motor_on)) { _cmd[COMMAND_THRUST] = 0; } \
+ commands[COMMAND_ROLL] = _cmd[COMMAND_ROLL]; \
+ commands[COMMAND_PITCH] = _cmd[COMMAND_PITCH]; \
+ commands[COMMAND_YAW] = _cmd[COMMAND_YAW]; \
+ commands[COMMAND_THRUST] = _cmd[COMMAND_THRUST]; \
+ }
+#else
+#define SetRotorcraftCommands(_cmd, _in_flight, _motor_on) { \
+ if (!(_motor_on)) { _cmd[COMMAND_THRUST] = 0; } \
+ commands[COMMAND_ROLL] = _cmd[COMMAND_ROLL]; \
+ commands[COMMAND_PITCH] = _cmd[COMMAND_PITCH]; \
+ commands[COMMAND_YAW] = _cmd[COMMAND_YAW]; \
+ commands[COMMAND_THRUST] = _cmd[COMMAND_THRUST]; \
+ }
+#endif
+#endif
+
+#endif // AUTOPILOT_UTILS_H
+
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
index 9a84134cda..e3742212c0 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
@@ -99,8 +99,6 @@ static void guidance_h_update_reference(void);
#if !GUIDANCE_INDI
static void guidance_h_traj_run(bool in_flight);
#endif
-static void guidance_h_hover_enter(void);
-static void guidance_h_nav_enter(void);
static inline void transition_run(void);
static void read_rc_setpoint_speed_i(struct Int32Vect2 *speed_sp, bool in_flight);
@@ -391,72 +389,11 @@ void guidance_h_run(bool in_flight)
/* fall trough to GUIDED to update ref, run traj and set final attitude setpoint */
case GUIDANCE_H_MODE_GUIDED:
- /* guidance_h.sp.pos and guidance_h.sp.heading need to be set from external source */
- if (!in_flight) {
- guidance_h_hover_enter();
- }
-
- guidance_h_update_reference();
-
-#if GUIDANCE_INDI
- guidance_indi_run(in_flight, guidance_h.sp.heading);
-#else
- /* compute x,y earth commands */
- guidance_h_traj_run(in_flight);
- /* set final attitude setpoint */
- stabilization_attitude_set_earth_cmd_i(&guidance_h_cmd_earth, guidance_h.sp.heading);
-#endif
- stabilization_attitude_run(in_flight);
+ guidance_h_guided_run(in_flight);
break;
case GUIDANCE_H_MODE_NAV:
- if (!in_flight) {
- guidance_h_nav_enter();
- }
-
- if (horizontal_mode == HORIZONTAL_MODE_MANUAL) {
- stabilization_cmd[COMMAND_ROLL] = nav_cmd_roll;
- stabilization_cmd[COMMAND_PITCH] = nav_cmd_pitch;
- stabilization_cmd[COMMAND_YAW] = nav_cmd_yaw;
- } else if (horizontal_mode == HORIZONTAL_MODE_ATTITUDE) {
- struct Int32Eulers sp_cmd_i;
- sp_cmd_i.phi = nav_roll;
- sp_cmd_i.theta = nav_pitch;
- sp_cmd_i.psi = nav_heading;
- stabilization_attitude_set_rpy_setpoint_i(&sp_cmd_i);
- stabilization_attitude_run(in_flight);
-
-#if HYBRID_NAVIGATION
- //make sure the heading is right before leaving horizontal_mode attitude
- guidance_hybrid_reset_heading(&sp_cmd_i);
-#endif
- } else {
-
-#if HYBRID_NAVIGATION
- INT32_VECT2_NED_OF_ENU(guidance_h.sp.pos, navigation_target);
- guidance_hybrid_run();
-#else
- INT32_VECT2_NED_OF_ENU(guidance_h.sp.pos, navigation_carrot);
-
- guidance_h_update_reference();
-
- /* set psi command */
- guidance_h.sp.heading = nav_heading;
- INT32_ANGLE_NORMALIZE(guidance_h.sp.heading);
-
-#if GUIDANCE_INDI
- guidance_indi_run(in_flight, guidance_h.sp.heading);
-#else
- /* compute x,y earth commands */
- guidance_h_traj_run(in_flight);
- /* set final attitude setpoint */
- stabilization_attitude_set_earth_cmd_i(&guidance_h_cmd_earth,
- guidance_h.sp.heading);
-#endif
-
-#endif
- stabilization_attitude_run(in_flight);
- }
+ guidance_h_from_nav(in_flight);
break;
#if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE
@@ -593,7 +530,7 @@ static void guidance_h_traj_run(bool in_flight)
}
#endif
-static void guidance_h_hover_enter(void)
+void guidance_h_hover_enter(void)
{
/* reset speed setting */
guidance_h.sp.speed.x = 0;
@@ -616,7 +553,7 @@ static void guidance_h_hover_enter(void)
guidance_h.sp.heading = guidance_h.rc_sp.psi;
}
-static void guidance_h_nav_enter(void)
+void guidance_h_nav_enter(void)
{
ClearBit(guidance_h.sp.mask, 5);
ClearBit(guidance_h.sp.mask, 7);
@@ -629,6 +566,57 @@ static void guidance_h_nav_enter(void)
nav_heading = stateGetNedToBodyEulers_i()->psi;
}
+void guidance_h_from_nav(bool in_flight)
+{
+ if (!in_flight) {
+ guidance_h_nav_enter();
+ }
+
+ if (horizontal_mode == HORIZONTAL_MODE_MANUAL) {
+ stabilization_cmd[COMMAND_ROLL] = nav_cmd_roll;
+ stabilization_cmd[COMMAND_PITCH] = nav_cmd_pitch;
+ stabilization_cmd[COMMAND_YAW] = nav_cmd_yaw;
+ } else if (horizontal_mode == HORIZONTAL_MODE_ATTITUDE) {
+ struct Int32Eulers sp_cmd_i;
+ sp_cmd_i.phi = nav_roll;
+ sp_cmd_i.theta = nav_pitch;
+ sp_cmd_i.psi = nav_heading;
+ stabilization_attitude_set_rpy_setpoint_i(&sp_cmd_i);
+ stabilization_attitude_run(in_flight);
+
+#if HYBRID_NAVIGATION
+ //make sure the heading is right before leaving horizontal_mode attitude
+ guidance_hybrid_reset_heading(&sp_cmd_i);
+#endif
+ } else {
+
+#if HYBRID_NAVIGATION
+ INT32_VECT2_NED_OF_ENU(guidance_h.sp.pos, navigation_target);
+ guidance_hybrid_run();
+#else
+ INT32_VECT2_NED_OF_ENU(guidance_h.sp.pos, navigation_carrot);
+
+ guidance_h_update_reference();
+
+ /* set psi command */
+ guidance_h.sp.heading = nav_heading;
+ INT32_ANGLE_NORMALIZE(guidance_h.sp.heading);
+
+#if GUIDANCE_INDI
+ guidance_indi_run(in_flight, guidance_h.sp.heading);
+#else
+ /* compute x,y earth commands */
+ guidance_h_traj_run(in_flight);
+ /* set final attitude setpoint */
+ stabilization_attitude_set_earth_cmd_i(&guidance_h_cmd_earth,
+ guidance_h.sp.heading);
+#endif
+
+#endif
+ stabilization_attitude_run(in_flight);
+ }
+}
+
static inline void transition_run(void)
{
//Add 0.00625%
@@ -678,6 +666,27 @@ void guidance_h_set_igain(uint32_t igain)
INT_VECT2_ZERO(guidance_h_trim_att_integrator);
}
+
+void guidance_h_guided_run(bool in_flight)
+{
+ /* guidance_h.sp.pos and guidance_h.sp.heading need to be set from external source */
+ if (!in_flight) {
+ guidance_h_hover_enter();
+ }
+
+ guidance_h_update_reference();
+
+#if GUIDANCE_INDI
+ guidance_indi_run(in_flight, guidance_h.sp.heading);
+#else
+ /* compute x,y earth commands */
+ guidance_h_traj_run(in_flight);
+ /* set final attitude setpoint */
+ stabilization_attitude_set_earth_cmd_i(&guidance_h_cmd_earth, guidance_h.sp.heading);
+#endif
+ stabilization_attitude_run(in_flight);
+}
+
bool guidance_h_set_guided_pos(float x, float y)
{
if (guidance_h.mode == GUIDANCE_H_MODE_GUIDED) {
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h
index db9a30f73c..39fda02140 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h
@@ -112,8 +112,19 @@ extern void guidance_h_mode_changed(uint8_t new_mode);
extern void guidance_h_read_rc(bool in_flight);
extern void guidance_h_run(bool in_flight);
+extern void guidance_h_hover_enter(void);
+extern void guidance_h_nav_enter(void);
+
+/** Set horizontal guidance from NAV and run control loop
+ */
+extern void guidance_h_from_nav(bool in_flight);
+
extern void guidance_h_set_igain(uint32_t igain);
+/** Run GUIDED mode control
+ */
+extern void guidance_h_guided_run(bool in_flight);
+
/** Set horizontal position setpoint in GUIDED mode.
* @param x North position (local NED frame) in meters.
* @param y East position (local NED frame) in meters.
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
index 5728eb1345..4007d2301a 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
@@ -138,15 +138,7 @@ int32_t guidance_v_z_sum_err;
int32_t guidance_v_thrust_coeff;
-#define GuidanceVSetRef(_pos, _speed, _accel) { \
- gv_set_ref(_pos, _speed, _accel); \
- guidance_v_z_ref = _pos; \
- guidance_v_zd_ref = _speed; \
- guidance_v_zdd_ref = _accel; \
- }
-
static int32_t get_vertical_thrust_coeff(void);
-static void run_hover_loop(bool in_flight);
#if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h"
@@ -194,6 +186,8 @@ void guidance_v_init(void)
guidance_v_adapt_throttle_enabled = GUIDANCE_V_ADAPT_THROTTLE_ENABLED;
guidance_v_guided_mode = GUIDANCE_V_GUIDED_MODE_ZHOLD;
+ guidance_v_thrust_coeff = BFP_OF_REAL(1.f, INT32_TRIG_FRAC);
+
gv_adapt_init();
#if GUIDANCE_V_MODE_MODULE_SETTING == GUIDANCE_V_MODE_MODULE
@@ -239,16 +233,7 @@ void guidance_v_mode_changed(uint8_t new_mode)
switch (new_mode) {
case GUIDANCE_V_MODE_GUIDED:
case GUIDANCE_V_MODE_HOVER:
- /* disable vertical velocity setpoints */
- guidance_v_guided_mode = GUIDANCE_V_GUIDED_MODE_ZHOLD;
-
- /* set current altitude as setpoint and reset speed setpoint */
- guidance_v_z_sp = stateGetPositionNed_i()->z;
- guidance_v_zd_sp = 0;
-
- /* reset guidance reference */
- guidance_v_z_sum_err = 0;
- GuidanceVSetRef(stateGetPositionNed_i()->z, stateGetSpeedNed_i()->z, 0);
+ guidance_v_guided_enter();
break;
case GUIDANCE_V_MODE_RC_CLIMB:
@@ -284,10 +269,8 @@ void guidance_v_notify_in_flight(bool in_flight)
}
}
-
-void guidance_v_run(bool in_flight)
+void guidance_v_thrust_adapt(bool in_flight)
{
-
// FIXME... SATURATIONS NOT TAKEN INTO ACCOUNT
// AKA SUPERVISION and co
guidance_v_thrust_coeff = get_vertical_thrust_coeff();
@@ -298,6 +281,11 @@ void guidance_v_run(bool in_flight)
/* reset estimate while not in_flight */
gv_adapt_init();
}
+}
+
+void guidance_v_run(bool in_flight)
+{
+ guidance_v_thrust_adapt(in_flight);
switch (guidance_v_mode) {
@@ -328,34 +316,7 @@ void guidance_v_run(bool in_flight)
case GUIDANCE_V_MODE_HOVER:
guidance_v_guided_mode = GUIDANCE_V_GUIDED_MODE_ZHOLD;
case GUIDANCE_V_MODE_GUIDED:
- switch(guidance_v_guided_mode)
- {
- case GUIDANCE_V_GUIDED_MODE_ZHOLD:
- // Altitude Hold
- guidance_v_zd_sp = 0;
- gv_update_ref_from_z_sp(guidance_v_z_sp);
- run_hover_loop(in_flight);
- break;
- case GUIDANCE_V_GUIDED_MODE_CLIMB:
- // Climb
- gv_update_ref_from_zd_sp(guidance_v_zd_sp, stateGetPositionNed_i()->z);
- run_hover_loop(in_flight);
- break;
- case GUIDANCE_V_GUIDED_MODE_THROTTLE:
- // Throttle
- guidance_v_z_sp = stateGetPositionNed_i()->z; // for display only
- stabilization_cmd[COMMAND_THRUST] = guidance_v_th_sp;
- break;
- default:
- break;
- }
-#if !NO_RC_THRUST_LIMIT
- /* use rc limitation if available */
- if (radio_control.status == RC_OK) {
- stabilization_cmd[COMMAND_THRUST] = Min(guidance_v_rc_delta_t, guidance_v_delta_t);
- } else
-#endif
- stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
+ guidance_v_guided_run(in_flight);
break;
#if GUIDANCE_V_MODE_MODULE_SETTING == GUIDANCE_V_MODE_MODULE
@@ -365,34 +326,7 @@ void guidance_v_run(bool in_flight)
#endif
case GUIDANCE_V_MODE_NAV: {
- if (vertical_mode == VERTICAL_MODE_ALT) {
- guidance_v_z_sp = -nav_flight_altitude;
- guidance_v_zd_sp = 0;
- gv_update_ref_from_z_sp(guidance_v_z_sp);
- run_hover_loop(in_flight);
- } else if (vertical_mode == VERTICAL_MODE_CLIMB) {
- guidance_v_z_sp = stateGetPositionNed_i()->z;
- guidance_v_zd_sp = -nav_climb;
- gv_update_ref_from_zd_sp(guidance_v_zd_sp, stateGetPositionNed_i()->z);
- run_hover_loop(in_flight);
- } else if (vertical_mode == VERTICAL_MODE_MANUAL) {
- guidance_v_z_sp = stateGetPositionNed_i()->z;
- guidance_v_zd_sp = stateGetSpeedNed_i()->z;
- GuidanceVSetRef(guidance_v_z_sp, guidance_v_zd_sp, 0);
- guidance_v_z_sum_err = 0;
- guidance_v_delta_t = nav_throttle;
- }
-#if HYBRID_NAVIGATION
- guidance_hybrid_vertical();
-#else
-#if !NO_RC_THRUST_LIMIT
- /* use rc limitation if available */
- if (radio_control.status == RC_OK) {
- stabilization_cmd[COMMAND_THRUST] = Min(guidance_v_rc_delta_t, guidance_v_delta_t);
- } else
-#endif
- stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
-#endif
+ guidance_v_from_nav(in_flight);
break;
}
@@ -435,7 +369,7 @@ static int32_t get_vertical_thrust_coeff(void)
#define FF_CMD_FRAC 18
-static void run_hover_loop(bool in_flight)
+void run_hover_loop(bool in_flight)
{
/* convert our reference to generic representation */
@@ -494,6 +428,84 @@ static void run_hover_loop(bool in_flight)
}
+void guidance_v_from_nav(bool in_flight)
+{
+ if (vertical_mode == VERTICAL_MODE_ALT) {
+ guidance_v_z_sp = -nav_flight_altitude;
+ guidance_v_zd_sp = 0;
+ gv_update_ref_from_z_sp(guidance_v_z_sp);
+ run_hover_loop(in_flight);
+ } else if (vertical_mode == VERTICAL_MODE_CLIMB) {
+ guidance_v_z_sp = stateGetPositionNed_i()->z;
+ guidance_v_zd_sp = -nav_climb;
+ gv_update_ref_from_zd_sp(guidance_v_zd_sp, stateGetPositionNed_i()->z);
+ run_hover_loop(in_flight);
+ } else if (vertical_mode == VERTICAL_MODE_MANUAL) {
+ guidance_v_z_sp = stateGetPositionNed_i()->z;
+ guidance_v_zd_sp = stateGetSpeedNed_i()->z;
+ GuidanceVSetRef(guidance_v_z_sp, guidance_v_zd_sp, 0);
+ guidance_v_z_sum_err = 0;
+ guidance_v_delta_t = nav_throttle;
+ }
+#if HYBRID_NAVIGATION
+ guidance_hybrid_vertical();
+#else
+#if !NO_RC_THRUST_LIMIT
+ /* use rc limitation if available */
+ if (radio_control.status == RC_OK) {
+ stabilization_cmd[COMMAND_THRUST] = Min(guidance_v_rc_delta_t, guidance_v_delta_t);
+ } else
+#endif
+ stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
+#endif
+}
+
+void guidance_v_guided_enter(void)
+{
+ /* disable vertical velocity setpoints */
+ guidance_v_guided_mode = GUIDANCE_V_GUIDED_MODE_ZHOLD;
+
+ /* set current altitude as setpoint and reset speed setpoint */
+ guidance_v_z_sp = stateGetPositionNed_i()->z;
+ guidance_v_zd_sp = 0;
+
+ /* reset guidance reference */
+ guidance_v_z_sum_err = 0;
+ GuidanceVSetRef(stateGetPositionNed_i()->z, stateGetSpeedNed_i()->z, 0);
+}
+
+void guidance_v_guided_run(bool in_flight)
+{
+ switch(guidance_v_guided_mode)
+ {
+ case GUIDANCE_V_GUIDED_MODE_ZHOLD:
+ // Altitude Hold
+ guidance_v_zd_sp = 0;
+ gv_update_ref_from_z_sp(guidance_v_z_sp);
+ run_hover_loop(in_flight);
+ break;
+ case GUIDANCE_V_GUIDED_MODE_CLIMB:
+ // Climb
+ gv_update_ref_from_zd_sp(guidance_v_zd_sp, stateGetPositionNed_i()->z);
+ run_hover_loop(in_flight);
+ break;
+ case GUIDANCE_V_GUIDED_MODE_THROTTLE:
+ // Throttle
+ guidance_v_z_sp = stateGetPositionNed_i()->z; // for display only
+ stabilization_cmd[COMMAND_THRUST] = guidance_v_th_sp;
+ break;
+ default:
+ break;
+ }
+#if !NO_RC_THRUST_LIMIT
+ /* use rc limitation if available */
+ if (radio_control.status == RC_OK) {
+ stabilization_cmd[COMMAND_THRUST] = Min(guidance_v_rc_delta_t, guidance_v_delta_t);
+ } else
+#endif
+ stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
+}
+
bool guidance_v_set_guided_z(float z)
{
if (guidance_v_mode == GUIDANCE_V_MODE_GUIDED) {
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h
index cbb276a90c..f793799000 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h
@@ -78,6 +78,11 @@ extern int32_t guidance_v_z_sum_err; ///< accumulator for I-gain
extern int32_t guidance_v_ff_cmd; ///< feed-forward command
extern int32_t guidance_v_fb_cmd; ///< feed-back command
+/** Direct throttle from radio control.
+ * range 0:#MAX_PPRZ
+ */
+extern int32_t guidance_v_rc_delta_t;
+
/** thrust command.
* summation of feed-forward and feed-back commands,
* valid range 0 : #MAX_PPRZ
@@ -94,8 +99,6 @@ extern float guidance_v_nominal_throttle;
*/
extern bool guidance_v_adapt_throttle_enabled;
-extern bool guidance_v_guided_vel_enabled;
-
extern int32_t guidance_v_thrust_coeff;
extern int32_t guidance_v_kp; ///< vertical control P-gain
@@ -105,9 +108,24 @@ extern int32_t guidance_v_ki; ///< vertical control I-gain
extern void guidance_v_init(void);
extern void guidance_v_read_rc(void);
extern void guidance_v_mode_changed(uint8_t new_mode);
+extern void guidance_v_thrust_adapt(bool in_flight);
extern void guidance_v_notify_in_flight(bool in_flight);
extern void guidance_v_run(bool in_flight);
+extern void run_hover_loop(bool in_flight);
+
+/** Set guidance setpoint from NAV and run hover loop
+ */
+extern void guidance_v_from_nav(bool in_flight);
+
+/** Enter GUIDED mode control
+ */
+extern void guidance_v_guided_enter(void);
+
+/** Run GUIDED mode control
+ */
+extern void guidance_v_guided_run(bool in_flight);
+
/** Set z setpoint in GUIDED mode.
* @param z Setpoint (down is positive) in meters.
* @return TRUE if setpoint was set (currently in GUIDANCE_V_MODE_GUIDED)
@@ -127,4 +145,25 @@ extern bool guidance_v_set_guided_th(float th);
guidance_v_z_sum_err = 0; \
}
+#define GuidanceVSetRef(_pos, _speed, _accel) { \
+ gv_set_ref(_pos, _speed, _accel); \
+ guidance_v_z_ref = _pos; \
+ guidance_v_zd_ref = _speed; \
+ guidance_v_zdd_ref = _accel; \
+ }
+
+#include "state.h"
+static inline void guidance_v_z_enter(void)
+{
+ /* set current altitude as setpoint */
+ guidance_v_z_sp = stateGetPositionNed_i()->z;
+
+ /* reset guidance reference */
+ guidance_v_z_sum_err = 0;
+ GuidanceVSetRef(stateGetPositionNed_i()->z, 0, 0);
+
+ /* reset speed setting */
+ guidance_v_zd_sp = 0;
+}
+
#endif /* GUIDANCE_V_H */
diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c
index a35ffc889c..877446886f 100644
--- a/sw/airborne/firmwares/rotorcraft/main.c
+++ b/sw/airborne/firmwares/rotorcraft/main.c
@@ -331,6 +331,7 @@ STATIC_INLINE void telemetry_periodic(void)
STATIC_INLINE void failsafe_check(void)
{
+#if !USE_GENERATED_AUTOPILOT
if (radio_control.status == RC_REALLY_LOST &&
autopilot_mode != AP_MODE_KILL &&
autopilot_mode != AP_MODE_HOME &&
@@ -365,6 +366,8 @@ STATIC_INLINE void failsafe_check(void)
}
#endif
+#endif // !USE_GENERATED_AUTOPILOT
+
autopilot_check_in_flight(autopilot_motors_on);
}
diff --git a/sw/airborne/firmwares/rotorcraft/rotorcraft_datalink.c b/sw/airborne/firmwares/rotorcraft/rotorcraft_datalink.c
index 14d332f990..cb24bb5a23 100644
--- a/sw/airborne/firmwares/rotorcraft/rotorcraft_datalink.c
+++ b/sw/airborne/firmwares/rotorcraft/rotorcraft_datalink.c
@@ -70,6 +70,7 @@ void firmware_parse_msg(struct link_device *dev __attribute__((unused)), struct
break;
#endif /* USE_NAVIGATION */
+#ifdef AP_MODE_GUIDED
case DL_GUIDED_SETPOINT_NED:
if (DL_GUIDED_SETPOINT_NED_ac_id(buf) != AC_ID) { break; }
@@ -79,6 +80,7 @@ void firmware_parse_msg(struct link_device *dev __attribute__((unused)), struct
DL_GUIDED_SETPOINT_NED_z(buf),
DL_GUIDED_SETPOINT_NED_yaw(buf));
break;
+#endif
default:
break;
diff --git a/sw/ground_segment/tmtc/aircraft.ml b/sw/ground_segment/tmtc/aircraft.ml
index c7ef93687a..3e8f58c9ef 100644
--- a/sw/ground_segment/tmtc/aircraft.ml
+++ b/sw/ground_segment/tmtc/aircraft.ml
@@ -193,7 +193,8 @@ type aircraft = {
mutable last_msg_date : float;
mutable time_since_last_survey_msg : float;
mutable dist_to_wp : float;
- inflight_calib : inflight_calib
+ inflight_calib : inflight_calib;
+ mutable ap_modes : string array option
}
let max_nb_dl_setting_values = 256 (** indexed iwth an uint8 (messages.xml) *)
@@ -225,5 +226,14 @@ let new_aircraft = fun id name fp airframe ->
waypoints = Hashtbl.create 3; survey = None; last_msg_date = 0.; dist_to_wp = 0.;
datalink_status = datalink_status_init (); link_status = Hashtbl.create 1;
time_since_last_survey_msg = 1729.;
- inflight_calib = { if_mode = 1 ; if_val1 = 0.; if_val2 = 0.}
+ inflight_calib = { if_mode = 1 ; if_val1 = 0.; if_val2 = 0.};
+ ap_modes = None
}
+
+let modes_of_aircraft = fun ac ->
+ match ac.ap_modes, ac.vehicle_type with
+ | Some m, _ -> m
+ | None, FixedWing -> Server_globals.fixedwing_ap_modes
+ | None, Rotorcraft -> Server_globals.rotorcraft_ap_modes
+ | None, _ -> [| "UKN" |]
+
diff --git a/sw/ground_segment/tmtc/aircraft.mli b/sw/ground_segment/tmtc/aircraft.mli
index ccff6d7577..bd570f008b 100644
--- a/sw/ground_segment/tmtc/aircraft.mli
+++ b/sw/ground_segment/tmtc/aircraft.mli
@@ -144,8 +144,10 @@ type aircraft = {
mutable last_msg_date : float;
mutable time_since_last_survey_msg : float;
mutable dist_to_wp : float;
- inflight_calib : inflight_calib
+ inflight_calib : inflight_calib;
+ mutable ap_modes : string array option
}
val new_aircraft : string -> string -> Xml.xml -> Xml.xml -> aircraft
val max_nb_dl_setting_values : int
+val modes_of_aircraft : aircraft -> string array
diff --git a/sw/ground_segment/tmtc/fw_server.ml b/sw/ground_segment/tmtc/fw_server.ml
index fb4c379416..cea9b93bb8 100644
--- a/sw/ground_segment/tmtc/fw_server.ml
+++ b/sw/ground_segment/tmtc/fw_server.ml
@@ -246,7 +246,7 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
if a.fbw.rc_mode = "FAILSAFE" then
a.ap_mode <- 5 (* Override and set FAIL(Safe) Mode *)
else
- a.ap_mode <- check_index (ivalue "ap_mode") fixedwing_ap_modes "AP_MODE"
+ a.ap_mode <- check_index (ivalue "ap_mode") (modes_of_aircraft a) "AP_MODE"
| "CAM" ->
a.cam.phi <- (Deg>>Rad) (fvalue "phi");
a.cam.theta <- (Deg>>Rad) (fvalue "theta");
@@ -337,7 +337,7 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
a.bat <- fvalue "vsupply" /. 10.;
a.energy <- ivalue "energy" * 100;
a.throttle <- fvalue "throttle";
- a.ap_mode <- check_index (ivalue "ap_mode") fixedwing_ap_modes "AP_MODE";
+ a.ap_mode <- check_index (ivalue "ap_mode") (modes_of_aircraft a) "AP_MODE";
a.cur_block <- ivalue "nav_block";
end
| "FORMATION_SLOT_TM" ->
diff --git a/sw/ground_segment/tmtc/rotorcraft_server.ml b/sw/ground_segment/tmtc/rotorcraft_server.ml
index 87c0736d0d..81d2c16b09 100644
--- a/sw/ground_segment/tmtc/rotorcraft_server.ml
+++ b/sw/ground_segment/tmtc/rotorcraft_server.ml
@@ -199,7 +199,7 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
a.fbw.rc_status <- get_rc_status (ivalue "rc_status");
a.fbw.rc_rate <- ivalue "frame_rate";
a.gps_mode <- check_index (ivalue "gps_status") gps_modes "GPS_MODE";
- a.ap_mode <- check_index (ivalue "ap_mode") rotorcraft_ap_modes "ROTORCRAFT_AP_MODE";
+ a.ap_mode <- check_index (ivalue "ap_mode") (modes_of_aircraft a) "ROTORCRAFT_AP_MODE";
a.kill_mode <- ivalue "ap_motors_on" == 0;
a.bat <- fvalue "vsupply" /. 10.
| "STATE_FILTER_STATUS" ->
diff --git a/sw/ground_segment/tmtc/server.ml b/sw/ground_segment/tmtc/server.ml
index 5054d8c79b..7c1332192e 100644
--- a/sw/ground_segment/tmtc/server.ml
+++ b/sw/ground_segment/tmtc/server.ml
@@ -52,12 +52,6 @@ let srtm_path = Env.paparazzi_home // "data" // "srtm"
let get_indexed_value = fun t i ->
if i >= 0 then t.(i) else "UNK"
-let modes_of_type = fun vt ->
- match vt with
- FixedWing -> fixedwing_ap_modes
- | Rotorcraft -> rotorcraft_ap_modes
- | UnknownVehicleType -> [| |]
-
(** The aircrafts store *)
let aircrafts = Hashtbl.create 3
@@ -422,7 +416,7 @@ let send_aircraft_msg = fun ac ->
"energy", PprzLink.Int a.energy] in
Ground_Pprz.message_send my_id "ENGINE_STATUS" values;
- let ap_mode = get_indexed_value (modes_of_type a.vehicle_type) a.ap_mode in
+ let ap_mode = get_indexed_value (modes_of_aircraft a) a.ap_mode in
let gaz_mode = get_indexed_value gaz_modes a.gaz_mode in
let lat_mode = get_indexed_value lat_modes a.lateral_mode in
let horiz_mode = get_indexed_value horiz_modes a.horizontal_mode in
@@ -545,6 +539,11 @@ let new_aircraft = fun get_alive_md5sum real_id ->
ac.svinfo.(i).age <- ac.svinfo.(i).age + 1;
done in
+ ignore (ac.ap_modes <- try
+ let (ap_file, _) = Gen_common.get_autopilot_of_airframe airframe_xml in
+ Some (modes_from_autopilot (ExtXml.parse_file ap_file))
+ with _ -> None);
+
ignore (Glib.Timeout.add 1000 (fun _ -> update (); true));
let messages_xml = ExtXml.parse_file (Env.paparazzi_home // root_dir // "var" // "messages.xml") in
diff --git a/sw/ground_segment/tmtc/server_globals.ml b/sw/ground_segment/tmtc/server_globals.ml
index 97171cfea3..f7600a1467 100644
--- a/sw/ground_segment/tmtc/server_globals.ml
+++ b/sw/ground_segment/tmtc/server_globals.ml
@@ -20,3 +20,14 @@ let if_modes = [|"OFF";"DOWN";"UP"|]
let string_of_values = fun values ->
Compat.bytes_concat " " (List.map (fun (_, v) -> PprzLink.string_of_value v) values)
+
+(** get modes from autopilot xml file *)
+let modes_from_autopilot = fun ap_xml ->
+ let ap = ExtXml.child ap_xml
+ ~select:(fun x -> String.uppercase (ExtXml.attrib_or_default x "gcs_mode" "") = "TRUE")
+ "state_machine"
+ in
+ let modes = List.filter (fun x -> Xml.tag x = "mode") (Xml.children ap) in
+ let l = List.map (fun x -> try Xml.attrib x "shortname" with _ -> ExtXml.attrib x "name") modes in
+ Array.of_list l
+
diff --git a/sw/tools/generators/gen_autopilot.ml b/sw/tools/generators/gen_autopilot.ml
index 3fbfb9ce6e..22c3519220 100644
--- a/sw/tools/generators/gen_autopilot.ml
+++ b/sw/tools/generators/gen_autopilot.ml
@@ -59,6 +59,10 @@ let get_includes = fun sm ->
try ExtXml.child sm "includes"
with _ -> Xml.Element ("includes", [], [])
+let get_settings = fun sm ->
+ try ExtXml.child sm "settings"
+ with _ -> Xml.Element ("settings", [], [])
+
let has_modules = fun sm ->
try
let m = ExtXml.child sm "modules" in
@@ -70,8 +74,22 @@ let get_mode_exceptions = fun mode ->
let print_includes = fun includes out_h ->
List.iter (fun i ->
- let name = Xml.attrib i "name" in
- lprintf out_h "#include \"%s\"\n" name
+ match Xml.tag i with
+ | "include" ->
+ let name = ExtXml.attrib i "name" in
+ lprintf out_h "#include \"%s\"\n" name
+ | "define" ->
+ let name = ExtXml.attrib i "name"
+ and value = ExtXml.attrib i "value"
+ and cond = try Some (Xml.attrib i "cond") with _ -> None in
+ begin match cond with
+ | None -> lprintf out_h "#define %s %s\n" name value
+ | Some c ->
+ lprintf out_h "#%s\n" c;
+ lprintf out_h "#define %s %s\n" name value;
+ lprintf out_h "#endif\n"
+ end
+ | _ -> failwith "[gen_autopilot] Unknown includes type"
) (Xml.children includes)
let print_mode_name = fun sm_name name ->
@@ -141,8 +159,8 @@ let print_test_exception = fun modes name out_h ->
let print_exception = fun ex ->
let deroute = Xml.attrib ex "deroute"
and cond = Xml.attrib ex "cond" in
- match name with
- "$LAST_MODE" -> lprintf out_h "if (%s) { return last_autopilot_mode_%s; }\n" cond name
+ match deroute with
+ | "$LAST_MODE" -> lprintf out_h "if (%s) { return last_autopilot_mode_%s; }\n" cond name
| _ -> lprintf out_h "if (%s) { return %s; }\n" cond (print_mode_name name deroute)
in
@@ -338,11 +356,11 @@ let parse_and_gen_modes xml_file ap_name main_freq h_dir sm =
(* Print mode names and variable *)
print_modes modes name_up out_h;
fprintf out_h "\nEXTERN_%s uint8_t autopilot_mode_%s;\n" name_up name;
- fprintf out_h "\n#ifdef AUTOPILOT_CORE_%s_C\n" name_up;
+ fprintf out_h "\n#ifdef AUTOPILOT_CORE_%s_C\n\n" name_up;
(* Print includes and private variables *)
print_includes (get_includes sm) out_h;
if has_modules sm then fprintf out_h "\n#include \"modules.h\"\n";
- fprintf out_h "uint8_t private_autopilot_mode_%s;\n" name;
+ fprintf out_h "\nuint8_t private_autopilot_mode_%s;\n" name;
fprintf out_h "uint8_t last_autopilot_mode_%s;\n\n" name;
(* Print functions *)
print_ap_init modes name out_h;
@@ -402,12 +420,24 @@ let write_settings = fun xml_file out_set ap ->
(current + 1, min, max, values @ n)
end
) (0, None, None, []) modes in
+ (* check handler *)
+ let handler = try
+ let sh = Xml.attrib sm "settings_handler" in
+ let s = Str.split (Str.regexp "|") sh in
+ match s with
+ | [m; h] -> sprintf " module=\"%s\" handler=\"%s\"" m h
+ | _ -> failwith "invalid handler format"
+ with _ -> "" in
(* Print if at least one mode has been found *)
- match min, max with
+ begin match min, max with
| Some min_idx, Some max_idx ->
- fprintf out_set " \n"
- min_idx max_idx name name (String.concat "|" values)
+ fprintf out_set " \n"
+ min_idx max_idx name name (String.concat "|" values) handler
| _, _ -> ()
+ end;
+ (* check for embedded custom settings *)
+ let extra_settings = get_settings sm in
+ List.iter (fun s -> fprintf out_set " %s\n" (Xml.to_string s)) (Xml.children extra_settings)
in
(* Iter and call print function *)
List.iter write_ap_mode sm_filtered;
@@ -422,17 +452,18 @@ let write_settings = fun xml_file out_set ap ->
* Usage: main_freq xml_file_input h_dir_output
*)
let gen_autopilot main_freq xml_file h_dir out_set =
- try
- let ap_xml = Xml.parse_file xml_file in
- let ap_name = ExtXml.attrib_or_default ap_xml "name" "Autopilot" in
- let state_machines = get_state_machines ap_xml in
- List.iter (parse_and_gen_modes xml_file ap_name main_freq h_dir) state_machines;
- write_settings xml_file out_set ap_xml
- with
- Xml.Error e -> fprintf stderr "%s: XML error:%s\n" xml_file (Xml.error e); exit 1
+ let ap_xml = try Xml.parse_file xml_file with
+ | Xml.Error e -> fprintf stderr "%s: XML error:%s\n" xml_file (Xml.error e); exit 1
| Dtd.Prove_error e -> fprintf stderr "%s: DTD error:%s\n%!" xml_file (Dtd.prove_error e); exit 1
| Dtd.Check_error e -> fprintf stderr "%s: DTD error:%s\n%!" xml_file (Dtd.check_error e); exit 1
| Dtd.Parse_error e -> fprintf stderr "%s: DTD error:%s\n%!" xml_file (Dtd.parse_error e); exit 1
+ | Xml.File_not_found _ -> (* invalid file, return empty Xml *)
+ Xml.Element ("autopilot", [], [])
+ in
+ let ap_name = ExtXml.attrib_or_default ap_xml "name" "Autopilot" in
+ let state_machines = get_state_machines ap_xml in
+ List.iter (parse_and_gen_modes xml_file ap_name main_freq h_dir) state_machines;
+ write_settings xml_file out_set ap_xml
(* Main call *)
let () =
@@ -448,7 +479,8 @@ let () =
| Dtd.Prove_error e -> fprintf stderr "%s: DTD error:%s\n%!" xml_file (Dtd.prove_error e); exit 1
| Dtd.Check_error e -> fprintf stderr "%s: DTD error:%s\n%!" xml_file (Dtd.check_error e); exit 1
| Dtd.Parse_error e -> fprintf stderr "%s: DTD error:%s\n%!" xml_file (Dtd.parse_error e); exit 1
- | Not_found -> exit 0 (* No autopilot file found *)
+ | Not_found -> (* No autopilot file found *)
+ ("", None)
in
try
gen_autopilot ap_freq autopilot h_dir out_set;