Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-28 09:58:23 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

Autopilot refactor (#2009)

Browse Source 
* [autopilot] refactor autopilot API for both firmwares

With this, fixedwing and rotorcraft are mostly using the same interface
for the autopilot. Some specific code and messages handling are still
firmware dependent.
A large part of the autopilot logic of the fixedwing is moved from
main_ap to autopilot_static.
More getter/setter functions are provided.

* [autopilot] update the rest of the system and the conf

for using the refactored autopilot API

* [autopilot] fix some errors from CI servers

* [actuators] use dummy actuators module to prevent autoloading

* Rename Bart_heliDD_INDI.xml to tudelft_bs_helidd_indi.xml

* Rename Bart_heliDD_pid.xml to tudelft_bs_helidd_pid.xml

* Delete tudelft_course2016_bebop_colorfilter.xml

* Delete tudelft_course2016_bebop_avoider.xml

* [actuators] don't autoload actuators when set to 'none'

* [gcs] autodetect firmware for strip mode button
This commit is contained in:
Gautier Hattenberger
2017-02-19 11:45:57 +01:00
committed by Felix Ruess
parent ded925a851
commit 363dec8693
275 changed files with 1931 additions and 1380 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/arch/sim/sim_ap.c
-1
View File
@@ -35,7 +35,6 @@ uint8_t ir_estim_mode;
uint8_t vertical_mode;
uint8_t inflight_calib_mode;
bool rc_event_1, rc_event_2;
bool launch;
uint8_t gps_nb_ovrn, link_fbw_fbw_nb_err, link_fbw_nb_err;
float alt_roll_pgain;
float roll_rate_pgain;
sw/airborne/arch/sim/sim_gps.c
-3
View File
@@ -63,9 +63,6 @@ value sim_use_gps_pos(value x, value y, value z, value c, value a, value s, valu
gps.pdop = gps.sacc = gps.pacc = 500 + 200 * sin(time / 100.);
gps.num_sv = 7;
//gps_verbose_downlink = !launch;
//gps_downlink();
gps_sim_publish();
return Val_unit;
sw/airborne/autopilot.c
+303
View File
@@ -0,0 +1,303 @@
/*
* Copyright (C) 2003 Pascal Brisset, Antoine Drouin
* Copyright (C) 2008-2012 The Paparazzi Team
* Copyright (C) 2016-2017 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* 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, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file autopilot.c
*
* Core autopilot interface common to all firmwares.
* Using either static or generated autopilot logic,
* which depends on the firmware.
*
*/
#include "autopilot.h"
#include "generated/modules.h"
#include "mcu_periph/uart.h"
#include "mcu_periph/sys_time.h"
#include "mcu_periph/gpio.h"
#include "subsystems/radio_control.h"
#include "subsystems/commands.h"
#include "subsystems/actuators.h"
//#include "subsystems/electrical.h"
#include "subsystems/datalink/telemetry.h"
#include "subsystems/settings.h"
#include "generated/settings.h"
#include "pprz_version.h"
struct pprz_autopilot autopilot;
static void send_autopilot_version(struct transport_tx *trans, struct link_device *dev)
{
static uint32_t ap_version = PPRZ_VERSION_INT;
static char *ver_desc = PPRZ_VERSION_DESC;
pprz_msg_send_AUTOPILOT_VERSION(trans, dev, AC_ID, &ap_version, strlen(ver_desc), ver_desc);
}
static void send_alive(struct transport_tx *trans, struct link_device *dev)
{
pprz_msg_send_ALIVE(trans, dev, AC_ID, 16, MD5SUM);
}
static void send_attitude(struct transport_tx *trans, struct link_device *dev)
{
struct FloatEulers *att = stateGetNedToBodyEulers_f();
pprz_msg_send_ATTITUDE(trans, dev, AC_ID, &(att->phi), &(att->psi), &(att->theta));
};
static void send_dl_value(struct transport_tx *trans, struct link_device *dev)
{
PeriodicSendDlValue(trans, dev);
}
#ifdef RADIO_CONTROL
static void send_rc(struct transport_tx *trans, struct link_device *dev)
{
pprz_msg_send_RC(trans, dev, AC_ID, RADIO_CONTROL_NB_CHANNEL, radio_control.values);
}
#endif
#ifdef ACTUATORS
static void send_actuators(struct transport_tx *trans, struct link_device *dev)
{
pprz_msg_send_ACTUATORS(trans, dev, AC_ID , ACTUATORS_NB, actuators);
}
#endif
void autopilot_init(void)
{
#ifdef MODE_AUTO2
autopilot.mode_auto2 = MODE_AUTO2; // FIXME
#endif
autopilot.flight_time = 0;
autopilot.motors_on = false;
autopilot.kill_throttle = true;
autopilot.in_flight = false;
autopilot.ground_detected = false;
autopilot.detect_ground_once = false;
autopilot.use_rc = true;
autopilot.power_switch = false;
#ifdef POWER_SWITCH_GPIO
gpio_setup_output(POWER_SWITCH_GPIO);
gpio_clear(POWER_SWITCH_GPIO); // POWER OFF
#endif
// call firmware specific init
autopilot_firmware_init();
// static / generated AP init part is called later by main program
// and will set the correct initial mode
// 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_ATTITUDE, send_attitude);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_DL_VALUE, send_dl_value);
#ifdef ACTUATORS
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ACTUATORS, send_actuators);
#endif
#ifdef RADIO_CONTROL
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_RC, send_rc);
#endif
}
/** AP periodic call
*/
void autopilot_periodic(void)
{
#if USE_GENERATED_AUTOPILOT
autopilot_generated_periodic();
#else
autopilot_static_periodic();
#endif
}
/** AP event call
*/
void WEAK autopilot_event(void) {}
/** 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
}
/** set autopilot mode
*/
bool 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
return (autopilot.mode != new_autopilot_mode);
}
/** AP mode setting handler
*/
void autopilot_SetModeHandler(float mode)
{
#if USE_GENERATED_AUTOPILOT
autopilot_generated_SetModeHandler(mode);
#else
autopilot_static_SetModeHandler(mode);
#endif
}
/** get autopilot mode
*/
uint8_t autopilot_get_mode(void)
{
return autopilot.mode;
}
/** reset flight time and launch
*/
void autopilot_reset_flight_time(void)
{
autopilot.flight_time = 0;
autopilot.launch = false;
}
/** turn motors on/off, eventually depending of the current mode
* set kill_throttle accordingly FIXME is it true for FW firmware ?
*/
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
autopilot.kill_throttle = ! autopilot.motors_on;
}
/** get motors status
*/
bool autopilot_get_motors_on(void)
{
return autopilot.motors_on;
}
/** set kill throttle
*/
void autopilot_set_kill_throttle(bool kill)
{
if (kill) {
autopilot_set_motors_on(false);
} else {
autopilot_set_motors_on(true);
}
}
/** get kill status
*/
bool autopilot_throttle_killed(void)
{
return autopilot.kill_throttle;
}
/** in flight check utility function
* actual implementation is firmware dependent
*/
void WEAK autopilot_check_in_flight(bool motors_on __attribute__((unused))) {}
/** reset in_flight counter
* actual implementation is firmware dependent
*/
void autopilot_reset_in_flight_counter(void);
void WEAK autopilot_reset_in_flight_counter(void) {}
/** set in_flight flag
*/
void autopilot_set_in_flight(bool in_flight)
{
autopilot.in_flight = in_flight;
if (!in_flight) {
autopilot_reset_in_flight_counter();
}
}
/** get in_flight flag
*/
bool autopilot_in_flight(void)
{
return autopilot.in_flight;
}
/** set power switch
*/
void autopilot_set_power_switch(bool power_switch)
{
#ifdef POWER_SWITCH_GPIO
if (power_switch) {
gpio_set(POWER_SWITCH_GPIO);
} else {
gpio_clear(POWER_SWITCH_GPIO);
}
#endif
autopilot.power_switch = power_switch;
}
/** store settings
*/
void autopilot_store_settings(void)
{
if (autopilot.kill_throttle) {
settings_store_flag = true;
settings_store();
}
}
/** clear settings
*/
void autopilot_clear_settings(void)
{
if (autopilot.kill_throttle) {
settings_clear_flag = true;
settings_clear();
}
}
/** send autopilot version
*/
void autopilot_send_version(void)
{
send_autopilot_version(&(DefaultChannel).trans_tx, &(DefaultDevice).device);
}
/** send autopilot mode
* actual implementation is firmware dependent
*/
void WEAK autopilot_send_mode(void) {}
sw/airborne/autopilot.h
+193
View File
@@ -0,0 +1,193 @@
/*
* Copyright (C) 2003 Pascal Brisset, Antoine Drouin
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
* Copyright (C) 2016-2017 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* 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, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file autopilot.h
*
* Core autopilot interface common to all firmwares.
* Using either static or generated autopilot logic,
* which depends on the firmware.
*
*/
#ifndef AUTOPILOT_H
#define AUTOPILOT_H
#include "std.h"
#include "paparazzi.h"
#include "generated/airframe.h"
#include "state.h"
#include "autopilot_utils.h"
// include firmware specific header
#include "autopilot_firmware.h"
// include static or generated autopilot
// static version by default
#ifndef USE_GENERATED_AUTOPILOT
#define USE_GENERATED_AUTOPILOT FALSE
#endif
#if USE_GENERATED_AUTOPILOT
#include "autopilot_generated.h"
#else
#include "autopilot_static.h"
#endif
/** PPRZ Autopilot structure definition
*/
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
bool motors_on; ///< motor status
bool kill_throttle; ///< allow autopilot to use throttle
bool in_flight; ///< in flight status
bool launch; ///< request launch
bool use_rc; ///< enable/disable RC input
bool power_switch; ///< enable/disable power from power switch (if any)
bool ground_detected; ///< automatic detection of landing
bool detect_ground_once; ///< enable automatic detection of ground (one shot)
};
/** Global autopilot structure
*/
extern struct pprz_autopilot autopilot;
/** Autopilot initialization function
*/
extern void autopilot_init(void);
/** Autopilot periodic call at PERIODIC_FREQUENCY
*/
extern void autopilot_periodic(void);
/** Autopilot event check function
*/
extern void autopilot_event(void);
/** Autopilot RC input event hadler
*/
extern void autopilot_on_rc_frame(void);
/** Set new autopilot mode
*
* @param[in] new_autopilot_mode new mode to set
* @return true if mode has changed
*/
extern bool autopilot_set_mode(uint8_t new_autopilot_mode);
/** Handler for setter function with dl_setting
*/
extern void autopilot_SetModeHandler(float new_autopilot_mode);
/** Get autopilot mode
*
* @return current autopilot mode
*/
extern uint8_t autopilot_get_mode(void);
/** Reset flight time and launch status
* Also provide macro for dl_setting backward compatibility
*/
extern void autopilot_reset_flight_time(void);
#define autopilot_ResetFlightTimeAndLaunch(_) autopilot_reset_flight_time()
/** Start or stop motors
* May have no effect if motors has auto-start based on throttle setpoint
*
* @param[in] motors_on true to start motors, false to stop
*/
extern void autopilot_set_motors_on(bool motors_on);
/** Get motor status
*
* @return true if motors are running
*/
extern bool autopilot_get_motors_on(void);
/** Enable or disable motor control from autopilot
* Also provide macro for dl_setting backward compatibility
*
* @param[in] kill true to disable (kill), false to enable (un-kill)
*/
extern void autopilot_set_kill_throttle(bool kill);
#define autopilot_KillThrottle(_kill) autopilot_set_kill_throttle(_kill)
/** Get kill status
*
* @return true if motors are killed
*/
extern bool autopilot_throttle_killed(void);
/** Check if airframe is in flight
*
* @param[in] motors_on motors status
*/
extern void autopilot_check_in_flight(bool motors_on);
/** Set in flight status
*
* @param[in] in_flight in flight status
*/
extern void autopilot_set_in_flight(bool in_flight);
/** Get in flight status
*
* @return true if airframe in flight
*/
extern bool autopilot_in_flight(void);
/** Set power switch state
* This will actually enable the switch if POWER_SWITCH_GPIO is defined
* Also provide macro for dl_setting backward compatibility
*
* @param[in] power_switch true to enable, false to disable
*/
extern void autopilot_set_power_switch(bool power_switch);
#define autopilot_SetPowerSwitch(_ps) autopilot_set_power_switch(_ps)
/** Store marked settings in flash
* Try to make sure that we don't write to flash while flying
* Also provide macro for dl_setting backward compatibility
*/
extern void autopilot_store_settings(void);
#define autopilot_StoreSettings(_) autopilot_store_settings()
/** Clear marked settings in flash
* try to make sure that we don't write to flash while flying
* Also provide macro for dl_setting backward compatibility
*/
extern void autopilot_clear_settings(void);
#define autopilot_ClearSettings(_) autopilot_clear_settings()
/** Report autopilot version on default downlink channel
*/
extern void autopilot_send_version(void);
/** Report autopilot mode on default downlink channel
*/
extern void autopilot_send_mode(void);
#endif /* AUTOPILOT_H */
sw/airborne/boards/ardrone/actuators.c
+2 -2
View File
@@ -189,7 +189,7 @@ void actuators_ardrone_motor_status(void)
// If a motor IRQ line is set
if (gpio_get(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_IRQ_INPUT) == 1) {
if (autopilot_motors_on) {
if (autopilot_get_motors_on()) {
if (last_motor_on) {
// Tell paparazzi that one motor has stalled
autopilot_set_motors_on(FALSE);
@@ -200,7 +200,7 @@ void actuators_ardrone_motor_status(void)
}
}
last_motor_on = autopilot_motors_on;
last_motor_on = autopilot_get_motors_on();
}
sw/airborne/boards/bebop/actuators.c
+3 -3
View File
@@ -91,7 +91,7 @@ void actuators_bebop_commit(void)
}
// Start the motors
if (actuators_bebop.i2c_trans.buf[10] != 4 && actuators_bebop.i2c_trans.buf[10] != 2 && autopilot_motors_on) {
if (actuators_bebop.i2c_trans.buf[10] != 4 && actuators_bebop.i2c_trans.buf[10] != 2 && autopilot_get_motors_on()) {
// Reset the error
actuators_bebop.i2c_trans.buf[0] = ACTUATORS_BEBOP_CLEAR_ERROR;
i2c_transmit(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 1);
@@ -107,10 +107,10 @@ void actuators_bebop_commit(void)
i2c_transmit(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 2);
}
// Stop the motors
else if (actuators_bebop.i2c_trans.buf[10] == 4 && !autopilot_motors_on) {
else if (actuators_bebop.i2c_trans.buf[10] == 4 && !autopilot_get_motors_on()) {
actuators_bebop.i2c_trans.buf[0] = ACTUATORS_BEBOP_STOP_PROP;
i2c_transmit(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 1);
} else if (actuators_bebop.i2c_trans.buf[10] == 4 && autopilot_motors_on) {
} else if (actuators_bebop.i2c_trans.buf[10] == 4 && autopilot_get_motors_on()) {
// Send the commands
actuators_bebop.i2c_trans.buf[0] = ACTUATORS_BEBOP_SET_REF_SPEED;
actuators_bebop.i2c_trans.buf[1] = actuators_bebop.rpm_ref[0] >> 8;
sw/airborne/boards/disco/actuators.c
+1 -1
View File
@@ -135,7 +135,7 @@ void actuators_disco_commit(void)
// When detected a suicide
uint8_t bldc_status = obs_data.status & 0x07;
if (obs_data.error == 2 && bldc_status != DISCO_BLDC_STATUS_STOPPED) {
kill_throttle = true;
autopilot_set_kill_throttle(true);
}
// Start the motors
sw/airborne/firmwares/fixedwing/autopilot.h
-172
View File
@@ -1,172 +0,0 @@
/*
* Copyright (C) 2003 Pascal Brisset, Antoine Drouin
*
* 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/fixedwing/autopilot.h
*
* Fixedwing autopilot modes.
*
*/
#ifndef AUTOPILOT_H
#define AUTOPILOT_H
#include <inttypes.h>
#include "std.h"
#include "paparazzi.h"
#include "generated/airframe.h"
/** Autopilot inititalization.
*/
extern void autopilot_init(void);
/** Threshold for RC mode detection.
*/
#define THRESHOLD_MANUAL_PPRZ (MIN_PPRZ / 2)
#define THRESHOLD1 THRESHOLD_MANUAL_PPRZ
#define THRESHOLD2 (MAX_PPRZ/2)
/** AP modes.
*/
#define PPRZ_MODE_MANUAL 0
#define PPRZ_MODE_AUTO1 1
#define PPRZ_MODE_AUTO2 2
#define PPRZ_MODE_HOME 3
#define PPRZ_MODE_GPS_OUT_OF_ORDER 4
#define PPRZ_MODE_NB 5
#define PPRZ_MODE_OF_PULSE(pprz) \
(pprz > THRESHOLD2 ? PPRZ_MODE_AUTO2 : \
(pprz > THRESHOLD1 ? PPRZ_MODE_AUTO1 : PPRZ_MODE_MANUAL))
extern uint8_t pprz_mode;
extern bool kill_throttle;
extern uint8_t mcu1_status;
/** flight time in seconds. */
extern uint16_t autopilot_flight_time;
#define autopilot_ResetFlightTimeAndLaunch(_) { \
autopilot_flight_time = 0; launch = false; \
}
// FIXME, move to control
#define LATERAL_MODE_MANUAL 0
#define LATERAL_MODE_ROLL_RATE 1
#define LATERAL_MODE_ROLL 2
#define LATERAL_MODE_COURSE 3
#define LATERAL_MODE_NB 4
extern uint8_t lateral_mode;
#define STICK_PUSHED(pprz) (pprz < THRESHOLD1 || pprz > THRESHOLD2)
#define FLOAT_OF_PPRZ(pprz, center, travel) ((float)pprz / (float)MAX_PPRZ * travel + center)
#define THROTTLE_THRESHOLD_TAKEOFF (pprz_t)(MAX_PPRZ * 0.9)
/** Supply voltage in deciVolt.
* This the ap copy of the measurement from fbw
*/
extern uint16_t vsupply;
/** Supply current in milliAmpere.
* This the ap copy of the measurement from fbw
*/
extern int32_t current; // milliAmpere
/** Energy consumption (mAh)
* This is the ap copy of the measurement from fbw
*/
extern float energy;
extern bool launch;
extern bool gps_lost;
/** Assignment, returning _old_value != _value
* Using GCC expression statements */
#define ModeUpdate(_mode, _value) ({ \
uint8_t new_mode = _value; \
(_mode != new_mode ? _mode = new_mode, TRUE : FALSE); \
})
/** Send mode over telemetry
*/
extern void autopilot_send_mode(void);
/** Power switch control.
*/
extern bool power_switch;
#ifdef POWER_SWITCH_GPIO
#include "mcu_periph/gpio.h"
#define autopilot_SetPowerSwitch(_x) { \
power_switch = _x; \
if (_x) { gpio_set(POWER_SWITCH_GPIO); } \
else { gpio_clear(POWER_SWITCH_GPIO); } \
}
#else // POWER_SWITCH_GPIO
#define autopilot_SetPowerSwitch(_x) { power_switch = _x; }
#endif // POWER_SWITCH_GPIO
/* CONTROL_RATE will be removed in the next release
* please use CONTROL_FREQUENCY instead
*/
#ifndef CONTROL_FREQUENCY
#ifdef CONTROL_RATE
#define CONTROL_FREQUENCY CONTROL_RATE
#warning "CONTROL_RATE is deprecated. Please use CONTROL_FREQUENCY instead. Defaults to 60Hz if not defined."
#else
#define CONTROL_FREQUENCY 60
#endif // CONTROL_RATE
#endif // CONTROL_FREQUENCY
#ifndef NAVIGATION_FREQUENCY
#define NAVIGATION_FREQUENCY 4
#endif
#include "subsystems/settings.h"
/* try to make sure that we don't write to flash while flying */
static inline void autopilot_StoreSettings(float store)
{
if (kill_throttle && store) {
settings_store_flag = store;
settings_store();
}
}
static inline void autopilot_ClearSettings(float clear)
{
if (kill_throttle && clear) {
settings_clear_flag = clear;
settings_clear();
}
}
#if DOWNLINK
#include "pprzlink/pprzlink_transport.h"
extern void send_autopilot_version(struct transport_tx *trans, struct link_device *dev);
#endif
#endif /* AUTOPILOT_H */
sw/airborne/firmwares/fixedwing/autopilot.c → sw/airborne/firmwares/fixedwing/autopilot_firmware.c
+16 -73
View File
@@ -1,5 +1,5 @@
/*
* Copyright (C) 2012 Gautier Hattenberger
* Copyright (C) 2012-2017 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* This file is part of paparazzi.
*
@@ -14,65 +14,35 @@
* 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.
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file firmwares/fixedwing/autopilot.c
* @file firmwares/fixedwing/autopilot_firmware.c
*
* Fixedwing autopilot inititalization.
* Fixedwing specific autopilot interface
* and initialization
*
*/
#include <stdint.h>
#include "firmwares/fixedwing/autopilot.h"
#include "firmwares/fixedwing/autopilot_firmware.h"
#include "state.h"
#include "firmwares/fixedwing/nav.h"
#ifdef POWER_SWITCH_GPIO
#include "mcu_periph/gpio.h"
#endif
#include "pprz_version.h"
uint8_t pprz_mode;
bool kill_throttle;
uint8_t mcu1_status;
bool launch;
/** flight time in seconds. */
uint16_t autopilot_flight_time;
uint8_t lateral_mode;
#include <stdint.h>
uint16_t vsupply;
int32_t current;
float energy;
bool gps_lost;
bool power_switch;
uint8_t lateral_mode;
uint8_t mcu1_status;
#if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h"
#include "generated/settings.h"
void send_autopilot_version(struct transport_tx *trans, struct link_device *dev)
{
static uint32_t ap_version = PPRZ_VERSION_INT;
static char *ver_desc = PPRZ_VERSION_DESC;
pprz_msg_send_AUTOPILOT_VERSION(trans, dev, AC_ID, &ap_version, strlen(ver_desc), ver_desc);
}
static void send_alive(struct transport_tx *trans, struct link_device *dev)
{
pprz_msg_send_ALIVE(trans, dev, AC_ID, 16, MD5SUM);
}
#if defined RADIO_CALIB && defined RADIO_CONTROL_SETTINGS
#include "modules/settings/rc_settings.h"
static void send_rc_settings(struct transport_tx *trans, struct link_device *dev)
@@ -88,16 +58,9 @@ uint8_t rc_settings_mode = 0;
static void send_mode(struct transport_tx *trans, struct link_device *dev)
{
pprz_msg_send_PPRZ_MODE(trans, dev, AC_ID,
&pprz_mode, &v_ctl_mode, &lateral_mode, &horizontal_mode, &rc_settings_mode, &mcu1_status);
&autopilot.mode, &v_ctl_mode, &lateral_mode, &horizontal_mode, &rc_settings_mode, &mcu1_status);
}
static void send_attitude(struct transport_tx *trans, struct link_device *dev)
{
struct FloatEulers *att = stateGetNedToBodyEulers_f();
pprz_msg_send_ATTITUDE(trans, dev, AC_ID,
&(att->phi), &(att->psi), &(att->theta));
};
static void send_estimator(struct transport_tx *trans, struct link_device *dev)
{
pprz_msg_send_ESTIMATOR(trans, dev, AC_ID,
@@ -114,7 +77,7 @@ static void send_bat(struct transport_tx *trans, struct link_device *dev)
}
pprz_msg_send_BAT(trans, dev, AC_ID,
&v_ctl_throttle_slewed, &vsupply, &amps,
&autopilot_flight_time, (uint8_t *)(&kill_throttle),
&autopilot.flight_time, (uint8_t *)(&autopilot.kill_throttle),
&block_time, &stage_time, &e);
}
@@ -130,11 +93,6 @@ static void send_energy(struct transport_tx *trans, struct link_device *dev)
pprz_msg_send_ENERGY(trans, dev, AC_ID, &vsup, &curs, &e, &power);
}
static void send_dl_value(struct transport_tx *trans, struct link_device *dev)
{
PeriodicSendDlValue(trans, dev);
}
// FIXME not the best place
#include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
#include CTRL_TYPE_H
@@ -172,34 +130,19 @@ void autopilot_send_mode(void)
#endif
}
void autopilot_init(void)
void autopilot_firmware_init(void)
{
pprz_mode = PPRZ_MODE_AUTO2;
kill_throttle = false;
launch = false;
autopilot_flight_time = 0;
lateral_mode = LATERAL_MODE_MANUAL;
gps_lost = false;
power_switch = false;
#ifdef POWER_SWITCH_GPIO
gpio_setup_output(POWER_SWITCH_GPIO);
gpio_clear(POWER_SWITCH_GPIO);
#endif
vsupply = 0;
current = 0;
energy = 0.f;
#if PERIODIC_TELEMETRY
/* register some periodic message */
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_PPRZ_MODE, send_mode);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ATTITUDE, send_attitude);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ESTIMATOR, send_estimator);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AIRSPEED, send_airspeed);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_BAT, send_bat);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ENERGY, send_energy);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_DL_VALUE, send_dl_value);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_DESIRED, send_desired);
#if defined RADIO_CALIB && defined RADIO_CONTROL_SETTINGS
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_RC_SETTINGS, send_rc_settings);
sw/airborne/firmwares/fixedwing/autopilot_firmware.h
+62
View File
@@ -0,0 +1,62 @@
/*
* Copyright (C) 2017 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* 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, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file firmwares/fixedwing/autopilot_firmware.h
*
* Fixedwing specific autopilot interface
* and initialization
*/
#ifndef AUTOPILOT_FIRMWARE_H
#define AUTOPILOT_FIRMWARE_H
#include "std.h"
#include "autopilot.h"
/** Supply voltage in deciVolt.
* This the ap copy of the measurement from fbw
* FIXME use electrical module ?
*/
extern uint16_t vsupply;
/** Supply current in milliAmpere.
* This the ap copy of the measurement from fbw
* FIXME use electrical module ?
*/
extern int32_t current; // milliAmpere
/** Energy consumption (mAh)
* This is the ap copy of the measurement from fbw
* FIXME use electrical module ?
*/
extern float energy;
/** Second MCU status (FBW part)
*/
extern uint8_t mcu1_status;
/** Init function
*/
extern void autopilot_firmware_init(void);
#endif /* AUTOPILOT_FIRMWARE_H */
sw/airborne/firmwares/fixedwing/autopilot_generated.c
+103
View File
@@ -0,0 +1,103 @@
/*
* Copyright (C) 2017 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* 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, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file firmwares/fixedwing/autopilot_generated.c
*
* Generated autopilot implementation.
*
*/
#define AUTOPILOT_CORE_AP_C
#include "firmwares/fixedwing/autopilot_generated.h"
#include "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"
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;
}
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;
}
/** 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)
{
// Do nothing on fixedwing ?
}
static inline void copy_from_to_fbw(void)
{
PPRZ_MUTEX_LOCK(fbw_state_mtx);
PPRZ_MUTEX_LOCK(ap_state_mtx);
#ifdef SetAutoCommandsFromRC
SetAutoCommandsFromRC(ap_state->commands, fbw_state->channels);
#elif defined RADIO_YAW && defined COMMAND_YAW
ap_state->commands[COMMAND_YAW] = fbw_state->channels[RADIO_YAW];
#endif
PPRZ_MUTEX_UNLOCK(ap_state_mtx);
PPRZ_MUTEX_UNLOCK(fbw_state_mtx);
}
void autopilot_generated_on_rc_frame(void)
{
// FIXME what to do here ?
copy_from_to_fbw();
}
sw/airborne/firmwares/fixedwing/autopilot_generated.h
+45
View File
@@ -0,0 +1,45 @@
/*
* Copyright (C) 2017 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* 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, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file firmwares/fixedwing/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 */
sw/airborne/firmwares/fixedwing/autopilot_static.c
+403
View File
@@ -0,0 +1,403 @@
/*
* Copyright (C) 2003 Pascal Brisset, Antoine Drouin
* Copyright (C) 2017 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* 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, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file firmwares/fixedwing/autopilot.c
*
* Fixedwing autopilot inititalization.
*
*/
#include "autopilot.h"
#include "firmwares/fixedwing/autopilot_static.h"
#include "inter_mcu.h"
#include "link_mcu.h"
#include "state.h"
#include "firmwares/fixedwing/nav.h"
#include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
#include "generated/flight_plan.h"
/* Geofence exceptions */
#include "modules/nav/nav_geofence.h"
#if USE_GPS
#include "subsystems/gps.h"
#endif
static bool gps_lost;
#include "subsystems/datalink/downlink.h"
#if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1
static uint8_t mcu1_ppm_cpt;
#endif
static inline uint8_t pprz_mode_update(void);
static inline uint8_t mcu1_status_update(void);
static inline void copy_from_to_fbw(void);
/** mode to enter when RC is lost in PPRZ_MODE_MANUAL or PPRZ_MODE_AUTO1 */
#ifndef RC_LOST_MODE
#define RC_LOST_MODE PPRZ_MODE_HOME
#endif
/// @todo, properly implement or remove
#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
#include "subsystems/abi.h"
volatile uint8_t new_ins_attitude = 0;
static abi_event new_att_ev;
static void new_att_cb(uint8_t sender_id __attribute__((unused)),
uint32_t stamp __attribute__((unused)),
struct Int32Rates *gyro __attribute__((unused)))
{
new_ins_attitude = 1;
}
/** use AP event in case of attitude control triggered by INS data
*/
void autopilot_event(void)
{
if (new_ins_attitude > 0) {
attitude_loop();
new_ins_attitude = 0;
}
}
#endif
/** Static autopilot API
*/
void autopilot_static_init(void)
{
autopilot.mode = PPRZ_MODE_AUTO2;
lateral_mode = LATERAL_MODE_MANUAL;
gps_lost = false;
///@todo: properly implement/fix a triggered attitude loop
#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
AbiBindMsgIMU_GYRO_INT32(ABI_BROADCAST, &new_att_ev, &new_att_cb);
#endif
}
void autopilot_static_periodic(void)
{
attitude_loop();
}
/**
* Function to be called when a message from FBW is available
*/
void autopilot_static_on_rc_frame(void)
{
uint8_t mode_changed = false;
copy_from_to_fbw();
/* really_lost is true if we lost RC in MANUAL or AUTO1 */
uint8_t really_lost = bit_is_set(imcu_get_status(), STATUS_RADIO_REALLY_LOST) &&
(autopilot_get_mode() == PPRZ_MODE_AUTO1 || autopilot_get_mode() == PPRZ_MODE_MANUAL);
if (autopilot_get_mode() != PPRZ_MODE_HOME && autopilot_get_mode() != PPRZ_MODE_GPS_OUT_OF_ORDER && autopilot.launch) {
if (too_far_from_home || datalink_lost() || higher_than_max_altitude()) {
mode_changed = autopilot_set_mode(PPRZ_MODE_HOME);
}
if (really_lost) {
mode_changed = autopilot_set_mode(RC_LOST_MODE);
}
}
if (bit_is_set(imcu_get_status(), AVERAGED_CHANNELS_SENT)) {
bool pprz_mode_changed = pprz_mode_update();
mode_changed |= pprz_mode_changed;
#if defined RADIO_CALIB && defined RADIO_CONTROL_SETTINGS
PPRZ_MUTEX_LOCK(fbw_state_mtx);
bool calib_mode_changed = RcSettingsModeUpdate(fbw_state->channels);
PPRZ_MUTEX_UNLOCK(fbw_state_mtx);
rc_settings(calib_mode_changed || pprz_mode_changed);
mode_changed |= calib_mode_changed;
#endif
}
mode_changed |= mcu1_status_update();
if (mode_changed) { autopilot_send_mode(); }
#if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1
/** In AUTO1 mode, compute roll setpoint and pitch setpoint from
* \a RADIO_ROLL and \a RADIO_PITCH \n
*/
if (autopilot_get_mode() == PPRZ_MODE_AUTO1) {
/** Roll is bounded between [-AUTO1_MAX_ROLL;AUTO1_MAX_ROLL] */
h_ctl_roll_setpoint = FLOAT_OF_PPRZ(imcu_get_radio(RADIO_ROLL), 0., AUTO1_MAX_ROLL);
/** Pitch is bounded between [-AUTO1_MAX_PITCH;AUTO1_MAX_PITCH] */
h_ctl_pitch_setpoint = FLOAT_OF_PPRZ(imcu_get_radio(RADIO_PITCH), 0., AUTO1_MAX_PITCH);
#if H_CTL_YAW_LOOP && defined RADIO_YAW
/** Yaw is bounded between [-AUTO1_MAX_YAW_RATE;AUTO1_MAX_YAW_RATE] */
h_ctl_yaw_rate_setpoint = FLOAT_OF_PPRZ(imcu_get_radio(RADIO_YAW), 0., AUTO1_MAX_YAW_RATE);
#endif
} /** Else asynchronously set by \a h_ctl_course_loop() */
/** In AUTO1, throttle comes from RADIO_THROTTLE
In MANUAL, the value is copied to get it in the telemetry */
if (autopilot_get_mode() == PPRZ_MODE_MANUAL || autopilot_get_mode() == PPRZ_MODE_AUTO1) {
v_ctl_throttle_setpoint = imcu_get_radio(RADIO_THROTTLE);
}
/** else asynchronously set by v_ctl_climb_loop(); */
mcu1_ppm_cpt = imcu_get_ppm_cpt();
#endif // RADIO_CONTROL
// update electrical from FBW
imcu_get_electrical(&vsupply, &current, &energy);
#ifdef RADIO_CONTROL
/* the SITL check is a hack to prevent "automatic" launch in NPS */
#ifndef SITL
if (!autopilot.flight_time) {
if (autopilot_get_mode() == PPRZ_MODE_AUTO2 && imcu_get_radio(RADIO_THROTTLE) > THROTTLE_THRESHOLD_TAKEOFF) {
autopilot.launch = true;
}
}
#endif
#endif
}
void autopilot_static_set_mode(uint8_t new_autopilot_mode)
{
if (new_autopilot_mode != autopilot.mode) {
autopilot.mode = new_autopilot_mode;
}
}
void autopilot_static_SetModeHandler(float new_autopilot_mode)
{
autopilot_static_set_mode(new_autopilot_mode);
}
void autopilot_static_set_motors_on(bool motors_on __attribute__((unused)))
{
// Do nothing on fixedwing ?
}
#ifdef FAILSAFE_DELAY_WITHOUT_GPS
#define GpsTimeoutError (sys_time.nb_sec - gps.last_3dfix_time > FAILSAFE_DELAY_WITHOUT_GPS)
#endif
/**
* Compute desired_course
*/
void navigation_task(void)
{
#if defined FAILSAFE_DELAY_WITHOUT_GPS
/** This section is used for the failsafe of GPS */
static uint8_t last_pprz_mode;
/** If aircraft is launched and is in autonomus mode, go into
PPRZ_MODE_GPS_OUT_OF_ORDER mode (Failsafe) if we lost the GPS */
if (autopilot.launch) {
if (GpsTimeoutError) {
if (autopilot_get_mode() == PPRZ_MODE_AUTO2 || autopilot_get_mode() == PPRZ_MODE_HOME) {
last_pprz_mode = autopilot_get_mode();
autopilot_set_mode(PPRZ_MODE_GPS_OUT_OF_ORDER);
autopilot_send_mode();
gps_lost = true;
}
} else if (gps_lost) { /* GPS is ok */
/** If aircraft was in failsafe mode, come back in previous mode */
autopilot_set_mode(last_pprz_mode);
gps_lost = false;
autopilot_send_mode();
}
}
#endif /* GPS && FAILSAFE_DELAY_WITHOUT_GPS */
common_nav_periodic_task_4Hz();
if (autopilot_get_mode() == PPRZ_MODE_HOME) {
nav_home();
} else if (autopilot_get_mode() == PPRZ_MODE_GPS_OUT_OF_ORDER) {
nav_without_gps();
} else {
nav_periodic_task();
}
#ifdef TCAS
callTCAS();
#endif
#if DOWNLINK && !defined PERIOD_NAVIGATION_Ap_0 // If not sent periodically (in default 0 mode)
SEND_NAVIGATION(&(DefaultChannel).trans_tx, &(DefaultDevice).device);
#endif
/* The nav task computes only nav_altitude. However, we are interested
by desired_altitude (= nav_alt+alt_shift) in any case.
So we always run the altitude control loop */
if (v_ctl_mode == V_CTL_MODE_AUTO_ALT) {
v_ctl_altitude_loop();
}
if (autopilot_get_mode() == PPRZ_MODE_AUTO2 || autopilot_get_mode() == PPRZ_MODE_HOME
|| autopilot_get_mode() == PPRZ_MODE_GPS_OUT_OF_ORDER) {
#ifdef H_CTL_RATE_LOOP
/* Be sure to be in attitude mode, not roll */
h_ctl_auto1_rate = false;
#endif
if (lateral_mode >= LATERAL_MODE_COURSE) {
h_ctl_course_loop(); /* aka compute nav_desired_roll */
}
// climb_loop(); //4Hz
}
}
void attitude_loop(void)
{
if (autopilot_get_mode() >= PPRZ_MODE_AUTO2) {
#if CTRL_VERTICAL_LANDING
if (v_ctl_mode == V_CTL_MODE_LANDING) {
v_ctl_landing_loop();
} else {
#endif
if (v_ctl_mode == V_CTL_MODE_AUTO_THROTTLE) {
v_ctl_throttle_setpoint = nav_throttle_setpoint;
v_ctl_pitch_setpoint = nav_pitch;
} else {
if (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB) {
v_ctl_climb_loop();
} /* v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB */
} /* v_ctl_mode == V_CTL_MODE_AUTO_THROTTLE */
#if CTRL_VERTICAL_LANDING
} /* v_ctl_mode == V_CTL_MODE_LANDING */
#endif
#if defined V_CTL_THROTTLE_IDLE
Bound(v_ctl_throttle_setpoint, TRIM_PPRZ(V_CTL_THROTTLE_IDLE * MAX_PPRZ), MAX_PPRZ);
#endif
#ifdef V_CTL_POWER_CTL_BAT_NOMINAL
if (vsupply > 0.) {
v_ctl_throttle_setpoint *= 10. * V_CTL_POWER_CTL_BAT_NOMINAL / (float)vsupply;
v_ctl_throttle_setpoint = TRIM_UPPRZ(v_ctl_throttle_setpoint);
}
#endif
// Copy the pitch setpoint from the guidance to the stabilization control
h_ctl_pitch_setpoint = v_ctl_pitch_setpoint;
Bound(h_ctl_pitch_setpoint, H_CTL_PITCH_MIN_SETPOINT, H_CTL_PITCH_MAX_SETPOINT);
if (autopilot.kill_throttle || (!autopilot.flight_time && !autopilot.launch)) {
v_ctl_throttle_setpoint = 0;
}
}
h_ctl_attitude_loop(); /* Set h_ctl_aileron_setpoint & h_ctl_elevator_setpoint */
v_ctl_throttle_slew();
PPRZ_MUTEX_LOCK(ap_state_mtx);
ap_state->commands[COMMAND_THROTTLE] = v_ctl_throttle_slewed;
ap_state->commands[COMMAND_ROLL] = -h_ctl_aileron_setpoint;
ap_state->commands[COMMAND_PITCH] = h_ctl_elevator_setpoint;
#if H_CTL_YAW_LOOP && defined COMMAND_YAW
ap_state->commands[COMMAND_YAW] = h_ctl_rudder_setpoint;
#endif
#if H_CTL_CL_LOOP && defined COMMAND_CL
ap_state->commands[COMMAND_CL] = h_ctl_flaps_setpoint;
#endif
PPRZ_MUTEX_UNLOCK(ap_state_mtx);
#if defined MCU_SPI_LINK || defined MCU_UART_LINK || defined MCU_CAN_LINK
link_mcu_send();
#elif defined INTER_MCU && defined SINGLE_MCU
/**Directly set the flag indicating to FBW that shared buffer is available*/
inter_mcu_received_ap = true;
#endif
}
/******************** Interaction with FBW *****************************/
/** Update paparazzi mode.
*/
#if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1
static inline uint8_t pprz_mode_update(void)
{
if ((autopilot_get_mode() != PPRZ_MODE_HOME &&
autopilot_get_mode() != PPRZ_MODE_GPS_OUT_OF_ORDER)
#ifdef UNLOCKED_HOME_MODE
|| true
#endif
) {
#ifndef RADIO_AUTO_MODE
return autopilot_set_mode(PPRZ_MODE_OF_PULSE(imcu_get_radio(RADIO_MODE)));
#else
INFO("Using RADIO_AUTO_MODE to switch between AUTO1 and AUTO2.")
/* If RADIO_AUTO_MODE is enabled mode swithing will be seperated between two switches/channels
* RADIO_MODE will switch between PPRZ_MODE_MANUAL and any PPRZ_MODE_AUTO mode selected by RADIO_AUTO_MODE.
*
* 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 (PPRZ_MODE_OF_PULSE(imcu_get_radio(RADIO_MODE)) == PPRZ_MODE_MANUAL) {
/* RADIO_MODE in MANUAL position */
return autopilot_set_mode(PPRZ_MODE_MANUAL);
} else {
/* RADIO_MODE not in MANUAL position.
* Select AUTO mode bassed on RADIO_AUTO_MODE channel
*/
return autopilot_set_mode((imcu_get_radio(RADIO_AUTO_MODE) > THRESHOLD2) ? PPRZ_MODE_AUTO2 : PPRZ_MODE_AUTO1);
}
#endif // RADIO_AUTO_MODE
} else {
return false;
}
}
#else // not RADIO_CONTROL
static inline uint8_t pprz_mode_update(void)
{
return false;
}
#endif
static inline uint8_t mcu1_status_update(void)
{
uint8_t new_status = imcu_get_status();
if (mcu1_status != new_status) {
bool changed = ((mcu1_status & MASK_FBW_CHANGED) != (new_status & MASK_FBW_CHANGED));
mcu1_status = new_status;
return changed;
}
return false;
}
/** Send back uncontrolled channels.
*/
static inline void copy_from_to_fbw(void)
{
PPRZ_MUTEX_LOCK(fbw_state_mtx);
PPRZ_MUTEX_LOCK(ap_state_mtx);
#ifdef SetAutoCommandsFromRC
SetAutoCommandsFromRC(ap_state->commands, fbw_state->channels);
#elif defined RADIO_YAW && defined COMMAND_YAW
ap_state->commands[COMMAND_YAW] = fbw_state->channels[RADIO_YAW];
#endif
PPRZ_MUTEX_UNLOCK(ap_state_mtx);
PPRZ_MUTEX_UNLOCK(fbw_state_mtx);
}
sw/airborne/firmwares/fixedwing/autopilot_static.h
+99
View File
@@ -0,0 +1,99 @@
/*
* Copyright (C) 2003 Pascal Brisset, Antoine Drouin
* Copyright (C) 2017 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* 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, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file firmwares/fixedwing/autopilot_static.h
*
* Fixedwing autopilot modes (static implementation).
*
*/
#ifndef AUTOPILOT_STATIC_H
#define AUTOPILOT_STATIC_H
#include "autopilot.h"
/** AP modes.
*/
#define PPRZ_MODE_MANUAL 0
#define PPRZ_MODE_AUTO1 1
#define PPRZ_MODE_AUTO2 2
#define PPRZ_MODE_HOME 3
#define PPRZ_MODE_GPS_OUT_OF_ORDER 4
#define PPRZ_MODE_NB 5
/** Static autopilot functions
*/
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);
/** Control loops
* FIXME should be somewhere else
*/
extern void navigation_task(void);
extern void attitude_loop(void);
/** Threshold for RC mode detection.
*/
#define THRESHOLD_MANUAL_PPRZ (MIN_PPRZ / 2)
#define THRESHOLD1 THRESHOLD_MANUAL_PPRZ
#define THRESHOLD2 (MAX_PPRZ/2)
#define PPRZ_MODE_OF_PULSE(pprz) \
(pprz > THRESHOLD2 ? PPRZ_MODE_AUTO2 : \
(pprz > THRESHOLD1 ? PPRZ_MODE_AUTO1 : PPRZ_MODE_MANUAL))
// FIXME, move to control
#define LATERAL_MODE_MANUAL 0
#define LATERAL_MODE_ROLL_RATE 1
#define LATERAL_MODE_ROLL 2
#define LATERAL_MODE_COURSE 3
#define LATERAL_MODE_NB 4
extern uint8_t lateral_mode;
#define STICK_PUSHED(pprz) (pprz < THRESHOLD1 || pprz > THRESHOLD2)
#define FLOAT_OF_PPRZ(pprz, center, travel) ((float)pprz / (float)MAX_PPRZ * travel + center)
#define THROTTLE_THRESHOLD_TAKEOFF (pprz_t)(MAX_PPRZ * 0.9)
/* CONTROL_RATE will be removed in the next release
* please use CONTROL_FREQUENCY instead
*/
#ifndef CONTROL_FREQUENCY
#ifdef CONTROL_RATE
#define CONTROL_FREQUENCY CONTROL_RATE
#warning "CONTROL_RATE is deprecated. Please use CONTROL_FREQUENCY instead. Defaults to 60Hz if not defined."
#else
#define CONTROL_FREQUENCY 60
#endif // CONTROL_RATE
#endif // CONTROL_FREQUENCY
#ifndef NAVIGATION_FREQUENCY
#define NAVIGATION_FREQUENCY 4
#endif
#endif /* AUTOPILOT_STATIC_H */
sw/airborne/firmwares/fixedwing/autopilot_utils.c
+30
View File
@@ -0,0 +1,30 @@
/*
* Copyright (C) 2017 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* 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/fixedwing/autopilot_utils.c
*
* Utility functions and includes for autopilots
*
*/
#include "firmwares/fixedwing/autopilot_utils.h"
sw/airborne/firmwares/fixedwing/autopilot_utils.h
+35
View File
@@ -0,0 +1,35 @@
/*
* Copyright (C) 2017 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* 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/fixedwing/autopilot_utils.h
*
* Utility functions and includes for autopilots
*
*/
#ifndef AUTOPILOT_UTILS_H
#define AUTOPILOT_UTILS_H
#include "std.h"
#endif // AUTOPILOT_UTILS_H
sw/airborne/firmwares/fixedwing/fixedwing_datalink.c
+1 -1
View File
@@ -52,7 +52,7 @@
#include "autopilot.h"
uint8_t joystick_block;
#define JoystickHandeDatalink(_roll_int8, _pitch_int8, _throttle_int8) { \
if (pprz_mode == PPRZ_MODE_AUTO2 && nav_block == joystick_block) { \
if (autopilot_get_mode() == PPRZ_MODE_AUTO2 && nav_block == joystick_block) { \
h_ctl_roll_setpoint = _roll_int8 * (AUTO1_MAX_ROLL / 0x7f); \
h_ctl_pitch_setpoint = _pitch_int8 * (AUTO1_MAX_PITCH / 0x7f); \
v_ctl_throttle_setpoint = (MAX_PPRZ/0x7f) * _throttle_int8; \
sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c
+5 -5
View File
@@ -65,7 +65,7 @@
#include "state.h"
#include "firmwares/fixedwing/nav.h"
#include "generated/airframe.h"
#include "firmwares/fixedwing/autopilot.h"
#include "autopilot.h"
#include "subsystems/abi.h"
/////// DEFAULT GUIDANCE_V NECESSITIES //////
@@ -391,7 +391,7 @@ void v_ctl_climb_loop(void)
float en_dis_err = gamma_err - vdot_err;
// Auto Cruise Throttle
if (launch && (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB)) {
if (autopilot.launch && (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB)) {
v_ctl_auto_throttle_nominal_cruise_throttle +=
v_ctl_auto_throttle_of_airspeed_igain * speed_error * dt_attidude
+ en_tot_err * v_ctl_energy_total_igain * dt_attidude;
@@ -404,7 +404,7 @@ void v_ctl_climb_loop(void)
+ v_ctl_auto_throttle_of_airspeed_pgain * speed_error
+ v_ctl_energy_total_pgain * en_tot_err;
if ((controlled_throttle >= 1.0f) || (controlled_throttle <= 0.0f) || (kill_throttle == 1)) {
if ((controlled_throttle >= 1.0f) || (controlled_throttle <= 0.0f) || (autopilot_throttle_killed() == 1)) {
// If your energy supply is not sufficient, then neglect the climb requirement
en_dis_err = -vdot_err;
@@ -415,7 +415,7 @@ void v_ctl_climb_loop(void)
/* pitch pre-command */
if (launch && (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB)) {
if (autopilot.launch && (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB)) {
v_ctl_auto_throttle_nominal_cruise_pitch += v_ctl_auto_pitch_of_airspeed_igain * (-speed_error) * dt_attidude
+ v_ctl_energy_diff_igain * en_dis_err * dt_attidude;
Bound(v_ctl_auto_throttle_nominal_cruise_pitch, H_CTL_PITCH_MIN_SETPOINT, H_CTL_PITCH_MAX_SETPOINT);
@@ -426,7 +426,7 @@ void v_ctl_climb_loop(void)
+ v_ctl_auto_pitch_of_airspeed_dgain * vdot
+ v_ctl_energy_diff_pgain * en_dis_err
+ v_ctl_auto_throttle_nominal_cruise_pitch;
if (kill_throttle) { v_ctl_pitch_of_vz = v_ctl_pitch_of_vz - 1 / V_CTL_GLIDE_RATIO; }
if (autopilot_throttle_killed()) { v_ctl_pitch_of_vz = v_ctl_pitch_of_vz - 1 / V_CTL_GLIDE_RATIO; }
v_ctl_pitch_setpoint = v_ctl_pitch_of_vz + nav_pitch;
Bound(v_ctl_pitch_setpoint, H_CTL_PITCH_MIN_SETPOINT, H_CTL_PITCH_MAX_SETPOINT)
sw/airborne/firmwares/fixedwing/guidance/guidance_v.c
+3 -3
View File
@@ -29,7 +29,7 @@
#include "state.h"
#include "firmwares/fixedwing/nav.h"
#include "generated/airframe.h"
#include "firmwares/fixedwing/autopilot.h"
#include "autopilot.h"
#include "firmwares/fixedwing/stabilization/stabilization_attitude.h" //> allow for roll control during landing final flare
/* mode */
@@ -324,7 +324,7 @@ void v_ctl_landing_loop(void)
float land_speed_err = v_ctl_landing_desired_speed - stateGetHorizontalSpeedNorm_f();
float land_alt_err = v_ctl_altitude_setpoint - stateGetPositionUtm_f()->alt;
if (kill_throttle
if (autopilot_throttle_killed()
&& (kill_alt - v_ctl_altitude_setpoint)
> (v_ctl_landing_alt_throttle_kill - v_ctl_landing_alt_flare)) {
v_ctl_throttle_setpoint = 0.0; // Throttle is already in KILL (command redundancy)
@@ -350,7 +350,7 @@ void v_ctl_landing_loop(void)
// update kill_alt until final kill throttle is initiated - allows for mode switch to first part of if statement above
// eliminates the need for knowing the altitude of TD
if (!kill_throttle) {
if (!autopilot_throttle_killed()) {
kill_alt = v_ctl_altitude_setpoint; //
if (land_alt_err > 0.0) {
nav_pitch = 0.01; // if below desired alt close to ground command level pitch
sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c
+4 -4
View File
@@ -30,7 +30,7 @@
#include "state.h"
#include "firmwares/fixedwing/nav.h"
#include "generated/airframe.h"
#include "firmwares/fixedwing/autopilot.h"
#include "autopilot.h"
/* mode */
uint8_t v_ctl_mode;
@@ -216,7 +216,7 @@ static inline void v_ctl_set_pitch(void)
{
static float last_err = 0.;
if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) {
if (autopilot_get_mode() == PPRZ_MODE_MANUAL || autopilot.launch == false) {
v_ctl_auto_pitch_sum_err = 0;
}
@@ -244,7 +244,7 @@ static inline void v_ctl_set_throttle(void)
{
static float last_err = 0.;
if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) {
if (autopilot_get_mode() == PPRZ_MODE_MANUAL || autopilot.launch == false) {
v_ctl_auto_throttle_sum_err = 0;
}
@@ -307,7 +307,7 @@ static inline void v_ctl_set_airspeed(void)
// Reset integrators in manual or before flight
if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) {
if (autopilot_get_mode() == PPRZ_MODE_MANUAL || autopilot.launch == false) {
v_ctl_auto_throttle_sum_err = 0.;
v_ctl_auto_pitch_sum_err = 0.;
v_ctl_auto_airspeed_throttle_sum_err = 0.;
sw/airborne/firmwares/fixedwing/main_ap.c
+32 -328
View File
@@ -59,9 +59,10 @@ PRINT_CONFIG_MSG_VALUE("USE_BARO_BOARD is TRUE, reading onboard baro: ", BARO_BO
#endif
// autopilot & control
// autopilot
#include "state.h"
#include "firmwares/fixedwing/autopilot.h"
#include "autopilot.h"
#include "firmwares/fixedwing/nav.h"
#include "generated/flight_plan.h"
// datalink & telemetry
@@ -97,9 +98,6 @@ PRINT_CONFIG_MSG_VALUE("USE_BARO_BOARD is TRUE, reading onboard baro: ", BARO_BO
#define COMMAND_YAW_TRIM 0
#endif
/* Geofence exceptions */
#include "modules/nav/nav_geofence.h"
/* if PRINT_CONFIG is defined, print some config options */
PRINT_CONFIG_VAR(PERIODIC_FREQUENCY)
PRINT_CONFIG_VAR(NAVIGATION_FREQUENCY)
@@ -135,10 +133,6 @@ INFO_VALUE("it is recommended to configure in your airframe PERIODIC_FREQUENCY t
#endif
#endif // USE_IMU
#if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1
static uint8_t mcu1_ppm_cpt;
#endif
tid_t modules_tid; ///< id for modules_periodic_task() timer
tid_t telemetry_tid; ///< id for telemetry_periodic() timer
@@ -151,19 +145,6 @@ tid_t baro_tid; ///< id for baro_periodic() timer
#endif
/// @todo, properly implement or remove
#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
volatile uint8_t new_ins_attitude = 0;
static abi_event new_att_ev;
static void new_att_cb(uint8_t sender_id __attribute__((unused)),
uint32_t stamp __attribute__((unused)),
struct Int32Rates *gyro __attribute__((unused)))
{
new_ins_attitude = 1;
}
#endif
void init_ap(void)
{
#ifndef SINGLE_MCU /** init done in main_fbw in single MCU */
@@ -184,11 +165,6 @@ void init_ap(void)
ahrs_aligner_init();
#endif
///@todo: properly implement/fix a triggered attitude loop
#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
AbiBindMsgIMU_GYRO_INT32(ABI_BROADCAST, &new_att_ev, &new_att_cb);
#endif
#if USE_AHRS
ahrs_init();
#endif
@@ -207,6 +183,14 @@ void init_ap(void)
modules_init();
// call autopilot implementation init after guidance modules init
// it will set startup mode
#if USE_GENERATED_AUTOPILOT
autopilot_generated_init();
#else
autopilot_static_init();
#endif
settings_init();
/**** start timers for periodic functions *****/
@@ -256,6 +240,12 @@ void handle_periodic_tasks_ap(void)
}
#endif
#if USE_GENERATED_AUTOPILOT
if (sys_time_check_and_ack_timer(attitude_tid)) {
autopilot_periodic();
}
#else
// static autopilot
if (sys_time_check_and_ack_timer(navigation_tid)) {
navigation_task();
}
@@ -264,6 +254,8 @@ void handle_periodic_tasks_ap(void)
if (sys_time_check_and_ack_timer(attitude_tid)) {
attitude_loop();
}
#endif
#endif
if (sys_time_check_and_ack_timer(modules_tid)) {
@@ -282,158 +274,6 @@ void handle_periodic_tasks_ap(void)
}
/******************** Interaction with FBW *****************************/
/** Update paparazzi mode.
*/
#if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1
static inline uint8_t pprz_mode_update(void)
{
if ((pprz_mode != PPRZ_MODE_HOME &&
pprz_mode != PPRZ_MODE_GPS_OUT_OF_ORDER)
#ifdef UNLOCKED_HOME_MODE
|| true
#endif
) {
#ifndef RADIO_AUTO_MODE
return ModeUpdate(pprz_mode, PPRZ_MODE_OF_PULSE(imcu_get_radio(RADIO_MODE)));
#else
INFO("Using RADIO_AUTO_MODE to switch between AUTO1 and AUTO2.")
/* If RADIO_AUTO_MODE is enabled mode swithing will be seperated between two switches/channels
* RADIO_MODE will switch between PPRZ_MODE_MANUAL and any PPRZ_MODE_AUTO mode selected by RADIO_AUTO_MODE.
*
* 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 (PPRZ_MODE_OF_PULSE(imcu_get_radio(RADIO_MODE)) == PPRZ_MODE_MANUAL) {
/* RADIO_MODE in MANUAL position */
return ModeUpdate(pprz_mode, PPRZ_MODE_MANUAL);
} else {
/* RADIO_MODE not in MANUAL position.
* Select AUTO mode bassed on RADIO_AUTO_MODE channel
*/
return ModeUpdate(pprz_mode, (imcu_get_radio(RADIO_AUTO_MODE) > THRESHOLD2) ? PPRZ_MODE_AUTO2 : PPRZ_MODE_AUTO1);
}
#endif // RADIO_AUTO_MODE
} else {
return false;
}
}
#else // not RADIO_CONTROL
static inline uint8_t pprz_mode_update(void)
{
return false;
}
#endif
static inline uint8_t mcu1_status_update(void)
{
uint8_t new_status = imcu_get_status();
if (mcu1_status != new_status) {
bool changed = ((mcu1_status & MASK_FBW_CHANGED) != (new_status & MASK_FBW_CHANGED));
mcu1_status = new_status;
return changed;
}
return false;
}
/** Send back uncontrolled channels.
*/
static inline void copy_from_to_fbw(void)
{
PPRZ_MUTEX_LOCK(fbw_state_mtx);
PPRZ_MUTEX_LOCK(ap_state_mtx);
#ifdef SetAutoCommandsFromRC
SetAutoCommandsFromRC(ap_state->commands, fbw_state->channels);
#elif defined RADIO_YAW && defined COMMAND_YAW
ap_state->commands[COMMAND_YAW] = fbw_state->channels[RADIO_YAW];
#endif
PPRZ_MUTEX_UNLOCK(ap_state_mtx);
PPRZ_MUTEX_UNLOCK(fbw_state_mtx);
}
/** mode to enter when RC is lost in PPRZ_MODE_MANUAL or PPRZ_MODE_AUTO1 */
#ifndef RC_LOST_MODE
#define RC_LOST_MODE PPRZ_MODE_HOME
#endif
/**
* Function to be called when a message from FBW is available
*/
static inline void telecommand_task(void)
{
uint8_t mode_changed = false;
copy_from_to_fbw();
/* really_lost is true if we lost RC in MANUAL or AUTO1 */
uint8_t really_lost = bit_is_set(imcu_get_status(), STATUS_RADIO_REALLY_LOST) &&
(pprz_mode == PPRZ_MODE_AUTO1 || pprz_mode == PPRZ_MODE_MANUAL);
if (pprz_mode != PPRZ_MODE_HOME && pprz_mode != PPRZ_MODE_GPS_OUT_OF_ORDER && launch) {
if (too_far_from_home || datalink_lost() || higher_than_max_altitude()) {
pprz_mode = PPRZ_MODE_HOME;
mode_changed = true;
}
if (really_lost) {
pprz_mode = RC_LOST_MODE;
mode_changed = true;
}
}
if (bit_is_set(imcu_get_status(), AVERAGED_CHANNELS_SENT)) {
bool pprz_mode_changed = pprz_mode_update();
mode_changed |= pprz_mode_changed;
#if defined RADIO_CALIB && defined RADIO_CONTROL_SETTINGS
PPRZ_MUTEX_LOCK(fbw_state_mtx);
bool calib_mode_changed = RcSettingsModeUpdate(fbw_state->channels);
PPRZ_MUTEX_UNLOCK(fbw_state_mtx);
rc_settings(calib_mode_changed || pprz_mode_changed);
mode_changed |= calib_mode_changed;
#endif
}
mode_changed |= mcu1_status_update();
if (mode_changed) { autopilot_send_mode(); }
#if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1
/** In AUTO1 mode, compute roll setpoint and pitch setpoint from
* \a RADIO_ROLL and \a RADIO_PITCH \n
*/
if (pprz_mode == PPRZ_MODE_AUTO1) {
/** Roll is bounded between [-AUTO1_MAX_ROLL;AUTO1_MAX_ROLL] */
h_ctl_roll_setpoint = FLOAT_OF_PPRZ(imcu_get_radio(RADIO_ROLL), 0., AUTO1_MAX_ROLL);
/** Pitch is bounded between [-AUTO1_MAX_PITCH;AUTO1_MAX_PITCH] */
h_ctl_pitch_setpoint = FLOAT_OF_PPRZ(imcu_get_radio(RADIO_PITCH), 0., AUTO1_MAX_PITCH);
#if H_CTL_YAW_LOOP && defined RADIO_YAW
/** Yaw is bounded between [-AUTO1_MAX_YAW_RATE;AUTO1_MAX_YAW_RATE] */
h_ctl_yaw_rate_setpoint = FLOAT_OF_PPRZ(imcu_get_radio(RADIO_YAW), 0., AUTO1_MAX_YAW_RATE);
#endif
} /** Else asynchronously set by \a h_ctl_course_loop() */
/** In AUTO1, throttle comes from RADIO_THROTTLE
In MANUAL, the value is copied to get it in the telemetry */
if (pprz_mode == PPRZ_MODE_MANUAL || pprz_mode == PPRZ_MODE_AUTO1) {
v_ctl_throttle_setpoint = imcu_get_radio(RADIO_THROTTLE);
}
/** else asynchronously set by v_ctl_climb_loop(); */
mcu1_ppm_cpt = imcu_get_ppm_cpt();
#endif // RADIO_CONTROL
// update electrical from FBW
imcu_get_electrical(&vsupply, &current, &energy);
#ifdef RADIO_CONTROL
/* the SITL check is a hack to prevent "automatic" launch in NPS */
#ifndef SITL
if (!autopilot_flight_time) {
if (pprz_mode == PPRZ_MODE_AUTO2 && imcu_get_radio(RADIO_THROTTLE) > THROTTLE_THRESHOLD_TAKEOFF) {
launch = true;
}
}
#endif
#endif
}
/**************************** Periodic tasks ***********************************/
@@ -448,7 +288,7 @@ void reporting_task(void)
/* initialisation phase during boot */
if (boot) {
#if DOWNLINK
send_autopilot_version(&(DefaultChannel).trans_tx, &(DefaultDevice).device);
autopilot_send_version();
#endif
boot = false;
}
@@ -462,140 +302,6 @@ void reporting_task(void)
}
#ifdef FAILSAFE_DELAY_WITHOUT_GPS
#define GpsTimeoutError (sys_time.nb_sec - gps.last_3dfix_time > FAILSAFE_DELAY_WITHOUT_GPS)
#endif
/**
* Compute desired_course
*/
void navigation_task(void)
{
#if defined FAILSAFE_DELAY_WITHOUT_GPS
/** This section is used for the failsafe of GPS */
static uint8_t last_pprz_mode;
/** If aircraft is launched and is in autonomus mode, go into
PPRZ_MODE_GPS_OUT_OF_ORDER mode (Failsafe) if we lost the GPS */
if (launch) {
if (GpsTimeoutError) {
if (pprz_mode == PPRZ_MODE_AUTO2 || pprz_mode == PPRZ_MODE_HOME) {
last_pprz_mode = pprz_mode;
pprz_mode = PPRZ_MODE_GPS_OUT_OF_ORDER;
autopilot_send_mode();
gps_lost = true;
}
} else if (gps_lost) { /* GPS is ok */
/** If aircraft was in failsafe mode, come back in previous mode */
pprz_mode = last_pprz_mode;
gps_lost = false;
autopilot_send_mode();
}
}
#endif /* GPS && FAILSAFE_DELAY_WITHOUT_GPS */
common_nav_periodic_task_4Hz();
if (pprz_mode == PPRZ_MODE_HOME) {
nav_home();
} else if (pprz_mode == PPRZ_MODE_GPS_OUT_OF_ORDER) {
nav_without_gps();
} else {
nav_periodic_task();
}
#ifdef TCAS
callTCAS();
#endif
#if DOWNLINK && !defined PERIOD_NAVIGATION_Ap_0 // If not sent periodically (in default 0 mode)
SEND_NAVIGATION(&(DefaultChannel).trans_tx, &(DefaultDevice).device);
#endif
/* The nav task computes only nav_altitude. However, we are interested
by desired_altitude (= nav_alt+alt_shift) in any case.
So we always run the altitude control loop */
if (v_ctl_mode == V_CTL_MODE_AUTO_ALT) {
v_ctl_altitude_loop();
}
if (pprz_mode == PPRZ_MODE_AUTO2 || pprz_mode == PPRZ_MODE_HOME
|| pprz_mode == PPRZ_MODE_GPS_OUT_OF_ORDER) {
#ifdef H_CTL_RATE_LOOP
/* Be sure to be in attitude mode, not roll */
h_ctl_auto1_rate = false;
#endif
if (lateral_mode >= LATERAL_MODE_COURSE) {
h_ctl_course_loop(); /* aka compute nav_desired_roll */
}
// climb_loop(); //4Hz
}
}
void attitude_loop(void)
{
if (pprz_mode >= PPRZ_MODE_AUTO2) {
#if CTRL_VERTICAL_LANDING
if (v_ctl_mode == V_CTL_MODE_LANDING) {
v_ctl_landing_loop();
} else {
#endif
if (v_ctl_mode == V_CTL_MODE_AUTO_THROTTLE) {
v_ctl_throttle_setpoint = nav_throttle_setpoint;
v_ctl_pitch_setpoint = nav_pitch;
} else {
if (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB) {
v_ctl_climb_loop();
} /* v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB */
} /* v_ctl_mode == V_CTL_MODE_AUTO_THROTTLE */
#if CTRL_VERTICAL_LANDING
} /* v_ctl_mode == V_CTL_MODE_LANDING */
#endif
#if defined V_CTL_THROTTLE_IDLE
Bound(v_ctl_throttle_setpoint, TRIM_PPRZ(V_CTL_THROTTLE_IDLE * MAX_PPRZ), MAX_PPRZ);
#endif
#ifdef V_CTL_POWER_CTL_BAT_NOMINAL
if (vsupply > 0.) {
v_ctl_throttle_setpoint *= 10. * V_CTL_POWER_CTL_BAT_NOMINAL / (float)vsupply;
v_ctl_throttle_setpoint = TRIM_UPPRZ(v_ctl_throttle_setpoint);
}
#endif
// Copy the pitch setpoint from the guidance to the stabilization control
h_ctl_pitch_setpoint = v_ctl_pitch_setpoint;
Bound(h_ctl_pitch_setpoint, H_CTL_PITCH_MIN_SETPOINT, H_CTL_PITCH_MAX_SETPOINT);
if (kill_throttle || (!autopilot_flight_time && !launch)) {
v_ctl_throttle_setpoint = 0;
}
}
h_ctl_attitude_loop(); /* Set h_ctl_aileron_setpoint & h_ctl_elevator_setpoint */
v_ctl_throttle_slew();
PPRZ_MUTEX_LOCK(ap_state_mtx);
ap_state->commands[COMMAND_THROTTLE] = v_ctl_throttle_slewed;
ap_state->commands[COMMAND_ROLL] = -h_ctl_aileron_setpoint;
ap_state->commands[COMMAND_PITCH] = h_ctl_elevator_setpoint;
#if H_CTL_YAW_LOOP && defined COMMAND_YAW
ap_state->commands[COMMAND_YAW] = h_ctl_rudder_setpoint;
#endif
#if H_CTL_CL_LOOP && defined COMMAND_CL
ap_state->commands[COMMAND_CL] = h_ctl_flaps_setpoint;
#endif
PPRZ_MUTEX_UNLOCK(ap_state_mtx);
#if defined MCU_SPI_LINK || defined MCU_UART_LINK || defined MCU_CAN_LINK
link_mcu_send();
#elif defined INTER_MCU && defined SINGLE_MCU
/**Directly set the flag indicating to FBW that shared buffer is available*/
inter_mcu_received_ap = true;
#endif
}
/** Run at PERIODIC_FREQUENCY (60Hz if not defined) */
void sensors_task(void)
@@ -628,8 +334,8 @@ void sensors_task(void)
/** monitor stuff run at 1Hz */
void monitor_task(void)
{
if (autopilot_flight_time) {
autopilot_flight_time++;
if (autopilot.flight_time) {
autopilot.flight_time++;
}
#if defined DATALINK || defined SITL
datalink_time++;
@@ -641,13 +347,16 @@ void monitor_task(void)
} else {
t = 0;
}
kill_throttle |= (t >= CATASTROPHIC_BAT_KILL_DELAY);
kill_throttle |= launch && (dist2_to_home > Square(KILL_MODE_DISTANCE));
#if !USE_GENERATED_AUTOPILOT
// only check for static autopilot
autopilot.kill_throttle |= (t >= CATASTROPHIC_BAT_KILL_DELAY);
autopilot.kill_throttle |= autopilot.launch && (dist2_to_home > Square(KILL_MODE_DISTANCE));
#endif
if (!autopilot_flight_time &&
if (!autopilot.flight_time &&
stateGetHorizontalSpeedNorm_f() > MIN_SPEED_FOR_TAKEOFF) {
autopilot_flight_time = 1;
launch = true; /* Not set in non auto launch */
autopilot.flight_time = 1;
autopilot.launch = true; /* Not set in non auto launch */
#if DOWNLINK
uint16_t time_sec = sys_time.nb_sec;
DOWNLINK_SEND_TAKEOFF(DefaultChannel, DefaultDevice, &time_sec);
@@ -680,15 +389,10 @@ void event_task_ap(void)
if (inter_mcu_received_fbw) {
/* receive radio control task from fbw */
inter_mcu_received_fbw = false;
telecommand_task();
autopilot_on_rc_frame();
}
#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
if (new_ins_attitude > 0) {
attitude_loop();
new_ins_attitude = 0;
}
#endif
autopilot_event();
} /* event_task_ap() */
sw/airborne/firmwares/fixedwing/main_ap.h
-2
View File
@@ -34,9 +34,7 @@ extern void handle_periodic_tasks_ap(void);
extern void event_task_ap(void);
extern void sensors_task(void);
extern void navigation_task(void);
extern void monitor_task(void);
extern void reporting_task(void);
extern void attitude_loop(void);
#endif
sw/airborne/firmwares/fixedwing/main_fbw.c
+2 -2
View File
@@ -40,7 +40,7 @@
#include "subsystems/actuators.h"
#include "subsystems/electrical.h"
#include "subsystems/radio_control.h"
#include "firmwares/fixedwing/autopilot.h"
#include "autopilot.h"
#include "paparazzi.h"
#include "mcu_periph/i2c.h"
#include "mcu_periph/uart.h"
@@ -173,7 +173,7 @@ void update_actuators(void)
trimmed_commands[COMMAND_YAW] += ChopAbs(command_yaw_trim, MAX_PPRZ);
#endif /* COMMAND_YAW */
SetActuatorsFromCommands(trimmed_commands, autopilot_mode);
SetActuatorsFromCommands(trimmed_commands, autopilot_get_mode());
fbw_new_actuators = 0;
}
}
sw/airborne/firmwares/fixedwing/nav.c
+2 -2
View File
@@ -33,7 +33,7 @@ static unit_t unit __attribute__((unused));
#define NAV_C
#include "firmwares/fixedwing/nav.h"
#include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
#include "firmwares/fixedwing/autopilot.h"
#include "autopilot.h"
#include "inter_mcu.h"
#include "subsystems/gps.h"
@@ -170,7 +170,7 @@ void nav_glide(uint8_t start_wp, uint8_t wp)
#define MAX_HEIGHT_CARROT 150.
#define Goto3D(radius) { \
if (pprz_mode == PPRZ_MODE_AUTO2) { \
if (autopilot_get_mode() == PPRZ_MODE_AUTO2) { \
int16_t yaw = imcu_get_radio(RADIO_YAW); \
if (yaw > MIN_DX || yaw < -MIN_DX) { \
carrot_x += FLOAT_OF_PPRZ(yaw, 0, -20.); \
sw/airborne/firmwares/fixedwing/nav.h
+2 -2
View File
@@ -206,7 +206,7 @@ bool nav_approaching_xy(float x, float y, float from_x, float from_y, float appr
#define NavAttitude(_roll) { \
lateral_mode = LATERAL_MODE_ROLL; \
if(pprz_mode != PPRZ_MODE_AUTO1) \
if(autopilot_get_mode() != PPRZ_MODE_AUTO1) \
{h_ctl_roll_setpoint = _roll;} \
}
@@ -217,7 +217,7 @@ bool nav_approaching_xy(float x, float y, float from_x, float from_y, float appr
#define nav_SetNavRadius(x) { if (x==1) nav_radius = DEFAULT_CIRCLE_RADIUS; else if (x==-1) nav_radius = -DEFAULT_CIRCLE_RADIUS; else nav_radius = x; }
#define NavKillThrottle() { kill_throttle = 1; }
#define NavKillThrottle() { autopilot_set_kill_throttle(true); }
/// Get current x (east) position in local coordinates
#define GetPosX() (stateGetPositionEnu_f()->x)
sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c
+5 -5
View File
@@ -73,7 +73,7 @@
#include "firmwares/fixedwing/nav.h"
#include "generated/airframe.h"
#include CTRL_TYPE_H
#include "firmwares/fixedwing/autopilot.h"
#include "autopilot.h"
/* outer loop parameters */
@@ -487,7 +487,7 @@ inline static void h_ctl_roll_loop(void)
struct FloatRates *body_rate = stateGetBodyRates_f();
float d_err = h_ctl_ref.roll_rate - body_rate->p;
if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) {
if (autopilot_get_mode() == PPRZ_MODE_MANUAL || autopilot.launch == false) {
h_ctl_roll_sum_err = 0.;
} else {
if (h_ctl_roll_igain > 0.) {
@@ -604,7 +604,7 @@ inline static void h_ctl_pitch_loop(void)
last_err = err;
#endif
if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) {
if (autopilot_get_mode() == PPRZ_MODE_MANUAL || autopilot.launch == false) {
h_ctl_pitch_sum_err = 0.;
} else {
if (h_ctl_pitch_igain > 0.) {
@@ -646,7 +646,7 @@ inline static void h_ctl_yaw_loop(void)
float ny = 0.f;
#endif
if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) {
if (autopilot_get_mode() == PPRZ_MODE_MANUAL || autopilot.launch == false) {
h_ctl_yaw_ny_sum_err = 0.;
} else {
if (h_ctl_yaw_ny_igain > 0.) {
@@ -729,7 +729,7 @@ inline static void h_ctl_cl_loop(void)
}
// no control in manual mode
if (pprz_mode == PPRZ_MODE_MANUAL) {
if (autopilot_get_mode() == PPRZ_MODE_MANUAL) {
cmd = 0.f;
}
// bound max flap angle
sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c
+2 -2
View File
@@ -33,7 +33,7 @@
#include "firmwares/fixedwing/nav.h"
#include "generated/airframe.h"
#include CTRL_TYPE_H
#include "firmwares/fixedwing/autopilot.h"
#include "autopilot.h"
/* outer loop parameters */
float h_ctl_course_setpoint; /* rad, CW/north */
@@ -267,7 +267,7 @@ void h_ctl_course_loop(void)
static float h_ctl_course_slew_rate = 0.;
float nav_angle_saturation = h_ctl_roll_max_setpoint / h_ctl_course_pgain; /* heading error corresponding to max_roll */
float half_nav_angle_saturation = nav_angle_saturation / 2.;
if (launch) { /* prevent accumulator run-up on the ground */
if (autopilot.launch) { /* prevent accumulator run-up on the ground */
if (err > half_nav_angle_saturation) {
h_ctl_course_slew_rate = Max(h_ctl_course_slew_rate, 0.);
err = Min(err, (half_nav_angle_saturation + h_ctl_course_slew_rate));
sw/airborne/firmwares/rotorcraft/autopilot.h
-140
View File
@@ -1,140 +0,0 @@
/*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
* Copyright (C) 2016 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* 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.h
*
* Core autopilot file.
* Using either static or generated autopilot logic.
*
*/
#ifndef AUTOPILOT_H
#define AUTOPILOT_H
#include "std.h"
#include "generated/airframe.h"
#include "state.h"
#include "firmwares/rotorcraft/autopilot_utils.h"
// 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;
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;
extern void autopilot_init(void);
extern void autopilot_periodic(void);
extern void autopilot_on_rc_frame(void);
extern void autopilot_set_mode(uint8_t new_autopilot_mode);
extern void autopilot_SetModeHandler(float new_autopilot_mode); // handler for dl_setting
extern void autopilot_set_motors_on(bool motors_on);
extern void autopilot_check_in_flight(bool motors_on);
extern bool autopilot_ground_detected;
extern bool autopilot_detect_ground_once;
extern uint16_t autopilot_flight_time;
#define autopilot_KillThrottle(_kill) { \
if (_kill) \
autopilot_set_motors_on(false); \
else \
autopilot_set_motors_on(true); \
}
#ifdef POWER_SWITCH_GPIO
#include "mcu_periph/gpio.h"
#define autopilot_SetPowerSwitch(_v) { \
autopilot_power_switch = _v; \
if (_v) { gpio_set(POWER_SWITCH_GPIO); } \
else { gpio_clear(POWER_SWITCH_GPIO); } \
}
#else
#define autopilot_SetPowerSwitch(_v) { \
autopilot_power_switch = _v; \
}
#endif
/** Z-acceleration threshold to detect ground in m/s^2 */
#ifndef THRESHOLD_GROUND_DETECT
#define THRESHOLD_GROUND_DETECT 25.0
#endif
/** Ground detection based on vertical acceleration.
*/
static inline void DetectGroundEvent(void)
{
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) {
autopilot_ground_detected = true;
autopilot_detect_ground_once = false;
}
}
}
#include "subsystems/settings.h"
/* try to make sure that we don't write to flash while flying */
static inline void autopilot_StoreSettings(float store)
{
if (kill_throttle && store) {
settings_store_flag = store;
settings_store();
}
}
static inline void autopilot_ClearSettings(float clear)
{
if (kill_throttle && clear) {
settings_clear_flag = clear;
settings_clear();
}
}
#if DOWNLINK
#include "pprzlink/pprzlink_transport.h"
extern void send_autopilot_version(struct transport_tx *trans, struct link_device *dev);
#endif
#endif /* AUTOPILOT_H */
sw/airborne/firmwares/rotorcraft/autopilot_arming_switch.h
+6 -6
View File
@@ -59,8 +59,8 @@ static inline void autopilot_arming_set(bool motors_on)
if (autopilot_arming_state == STATE_MOTORS_ON) {
autopilot_arming_state = STATE_STARTABLE;
/* if turned off in an AUTO mode, remember it so it can be turned on again in AUTO */
if (autopilot_mode != MODE_MANUAL && autopilot_mode != AP_MODE_KILL &&
autopilot_mode != AP_MODE_FAILSAFE) {
if (autopilot_get_mode() != MODE_MANUAL && autopilot_get_mode() != AP_MODE_KILL &&
autopilot_get_mode() != AP_MODE_FAILSAFE) {
autopilot_unarmed_in_auto = true;
} else {
autopilot_unarmed_in_auto = false;
@@ -96,11 +96,11 @@ static inline void autopilot_arming_check_motors_on(void)
case STATE_STARTABLE:
autopilot_motors_on = false;
/* don't allow to start if in KILL mode or kill switch is on */
if (autopilot_mode == AP_MODE_KILL || kill_switch_is_on()) {
if (autopilot_get_mode() == AP_MODE_KILL || kill_switch_is_on()) {
break;
}
else if (THROTTLE_STICK_DOWN() && rc_attitude_sticks_centered() &&
(autopilot_mode == MODE_MANUAL || autopilot_unarmed_in_auto)) {
(autopilot_get_mode() == MODE_MANUAL || autopilot_unarmed_in_auto)) {
autopilot_arming_state = STATE_MOTORS_ON;
}
break;
@@ -110,8 +110,8 @@ static inline void autopilot_arming_check_motors_on(void)
autopilot_motors_on = false;
autopilot_arming_state = STATE_STARTABLE;
/* if turned off in an AUTO mode, remember it so it can be turned on again in AUTO */
if (autopilot_mode != MODE_MANUAL && autopilot_mode != AP_MODE_KILL &&
autopilot_mode != AP_MODE_FAILSAFE) {
if (autopilot_get_mode() != MODE_MANUAL && autopilot_get_mode() != AP_MODE_KILL &&
autopilot_get_mode() != AP_MODE_FAILSAFE) {
autopilot_unarmed_in_auto = true;
} else {
autopilot_unarmed_in_auto = false;
sw/airborne/firmwares/rotorcraft/autopilot_arming_throttle.h
+4 -4
View File
@@ -75,7 +75,7 @@ static inline void autopilot_arming_check_motors_on(void)
{
/* only allow switching motor if not in KILL mode */
if (autopilot_mode != AP_MODE_KILL) {
if (autopilot_get_mode() != AP_MODE_KILL) {
switch (autopilot_arming_state) {
case STATE_UNINIT:
@@ -99,7 +99,7 @@ static inline void autopilot_arming_check_motors_on(void)
autopilot_arming_delay_counter = 0;
if (!THROTTLE_STICK_DOWN() &&
rc_attitude_sticks_centered() &&
(autopilot_mode == MODE_MANUAL || autopilot_unarmed_in_auto)) {
(autopilot_get_mode() == MODE_MANUAL || autopilot_unarmed_in_auto)) {
autopilot_arming_state = STATE_ARMING;
}
break;
@@ -108,7 +108,7 @@ static inline void autopilot_arming_check_motors_on(void)
autopilot_arming_delay_counter++;
if (THROTTLE_STICK_DOWN() ||
!rc_attitude_sticks_centered() ||
(autopilot_mode != MODE_MANUAL && !autopilot_unarmed_in_auto)) {
(autopilot_get_mode() != MODE_MANUAL && !autopilot_unarmed_in_auto)) {
autopilot_arming_state = STATE_MOTORS_OFF_READY;
} else if (autopilot_arming_delay_counter >= AUTOPILOT_ARMING_DELAY) {
autopilot_arming_state = STATE_MOTORS_ON;
@@ -128,7 +128,7 @@ static inline void autopilot_arming_check_motors_on(void)
autopilot_arming_state = STATE_MOTORS_ON;
} else if (autopilot_arming_delay_counter == 0) {
autopilot_arming_state = STATE_MOTORS_OFF_READY;
if (autopilot_mode != MODE_MANUAL) {
if (autopilot_get_mode() != MODE_MANUAL) {
autopilot_unarmed_in_auto = true;
} else {
autopilot_unarmed_in_auto = false;
sw/airborne/firmwares/rotorcraft/autopilot_arming_yaw.h
+8 -8
View File
@@ -83,7 +83,7 @@ static inline void autopilot_arming_set(bool motors_on)
static inline void autopilot_arming_check_motors_on(void)
{
/* only allow switching motor if not in KILL mode */
if (autopilot_mode != AP_MODE_KILL) {
if (autopilot_get_mode() != AP_MODE_KILL) {
switch (autopilot_check_motor_status) {
case STATUS_INITIALISE_RC: // Wait until RC is initialised (it being centered is a good pointer to this)
@@ -94,7 +94,7 @@ static inline void autopilot_arming_check_motors_on(void)
case STATUS_MOTORS_AUTOMATICALLY_OFF: // Motors were disarmed externally
//(possibly due to crash)
//wait extra delay before enabling the normal arming state machine
autopilot_motors_on = false;
autopilot.motors_on = false;
autopilot_motors_on_counter = 0;
if (THROTTLE_STICK_DOWN() && YAW_STICK_CENTERED()) { // stick released
autopilot_check_motor_status = STATUS_MOTORS_AUTOMATICALLY_OFF_SAFETY_WAIT;
@@ -107,14 +107,14 @@ static inline void autopilot_arming_check_motors_on(void)
}
break;
case STATUS_MOTORS_OFF:
autopilot_motors_on = false;
autopilot.motors_on = false;
autopilot_motors_on_counter = 0;
if (THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED()) { // stick pushed
autopilot_check_motor_status = STATUS_M_OFF_STICK_PUSHED;
}
break;
case STATUS_M_OFF_STICK_PUSHED:
autopilot_motors_on = false;
autopilot.motors_on = false;
autopilot_motors_on_counter++;
if (autopilot_motors_on_counter >= MOTOR_ARMING_DELAY) {
autopilot_check_motor_status = STATUS_START_MOTORS;
@@ -123,21 +123,21 @@ static inline void autopilot_arming_check_motors_on(void)
}
break;
case STATUS_START_MOTORS:
autopilot_motors_on = true;
autopilot.motors_on = true;
autopilot_motors_on_counter = MOTOR_ARMING_DELAY;
if (!(THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED())) { // wait until stick released
autopilot_check_motor_status = STATUS_MOTORS_ON;
}
break;
case STATUS_MOTORS_ON:
autopilot_motors_on = true;
autopilot.motors_on = true;
autopilot_motors_on_counter = MOTOR_ARMING_DELAY;
if (THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED()) { // stick pushed
autopilot_check_motor_status = STATUS_M_ON_STICK_PUSHED;
}
break;
case STATUS_M_ON_STICK_PUSHED:
autopilot_motors_on = true;
autopilot.motors_on = true;
autopilot_motors_on_counter--;
if (autopilot_motors_on_counter == 0) {
autopilot_check_motor_status = STATUS_STOP_MOTORS;
@@ -146,7 +146,7 @@ static inline void autopilot_arming_check_motors_on(void)
}
break;
case STATUS_STOP_MOTORS:
autopilot_motors_on = false;
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;
sw/airborne/firmwares/rotorcraft/autopilot.c → sw/airborne/firmwares/rotorcraft/autopilot_firmware.c
+39 -144
View File
@@ -1,6 +1,6 @@
/*
* Copyright (C) 2008-2012 The Paparazzi Team
* Copyright (C) 2016 Gautier Hattenberger <gautier.hattenberger@enac.fr>
* Copyright (C) 2016-2017 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* This file is part of paparazzi.
*
@@ -15,9 +15,8 @@
* 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.
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
@@ -27,21 +26,15 @@
*
*/
#include <stdint.h>
#include "firmwares/rotorcraft/autopilot.h"
#include "firmwares/rotorcraft/autopilot_firmware.h"
#include "generated/modules.h"
#include "mcu_periph/uart.h"
#include "mcu_periph/sys_time.h"
#include "subsystems/radio_control.h"
#include "subsystems/commands.h"
#include "subsystems/actuators.h"
#include <stdint.h>
//#include "mcu_periph/sys_time.h"
#include "subsystems/electrical.h"
#include "subsystems/settings.h"
#include "subsystems/datalink/telemetry.h"
#include "generated/settings.h"
#include "subsystems/radio_control.h"
#if USE_GPS
#include "subsystems/gps.h"
@@ -53,27 +46,8 @@
#endif
#endif
#ifdef POWER_SWITCH_GPIO
#include "mcu_periph/gpio.h"
#endif
#include "pprz_version.h"
uint8_t autopilot_mode;
uint8_t autopilot_mode_auto2;
bool autopilot_in_flight;
uint32_t autopilot_in_flight_counter;
uint16_t autopilot_flight_time;
bool autopilot_motors_on;
bool kill_throttle;
bool autopilot_rc;
bool autopilot_power_switch;
bool autopilot_ground_detected;
bool autopilot_detect_ground_once;
static uint32_t autopilot_in_flight_counter;
/* Geofence exceptions */
#include "modules/nav/nav_geofence.h"
@@ -98,24 +72,11 @@ bool autopilot_detect_ground_once;
#define AUTOPILOT_IN_FLIGHT_MIN_THRUST 500
#endif
/** Z-acceleration threshold to detect ground in m/s^2 */
#ifndef THRESHOLD_GROUND_DETECT
#define THRESHOLD_GROUND_DETECT 25.0
#endif
void send_autopilot_version(struct transport_tx *trans, struct link_device *dev)
{
static uint32_t ap_version = PPRZ_VERSION_INT;
static char *ver_desc = PPRZ_VERSION_DESC;
pprz_msg_send_AUTOPILOT_VERSION(trans, dev, AC_ID, &ap_version, strlen(ver_desc), ver_desc);
}
static void send_alive(struct transport_tx *trans, struct link_device *dev)
{
pprz_msg_send_ALIVE(trans, dev, AC_ID, 16, MD5SUM);
}
static void send_attitude(struct transport_tx *trans, struct link_device *dev)
{
struct FloatEulers *att = stateGetNedToBodyEulers_f();
pprz_msg_send_ATTITUDE(trans, dev, AC_ID, &(att->phi), &(att->psi), &(att->theta));
};
#if USE_MOTOR_MIXING
#include "subsystems/actuators/motor_mixing.h"
@@ -134,13 +95,13 @@ static void send_status(struct transport_tx *trans, struct link_device *dev)
#else
uint8_t fix = 0;
#endif
uint8_t in_flight = autopilot_in_flight;
uint8_t motors_on = autopilot_motors_on;
uint8_t in_flight = autopilot.in_flight;
uint8_t motors_on = autopilot.motors_on;
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,
&fix, &autopilot.mode, &in_flight, &motors_on,
&guidance_h.mode, &guidance_v_mode,
&electrical.vsupply, &time_sec);
}
@@ -175,7 +136,7 @@ static void send_fp(struct transport_tx *trans, struct link_device *dev)
&carrot_up,
&guidance_h.sp.heading,
&stabilization_cmd[COMMAND_THRUST],
&autopilot_flight_time);
&autopilot.flight_time);
}
static void send_fp_min(struct transport_tx *trans, struct link_device *dev)
@@ -194,11 +155,6 @@ static void send_fp_min(struct transport_tx *trans, struct link_device *dev)
}
#ifdef RADIO_CONTROL
static void send_rc(struct transport_tx *trans, struct link_device *dev)
{
pprz_msg_send_RC(trans, dev, AC_ID, RADIO_CONTROL_NB_CHANNEL, radio_control.values);
}
static void send_rotorcraft_rc(struct transport_tx *trans, struct link_device *dev)
{
#ifdef RADIO_KILL_SWITCH
@@ -217,18 +173,6 @@ static void send_rotorcraft_rc(struct transport_tx *trans, struct link_device *d
}
#endif
#ifdef ACTUATORS
static void send_actuators(struct transport_tx *trans, struct link_device *dev)
{
pprz_msg_send_ACTUATORS(trans, dev, AC_ID , ACTUATORS_NB, actuators);
}
#endif
static void send_dl_value(struct transport_tx *trans, struct link_device *dev)
{
PeriodicSendDlValue(trans, dev);
}
static void send_rotorcraft_cmd(struct transport_tx *trans, struct link_device *dev)
{
pprz_msg_send_ROTORCRAFT_CMD(trans, dev, AC_ID,
@@ -239,104 +183,55 @@ static void send_rotorcraft_cmd(struct transport_tx *trans, struct link_device *
}
void autopilot_init(void)
void autopilot_firmware_init(void)
{
autopilot_motors_on = false;
kill_throttle = ! autopilot_motors_on;
autopilot_in_flight = false;
autopilot_in_flight_counter = 0;
autopilot_mode_auto2 = MODE_AUTO2;
autopilot_ground_detected = false;
autopilot_detect_ground_once = false;
autopilot_flight_time = 0;
autopilot_rc = true;
autopilot_power_switch = false;
#ifdef POWER_SWITCH_GPIO
gpio_setup_output(POWER_SWITCH_GPIO);
gpio_clear(POWER_SWITCH_GPIO); // POWER OFF
#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);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ATTITUDE, send_attitude);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ENERGY, send_energy);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_FP, send_fp);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_FP_MIN, send_fp_min);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_CMD, send_rotorcraft_cmd);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_DL_VALUE, send_dl_value);
#ifdef ACTUATORS
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ACTUATORS, send_actuators);
#endif
#ifdef RADIO_CONTROL
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_RC, send_rc);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_RADIO_CONTROL, send_rotorcraft_rc);
#endif
}
/** AP periodic call
/** autopilot event function
*
* used for automatic ground detection
*/
void autopilot_periodic(void)
void autopilot_event(void)
{
#if USE_GENERATED_AUTOPILOT
autopilot_generated_periodic();
#else
autopilot_static_periodic();
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) {
autopilot.ground_detected = true;
autopilot.detect_ground_once = false;
}
}
}
/** set autopilot mode
/** reset in_flight counter
*/
void autopilot_set_mode(uint8_t new_autopilot_mode)
void autopilot_reset_in_flight_counter(void);
void autopilot_reset_in_flight_counter(void)
{
#if USE_GENERATED_AUTOPILOT
autopilot_generated_set_mode(new_autopilot_mode);
#else
autopilot_static_set_mode(new_autopilot_mode);
#endif
}
/** AP mode setting handler
*/
void autopilot_SetModeHandler(float mode)
{
#if USE_GENERATED_AUTOPILOT
autopilot_generated_SetModeHandler(mode);
#else
autopilot_static_SetModeHandler(mode);
#endif
}
/** 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;
autopilot_in_flight_counter = 0;
}
/** in flight check utility function
*/
void autopilot_check_in_flight(bool motors_on)
{
if (autopilot_in_flight) {
if (autopilot.in_flight) {
if (autopilot_in_flight_counter > 0) {
/* probably in_flight if thrust, speed and accel above IN_FLIGHT_MIN thresholds */
if ((stabilization_cmd[COMMAND_THRUST] <= AUTOPILOT_IN_FLIGHT_MIN_THRUST) &&
@@ -344,7 +239,7 @@ void autopilot_check_in_flight(bool motors_on)
(fabsf(stateGetAccelNed_f()->z) < AUTOPILOT_IN_FLIGHT_MIN_ACCEL)) {
autopilot_in_flight_counter--;
if (autopilot_in_flight_counter == 0) {
autopilot_in_flight = false;
autopilot.in_flight = false;
}
} else { /* thrust, speed or accel not above min threshold, reset counter */
autopilot_in_flight_counter = AUTOPILOT_IN_FLIGHT_TIME;
@@ -359,7 +254,7 @@ void autopilot_check_in_flight(bool motors_on)
if (stabilization_cmd[COMMAND_THRUST] > AUTOPILOT_IN_FLIGHT_MIN_THRUST) {
autopilot_in_flight_counter++;
if (autopilot_in_flight_counter == AUTOPILOT_IN_FLIGHT_TIME) {
autopilot_in_flight = true;
autopilot.in_flight = true;
}
} else { /* currently not in_flight and thrust below threshold, reset counter */
autopilot_in_flight_counter = 0;
sw/airborne/firmwares/rotorcraft/autopilot_firmware.h
+38
View File
@@ -0,0 +1,38 @@
/*
* Copyright (C) 2017 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* 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, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file firmwares/rotorcraft/autopilot_firmware.h
*
* Rotorcraft specific autopilot interface
* and initialization
*/
#ifndef AUTOPILOT_FIRMWARE_H
#define AUTOPILOT_FIRMWARE_H
#include "std.h"
#include "autopilot.h"
extern uint8_t autopilot_mode_auto2;
extern void autopilot_firmware_init(void);
#endif /* AUTOPILOT_FIRMWARE_H */
sw/airborne/firmwares/rotorcraft/autopilot_generated.c
+17 -18
View File
@@ -14,9 +14,8 @@
* 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.
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
@@ -28,7 +27,7 @@
#define AUTOPILOT_CORE_AP_C
#include "firmwares/rotorcraft/autopilot_generated.h"
#include "firmwares/rotorcraft/autopilot.h"
#include "autopilot.h"
#include "subsystems/radio_control.h"
#include "subsystems/commands.h"
@@ -55,7 +54,7 @@ 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;
autopilot.mode = autopilot_mode_ap;
// init arming
autopilot_arming_init();
@@ -68,13 +67,13 @@ 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;
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;
if (!autopilot_in_flight()) {
autopilot.ground_detected = false;
autopilot.detect_ground_once = false;
}
}
@@ -93,7 +92,7 @@ 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;
autopilot.mode = autopilot_mode_ap;
}
@@ -101,14 +100,14 @@ 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
&& autopilot.mode != AP_MODE_KILL
#endif
) {
autopilot_motors_on = true;
autopilot.motors_on = true;
} else {
autopilot_motors_on = false;
autopilot.motors_on = false;
}
autopilot_arming_set(autopilot_motors_on);
autopilot_arming_set(autopilot.motors_on);
}
@@ -122,11 +121,11 @@ void autopilot_generated_on_rc_frame(void)
*/
if (ap_ahrs_is_aligned()) {
autopilot_arming_check_motors_on();
kill_throttle = ! autopilot_motors_on;
autopilot.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 (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
@@ -135,13 +134,13 @@ void autopilot_generated_on_rc_frame(void)
//
// /* if not in NAV_MODE set commands from the rc */
//#ifdef SetCommandsFromRC
// if (autopilot_mode != AP_MODE_NAV) {
// if (autopilot.mode != AP_MODE_NAV) {
// SetCommandsFromRC(commands, radio_control.values);
// }
//#endif
//
// guidance_v_read_rc();
// guidance_h_read_rc(autopilot_in_flight);
// guidance_h_read_rc(autopilot.in_flight);
// }
}
sw/airborne/firmwares/rotorcraft/autopilot_guided.c
+6 -6
View File
@@ -27,14 +27,14 @@
*/
#include "firmwares/rotorcraft/autopilot_guided.h"
#include "firmwares/rotorcraft/autopilot.h"
#include "autopilot.h"
#include "firmwares/rotorcraft/guidance.h"
#include "state.h"
bool autopilot_guided_goto_ned(float x, float y, float z, float heading)
{
if (autopilot_mode == AP_MODE_GUIDED) {
if (autopilot_get_mode() == AP_MODE_GUIDED) {
guidance_h_set_guided_pos(x, y);
guidance_h_set_guided_heading(heading);
guidance_v_set_guided_z(z);
@@ -45,7 +45,7 @@ bool autopilot_guided_goto_ned(float x, float y, float z, float heading)
bool autopilot_guided_goto_ned_relative(float dx, float dy, float dz, float dyaw)
{
if (autopilot_mode == AP_MODE_GUIDED && stateIsLocalCoordinateValid()) {
if (autopilot_get_mode() == AP_MODE_GUIDED && stateIsLocalCoordinateValid()) {
float x = stateGetPositionNed_f()->x + dx;
float y = stateGetPositionNed_f()->y + dy;
float z = stateGetPositionNed_f()->z + dz;
@@ -57,7 +57,7 @@ bool autopilot_guided_goto_ned_relative(float dx, float dy, float dz, float dyaw
bool autopilot_guided_goto_body_relative(float dx, float dy, float dz, float dyaw)
{
if (autopilot_mode == AP_MODE_GUIDED && stateIsLocalCoordinateValid()) {
if (autopilot_get_mode() == AP_MODE_GUIDED && stateIsLocalCoordinateValid()) {
float psi = stateGetNedToBodyEulers_f()->psi;
float x = stateGetPositionNed_f()->x + cosf(-psi) * dx + sinf(-psi) * dy;
float y = stateGetPositionNed_f()->y - sinf(-psi) * dx + cosf(-psi) * dy;
@@ -70,7 +70,7 @@ bool autopilot_guided_goto_body_relative(float dx, float dy, float dz, float dya
bool autopilot_guided_move_ned(float vx, float vy, float vz, float heading)
{
if (autopilot_mode == AP_MODE_GUIDED) {
if (autopilot_get_mode() == AP_MODE_GUIDED) {
guidance_h_set_guided_vel(vx, vy);
guidance_h_set_guided_heading(heading);
guidance_v_set_guided_vz(vz);
@@ -95,7 +95,7 @@ bool autopilot_guided_move_ned(float vx, float vy, float vz, float heading)
void autopilot_guided_update(uint8_t flags, float x, float y, float z, float yaw)
{
/* only update setpoints when in guided mode */
if (autopilot_mode != AP_MODE_GUIDED) {
if (autopilot_get_mode() != AP_MODE_GUIDED) {
return;
}
sw/airborne/firmwares/rotorcraft/autopilot_static.c
+26 -27
View File
@@ -27,7 +27,7 @@
*
*/
#include "firmwares/rotorcraft/autopilot.h"
#include "autopilot.h"
#include "subsystems/radio_control.h"
#include "subsystems/commands.h"
@@ -108,7 +108,7 @@ void autopilot_static_init(void)
* For autopilot_static_set_mode to do anything, the requested mode needs to differ
* from previous mode, so we set it to a safe KILL first.
*/
autopilot_mode = AP_MODE_KILL;
autopilot.mode = AP_MODE_KILL;
/* set startup mode, propagates through to guidance h/v */
autopilot_static_set_mode(MODE_STARTUP);
@@ -124,7 +124,7 @@ void autopilot_static_periodic(void)
RunOnceEvery(NAV_PRESCALER, compute_dist2_to_home());
if (autopilot_in_flight && autopilot_mode == AP_MODE_NAV) {
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);
@@ -134,7 +134,7 @@ void autopilot_static_periodic(void)
}
}
if (autopilot_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
@@ -145,14 +145,14 @@ void autopilot_static_periodic(void)
/* 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) {
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) {
if (autopilot.ground_detected) {
autopilot_static_set_mode(AP_MODE_KILL);
}
#endif
@@ -160,21 +160,21 @@ void autopilot_static_periodic(void)
/* Reset ground detection _after_ running flight plan
*/
if (!autopilot_in_flight) {
autopilot_ground_detected = false;
autopilot_detect_ground_once = false;
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) {
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);
guidance_v_run(autopilot_in_flight());
guidance_h_run(autopilot_in_flight());
SetRotorcraftCommands(stabilization_cmd, autopilot.in_flight, autopilot.motors_on);
}
}
@@ -207,7 +207,7 @@ void autopilot_static_set_mode(uint8_t new_autopilot_mode)
new_autopilot_mode = MODE_STARTUP;
}
if (new_autopilot_mode != autopilot_mode) {
if (new_autopilot_mode != autopilot.mode) {
/* horizontal mode */
switch (new_autopilot_mode) {
case AP_MODE_FAILSAFE:
@@ -217,8 +217,7 @@ void autopilot_static_set_mode(uint8_t new_autopilot_mode)
break;
#endif
case AP_MODE_KILL:
autopilot_in_flight = false;
autopilot_in_flight_counter = 0;
autopilot_set_in_flight(false);
guidance_h_mode_changed(GUIDANCE_H_MODE_KILL);
break;
case AP_MODE_RC_DIRECT:
@@ -321,19 +320,19 @@ void autopilot_static_set_mode(uint8_t new_autopilot_mode)
break;
}
//if switching to rate mode but rate mode is not defined, the function returned
autopilot_mode = new_autopilot_mode;
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;
if (autopilot.mode != AP_MODE_KILL && ap_ahrs_is_aligned() && motors_on) {
autopilot.motors_on = true;
} else {
autopilot_motors_on = false;
autopilot.motors_on = false;
}
autopilot_arming_set(autopilot_motors_on);
autopilot_arming_set(autopilot.motors_on);
}
void autopilot_static_on_rc_frame(void)
@@ -360,7 +359,7 @@ void autopilot_static_on_rc_frame(void)
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)
else if ((autopilot.mode != AP_MODE_HOME)
#if UNLOCKED_HOME_MODE
/* Allowed to leave home mode when UNLOCKED_HOME_MODE */
|| !too_far_from_home
@@ -376,11 +375,11 @@ void autopilot_static_on_rc_frame(void)
*/
if (ap_ahrs_is_aligned()) {
autopilot_arming_check_motors_on();
kill_throttle = ! autopilot_motors_on;
autopilot.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 (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
@@ -389,13 +388,13 @@ void autopilot_static_on_rc_frame(void)
/* if not in NAV_MODE set commands from the rc */
#ifdef SetCommandsFromRC
if (autopilot_mode != AP_MODE_NAV) {
if (autopilot.mode != AP_MODE_NAV) {
SetCommandsFromRC(commands, radio_control.values);
}
#endif
guidance_v_read_rc();
guidance_h_read_rc(autopilot_in_flight);
guidance_h_read_rc(autopilot_in_flight());
}
}
sw/airborne/firmwares/rotorcraft/autopilot_utils.c
+1 -1
View File
@@ -27,7 +27,7 @@
*/
#include "firmwares/rotorcraft/autopilot_utils.h"
#include "firmwares/rotorcraft/autopilot.h"
#include "autopilot.h"
#include "firmwares/rotorcraft/autopilot_rc_helpers.h"
#include "state.h"
#include "subsystems/radio_control.h"
sw/airborne/firmwares/rotorcraft/guidance/guidance_flip.c
+3 -3
View File
@@ -59,7 +59,7 @@ void guidance_flip_enter(void)
flip_state = 0;
flip_rollout = false;
heading_save = stabilization_attitude_get_heading_i();
autopilot_mode_old = autopilot_mode;
autopilot_mode_old = autopilot_get_mode();
}
void guidance_flip_run(void)
@@ -77,7 +77,7 @@ void guidance_flip_run(void)
flip_cmd_earth.y = 0;
stabilization_attitude_set_earth_cmd_i(&flip_cmd_earth,
heading_save);
stabilization_attitude_run(autopilot_in_flight);
stabilization_attitude_run(autopilot_in_flight());
stabilization_cmd[COMMAND_THRUST] = 8000; //Thrust to go up first
timer_save = 0;
@@ -114,7 +114,7 @@ void guidance_flip_run(void)
flip_cmd_earth.y = 0;
stabilization_attitude_set_earth_cmd_i(&flip_cmd_earth,
heading_save);
stabilization_attitude_run(autopilot_in_flight);
stabilization_attitude_run(autopilot_in_flight());
stabilization_cmd[COMMAND_THRUST] = FINAL_THRUST_LEVEL; //Thrust to stop falling
sw/airborne/firmwares/rotorcraft/main_ap.c
+22 -24
View File
@@ -60,7 +60,7 @@ PRINT_CONFIG_MSG_VALUE("USE_BARO_BOARD is TRUE, reading onboard baro: ", BARO_BO
#include "subsystems/electrical.h"
#include "firmwares/rotorcraft/autopilot.h"
#include "autopilot.h"
#include "subsystems/radio_control.h"
@@ -259,15 +259,15 @@ void main_periodic(void)
/* run control loops */
autopilot_periodic();
/* set actuators */
//actuators_set(autopilot_motors_on);
//actuators_set(autopilot_get_motors_on());
#ifndef INTER_MCU_AP
SetActuatorsFromCommands(commands, autopilot_mode);
SetActuatorsFromCommands(commands, autopilot_get_mode());
#else
intermcu_set_actuators(commands, autopilot_mode);
intermcu_set_actuators(commands, autopilot_get_mode());
#endif
if (autopilot_in_flight) {
RunOnceEvery(PERIODIC_FREQUENCY, autopilot_flight_time++);
if (autopilot_in_flight()) {
RunOnceEvery(PERIODIC_FREQUENCY, autopilot.flight_time++);
}
#if defined DATALINK || defined SITL
@@ -284,7 +284,7 @@ void telemetry_periodic(void)
/* initialisation phase during boot */
if (boot) {
#if DOWNLINK
send_autopilot_version(&(DefaultChannel).trans_tx, &(DefaultDevice).device);
autopilot_send_version();
#endif
boot = false;
}
@@ -305,26 +305,26 @@ 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 &&
autopilot_mode != AP_MODE_FAILSAFE &&
autopilot_mode != AP_MODE_NAV &&
autopilot_mode != AP_MODE_MODULE &&
autopilot_mode != AP_MODE_FLIP &&
autopilot_mode != AP_MODE_GUIDED) {
autopilot_get_mode() != AP_MODE_KILL &&
autopilot_get_mode() != AP_MODE_HOME &&
autopilot_get_mode() != AP_MODE_FAILSAFE &&
autopilot_get_mode() != AP_MODE_NAV &&
autopilot_get_mode() != AP_MODE_MODULE &&
autopilot_get_mode() != AP_MODE_FLIP &&
autopilot_get_mode() != AP_MODE_GUIDED) {
autopilot_set_mode(RC_LOST_MODE);
}
#if FAILSAFE_ON_BAT_CRITICAL
if (autopilot_mode != AP_MODE_KILL &&
if (autopilot_get_mode() != AP_MODE_KILL &&
electrical.bat_critical) {
autopilot_set_mode(AP_MODE_FAILSAFE);
}
#endif
#if USE_GPS
if (autopilot_mode == AP_MODE_NAV &&
autopilot_motors_on &&
if (autopilot_get_mode() == AP_MODE_NAV &&
autopilot_get_motors_on() &&
#if NO_GPS_LOST_WITH_RC_VALID
radio_control.status != RC_OK &&
#endif
@@ -332,15 +332,15 @@ void failsafe_check(void)
autopilot_set_mode(AP_MODE_FAILSAFE);
}
if (autopilot_mode == AP_MODE_HOME &&
autopilot_motors_on && GpsIsLost()) {
if (autopilot_get_mode() == AP_MODE_HOME &&
autopilot_get_motors_on() && GpsIsLost()) {
autopilot_set_mode(AP_MODE_FAILSAFE);
}
#endif
#endif // !USE_GENERATED_AUTOPILOT
autopilot_check_in_flight(autopilot_motors_on);
autopilot_check_in_flight(autopilot_get_motors_on());
}
void main_event(void)
@@ -348,7 +348,7 @@ void main_event(void)
/* event functions for mcu peripherals: i2c, usb_serial.. */
mcu_event();
if (autopilot_rc) {
if (autopilot.use_rc) {
RadioControlEvent(autopilot_on_rc_frame);
}
@@ -356,9 +356,7 @@ void main_event(void)
BaroEvent();
#endif
#if FAILSAFE_GROUND_DETECT || KILL_ON_GROUND_DETECT
DetectGroundEvent();
#endif
autopilot_event();
modules_event_task();
}
sw/airborne/firmwares/rotorcraft/main_fbw.c
+1 -1
View File
@@ -217,7 +217,7 @@ void main_periodic(void)
}
/* Set actuators */
SetActuatorsFromCommands(commands, autopilot_mode);
SetActuatorsFromCommands(commands, autopilot_get_mode());
/* Periodic blinking */
RunOnceEvery(10, LED_PERIODIC());
sw/airborne/firmwares/rotorcraft/navigation.c
+6 -6
View File
@@ -35,7 +35,7 @@
#include "subsystems/ins.h"
#include "state.h"
#include "firmwares/rotorcraft/autopilot.h"
#include "autopilot.h"
#include "generated/modules.h"
#include "generated/flight_plan.h"
@@ -315,10 +315,10 @@ bool nav_check_wp_time(struct EnuCoor_i *wp, uint16_t stay_time)
if (dist_to_point < ARRIVED_AT_WAYPOINT) {
if (!wp_reached) {
wp_reached = true;
wp_entry_time = autopilot_flight_time;
wp_entry_time = autopilot.flight_time;
time_at_wp = 0;
} else {
time_at_wp = autopilot_flight_time - wp_entry_time;
time_at_wp = autopilot.flight_time - wp_entry_time;
}
} else {
time_at_wp = 0;
@@ -404,14 +404,14 @@ void navigation_update_wp_from_speed(uint8_t wp, struct Int16Vect3 speed_sp, int
bool nav_detect_ground(void)
{
if (!autopilot_ground_detected) { return false; }
autopilot_ground_detected = false;
if (!autopilot.ground_detected) { return false; }
autopilot.ground_detected = false;
return true;
}
bool nav_is_in_flight(void)
{
return autopilot_in_flight;
return autopilot_in_flight();
}
/** Home mode navigation */
sw/airborne/firmwares/rotorcraft/navigation.h
+4 -4
View File
@@ -33,7 +33,7 @@
#include "subsystems/navigation/waypoints.h"
#include "subsystems/navigation/common_flight_plan.h"
#include "firmwares/rotorcraft/autopilot.h"
#include "autopilot.h"
/** default approaching_time for a wp */
#ifndef CARROT
@@ -135,17 +135,17 @@ extern void nav_set_failsafe(void);
/* ground detection */
extern bool nav_detect_ground(void);
#define NavStartDetectGround() ({ autopilot_detect_ground_once = true; false; })
#define NavStartDetectGround() ({ autopilot.detect_ground_once = true; false; })
#define NavDetectGround() nav_detect_ground()
/* switching motors on/off */
static inline void NavKillThrottle(void)
{
if (autopilot_mode == AP_MODE_NAV) { autopilot_set_motors_on(FALSE); }
if (autopilot_get_mode() == AP_MODE_NAV) { autopilot_set_motors_on(FALSE); }
}
static inline void NavResurrect(void)
{
if (autopilot_mode == AP_MODE_NAV) { autopilot_set_motors_on(TRUE); }
if (autopilot_get_mode() == AP_MODE_NAV) { autopilot_set_motors_on(TRUE); }
}
sw/airborne/firmwares/rotorcraft/rotorcraft_datalink.c
+1 -1
View File
@@ -34,7 +34,7 @@
#include "math/pprz_geodetic_int.h"
#endif
#include "firmwares/rotorcraft/autopilot.h"
#include "autopilot.h"
#include "firmwares/rotorcraft/autopilot_guided.h"
void firmware_parse_msg(struct link_device *dev __attribute__((unused)), struct transport_tx *trans __attribute__((unused)), uint8_t *buf)
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_heli_indi.c
+1 -1
View File
@@ -557,7 +557,7 @@ void stabilization_attitude_run(bool in_flight)
/* Thrust is not applied */
/* Disable tail when not armed, because this thing goes crazy */
if (!autopilot_motors_on) {
if (!autopilot_get_motors_on()) {
stabilization_cmd[COMMAND_YAW] = 0;
}
}
sw/airborne/modules/cam_control/cam.c
+2 -2
View File
@@ -130,7 +130,7 @@ void cam_periodic(void)
{
#if defined(CAM_FIXED_FOR_FPV_IN_AUTO1) && CAM_FIXED_FOR_FPV_IN_AUTO1 == 1
//Position the camera for straight view.
if (pprz_mode == PPRZ_MODE_AUTO2) {
if (autopilot_get_mode() == PPRZ_MODE_AUTO2) {
#endif
switch (cam_mode) {
case CAM_MODE_OFF:
@@ -169,7 +169,7 @@ void cam_periodic(void)
break;
}
#if defined(CAM_FIXED_FOR_FPV_IN_AUTO1) && CAM_FIXED_FOR_FPV_IN_AUTO1 == 1
} else if (pprz_mode == PPRZ_MODE_AUTO1) {
} else if (autopilot_get_mode() == PPRZ_MODE_AUTO1) {
//Position the camera for straight view.
#if defined(CAM_TILT_POSITION_FOR_FPV)
sw/airborne/modules/cam_control/point.c
+2 -2
View File
@@ -345,8 +345,8 @@ void vPoint(float fPlaneEast, float fPlaneNorth, float fPlaneAltitude,
#if defined(RADIO_CAM_LOCK)
float radio_cam_lock = imcu_get_radio(RADIO_CAM_LOCK);
if ((radio_cam_lock > MAX_PPRZ / 2) && pprz_mode == PPRZ_MODE_AUTO2) { cam_lock = true; }
if ((radio_cam_lock < MIN_PPRZ / 2) && pprz_mode == PPRZ_MODE_AUTO2) { cam_lock = false; }
if ((radio_cam_lock > MAX_PPRZ / 2) && autopilot_get_mode() == PPRZ_MODE_AUTO2) { cam_lock = true; }
if ((radio_cam_lock < MIN_PPRZ / 2) && autopilot_get_mode() == PPRZ_MODE_AUTO2) { cam_lock = false; }
#endif
// When the variable "cam_lock" is set then the last calculated position is set as the target waypoint.
if (cam_lock == FALSE) {
sw/airborne/modules/com/generic_com.c
+3 -3
View File
@@ -32,7 +32,7 @@
#include "subsystems/electrical.h"
#include "generated/airframe.h"
#include "inter_mcu.h"
#include "firmwares/fixedwing/autopilot.h"
#include "autopilot.h"
#include "subsystems/navigation/common_nav.h"
#define NB_DATA 24
@@ -82,9 +82,9 @@ void generic_com_periodic(void)
com_trans.buf[17] = electrical.vsupply; // decivolts
com_trans.buf[18] = (uint8_t)(energy / 100); // deciAh
com_trans.buf[19] = (uint8_t)(imcu_get_command(COMMAND_THROTTLE) * 100 / MAX_PPRZ);
com_trans.buf[20] = pprz_mode;
com_trans.buf[20] = autopilot_get_mode();
com_trans.buf[21] = nav_block;
FillBufWith16bit(com_trans.buf, 22, autopilot_flight_time);
FillBufWith16bit(com_trans.buf, 22, autopilot.flight_time);
i2c_transmit(&GENERIC_COM_I2C_DEV, &com_trans, GENERIC_COM_SLAVE_ADDR, NB_DATA);
}
sw/airborne/modules/ctrl/optical_flow_landing.c
+1 -1
View File
@@ -53,7 +53,7 @@ float previous_cov_err;
#define MINIMUM_GAIN 0.1
// used for automated landing:
#include "firmwares/rotorcraft/autopilot.h"
#include "autopilot.h"
#include "subsystems/navigation/common_flight_plan.h"
#include "subsystems/datalink/telemetry.h"
sw/airborne/modules/datalink/mavlink.c
+6 -6
View File
@@ -323,7 +323,7 @@ void mavlink_common_message_handler(const mavlink_message_t *msg)
result = MAV_RESULT_FAILED;
if (cmd.param1 > 0.5) {
autopilot_set_mode(AP_MODE_GUIDED);
if (autopilot_mode == AP_MODE_GUIDED) {
if (autopilot_get_mode() == AP_MODE_GUIDED) {
result = MAV_RESULT_ACCEPTED;
}
}
@@ -338,12 +338,12 @@ void mavlink_common_message_handler(const mavlink_message_t *msg)
result = MAV_RESULT_FAILED;
if (cmd.param1 > 0.5) {
autopilot_set_motors_on(TRUE);
if (autopilot_motors_on)
if (autopilot_get_motors_on())
result = MAV_RESULT_ACCEPTED;
}
else {
autopilot_set_motors_on(FALSE);
if (!autopilot_motors_on)
if (!autopilot_get_motors_on())
result = MAV_RESULT_ACCEPTED;
}
break;
@@ -440,7 +440,7 @@ static void mavlink_send_heartbeat(struct transport_tx *trans, struct link_devic
uint8_t mav_mode = 0;
#ifdef AP
uint8_t mav_type = MAV_TYPE_FIXED_WING;
switch (pprz_mode) {
switch (autopilot_get_mode()) {
case PPRZ_MODE_MANUAL:
mav_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
break;
@@ -458,7 +458,7 @@ static void mavlink_send_heartbeat(struct transport_tx *trans, struct link_devic
}
#else
uint8_t mav_type = MAV_TYPE_QUADROTOR;
switch (autopilot_mode) {
switch (autopilot_get_mode()) {
case AP_MODE_HOME:
mav_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
break;
@@ -488,7 +488,7 @@ static void mavlink_send_heartbeat(struct transport_tx *trans, struct link_devic
}
#endif
if (stateIsAttitudeValid()) {
if (kill_throttle) {
if (autopilot_throttle_killed()) {
mav_state = MAV_STATE_STANDBY;
} else {
mav_state = MAV_STATE_ACTIVE;
sw/airborne/modules/geo_mag/geo_mag.c
+1 -1
View File
@@ -51,7 +51,7 @@ void geo_mag_init(void)
void geo_mag_periodic(void)
{
//FIXME: kill_throttle has no place in a geomag module
if (!geo_mag.ready && GpsFixValid() && kill_throttle) {
if (!geo_mag.ready && GpsFixValid() && autopilot_throttle_killed()) {
geo_mag.calc_once = true;
}
}

Some files were not shown because too many files have changed in this diff Show More