2011-09-08 22:29:39 -03:00
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
2013-11-05 00:55:08 -04:00
#define THISFIRMWARE "ArduPlane V2.76"
2011-09-08 22:29:39 -03:00
/*
2013-07-19 19:20:50 -03:00
Lead developer: Andrew Tridgell
Authors: Doug Weibel, Jose Julio, Jordi Munoz, Jason Short, Randy Mackay, Pat Hickey, John Arne Birkeland, Olivier Adler, Amilcar Lucas, Gregory Fletcher, Paul Riseborough, Brandon Jones, Jon Challinger
Thanks to: Chris Anderson, Michael Oborne, Paul Mather, Bill Premerlani, James Cohen, JB from rotorFX, Automatik, Fefenin, Peter Meister, Remzibi, Yury Smirnov, Sandro Benigno, Max Levine, Roberto Navoni, Lorenz Meier, Yury MonZon
Please contribute your ideas! See http://dev.ardupilot.com for details
This program 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 3 of the License, or
(at your option) any later version.
This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
2012-08-21 23:19:51 -03:00
*/
2011-09-08 22:29:39 -03:00
////////////////////////////////////////////////////////////////////////////////
// Header includes
////////////////////////////////////////////////////////////////////////////////
#include <math.h>
2012-12-04 18:22:21 -04:00
#include <stdarg.h>
#include <stdio.h>
2011-09-08 22:29:39 -03:00
#include <AP_Common.h>
2012-10-27 00:55:17 -03:00
#include <AP_Progmem.h>
2012-12-04 18:22:21 -04:00
#include <AP_HAL.h>
2012-10-19 00:53:39 -03:00
#include <AP_Menu.h>
2012-08-20 20:22:44 -03:00
#include <AP_Param.h>
2011-09-08 22:29:39 -03:00
#include <AP_GPS.h> // ArduPilot GPS library
2011-11-27 01:51:47 -04:00
#include <AP_Baro.h> // ArduPilot barometer library
2011-09-08 22:29:39 -03:00
#include <AP_Compass.h> // ArduPilot Mega Magnetometer Library
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
2012-12-04 18:22:21 -04:00
#include <AP_ADC.h> // ArduPilot Mega Analog to Digital Converter Library
#include <AP_ADC_AnalogSource.h>
2012-11-05 00:32:13 -04:00
#include <AP_InertialSensor.h> // Inertial Sensor Library
2012-03-11 05:13:31 -03:00
#include <AP_AHRS.h> // ArduPilot Mega DCM Library
2011-09-08 22:29:39 -03:00
#include <RC_Channel.h> // RC Channel Library
2012-08-21 23:19:51 -03:00
#include <AP_RangeFinder.h> // Range finder library
#include <Filter.h> // Filter library
2012-11-05 00:32:13 -04:00
#include <AP_Buffer.h> // APM FIFO Buffer
2011-10-01 19:55:41 -03:00
#include <AP_Relay.h> // APM relay
2012-08-21 23:19:51 -03:00
#include <AP_Camera.h> // Photo or video camera
2012-07-15 22:21:50 -03:00
#include <AP_Airspeed.h>
2011-09-09 11:18:38 -03:00
#include <memcheck.h>
2011-09-08 22:29:39 -03:00
2012-12-04 18:22:21 -04:00
#include <APM_OBC.h>
#include <APM_Control.h>
#include <GCS_MAVLink.h> // MAVLink GCS definitions
#include <AP_Mount.h> // Camera/Antenna mount
#include <AP_Declination.h> // ArduPilot Mega Declination Helper Library
2012-12-08 00:35:49 -04:00
#include <DataFlash.h>
2012-12-18 05:04:31 -04:00
#include <SITL.h>
2013-06-04 00:34:58 -03:00
#include <AP_Scheduler.h> // main loop scheduler
2012-12-04 18:22:21 -04:00
2013-04-11 21:25:46 -03:00
#include <AP_Navigation.h>
#include <AP_L1_Control.h>
2013-06-03 02:32:08 -03:00
#include <AP_RCMapper.h> // RC input mapping library
2013-04-11 21:25:46 -03:00
2013-09-12 22:46:54 -03:00
#include <AP_Vehicle.h>
2013-06-26 05:36:53 -03:00
#include <AP_SpdHgtControl.h>
#include <AP_TECS.h>
2013-08-24 05:58:32 -03:00
#include <AP_Notify.h> // Notify library
2013-09-29 09:54:39 -03:00
#include <AP_BattMonitor.h> // Battery monitor library
2013-08-24 05:58:32 -03:00
2012-12-04 18:22:21 -04:00
// Pre-AP_HAL compatibility
#include "compat.h"
2011-09-08 22:29:39 -03:00
// Configuration
#include "config.h"
// Local modules
#include "defines.h"
2013-07-17 22:58:23 -03:00
2013-09-12 22:46:54 -03:00
// key aircraft parameters passed to multiple libraries
static AP_Vehicle::FixedWing aparm;
2013-07-17 22:58:23 -03:00
2011-09-08 22:29:39 -03:00
#include "Parameters.h"
#include "GCS.h"
2012-12-04 18:22:21 -04:00
#include <AP_HAL_AVR.h>
2012-12-12 18:00:17 -04:00
#include <AP_HAL_AVR_SITL.h>
2013-01-01 21:50:42 -04:00
#include <AP_HAL_PX4.h>
2013-09-23 00:44:03 -03:00
#include <AP_HAL_FLYMAPLE.h>
2013-09-22 03:04:36 -03:00
#include <AP_HAL_Linux.h>
2012-12-17 22:31:26 -04:00
#include <AP_HAL_Empty.h>
2012-03-11 07:46:41 -03:00
2012-12-04 18:22:21 -04:00
AP_HAL::BetterStream* cliSerial;
2012-12-18 05:04:31 -04:00
const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
2011-09-08 22:29:39 -03:00
2012-11-21 01:19:16 -04:00
2012-12-04 18:22:21 -04:00
////////////////////////////////////////////////////////////////////////////////
// Outback Challenge Failsafe Support
////////////////////////////////////////////////////////////////////////////////
2012-07-08 22:49:26 -03:00
#if OBC_FAILSAFE == ENABLED
APM_OBC obc;
#endif
2012-11-29 07:56:40 -04:00
////////////////////////////////////////////////////////////////////////////////
// the rate we run the main loop at
////////////////////////////////////////////////////////////////////////////////
static const AP_InertialSensor::Sample_rate ins_sample_rate = AP_InertialSensor::RATE_50HZ;
2011-09-08 22:29:39 -03:00
////////////////////////////////////////////////////////////////////////////////
// Parameters
////////////////////////////////////////////////////////////////////////////////
//
// Global parameters are all contained within the 'g' class.
//
2012-08-21 23:19:51 -03:00
static Parameters g;
2011-09-08 22:29:39 -03:00
2013-06-04 00:34:58 -03:00
// main loop scheduler
static AP_Scheduler scheduler;
2013-08-24 05:58:32 -03:00
2013-06-03 02:32:08 -03:00
// mapping between input channels
static RCMapper rcmap;
// primary control channels
static RC_Channel *channel_roll;
static RC_Channel *channel_pitch;
static RC_Channel *channel_throttle;
static RC_Channel *channel_rudder;
2013-11-30 09:53:55 -04:00
// notification object for LEDs, buzzers etc (parameter set to false disables external leds)
2013-08-29 00:13:58 -03:00
static AP_Notify notify;
2011-09-08 22:29:39 -03:00
////////////////////////////////////////////////////////////////////////////////
// prototypes
2011-09-09 11:18:38 -03:00
static void update_events(void);
2013-03-21 21:53:45 -03:00
void gcs_send_text_fmt(const prog_char_t *fmt, ...);
2013-04-20 02:18:10 -03:00
static void print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode);
2011-09-08 22:29:39 -03:00
2012-12-08 00:35:49 -04:00
////////////////////////////////////////////////////////////////////////////////
// DataFlash
////////////////////////////////////////////////////////////////////////////////
2013-04-19 18:24:22 -03:00
#if LOGGING_ENABLED == ENABLED
2012-12-12 18:00:17 -04:00
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
2013-11-04 06:37:50 -04:00
static DataFlash_APM1 DataFlash;
2012-12-12 18:00:17 -04:00
#elif CONFIG_HAL_BOARD == HAL_BOARD_APM2
2013-11-04 06:37:50 -04:00
static DataFlash_APM2 DataFlash;
2012-12-12 18:00:17 -04:00
#elif CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
2013-11-04 06:37:50 -04:00
//static DataFlash_File DataFlash("logs");
static DataFlash_SITL DataFlash;
2013-04-17 08:35:27 -03:00
#elif CONFIG_HAL_BOARD == HAL_BOARD_PX4
static DataFlash_File DataFlash("/fs/microsd/APM/logs");
2013-09-28 02:00:14 -03:00
#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX
2013-10-01 00:05:18 -03:00
static DataFlash_File DataFlash("logs");
2013-01-01 21:50:42 -04:00
#else
// no dataflash driver
DataFlash_Empty DataFlash;
2012-12-08 00:35:49 -04:00
#endif
2013-04-19 18:24:22 -03:00
#endif
2012-12-12 18:00:17 -04:00
2013-11-04 04:48:22 -04:00
// scaled roll limit based on pitch
static int32_t roll_limit_cd;
static int32_t pitch_limit_min_cd;
2011-09-08 22:29:39 -03:00
////////////////////////////////////////////////////////////////////////////////
// Sensors
////////////////////////////////////////////////////////////////////////////////
//
// There are three basic options related to flight sensor selection.
//
// - Normal flight mode. Real sensors are used.
// - HIL Attitude mode. Most sensors are disabled, as the HIL
// protocol supplies attitude information directly.
// - HIL Sensors mode. Synthetic sensors are configured that
// supply data from the simulation.
//
// All GPS access should be through this pointer.
2011-09-09 11:18:38 -03:00
static GPS *g_gps;
// flight modes convenience array
2012-08-21 23:19:51 -03:00
static AP_Int8 *flight_modes = &g.flight_mode1;
2011-09-08 22:29:39 -03:00
2013-01-21 01:08:55 -04:00
#if CONFIG_BARO == AP_BARO_BMP085
2012-12-12 18:00:17 -04:00
static AP_Baro_BMP085 barometer;
2013-01-21 01:08:55 -04:00
#elif CONFIG_BARO == AP_BARO_PX4
2013-01-04 06:07:56 -04:00
static AP_Baro_PX4 barometer;
2013-01-21 01:08:55 -04:00
#elif CONFIG_BARO == AP_BARO_HIL
2013-05-02 02:09:27 -03:00
static AP_Baro_HIL barometer;
2013-01-21 01:08:55 -04:00
#elif CONFIG_BARO == AP_BARO_MS5611
#if CONFIG_MS5611_SERIAL == AP_BARO_MS5611_SPI
static AP_Baro_MS5611 barometer(&AP_Baro_MS5611::spi);
#elif CONFIG_MS5611_SERIAL == AP_BARO_MS5611_I2C
static AP_Baro_MS5611 barometer(&AP_Baro_MS5611::i2c);
#else
#error Unrecognized CONFIG_MS5611_SERIAL setting.
#endif
2013-01-04 06:07:56 -04:00
#else
2013-01-21 01:08:55 -04:00
#error Unrecognized CONFIG_BARO setting
#endif
#if CONFIG_COMPASS == AP_COMPASS_PX4
static AP_Compass_PX4 compass;
#elif CONFIG_COMPASS == AP_COMPASS_HMC5843
2012-08-21 23:19:51 -03:00
static AP_Compass_HMC5843 compass;
2013-01-21 01:08:55 -04:00
#elif CONFIG_COMPASS == AP_COMPASS_HIL
static AP_Compass_HIL compass;
#else
#error Unrecognized CONFIG_COMPASS setting
2013-01-04 06:07:56 -04:00
#endif
2011-09-08 22:29:39 -03:00
2013-01-21 01:08:55 -04:00
// GPS selection
#if GPS_PROTOCOL == GPS_PROTOCOL_AUTO
2012-12-17 22:31:26 -04:00
AP_GPS_Auto g_gps_driver(&g_gps);
2011-09-08 22:29:39 -03:00
2013-01-21 01:08:55 -04:00
#elif GPS_PROTOCOL == GPS_PROTOCOL_NMEA
AP_GPS_NMEA g_gps_driver;
2011-09-08 22:29:39 -03:00
2013-01-21 01:08:55 -04:00
#elif GPS_PROTOCOL == GPS_PROTOCOL_SIRF
AP_GPS_SIRF g_gps_driver;
2011-09-08 22:29:39 -03:00
2013-01-21 01:08:55 -04:00
#elif GPS_PROTOCOL == GPS_PROTOCOL_UBLOX
AP_GPS_UBLOX g_gps_driver;
2011-09-08 22:29:39 -03:00
2013-01-21 01:08:55 -04:00
#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK
AP_GPS_MTK g_gps_driver;
2011-09-08 22:29:39 -03:00
2013-01-21 01:08:55 -04:00
#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK19
AP_GPS_MTK19 g_gps_driver;
2011-09-08 22:29:39 -03:00
2013-01-21 01:08:55 -04:00
#elif GPS_PROTOCOL == GPS_PROTOCOL_NONE
AP_GPS_None g_gps_driver;
2011-09-08 22:29:39 -03:00
2013-01-21 01:08:55 -04:00
#elif GPS_PROTOCOL == GPS_PROTOCOL_HIL
AP_GPS_HIL g_gps_driver;
#else
2012-08-21 23:19:51 -03:00
#error Unrecognised GPS_PROTOCOL setting.
2013-01-21 01:08:55 -04:00
#endif // GPS PROTOCOL
2012-08-21 23:19:51 -03:00
2013-09-28 11:52:51 -03:00
#if CONFIG_INS_TYPE == CONFIG_INS_OILPAN || CONFIG_HAL_BOARD == HAL_BOARD_APM1
AP_ADC_ADS7844 apm1_adc;
#endif
2013-01-21 01:08:55 -04:00
#if CONFIG_INS_TYPE == CONFIG_INS_MPU6000
2012-09-28 07:21:59 -03:00
AP_InertialSensor_MPU6000 ins;
2013-01-21 01:08:55 -04:00
#elif CONFIG_INS_TYPE == CONFIG_INS_PX4
2013-01-04 06:07:56 -04:00
AP_InertialSensor_PX4 ins;
2013-09-28 06:34:21 -03:00
#elif CONFIG_INS_TYPE == CONFIG_INS_HIL
AP_InertialSensor_HIL ins;
2013-01-21 01:08:55 -04:00
#elif CONFIG_INS_TYPE == CONFIG_INS_OILPAN
2013-06-02 22:40:44 -03:00
AP_InertialSensor_Oilpan ins( &apm1_adc );
2013-09-23 00:44:03 -03:00
#elif CONFIG_INS_TYPE == CONFIG_INS_FLYMAPLE
AP_InertialSensor_Flymaple ins;
2013-10-07 21:24:21 -03:00
#elif CONFIG_INS_TYPE == CONFIG_INS_L3G4200D
AP_InertialSensor_L3G4200D ins;
2013-01-21 01:08:55 -04:00
#else
#error Unrecognised CONFIG_INS_TYPE setting.
#endif // CONFIG_INS_TYPE
2012-03-03 03:33:40 -04:00
2013-11-03 23:37:32 -04:00
AP_AHRS_DCM ahrs(ins, g_gps);
2011-11-13 02:15:45 -04:00
2013-08-13 00:49:34 -03:00
static AP_L1_Control L1_controller(ahrs);
2013-08-14 01:59:01 -03:00
static AP_TECS TECS_controller(ahrs, aparm);
2013-04-11 21:25:46 -03:00
2013-08-14 01:57:41 -03:00
// Attitude to servo controllers
static AP_RollController rollController(ahrs, aparm);
static AP_PitchController pitchController(ahrs, aparm);
static AP_YawController yawController(ahrs, aparm);
2013-10-03 09:01:43 -03:00
static AP_SteerController steerController(ahrs);
2013-08-14 01:57:41 -03:00
2013-01-21 01:08:55 -04:00
#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
SITL sitl;
#endif
2011-09-08 22:29:39 -03:00
2012-12-04 02:32:37 -04:00
// Training mode
static bool training_manual_roll; // user has manual roll control
static bool training_manual_pitch; // user has manual pitch control
2013-09-07 18:31:10 -03:00
// should throttle be pass-thru in guided?
static bool guided_throttle_passthru;
2011-09-08 22:29:39 -03:00
////////////////////////////////////////////////////////////////////////////////
// GCS selection
////////////////////////////////////////////////////////////////////////////////
2013-11-22 04:18:19 -04:00
static const uint8_t num_gcs = MAVLINK_COMM_NUM_BUFFERS;
static GCS_MAVLINK gcs[MAVLINK_COMM_NUM_BUFFERS];
2011-09-08 22:29:39 -03:00
2013-04-11 21:25:46 -03:00
// selected navigation controller
static AP_Navigation *nav_controller = &L1_controller;
2013-06-26 05:36:53 -03:00
// selected navigation controller
static AP_SpdHgtControl *SpdHgt_Controller = &TECS_controller;
2011-09-09 11:18:38 -03:00
////////////////////////////////////////////////////////////////////////////////
2012-12-04 18:22:21 -04:00
// Analog Inputs
2011-09-09 11:18:38 -03:00
////////////////////////////////////////////////////////////////////////////////
2013-05-13 02:14:56 -03:00
// a pin for reading the receiver RSSI voltage.
2013-04-15 01:46:01 -03:00
static AP_HAL::AnalogSource *rssi_analog_source;
2012-12-04 18:22:21 -04:00
2013-04-15 01:46:01 -03:00
static AP_HAL::AnalogSource *vcc_pin;
2012-12-04 18:22:21 -04:00
2013-10-30 19:23:21 -03:00
////////////////////////////////////////////////////////////////////////////////
// Sonar
////////////////////////////////////////////////////////////////////////////////
static AP_RangeFinder_analog sonar;
2012-12-04 18:22:21 -04:00
////////////////////////////////////////////////////////////////////////////////
// Relay
////////////////////////////////////////////////////////////////////////////////
2013-04-15 01:46:01 -03:00
static AP_Relay relay;
2012-01-16 13:28:03 -04:00
2013-01-01 19:18:45 -04:00
// Camera
#if CAMERA == ENABLED
2013-04-15 01:46:01 -03:00
static AP_Camera camera(&relay);
2013-01-01 19:18:45 -04:00
#endif
2012-01-16 13:28:03 -04:00
2011-09-08 22:29:39 -03:00
////////////////////////////////////////////////////////////////////////////////
// Global variables
////////////////////////////////////////////////////////////////////////////////
2013-09-19 03:23:58 -03:00
// remember if USB is connected, so we can adjust baud rate
2011-11-20 05:31:45 -04:00
static bool usb_connected;
2011-09-08 22:29:39 -03:00
/* Radio values
2012-08-21 23:19:51 -03:00
* Channel assignments
2012-09-18 00:40:01 -03:00
* 1 Ailerons
2012-08-21 23:19:51 -03:00
* 2 Elevator
* 3 Throttle
2012-09-18 00:40:01 -03:00
* 4 Rudder
2012-08-21 23:19:51 -03:00
* 5 Aux5
* 6 Aux6
* 7 Aux7
* 8 Aux8/Mode
* Each Aux channel can be configured to have any of the available auxiliary functions assigned to it.
* See libraries/RC_Channel/RC_Channel_aux.h for more information
*/
2011-09-08 22:29:39 -03:00
2012-01-11 15:37:43 -04:00
////////////////////////////////////////////////////////////////////////////////
2011-09-08 22:29:39 -03:00
// Radio
2012-01-11 15:37:43 -04:00
////////////////////////////////////////////////////////////////////////////////
// This is the state of the flight control system
// There are multiple states defined such as MANUAL, FBW-A, AUTO
2012-11-30 17:15:48 -04:00
enum FlightMode control_mode = INITIALISING;
2012-01-11 15:37:43 -04:00
// Used to maintain the state of the previous control switch position
// This is set to -1 when we need to re-read the switch
2012-12-04 18:22:21 -04:00
uint8_t oldSwitchPosition;
2012-01-11 15:37:43 -04:00
// This is used to enable the inverted flight feature
2012-08-21 23:19:51 -03:00
bool inverted_flight = false;
2013-04-15 01:53:56 -03:00
static struct {
// These are trim values used for elevon control
// For elevons radio_in[CH_ROLL] and radio_in[CH_PITCH] are
// equivalent aileron and elevator, not left and right elevon
uint16_t trim1;
uint16_t trim2;
// These are used in the calculation of elevon1_trim and elevon2_trim
uint16_t ch1_temp;
uint16_t ch2_temp;
} elevon = {
2013-04-15 08:59:07 -03:00
trim1 : 1500,
trim2 : 1500,
ch1_temp : 1500,
ch2_temp : 1500
2013-04-15 01:53:56 -03:00
};
2013-04-15 01:46:01 -03:00
2012-01-11 15:37:43 -04:00
////////////////////////////////////////////////////////////////////////////////
// Failsafe
////////////////////////////////////////////////////////////////////////////////
2013-07-19 01:11:16 -03:00
static struct {
// A flag if GCS joystick control is in use
2013-07-20 04:08:58 -03:00
uint8_t rc_override_active:1;
// Used to track if the value on channel 3 (throtttle) has fallen below the failsafe threshold
// RC receiver should be set up to output a low throttle value when signal is lost
uint8_t ch3_failsafe:1;
// has the saved mode for failsafe been set?
uint8_t saved_mode_set:1;
2013-09-29 09:54:39 -03:00
// flag to hold whether battery low voltage threshold has been breached
uint8_t low_battery:1;
2013-07-20 04:08:58 -03:00
// saved flight mode
enum FlightMode saved_mode;
2013-07-19 01:11:16 -03:00
// A tracking variable for type of failsafe active
// Used for failsafe based on loss of RC signal or GCS signal
int16_t state;
// number of low ch3 values
uint8_t ch3_counter;
// the time when the last HEARTBEAT message arrived from a GCS
uint32_t last_heartbeat_ms;
// A timer used to track how long we have been in a "short failsafe" condition due to loss of RC signal
uint32_t ch3_timer_ms;
} failsafe;
2011-09-09 11:18:38 -03:00
2012-01-11 15:37:43 -04:00
////////////////////////////////////////////////////////////////////////////////
2011-09-08 22:29:39 -03:00
// GPS variables
2012-01-11 15:37:43 -04:00
////////////////////////////////////////////////////////////////////////////////
// This is used to scale GPS values for EEPROM storage
// 10^7 times Decimal GPS means 1 == 1cm
// This approximation makes calculations integer and it's easy to read
2012-08-21 23:19:51 -03:00
static const float t7 = 10000000.0;
2012-01-11 15:37:43 -04:00
// We use atan2 and other trig techniques to calaculate angles
// A counter used to count down valid gps fixes to allow the gps estimate to settle
// before recording our home position (and executing a ground start if we booted with an air start)
2012-12-04 18:22:21 -04:00
static uint8_t ground_start_count = 5;
2012-06-10 17:10:07 -03:00
// Used to compute a speed estimate from the first valid gps fixes to decide if we are
2012-01-11 15:37:43 -04:00
// on the ground or in the air. Used to decide if a ground start is appropriate if we
// booted with an air start.
2012-08-21 23:19:51 -03:00
static int16_t ground_start_avg;
2011-09-08 22:29:39 -03:00
2012-08-10 23:01:08 -03:00
// true if we have a position estimate from AHRS
static bool have_position;
2012-01-15 19:11:02 -04:00
////////////////////////////////////////////////////////////////////////////////
2011-09-08 22:29:39 -03:00
// Location & Navigation
2012-01-15 19:11:02 -04:00
////////////////////////////////////////////////////////////////////////////////
2012-08-07 03:05:51 -03:00
2012-06-10 17:10:07 -03:00
// There may be two active commands in Auto mode.
2012-01-16 12:45:42 -04:00
// This indicates the active navigation command by index number
2012-12-04 18:22:21 -04:00
static uint8_t nav_command_index;
2012-01-16 12:45:42 -04:00
// This indicates the active non-navigation command by index number
2012-12-04 18:22:21 -04:00
static uint8_t non_nav_command_index;
2012-01-16 12:45:42 -04:00
// This is the command type (eg navigate to waypoint) of the active navigation command
2012-12-04 18:22:21 -04:00
static uint8_t nav_command_ID = NO_COMMAND;
static uint8_t non_nav_command_ID = NO_COMMAND;
2011-09-08 22:29:39 -03:00
2012-01-16 12:45:42 -04:00
////////////////////////////////////////////////////////////////////////////////
2011-09-08 22:29:39 -03:00
// Airspeed
2012-01-16 12:45:42 -04:00
////////////////////////////////////////////////////////////////////////////////
// The calculated airspeed to use in FBW-B. Also used in higher modes for insuring min ground speed is met.
2012-08-07 03:05:51 -03:00
// Also used for flap deployment criteria. Centimeters per second.
2012-08-21 23:19:51 -03:00
static int32_t target_airspeed_cm;
2012-08-07 03:05:51 -03:00
2012-01-16 12:45:42 -04:00
// The difference between current and desired airspeed. Used in the pitch controller. Centimeters per second.
2012-08-21 23:19:51 -03:00
static float airspeed_error_cm;
2012-08-07 03:05:51 -03:00
2012-06-10 17:10:07 -03:00
// An amount that the airspeed should be increased in auto modes based on the user positioning the
2012-01-16 12:45:42 -04:00
// throttle stick in the top half of the range. Centimeters per second.
2012-08-21 23:19:51 -03:00
static int16_t airspeed_nudge_cm;
2012-08-07 03:05:51 -03:00
2012-01-16 12:45:42 -04:00
// Similar to airspeed_nudge, but used when no airspeed sensor.
// 0-(throttle_max - throttle_cruise) : throttle nudge in Auto mode using top 1/2 of throttle stick travel
2012-08-21 23:19:51 -03:00
static int16_t throttle_nudge = 0;
2011-12-09 19:40:56 -04:00
2012-08-22 00:58:25 -03:00
// receiver RSSI
static uint8_t receiver_rssi;
2012-01-16 12:45:42 -04:00
////////////////////////////////////////////////////////////////////////////////
2011-12-09 19:40:56 -04:00
// Ground speed
2012-01-16 12:45:42 -04:00
////////////////////////////////////////////////////////////////////////////////
// The amount current ground speed is below min ground speed. Centimeters per second
2012-08-21 23:19:51 -03:00
static int32_t groundspeed_undershoot = 0;
2011-09-08 22:29:39 -03:00
2012-01-16 12:45:42 -04:00
// Difference between current altitude and desired altitude. Centimeters
2012-08-21 23:19:51 -03:00
static int32_t altitude_error_cm;
2012-08-07 03:05:51 -03:00
2012-01-16 12:45:42 -04:00
////////////////////////////////////////////////////////////////////////////////
2011-09-08 22:29:39 -03:00
// Battery Sensors
2012-01-16 12:45:42 -04:00
////////////////////////////////////////////////////////////////////////////////
2013-09-30 11:15:35 -03:00
static AP_BattMonitor battery;
2011-09-08 22:29:39 -03:00
2012-01-16 12:45:42 -04:00
////////////////////////////////////////////////////////////////////////////////
2011-09-08 22:29:39 -03:00
// Airspeed Sensors
2012-01-16 12:45:42 -04:00
////////////////////////////////////////////////////////////////////////////////
2013-08-28 09:36:37 -03:00
AP_Airspeed airspeed(aparm);
2011-09-08 22:29:39 -03:00
2013-07-10 10:25:38 -03:00
////////////////////////////////////////////////////////////////////////////////
// ACRO controller state
////////////////////////////////////////////////////////////////////////////////
static struct {
bool locked_roll;
bool locked_pitch;
2013-07-13 08:06:45 -03:00
float locked_roll_err;
2013-07-10 10:25:38 -03:00
int32_t locked_pitch_cd;
} acro_state;
2013-07-13 07:05:53 -03:00
////////////////////////////////////////////////////////////////////////////////
// CRUISE controller state
////////////////////////////////////////////////////////////////////////////////
static struct {
bool locked_heading;
int32_t locked_heading_cd;
uint32_t lock_timer_ms;
} cruise_state;
2013-10-03 09:01:43 -03:00
////////////////////////////////////////////////////////////////////////////////
// ground steering controller state
////////////////////////////////////////////////////////////////////////////////
static struct {
// Direction held during phases of takeoff and landing centidegrees
// A value of -1 indicates the course has not been set/is not in use
// this is a 0..36000 value, or -1 for disabled
int32_t hold_course_cd;
// locked_course and locked_course_cd are used in stabilize mode
// when ground steering is active
bool locked_course;
float locked_course_err;
} steer_state = {
hold_course_cd : -1,
};
2012-01-16 12:45:42 -04:00
////////////////////////////////////////////////////////////////////////////////
2011-09-08 22:29:39 -03:00
// flight mode specific
2012-01-16 12:45:42 -04:00
////////////////////////////////////////////////////////////////////////////////
2012-11-05 00:32:13 -04:00
// Flag for using gps ground course instead of INS yaw. Set false when takeoff command in process.
2012-06-10 17:10:07 -03:00
static bool takeoff_complete = true;
2012-01-16 12:45:42 -04:00
// Flag to indicate if we have landed.
//Set land_complete if we are within 2 seconds distance or within 3 meters altitude of touchdown
2012-08-21 23:19:51 -03:00
static bool land_complete;
2012-01-16 12:45:42 -04:00
// Altitude threshold to complete a takeoff command in autonomous modes. Centimeters
2013-06-26 05:36:53 -03:00
static int32_t takeoff_altitude_cm;
2012-08-07 03:05:51 -03:00
2012-01-16 12:45:42 -04:00
// Minimum pitch to hold during takeoff command execution. Hundredths of a degree
2012-08-21 23:19:51 -03:00
static int16_t takeoff_pitch_cd;
2011-09-08 22:29:39 -03:00
2013-07-05 01:55:22 -03:00
// true if we are in an auto-throttle mode, which means
// we need to run the speed/height controller
static bool auto_throttle_mode;
2012-08-27 03:26:53 -03:00
// this controls throttle suppression in auto modes
static bool throttle_suppressed;
2012-01-16 12:45:42 -04:00
////////////////////////////////////////////////////////////////////////////////
2011-09-08 22:29:39 -03:00
// Loiter management
2012-01-16 12:45:42 -04:00
////////////////////////////////////////////////////////////////////////////////
2012-08-07 03:05:51 -03:00
2012-01-16 12:45:42 -04:00
////////////////////////////////////////////////////////////////////////////////
// Navigation control variables
////////////////////////////////////////////////////////////////////////////////
// The instantaneous desired bank angle. Hundredths of a degree
2012-08-21 23:19:51 -03:00
static int32_t nav_roll_cd;
2012-08-07 03:05:51 -03:00
2012-01-16 12:45:42 -04:00
// The instantaneous desired pitch angle. Hundredths of a degree
2012-08-21 23:19:51 -03:00
static int32_t nav_pitch_cd;
2012-01-16 12:45:42 -04:00
////////////////////////////////////////////////////////////////////////////////
// Waypoint distances
////////////////////////////////////////////////////////////////////////////////
// Distance between plane and next waypoint. Meters
2013-07-14 20:56:28 -03:00
static uint32_t wp_distance;
2012-08-07 03:05:51 -03:00
2012-01-16 12:45:42 -04:00
// Distance between previous and next waypoint. Meters
2013-01-26 22:16:26 -04:00
static uint32_t wp_totalDistance;
2012-01-16 12:45:42 -04:00
2013-04-11 21:25:46 -03:00
/*
meta data to support counting the number of circles in a loiter
*/
static struct {
2013-04-15 08:31:11 -03:00
// previous target bearing, used to update sum_cd
2013-04-11 21:25:46 -03:00
int32_t old_target_bearing_cd;
// Total desired rotation in a loiter. Used for Loiter Turns commands.
2013-04-15 08:31:11 -03:00
int32_t total_cd;
// total angle completed in the loiter so far
int32_t sum_cd;
// Direction for loiter. 1 for clockwise, -1 for counter-clockwise
int8_t direction;
// start time of the loiter. Milliseconds.
uint32_t start_time_ms;
// The amount of time we should stay in a loiter for the Loiter Time command. Milliseconds.
uint32_t time_max_ms;
2013-04-11 21:25:46 -03:00
} loiter;
2012-09-16 20:24:00 -03:00
// event control state
enum event_type {
EVENT_TYPE_RELAY=0,
EVENT_TYPE_SERVO=1
};
static struct {
enum event_type type;
// when the event was started in ms
uint32_t start_time_ms;
// how long to delay the next firing of event in millis
uint16_t delay_ms;
// how many times to cycle : -1 (or -2) = forever, 2 = do one cycle, 4 = do two cycles
int16_t repeat;
// RC channel for servos
uint8_t rc_channel;
2012-08-07 03:05:51 -03:00
2012-09-16 20:24:00 -03:00
// PWM for servos
uint16_t servo_value;
2012-08-07 03:05:51 -03:00
2012-09-16 20:24:00 -03:00
// the value used to cycle events (alternate value to event_value)
uint16_t undo_value;
} event_state;
2012-08-07 03:05:51 -03:00
2012-01-16 12:45:42 -04:00
////////////////////////////////////////////////////////////////////////////////
// Conditional command
////////////////////////////////////////////////////////////////////////////////
// A value used in condition commands (eg delay, change alt, etc.)
// For example in a change altitude command, it is the altitude to change to.
2012-08-21 23:19:51 -03:00
static int32_t condition_value;
2012-01-16 12:45:42 -04:00
// A starting value used to check the status of a conditional command.
// For example in a delay command the condition_start records that start time for the delay
2012-07-06 06:22:19 -03:00
static uint32_t condition_start;
2012-01-16 12:45:42 -04:00
// A value used in condition commands. For example the rate at which to change altitude.
2012-08-21 23:19:51 -03:00
static int16_t condition_rate;
2011-09-08 22:29:39 -03:00
2012-01-16 13:28:03 -04:00
////////////////////////////////////////////////////////////////////////////////
2011-09-08 22:29:39 -03:00
// 3D Location vectors
2012-01-16 13:28:03 -04:00
// Location structure defined in AP_Common
////////////////////////////////////////////////////////////////////////////////
// The home location used for RTL. The location is set when we first get stable GPS lock
2012-08-21 23:19:51 -03:00
static struct Location home;
2012-01-16 13:28:03 -04:00
// Flag for if we have g_gps lock and have set the home location
2012-08-21 23:19:51 -03:00
static bool home_is_set;
2012-01-16 13:28:03 -04:00
// The location of the previous waypoint. Used for track following and altitude ramp calculations
2012-08-21 23:19:51 -03:00
static struct Location prev_WP;
2012-01-16 13:28:03 -04:00
// The plane's current location
2012-08-21 23:19:51 -03:00
static struct Location current_loc;
2012-01-16 13:28:03 -04:00
// The location of the current/active waypoint. Used for altitude ramp, track following and loiter calculations.
2012-08-21 23:19:51 -03:00
static struct Location next_WP;
2012-01-16 13:28:03 -04:00
// The location of the active waypoint in Guided mode.
2012-08-21 23:19:51 -03:00
static struct Location guided_WP;
2012-01-16 13:28:03 -04:00
// The location structure information from the Nav command being processed
2012-08-21 23:19:51 -03:00
static struct Location next_nav_command;
2012-01-16 13:28:03 -04:00
// The location structure information from the Non-Nav command being processed
2012-08-21 23:19:51 -03:00
static struct Location next_nonnav_command;
2012-01-16 13:28:03 -04:00
////////////////////////////////////////////////////////////////////////////////
// Altitude / Climb rate control
////////////////////////////////////////////////////////////////////////////////
// The current desired altitude. Altitude is linearly ramped between waypoints. Centimeters
2012-08-21 23:19:51 -03:00
static int32_t target_altitude_cm;
2012-01-16 13:28:03 -04:00
// Altitude difference between previous and current waypoint. Centimeters
2012-08-21 23:19:51 -03:00
static int32_t offset_altitude_cm;
2011-09-08 22:29:39 -03:00
2012-01-16 13:28:03 -04:00
////////////////////////////////////////////////////////////////////////////////
2012-11-05 00:32:13 -04:00
// INS variables
2012-01-16 13:28:03 -04:00
////////////////////////////////////////////////////////////////////////////////
// The main loop execution time. Seconds
//This is the time between calls to the DCM algorithm and is the Integration time for the gyros.
2013-10-11 23:30:27 -03:00
static float G_Dt = 0.02f;
2011-09-08 22:29:39 -03:00
2012-01-16 13:28:03 -04:00
////////////////////////////////////////////////////////////////////////////////
2011-09-08 22:29:39 -03:00
// Performance monitoring
2012-01-16 13:28:03 -04:00
////////////////////////////////////////////////////////////////////////////////
// Timer used to accrue data and trigger recording of the performanc monitoring log message
2013-10-11 23:30:27 -03:00
static uint32_t perf_mon_timer;
2012-01-16 13:28:03 -04:00
// The maximum main loop execution time recorded in the current performance monitoring interval
2013-10-11 23:30:27 -03:00
static uint32_t G_Dt_max = 0;
2012-01-16 13:28:03 -04:00
// The number of gps fixes recorded in the current performance monitoring interval
2013-01-26 21:58:50 -04:00
static uint8_t gps_fix_count = 0;
2011-09-08 22:29:39 -03:00
2012-01-16 13:28:03 -04:00
////////////////////////////////////////////////////////////////////////////////
2011-09-08 22:29:39 -03:00
// System Timers
2012-01-16 13:28:03 -04:00
////////////////////////////////////////////////////////////////////////////////
2013-10-11 23:30:27 -03:00
// Time in microseconds of start of main control loop
static uint32_t fast_loopTimer_us;
2012-08-07 03:05:51 -03:00
2012-01-16 13:28:03 -04:00
// Number of milliseconds used in last main loop cycle
2013-10-11 23:30:27 -03:00
static uint32_t delta_us_fast_loop;
2012-08-07 03:05:51 -03:00
2012-01-16 13:28:03 -04:00
// Counter of main loop executions. Used for performance monitoring and failsafe processing
2012-08-21 23:19:51 -03:00
static uint16_t mainLoop_count;
2011-09-09 11:18:38 -03:00
2012-06-13 15:55:19 -03:00
// Camera/Antenna mount tracking and stabilisation stuff
// --------------------------------------
#if MOUNT == ENABLED
// current_loc uses the baro/gps soloution for altitude rather than gps only.
// mabe one could use current_loc for lat/lon too and eliminate g_gps alltogether?
2013-10-12 03:59:00 -03:00
AP_Mount camera_mount(¤t_loc, g_gps, ahrs, 0);
2012-08-08 18:22:24 -03:00
#endif
#if MOUNT2 == ENABLED
// current_loc uses the baro/gps soloution for altitude rather than gps only.
// mabe one could use current_loc for lat/lon too and eliminate g_gps alltogether?
2013-10-12 03:59:00 -03:00
AP_Mount camera_mount2(¤t_loc, g_gps, ahrs, 1);
2012-06-13 15:55:19 -03:00
#endif
2011-09-08 22:29:39 -03:00
////////////////////////////////////////////////////////////////////////////////
// Top-level logic
////////////////////////////////////////////////////////////////////////////////
2013-06-04 00:34:58 -03:00
/*
2013-10-14 19:52:23 -03:00
scheduler table - all regular tasks are listed here, along with how
often they should be called (in 20ms units) and the maximum time
they are expected to take (in microseconds)
2013-06-04 00:34:58 -03:00
*/
static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
2013-10-14 19:52:23 -03:00
{ read_radio, 1, 700 }, // 0
{ check_short_failsafe, 1, 1000 },
{ ahrs_update, 1, 6400 },
{ update_speed_height, 1, 1600 },
{ update_flight_mode, 1, 1400 },
{ stabilize, 1, 3500 },
{ set_servos, 1, 1600 },
2013-07-19 01:13:25 -03:00
{ read_control_switch, 7, 1000 },
2013-10-14 19:52:23 -03:00
{ gcs_retry_deferred, 1, 1000 },
{ update_GPS, 5, 3700 },
{ navigate, 5, 3000 }, // 10
{ update_compass, 5, 1200 },
{ read_airspeed, 5, 1200 },
2013-07-05 05:05:27 -03:00
{ update_alt, 5, 3400 },
2013-10-14 19:52:23 -03:00
{ calc_altitude_error, 5, 1000 },
{ update_commands, 5, 5000 },
2013-06-04 00:34:58 -03:00
{ obc_fs_check, 5, 1000 },
2013-07-05 05:05:27 -03:00
{ gcs_update, 1, 1700 },
2013-07-23 04:08:23 -03:00
{ gcs_data_stream_send, 1, 3000 },
2013-12-08 23:17:20 -04:00
{ update_events, 15, 1500 },
{ check_usb_mux, 5, 300 }, // 20
2013-06-04 00:34:58 -03:00
{ read_battery, 5, 1000 },
2013-07-05 05:05:27 -03:00
{ compass_accumulate, 1, 1500 },
2013-10-14 19:52:23 -03:00
{ barometer_accumulate, 1, 900 },
{ update_notify, 1, 300 },
2013-10-30 19:23:21 -03:00
{ read_sonars, 1, 500 },
2013-12-08 23:15:16 -04:00
{ one_second_loop, 50, 1000 },
2013-07-23 04:08:23 -03:00
{ check_long_failsafe, 15, 1000 },
2013-12-08 23:17:20 -04:00
{ read_receiver_rssi, 5, 1000 },
2013-07-19 19:15:12 -03:00
{ airspeed_ratio_update, 50, 1000 },
2013-12-08 23:17:20 -04:00
{ update_mount, 1, 1500 }, // 30
{ log_perf_info, 500, 1000 },
{ compass_save, 3000, 2500 },
2013-07-23 04:08:23 -03:00
{ update_logging, 5, 1200 },
2013-06-04 00:34:58 -03:00
};
2012-12-13 18:57:46 -04:00
// setup the var_info table
AP_Param param_loader(var_info, WP_START_BYTE);
2011-09-08 22:29:39 -03:00
void setup() {
2013-01-10 19:25:09 -04:00
// this needs to be the first call, as it fills memory with
// sentinel values
memcheck_init();
2012-12-04 18:22:21 -04:00
cliSerial = hal.console;
2012-12-12 18:00:17 -04:00
2012-12-13 18:57:46 -04:00
// load the default values of variables listed in var_info[]
AP_Param::setup_sketch_defaults();
2012-12-12 18:00:17 -04:00
2013-08-24 05:58:32 -03:00
// arduplane does not use arming nor pre-arm checks
AP_Notify::flags.armed = true;
AP_Notify::flags.pre_arm_check = true;
2013-09-17 21:50:34 -03:00
AP_Notify::flags.failsafe_battery = false;
2013-08-24 05:58:32 -03:00
2013-11-30 09:53:55 -04:00
notify.init(false);
2013-09-17 21:56:06 -03:00
2013-09-29 09:54:39 -03:00
battery.init();
2013-05-13 02:14:56 -03:00
rssi_analog_source = hal.analogin->channel(ANALOG_INPUT_NONE);
2012-12-04 18:22:21 -04:00
vcc_pin = hal.analogin->channel(ANALOG_INPUT_BOARD_VCC);
2012-08-21 23:19:51 -03:00
init_ardupilot();
2013-06-04 00:34:58 -03:00
// initialise the main loop scheduler
scheduler.init(&scheduler_tasks[0], sizeof(scheduler_tasks)/sizeof(scheduler_tasks[0]));
2011-09-08 22:29:39 -03:00
}
void loop()
{
2013-10-08 03:29:12 -03:00
// wait for an INS sample
if (!ins.wait_for_sample(1000)) {
return;
2013-09-28 07:09:52 -03:00
}
2013-10-11 23:30:27 -03:00
uint32_t timer = hal.scheduler->micros();
2013-10-08 03:29:12 -03:00
2013-10-11 23:30:27 -03:00
delta_us_fast_loop = timer - fast_loopTimer_us;
G_Dt = delta_us_fast_loop * 1.0e-6f;
fast_loopTimer_us = timer;
2013-10-08 03:29:12 -03:00
2013-10-28 03:10:00 -03:00
if (delta_us_fast_loop > G_Dt_max)
G_Dt_max = delta_us_fast_loop;
2013-10-08 03:29:12 -03:00
mainLoop_count++;
// tell the scheduler one tick has passed
scheduler.tick();
// run all the tasks that are due to run. Note that we only
// have to call this once per loop, as the tasks are scheduled
// in multiples of the main loop tick. So if they don't run on
// the first call to the scheduler they won't run on a later
// call until scheduler.tick() is called again
2013-10-11 23:30:27 -03:00
uint32_t remaining = (timer + 20000) - hal.scheduler->micros();
if (remaining > 19500) {
remaining = 19500;
}
scheduler.run(remaining);
2011-09-08 22:29:39 -03:00
}
2013-10-14 19:52:23 -03:00
// update AHRS system
static void ahrs_update()
2011-09-08 22:29:39 -03:00
{
2013-04-12 04:44:40 -03:00
#if HIL_MODE != HIL_MODE_DISABLED
2012-08-21 23:19:51 -03:00
// update hil before AHRS update
gcs_update();
#endif
2011-09-08 22:29:39 -03:00
2012-03-11 05:13:31 -03:00
ahrs.update();
2011-09-08 22:29:39 -03:00
2012-08-21 23:19:51 -03:00
if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST)
2013-01-12 07:18:58 -04:00
Log_Write_Attitude();
2011-09-08 22:29:39 -03:00
2013-01-26 04:29:29 -04:00
if (g.log_bitmask & MASK_LOG_IMU)
2013-04-19 18:24:22 -03:00
Log_Write_IMU();
2013-11-04 04:48:22 -04:00
// calculate a scaled roll limit based on current pitch
roll_limit_cd = g.roll_limit_cd * cosf(ahrs.pitch);
pitch_limit_min_cd = aparm.pitch_limit_min_cd * fabsf(cosf(ahrs.roll));
2013-07-05 05:05:27 -03:00
}
2011-09-08 22:29:39 -03:00
2013-07-05 05:05:27 -03:00
/*
update 50Hz speed/height controller
*/
static void update_speed_height(void)
{
2013-07-23 04:08:58 -03:00
if (auto_throttle_mode) {
// Call TECS 50Hz update. Note that we call this regardless of
// throttle suppressed, as this needs to be running for
// takeoff detection
2013-07-10 00:40:13 -03:00
SpdHgt_Controller->update_50hz(relative_altitude());
2013-06-26 05:36:53 -03:00
}
2011-09-08 22:29:39 -03:00
}
2013-07-05 05:05:27 -03:00
2013-06-04 00:34:58 -03:00
/*
update camera mount
*/
static void update_mount(void)
2011-09-08 22:29:39 -03:00
{
2011-10-31 18:55:58 -03:00
#if MOUNT == ENABLED
2012-08-21 23:19:51 -03:00
camera_mount.update_mount_position();
2011-10-31 18:55:58 -03:00
#endif
2012-08-08 18:22:24 -03:00
#if MOUNT2 == ENABLED
2012-08-21 23:19:51 -03:00
camera_mount2.update_mount_position();
2012-08-08 18:22:24 -03:00
#endif
2012-06-13 16:00:20 -03:00
#if CAMERA == ENABLED
2013-01-01 19:18:45 -04:00
camera.trigger_pic_cleanup();
2012-06-13 16:00:20 -03:00
#endif
2013-06-04 00:34:58 -03:00
}
2012-06-13 16:00:20 -03:00
2013-06-04 00:34:58 -03:00
/*
read and update compass
*/
static void update_compass(void)
{
if (g.compass_enabled && compass.read()) {
ahrs.set_compass(&compass);
compass.null_offsets();
if (g.log_bitmask & MASK_LOG_COMPASS) {
Log_Write_Compass();
2012-08-21 23:19:51 -03:00
}
2013-06-04 00:34:58 -03:00
} else {
ahrs.set_compass(NULL);
2012-08-21 23:19:51 -03:00
}
2011-09-08 22:29:39 -03:00
}
2013-07-04 23:53:04 -03:00
/*
if the compass is enabled then try to accumulate a reading
*/
static void compass_accumulate(void)
{
if (g.compass_enabled) {
compass.accumulate();
}
}
/*
try to accumulate a baro reading
*/
static void barometer_accumulate(void)
{
barometer.accumulate();
}
2013-06-04 00:34:58 -03:00
/*
do 10Hz logging
*/
static void update_logging(void)
2011-09-08 22:29:39 -03:00
{
2013-06-04 00:34:58 -03:00
if ((g.log_bitmask & MASK_LOG_ATTITUDE_MED) && !(g.log_bitmask & MASK_LOG_ATTITUDE_FAST))
Log_Write_Attitude();
2013-11-06 19:19:13 -04:00
if ((g.log_bitmask & MASK_LOG_ATTITUDE_MED) && !(g.log_bitmask & MASK_LOG_IMU))
Log_Write_IMU();
2013-06-04 00:34:58 -03:00
if (g.log_bitmask & MASK_LOG_CTUN)
Log_Write_Control_Tuning();
if (g.log_bitmask & MASK_LOG_NTUN)
Log_Write_Nav_Tuning();
2013-11-25 17:44:17 -04:00
if (g.log_bitmask & MASK_LOG_RC)
Log_Write_RC();
2013-06-04 00:34:58 -03:00
}
2012-08-21 23:19:51 -03:00
2013-11-25 17:44:17 -04:00
2013-06-04 00:34:58 -03:00
/*
check for OBC failsafe check
*/
static void obc_fs_check(void)
{
#if OBC_FAILSAFE == ENABLED
// perform OBC failsafe checks
obc.check(OBC_MODE(control_mode),
2013-08-02 20:16:51 -03:00
failsafe.last_heartbeat_ms,
2013-06-04 00:34:58 -03:00
g_gps ? g_gps->last_fix_time : 0);
#endif
}
2012-08-21 23:19:51 -03:00
2011-09-08 22:29:39 -03:00
2013-06-04 00:34:58 -03:00
/*
update aux servo mappings
*/
static void update_aux(void)
{
2013-04-25 07:10:40 -03:00
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8, &g.rc_9, &g.rc_10, &g.rc_11, &g.rc_12);
#elif CONFIG_HAL_BOARD == HAL_BOARD_APM2
2013-06-23 20:04:36 -03:00
update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8, &g.rc_10, &g.rc_11);
2013-01-01 21:50:42 -04:00
#else
update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8);
2012-07-18 17:01:19 -03:00
#endif
2012-08-21 23:19:51 -03:00
enable_aux_servos();
2012-08-04 13:44:08 -03:00
#if MOUNT == ENABLED
2012-08-21 23:19:51 -03:00
camera_mount.update_mount_type();
2012-08-08 18:22:24 -03:00
#endif
#if MOUNT2 == ENABLED
2012-08-21 23:19:51 -03:00
camera_mount2.update_mount_type();
2012-08-04 13:44:08 -03:00
#endif
2011-09-08 22:29:39 -03:00
}
2011-09-09 11:18:38 -03:00
static void one_second_loop()
2011-09-08 22:29:39 -03:00
{
2013-01-26 04:19:42 -04:00
if (g.log_bitmask & MASK_LOG_CURRENT)
2012-08-21 23:19:51 -03:00
Log_Write_Current();
2011-09-08 22:29:39 -03:00
2012-08-21 23:19:51 -03:00
// send a heartbeat
gcs_send_message(MSG_HEARTBEAT);
2013-06-03 03:42:38 -03:00
// make it possible to change control channel ordering at runtime
set_control_channels();
2013-06-03 03:52:44 -03:00
// make it possible to change orientation at runtime
ahrs.set_orientation();
2013-06-04 00:34:58 -03:00
// sync MAVLink system ID
mavlink_system.sysid = g.sysid_this_mav;
update_aux();
2013-12-08 23:15:16 -04:00
}
2013-06-04 00:34:58 -03:00
2013-12-08 23:15:16 -04:00
static void log_perf_info()
{
if (scheduler.debug() != 0) {
hal.console->printf_P(PSTR("G_Dt_max=%lu\n"), (unsigned long)G_Dt_max);
2013-06-04 00:34:58 -03:00
}
2013-12-08 23:15:16 -04:00
if (g.log_bitmask & MASK_LOG_PM)
Log_Write_Performance();
G_Dt_max = 0;
resetPerfData();
}
2013-06-04 00:34:58 -03:00
2013-12-08 23:15:16 -04:00
static void compass_save()
{
if (g.compass_enabled) {
compass.save_offsets();
2013-06-04 00:34:58 -03:00
}
2011-09-08 22:29:39 -03:00
}
2013-07-19 19:15:12 -03:00
/*
once a second update the airspeed calibration ratio
*/
static void airspeed_ratio_update(void)
{
if (!airspeed.enabled() ||
g_gps->status() < GPS::GPS_OK_FIX_3D ||
2013-08-16 18:01:01 -03:00
g_gps->ground_speed_cm < 400 ||
airspeed.get_airspeed() < aparm.airspeed_min) {
2013-08-28 09:36:37 -03:00
// don't calibrate when not moving
2013-07-19 19:15:12 -03:00
return;
}
2013-11-04 04:48:22 -04:00
if (abs(ahrs.roll_sensor) > roll_limit_cd ||
2013-08-28 09:36:37 -03:00
ahrs.pitch_sensor > aparm.pitch_limit_max_cd ||
2013-11-04 04:48:22 -04:00
ahrs.pitch_sensor < pitch_limit_min_cd) {
2013-08-28 09:36:37 -03:00
// don't calibrate when going beyond normal flight envelope
return;
}
Vector3f vg = g_gps->velocity_vector();
airspeed.update_calibration(vg);
gcs_send_airspeed_calibration(vg);
2013-07-19 19:15:12 -03:00
}
2013-06-04 00:34:58 -03:00
/*
read the GPS and update position
*/
2011-09-09 11:18:38 -03:00
static void update_GPS(void)
2011-09-08 22:29:39 -03:00
{
2013-04-28 01:56:31 -03:00
static uint32_t last_gps_reading;
2012-08-21 23:19:51 -03:00
g_gps->update();
2011-09-08 22:29:39 -03:00
2013-04-28 01:56:31 -03:00
if (g_gps->last_message_time_ms() != last_gps_reading) {
last_gps_reading = g_gps->last_message_time_ms();
if (g.log_bitmask & MASK_LOG_GPS) {
Log_Write_GPS();
}
}
2012-08-10 23:01:08 -03:00
// get position from AHRS
2013-09-17 08:05:25 -03:00
have_position = ahrs.get_projected_position(current_loc);
2012-08-10 23:01:08 -03:00
2013-03-25 07:09:57 -03:00
if (g_gps->new_data && g_gps->status() >= GPS::GPS_OK_FIX_3D) {
2012-08-10 23:01:08 -03:00
g_gps->new_data = false;
2012-08-21 23:19:51 -03:00
// for performance
// ---------------
gps_fix_count++;
if(ground_start_count > 1) {
ground_start_count--;
2013-07-10 01:03:03 -03:00
ground_start_avg += g_gps->ground_speed_cm;
2012-08-21 23:19:51 -03:00
} else if (ground_start_count == 1) {
// We countdown N number of good GPS fixes
// so that the altitude is more accurate
// -------------------------------------
if (current_loc.lat == 0) {
ground_start_count = 5;
} else {
2013-07-19 22:15:48 -03:00
init_home();
2012-08-21 23:19:51 -03:00
2013-10-23 09:27:22 -03:00
// set system clock for log timestamps
hal.util->set_system_clock(g_gps->time_epoch_usec());
2012-08-21 23:19:51 -03:00
if (g.compass_enabled) {
// Set compass declination automatically
compass.set_initial_location(g_gps->latitude, g_gps->longitude);
}
ground_start_count = 0;
}
}
2011-09-08 22:29:39 -03:00
2011-12-15 03:09:29 -04:00
// see if we've breached the geo-fence
2011-12-15 21:41:11 -04:00
geofence_check(false);
2013-06-24 09:40:47 -03:00
#if CAMERA == ENABLED
2013-07-09 19:33:35 -03:00
if (camera.update_location(current_loc) == true) {
do_take_picture();
}
2013-06-24 09:40:47 -03:00
#endif
2013-10-05 05:44:44 -03:00
if (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) {
update_home();
}
2012-08-21 23:19:51 -03:00
}
2013-06-04 00:34:58 -03:00
calc_gndspeed_undershoot();
2011-09-08 22:29:39 -03:00
}
2013-09-13 01:37:48 -03:00
/*
main handling for AUTO mode
*/
static void handle_auto_mode(void)
2011-09-08 22:29:39 -03:00
{
2013-09-13 01:37:48 -03:00
switch(nav_command_ID) {
case MAV_CMD_NAV_TAKEOFF:
2013-10-03 09:01:43 -03:00
if (steer_state.hold_course_cd == -1) {
2013-09-13 01:37:48 -03:00
// we don't yet have a heading to hold - just level
// the wings until we get up enough speed to get a GPS heading
nav_roll_cd = 0;
} else {
2013-04-12 07:27:56 -03:00
calc_nav_roll();
2013-09-13 01:37:48 -03:00
// during takeoff use the level flight roll limit to
// prevent large course corrections
nav_roll_cd = constrain_int32(nav_roll_cd, -g.level_roll_limit*100UL, g.level_roll_limit*100UL);
}
if (airspeed.use()) {
calc_nav_pitch();
if (nav_pitch_cd < takeoff_pitch_cd)
nav_pitch_cd = takeoff_pitch_cd;
} else {
nav_pitch_cd = (g_gps->ground_speed_cm / (float)g.airspeed_cruise_cm) * takeoff_pitch_cd;
nav_pitch_cd = constrain_int32(nav_pitch_cd, 500, takeoff_pitch_cd);
}
// max throttle for takeoff
channel_throttle->servo_out = aparm.throttle_max;
break;
2012-08-21 23:19:51 -03:00
2013-09-13 01:37:48 -03:00
case MAV_CMD_NAV_LAND:
calc_nav_roll();
if (land_complete) {
// during final approach constrain roll to the range
// allowed for level flight
nav_roll_cd = constrain_int32(nav_roll_cd, -g.level_roll_limit*100UL, g.level_roll_limit*100UL);
// hold pitch constant in final approach
nav_pitch_cd = g.land_pitch_cd;
} else {
calc_nav_pitch();
if (!airspeed.use()) {
// when not under airspeed control, don't allow
// down pitch in landing
nav_pitch_cd = constrain_int32(nav_pitch_cd, 0, nav_pitch_cd);
2012-08-21 23:19:51 -03:00
}
2013-09-13 01:37:48 -03:00
}
calc_throttle();
if (land_complete) {
// we are in the final stage of a landing - force
// zero throttle
channel_throttle->servo_out = 0;
}
break;
default:
// we are doing normal AUTO flight, the special cases
// are for takeoff and landing
2013-10-03 09:01:43 -03:00
steer_state.hold_course_cd = -1;
2013-09-13 01:37:48 -03:00
land_complete = false;
calc_nav_roll();
calc_nav_pitch();
calc_throttle();
break;
}
}
2012-08-21 23:19:51 -03:00
2013-09-13 01:37:48 -03:00
/*
main flight mode dependent update code
*/
static void update_flight_mode(void)
{
enum FlightMode effective_mode = control_mode;
if (control_mode == AUTO && g.auto_fbw_steer) {
effective_mode = FLY_BY_WIRE_A;
}
2012-08-21 23:19:51 -03:00
2013-09-13 01:37:48 -03:00
if (effective_mode != AUTO) {
2012-02-14 17:44:33 -04:00
// hold_course is only used in takeoff and landing
2013-10-03 09:01:43 -03:00
steer_state.hold_course_cd = -1;
2013-09-13 01:37:48 -03:00
}
2012-02-14 17:44:33 -04:00
2013-09-13 01:37:48 -03:00
switch (effective_mode)
{
case AUTO:
handle_auto_mode();
break;
2012-12-04 02:32:37 -04:00
2013-09-13 01:37:48 -03:00
case RTL:
case LOITER:
case GUIDED:
calc_nav_roll();
calc_nav_pitch();
calc_throttle();
break;
case TRAINING: {
training_manual_roll = false;
training_manual_pitch = false;
// if the roll is past the set roll limit, then
// we set target roll to the limit
2013-11-04 04:48:22 -04:00
if (ahrs.roll_sensor >= roll_limit_cd) {
nav_roll_cd = roll_limit_cd;
} else if (ahrs.roll_sensor <= -roll_limit_cd) {
nav_roll_cd = -roll_limit_cd;
2013-09-13 01:37:48 -03:00
} else {
training_manual_roll = true;
nav_roll_cd = 0;
}
// if the pitch is past the set pitch limits, then
// we set target pitch to the limit
if (ahrs.pitch_sensor >= aparm.pitch_limit_max_cd) {
nav_pitch_cd = aparm.pitch_limit_max_cd;
2013-11-04 04:48:22 -04:00
} else if (ahrs.pitch_sensor <= pitch_limit_min_cd) {
nav_pitch_cd = pitch_limit_min_cd;
2013-09-13 01:37:48 -03:00
} else {
training_manual_pitch = true;
nav_pitch_cd = 0;
2012-12-04 02:32:37 -04:00
}
2013-09-13 01:37:48 -03:00
if (inverted_flight) {
nav_pitch_cd = -nav_pitch_cd;
}
break;
}
2012-12-04 02:32:37 -04:00
2013-09-13 01:37:48 -03:00
case ACRO: {
// handle locked/unlocked control
if (acro_state.locked_roll) {
nav_roll_cd = acro_state.locked_roll_err;
} else {
nav_roll_cd = ahrs.roll_sensor;
2013-07-10 10:25:38 -03:00
}
2013-09-13 01:37:48 -03:00
if (acro_state.locked_pitch) {
nav_pitch_cd = acro_state.locked_pitch_cd;
} else {
nav_pitch_cd = ahrs.pitch_sensor;
}
break;
}
2013-07-10 10:25:38 -03:00
2013-09-13 01:37:48 -03:00
case FLY_BY_WIRE_A: {
// set nav_roll and nav_pitch using sticks
2013-11-04 04:48:22 -04:00
nav_roll_cd = channel_roll->norm_input() * roll_limit_cd;
nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd);
2013-09-13 01:37:48 -03:00
float pitch_input = channel_pitch->norm_input();
if (pitch_input > 0) {
nav_pitch_cd = pitch_input * aparm.pitch_limit_max_cd;
} else {
2013-11-04 04:48:22 -04:00
nav_pitch_cd = -(pitch_input * pitch_limit_min_cd);
2013-09-13 01:37:48 -03:00
}
2013-11-04 04:48:22 -04:00
nav_pitch_cd = constrain_int32(nav_pitch_cd, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get());
2013-09-13 01:37:48 -03:00
if (inverted_flight) {
nav_pitch_cd = -nav_pitch_cd;
2012-08-30 07:15:15 -03:00
}
2013-09-13 21:30:13 -03:00
if (failsafe.ch3_failsafe && g.short_fs_action == 2) {
// FBWA failsafe glide
nav_roll_cd = 0;
nav_pitch_cd = 0;
}
2013-09-13 01:37:48 -03:00
break;
}
2012-08-21 23:19:51 -03:00
2013-09-13 01:37:48 -03:00
case FLY_BY_WIRE_B:
// Thanks to Yury MonZon for the altitude limit code!
2013-11-04 04:48:22 -04:00
nav_roll_cd = channel_roll->norm_input() * roll_limit_cd;
2013-09-13 01:37:48 -03:00
update_fbwb_speed_height();
break;
case CRUISE:
/*
in CRUISE mode we use the navigation code to control
roll when heading is locked. Heading becomes unlocked on
any aileron or rudder input
*/
if ((channel_roll->control_in != 0 ||
channel_rudder->control_in != 0)) {
cruise_state.locked_heading = false;
cruise_state.lock_timer_ms = 0;
}
if (!cruise_state.locked_heading) {
2013-11-04 04:48:22 -04:00
nav_roll_cd = channel_roll->norm_input() * roll_limit_cd;
2013-09-13 01:37:48 -03:00
} else {
calc_nav_roll();
2012-08-21 23:19:51 -03:00
}
2013-09-13 01:37:48 -03:00
update_fbwb_speed_height();
break;
case STABILIZE:
nav_roll_cd = 0;
nav_pitch_cd = 0;
// throttle is passthrough
break;
case CIRCLE:
// we have no GPS installed and have lost radio contact
// or we just want to fly around in a gentle circle w/o GPS,
// holding altitude at the altitude we set when we
// switched into the mode
2013-11-04 04:48:22 -04:00
nav_roll_cd = roll_limit_cd / 3;
2013-09-13 01:37:48 -03:00
calc_nav_pitch();
calc_throttle();
break;
case MANUAL:
// servo_out is for Sim control only
// ---------------------------------
channel_roll->servo_out = channel_roll->pwm_to_angle();
channel_pitch->servo_out = channel_pitch->pwm_to_angle();
channel_rudder->servo_out = channel_rudder->pwm_to_angle();
break;
//roll: -13788.000, pitch: -13698.000, thr: 0.000, rud: -13742.000
case INITIALISING:
// handled elsewhere
break;
2012-08-21 23:19:51 -03:00
}
2011-09-08 22:29:39 -03:00
}
2011-09-09 11:18:38 -03:00
static void update_navigation()
2011-09-08 22:29:39 -03:00
{
2012-08-21 23:19:51 -03:00
// wp_distance is in ACTUAL meters, not the *100 meters we get from the GPS
// ------------------------------------------------------------------------
// distance and bearing calcs only
2012-11-30 17:15:48 -04:00
switch(control_mode) {
case AUTO:
2012-08-21 23:19:51 -03:00
verify_commands();
2012-11-30 17:15:48 -04:00
break;
case LOITER:
case RTL:
case GUIDED:
2013-11-06 20:51:36 -04:00
// allow loiter direction to be changed in flight
if (g.loiter_radius < 0) {
loiter.direction = -1;
} else {
loiter.direction = 1;
}
2012-11-30 17:15:48 -04:00
update_loiter();
break;
2012-08-21 23:19:51 -03:00
2013-07-13 07:05:53 -03:00
case CRUISE:
update_cruise();
break;
2012-11-30 17:15:48 -04:00
case MANUAL:
2012-12-04 02:32:37 -04:00
case STABILIZE:
case TRAINING:
2012-11-30 17:15:48 -04:00
case INITIALISING:
2013-07-10 10:25:38 -03:00
case ACRO:
2012-11-30 17:15:48 -04:00
case FLY_BY_WIRE_A:
case FLY_BY_WIRE_B:
case CIRCLE:
// nothing to do
break;
2012-08-21 23:19:51 -03:00
}
2011-09-08 22:29:39 -03:00
}
2011-09-09 11:18:38 -03:00
static void update_alt()
2011-09-08 22:29:39 -03:00
{
2012-08-21 23:19:51 -03:00
// this function is in place to potentially add a sonar sensor in the future
//altitude_sensor = BARO;
if (barometer.healthy) {
2013-07-10 01:03:03 -03:00
// alt_MSL centimeters (centimeters)
current_loc.alt = (1 - g.altitude_mix) * g_gps->altitude_cm;
2012-08-21 23:19:51 -03:00
current_loc.alt += g.altitude_mix * (read_barometer() + home.alt);
2013-03-25 07:09:57 -03:00
} else if (g_gps->status() >= GPS::GPS_OK_FIX_3D) {
2013-07-10 01:03:03 -03:00
// alt_MSL centimeters (centimeters)
current_loc.alt = g_gps->altitude_cm;
2012-08-21 23:19:51 -03:00
}
2011-09-08 22:29:39 -03:00
2012-08-21 23:19:51 -03:00
geofence_check(true);
2011-12-15 21:41:11 -04:00
2013-06-26 05:36:53 -03:00
// Update the speed & height controller states
2013-07-22 00:28:11 -03:00
if (auto_throttle_mode && !throttle_suppressed) {
2013-10-11 23:14:30 -03:00
AP_SpdHgtControl::FlightStage flight_stage = AP_SpdHgtControl::FLIGHT_NORMAL;
if (control_mode==AUTO) {
if (takeoff_complete == false) {
flight_stage = AP_SpdHgtControl::FLIGHT_TAKEOFF;
} else if (nav_command_ID == MAV_CMD_NAV_LAND && land_complete == true) {
flight_stage = AP_SpdHgtControl::FLIGHT_LAND_FINAL;
} else if (nav_command_ID == MAV_CMD_NAV_LAND) {
flight_stage = AP_SpdHgtControl::FLIGHT_LAND_APPROACH;
}
}
2013-07-08 20:25:08 -03:00
SpdHgt_Controller->update_pitch_throttle(target_altitude_cm - home.alt + (int32_t(g.alt_offset)*100),
2013-07-09 07:51:03 -03:00
target_airspeed_cm,
2013-10-11 23:14:30 -03:00
flight_stage,
2013-07-09 07:51:03 -03:00
takeoff_pitch_cd,
2013-07-10 00:31:29 -03:00
throttle_nudge,
2013-07-10 00:40:13 -03:00
relative_altitude());
2013-06-26 05:36:53 -03:00
if (g.log_bitmask & MASK_LOG_TECS) {
Log_Write_TECS_Tuning();
}
}
2013-07-19 22:12:43 -03:00
// tell AHRS the airspeed to true airspeed ratio
airspeed.set_EAS2TAS(barometer.get_EAS2TAS());
2011-09-08 22:29:39 -03:00
}
2012-12-04 18:22:21 -04:00
AP_HAL_MAIN();