Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Sub: New vehicle type, derived from ArduCopter.

This commit is contained in:
Rustom Jehangir 2015-12-30 14:57:56 -08:00 committed by Andrew Tridgell
parent 1217256898
commit 3da7c95e9b
74 changed files with 21837 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

59
ArduSub/APM_Config.h Normal file
View File

@ -0,0 +1,59 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// User specific config file. Any items listed in config.h can be overridden here.
// If you used to define your CONFIG_APM_HARDWARE setting here, it is no longer
// valid! You should switch to using a HAL_BOARD flag in your local config.mk.
//#define FRAME_CONFIG QUAD_FRAME
/* options:
* QUAD_FRAME
* TRI_FRAME
* HEXA_FRAME
* Y6_FRAME
* OCTA_FRAME
* OCTA_QUAD_FRAME
* HELI_FRAME
* SINGLE_FRAME
* COAX_FRAME
*/
// uncomment the lines below to disable features (flash sizes listed are for APM2 boards and will underestimate savings on Pixhawk and other boards)
//#define LOGGING_ENABLED DISABLED // disable dataflash logging to save 11K of flash space
//#define MOUNT DISABLED // disable the camera gimbal to save 8K of flash space
//#define AUTOTUNE_ENABLED DISABLED // disable the auto tune functionality to save 7k of flash
//#define AC_FENCE DISABLED // disable fence to save 2k of flash
//#define CAMERA DISABLED // disable camera trigger to save 1k of flash
//#define CONFIG_SONAR DISABLED // disable sonar to save 1k of flash
//#define POSHOLD_ENABLED DISABLED // disable PosHold flight mode to save 4.5k of flash
//#define AC_RALLY DISABLED // disable rally points library (must also disable terrain which relies on rally)
//#define AC_TERRAIN DISABLED // disable terrain library
//#define PARACHUTE DISABLED // disable parachute release to save 1k of flash
//#define EPM_ENABLED DISABLED // disable epm cargo gripper to save 500bytes of flash
//#define CLI_ENABLED DISABLED // disable the CLI (command-line-interface) to save 21K of flash space
//#define NAV_GUIDED DISABLED // disable external navigation computer ability to control vehicle through MAV_CMD_NAV_GUIDED mission commands
//#define OPTFLOW DISABLED // disable optical flow sensor to save 5K of flash space
//#define FRSKY_TELEM_ENABLED DISABLED // disable FRSky telemetry
//#define ADSB_ENABLED DISABLED // disable ADSB support
// features below are disabled by default on all boards
//#define SPRAYER ENABLED // enable the crop sprayer feature (two ESC controlled pumps the speed of which depends upon the vehicle's horizontal velocity)
//#define PRECISION_LANDING ENABLED // enable precision landing using companion computer or IRLock sensor
//#define GNDEFFECT_COMPENSATION ENABLED // enable ground effect compensation for barometer (if propwash interferes with the barometer on the ground)
// other settings
//#define THROTTLE_IN_DEADBAND 100 // redefine size of throttle deadband in pwm (0 ~ 1000)
//#define LAND_REQUIRE_MIN_THROTTLE_TO_DISARM ENABLED // when set to ENABLED vehicle will only disarm after landing (in AUTO, LAND or RTL) if pilot has put throttle to zero
//#define HIL_MODE HIL_MODE_SENSORS // build for hardware-in-the-loop simulation
// User Hooks : For User Developed code that you wish to run
// Put your variable definitions into the UserVariables.h file (or another file name and then change the #define below).
//#define USERHOOK_VARIABLES "UserVariables.h"
// Put your custom code into the UserCode.pde with function names matching those listed below and ensure the appropriate #define below is uncommented below
//#define USERHOOK_INIT userhook_init(); // for code to be run once at startup
//#define USERHOOK_FASTLOOP userhook_FastLoop(); // for code to be run at 100hz
//#define USERHOOK_50HZLOOP userhook_50Hz(); // for code to be run at 50hz
//#define USERHOOK_MEDIUMLOOP userhook_MediumLoop(); // for code to be run at 10hz
//#define USERHOOK_SLOWLOOP userhook_SlowLoop(); // for code to be run at 3.3hz
//#define USERHOOK_SUPERSLOWLOOP userhook_SuperSlowLoop(); // for code to be run at 1hz

30
ArduSub/APM_Config_mavlink_hil.h Normal file
View File

@ -0,0 +1,30 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// HIL_MODE SELECTION
//
// Mavlink supports
// 1. HIL_MODE_SENSORS: full sensor simulation
#define HIL_MODE HIL_MODE_SENSORS
// HIL_PORT SELCTION
//
// PORT 1
// If you would like to run telemetry communications for a groundstation
// while you are running hardware in the loop it is necessary to set
// HIL_PORT to 1. This uses the port that would have been used for the gps
// as the hardware in the loop port. You will have to solder
// headers onto the gps port connection on the apm
// and connect via an ftdi cable.
//
// The baud rate is set to 115200 in this mode.
//
// PORT 3
// If you don't require telemetry communication with a gcs while running
// hardware in the loop you may use the telemetry port as the hardware in
// the loop port. Alternatively, use a telemetry/HIL shim like FGShim
// https://ardupilot-mega.googlecode.com/svn/Tools/trunk/FlightGear
//
// The buad rate is controlled by SERIAL1_BAUD in this mode.
#define HIL_PORT 3

140
ArduSub/AP_State.cpp Normal file
View File

@ -0,0 +1,140 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
// set_home_state - update home state
void Copter::set_home_state(enum HomeState new_home_state)
{
// if no change, exit immediately
if (ap.home_state == new_home_state)
return;
// update state
ap.home_state = new_home_state;
// log if home has been set
if (new_home_state == HOME_SET_NOT_LOCKED || new_home_state == HOME_SET_AND_LOCKED) {
Log_Write_Event(DATA_SET_HOME);
}
}
// home_is_set - returns true if home positions has been set (to GPS location, armed location or EKF origin)
bool Copter::home_is_set()
{
return (ap.home_state == HOME_SET_NOT_LOCKED || ap.home_state == HOME_SET_AND_LOCKED);
}
// ---------------------------------------------
void Copter::set_auto_armed(bool b)
{
// if no change, exit immediately
if( ap.auto_armed == b )
return;
ap.auto_armed = b;
if(b){
Log_Write_Event(DATA_AUTO_ARMED);
}
}
// ---------------------------------------------
void Copter::set_simple_mode(uint8_t b)
{
if(ap.simple_mode != b){
if(b == 0){
Log_Write_Event(DATA_SET_SIMPLE_OFF);
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "SIMPLE mode off");
}else if(b == 1){
Log_Write_Event(DATA_SET_SIMPLE_ON);
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "SIMPLE mode on");
}else{
// initialise super simple heading
update_super_simple_bearing(true);
Log_Write_Event(DATA_SET_SUPERSIMPLE_ON);
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "SUPERSIMPLE mode on");
}
ap.simple_mode = b;
}
}
// ---------------------------------------------
void Copter::set_failsafe_radio(bool b)
{
// only act on changes
// -------------------
if(failsafe.radio != b) {
// store the value so we don't trip the gate twice
// -----------------------------------------------
failsafe.radio = b;
if (failsafe.radio == false) {
// We've regained radio contact
// ----------------------------
failsafe_radio_off_event();
}else{
// We've lost radio contact
// ------------------------
failsafe_radio_on_event();
}
// update AP_Notify
AP_Notify::flags.failsafe_radio = b;
}
}
// ---------------------------------------------
void Copter::set_failsafe_battery(bool b)
{
failsafe.battery = b;
AP_Notify::flags.failsafe_battery = b;
}
// ---------------------------------------------
void Copter::set_failsafe_gcs(bool b)
{
failsafe.gcs = b;
}
// ---------------------------------------------
void Copter::set_pre_arm_check(bool b)
{
if(ap.pre_arm_check != b) {
ap.pre_arm_check = b;
AP_Notify::flags.pre_arm_check = b;
}
}
void Copter::set_pre_arm_rc_check(bool b)
{
if(ap.pre_arm_rc_check != b) {
ap.pre_arm_rc_check = b;
}
}
void Copter::update_using_interlock()
{
#if FRAME_CONFIG == HELI_FRAME
// helicopters are always using motor interlock
ap.using_interlock = true;
#else
// check if we are using motor interlock control on an aux switch
ap.using_interlock = check_if_auxsw_mode_used(AUXSW_MOTOR_INTERLOCK);
#endif
}
void Copter::set_motor_emergency_stop(bool b)
{
if(ap.motor_emergency_stop != b) {
ap.motor_emergency_stop = b;
}
// Log new status
if (ap.motor_emergency_stop){
Log_Write_Event(DATA_MOTORS_EMERGENCY_STOPPED);
} else {
Log_Write_Event(DATA_MOTORS_EMERGENCY_STOP_CLEARED);
}
}

642
ArduSub/ArduSub.cpp Normal file
View File

@ -0,0 +1,642 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
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/>.
*/
/*
* ArduCopter Version 3.0
* Creator: Jason Short
* Lead Developer: Randy Mackay
* Lead Tester: Marco Robustini
* Based on code and ideas from the Arducopter team: Leonard Hall, Andrew Tridgell, Robert Lefebvre, Pat Hickey, Michael Oborne, Jani Hirvinen,
Olivier Adler, Kevin Hester, Arthur Benemann, Jonathan Challinger, John Arne Birkeland,
Jean-Louis Naudin, Mike Smith, and more
* Thanks to: Chris Anderson, Jordi Munoz, Jason Short, Doug Weibel, Jose Julio
*
* Special Thanks to contributors (in alphabetical order by first name):
*
* Adam M Rivera :Auto Compass Declination
* Amilcar Lucas :Camera mount library
* Andrew Tridgell :General development, Mavlink Support
* Angel Fernandez :Alpha testing
* AndreasAntonopoulous:GeoFence
* Arthur Benemann :DroidPlanner GCS
* Benjamin Pelletier :Libraries
* Bill King :Single Copter
* Christof Schmid :Alpha testing
* Craig Elder :Release Management, Support
* Dani Saez :V Octo Support
* Doug Weibel :DCM, Libraries, Control law advice
* Emile Castelnuovo :VRBrain port, bug fixes
* Gregory Fletcher :Camera mount orientation math
* Guntars :Arming safety suggestion
* HappyKillmore :Mavlink GCS
* Hein Hollander :Octo Support, Heli Testing
* Igor van Airde :Control Law optimization
* Jack Dunkle :Alpha testing
* James Goppert :Mavlink Support
* Jani Hiriven :Testing feedback
* Jean-Louis Naudin :Auto Landing
* John Arne Birkeland :PPM Encoder
* Jose Julio :Stabilization Control laws, MPU6k driver
* Julien Dubois :PosHold flight mode
* Julian Oes :Pixhawk
* Jonathan Challinger :Inertial Navigation, CompassMot, Spin-When-Armed
* Kevin Hester :Andropilot GCS
* Max Levine :Tri Support, Graphics
* Leonard Hall :Flight Dynamics, Throttle, Loiter and Navigation Controllers
* Marco Robustini :Lead tester
* Michael Oborne :Mission Planner GCS
* Mike Smith :Pixhawk driver, coding support
* Olivier Adler :PPM Encoder, piezo buzzer
* Pat Hickey :Hardware Abstraction Layer (HAL)
* Robert Lefebvre :Heli Support, Copter LEDs
* Roberto Navoni :Library testing, Porting to VRBrain
* Sandro Benigno :Camera support, MinimOSD
* Sandro Tognana :PosHold flight mode
* ..and many more.
*
* Code commit statistics can be found here: https://github.com/diydrones/ardupilot/graphs/contributors
* Wiki: http://copter.ardupilot.com/
* Requires modified version of Arduino, which can be found here: http://ardupilot.com/downloads/?category=6
*
*/
#include "Copter.h"
#define SCHED_TASK(func, rate_hz, max_time_micros) SCHED_TASK_CLASS(Copter, &copter, func, rate_hz, max_time_micros)
/*
scheduler table for fast CPUs - all regular tasks apart from the fast_loop()
should be listed here, along with how often they should be called
(in 2.5ms units) and the maximum time they are expected to take (in
microseconds)
1 = 400hz
2 = 200hz
4 = 100hz
8 = 50hz
20 = 20hz
40 = 10hz
133 = 3hz
400 = 1hz
4000 = 0.1hz
*/
const AP_Scheduler::Task Copter::scheduler_tasks[] = {
SCHED_TASK(rc_loop, 100, 130),
SCHED_TASK(throttle_loop, 50, 75),
SCHED_TASK(update_GPS, 50, 200),
#if OPTFLOW == ENABLED
SCHED_TASK(update_optical_flow, 200, 160),
#endif
SCHED_TASK(update_batt_compass, 10, 120),
SCHED_TASK(read_aux_switches, 10, 50),
SCHED_TASK(arm_motors_check, 10, 50),
SCHED_TASK(auto_disarm_check, 10, 50),
SCHED_TASK(auto_trim, 10, 75),
SCHED_TASK(update_altitude, 10, 140),
SCHED_TASK(run_nav_updates, 50, 100),
SCHED_TASK(update_thr_average, 100, 90),
SCHED_TASK(three_hz_loop, 3, 75),
SCHED_TASK(compass_accumulate, 100, 100),
SCHED_TASK(barometer_accumulate, 50, 90),
#if PRECISION_LANDING == ENABLED
SCHED_TASK(update_precland, 50, 50),
#endif
#if FRAME_CONFIG == HELI_FRAME
SCHED_TASK(check_dynamic_flight, 50, 75),
#endif
SCHED_TASK(update_notify, 50, 90),
SCHED_TASK(one_hz_loop, 1, 100),
SCHED_TASK(ekf_check, 10, 75),
SCHED_TASK(landinggear_update, 10, 75),
SCHED_TASK(lost_vehicle_check, 10, 50),
SCHED_TASK(gcs_check_input, 400, 180),
SCHED_TASK(gcs_send_heartbeat, 1, 110),
SCHED_TASK(gcs_send_deferred, 50, 550),
SCHED_TASK(gcs_data_stream_send, 50, 550),
SCHED_TASK(update_mount, 50, 75),
SCHED_TASK(ten_hz_logging_loop, 10, 350),
SCHED_TASK(fifty_hz_logging_loop, 50, 110),
SCHED_TASK(full_rate_logging_loop,400, 100),
SCHED_TASK(dataflash_periodic, 400, 300),
SCHED_TASK(perf_update, 0.1, 75),
SCHED_TASK(read_receiver_rssi, 10, 75),
SCHED_TASK(rpm_update, 10, 200),
SCHED_TASK(compass_cal_update, 100, 100),
SCHED_TASK(accel_cal_update, 10, 100),
#if ADSB_ENABLED == ENABLED
SCHED_TASK(adsb_update, 1, 100),
#endif
#if FRSKY_TELEM_ENABLED == ENABLED
SCHED_TASK(frsky_telemetry_send, 5, 75),
#endif
#if EPM_ENABLED == ENABLED
SCHED_TASK(epm_update, 10, 75),
#endif
#ifdef USERHOOK_FASTLOOP
SCHED_TASK(userhook_FastLoop, 100, 75),
#endif
#ifdef USERHOOK_50HZLOOP
SCHED_TASK(userhook_50Hz, 50, 75),
#endif
#ifdef USERHOOK_MEDIUMLOOP
SCHED_TASK(userhook_MediumLoop, 10, 75),
#endif
#ifdef USERHOOK_SLOWLOOP
SCHED_TASK(userhook_SlowLoop, 3.3, 75),
#endif
#ifdef USERHOOK_SUPERSLOWLOOP
SCHED_TASK(userhook_SuperSlowLoop, 1, 75),
#endif
};
void Copter::setup()
{
cliSerial = hal.console;
// Load the default values of variables listed in var_info[]s
AP_Param::setup_sketch_defaults();
// setup storage layout for copter
StorageManager::set_layout_copter();
init_ardupilot();
// initialise the main loop scheduler
scheduler.init(&scheduler_tasks[0], ARRAY_SIZE(scheduler_tasks));
// setup initial performance counters
perf_info_reset();
fast_loopTimer = AP_HAL::micros();
}
/*
if the compass is enabled then try to accumulate a reading
*/
void Copter::compass_accumulate(void)
{
if (g.compass_enabled) {
compass.accumulate();
}
}
/*
try to accumulate a baro reading
*/
void Copter::barometer_accumulate(void)
{
barometer.accumulate();
}
void Copter::perf_update(void)
{
if (should_log(MASK_LOG_PM))
Log_Write_Performance();
if (scheduler.debug()) {
gcs_send_text_fmt(MAV_SEVERITY_WARNING, "PERF: %u/%u %lu %lu\n",
(unsigned)perf_info_get_num_long_running(),
(unsigned)perf_info_get_num_loops(),
(unsigned long)perf_info_get_max_time(),
(unsigned long)perf_info_get_min_time());
}
perf_info_reset();
pmTest1 = 0;
}
void Copter::loop()
{
// wait for an INS sample
ins.wait_for_sample();
uint32_t timer = micros();
// check loop time
perf_info_check_loop_time(timer - fast_loopTimer);
// used by PI Loops
G_Dt = (float)(timer - fast_loopTimer) / 1000000.0f;
fast_loopTimer = timer;
// for mainloop failure monitoring
mainLoop_count++;
// Execute the fast loop
// ---------------------
fast_loop();
// 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
uint32_t time_available = (timer + MAIN_LOOP_MICROS) - micros();
scheduler.run(time_available);
}
// Main loop - 400hz
void Copter::fast_loop()
{
// IMU DCM Algorithm
// --------------------
read_AHRS();
// run low level rate controllers that only require IMU data
attitude_control.rate_controller_run();
#if FRAME_CONFIG == HELI_FRAME
update_heli_control_dynamics();
#endif //HELI_FRAME
// send outputs to the motors library
motors_output();
// Inertial Nav
// --------------------
read_inertia();
// check if ekf has reset target heading
check_ekf_yaw_reset();
// run the attitude controllers
update_flight_mode();
// update home from EKF if necessary
update_home_from_EKF();
// check if we've landed or crashed
update_land_and_crash_detectors();
// log sensor health
if (should_log(MASK_LOG_ANY)) {
Log_Sensor_Health();
}
}
// rc_loops - reads user input from transmitter/receiver
// called at 100hz
void Copter::rc_loop()
{
// Read radio and 3-position switch on radio
// -----------------------------------------
read_radio();
read_control_switch();
}
// throttle_loop - should be run at 50 hz
// ---------------------------
void Copter::throttle_loop()
{
// get altitude and climb rate from inertial lib
read_inertial_altitude();
// update throttle_low_comp value (controls priority of throttle vs attitude control)
update_throttle_thr_mix();
// check auto_armed status
update_auto_armed();
#if FRAME_CONFIG == HELI_FRAME
// update rotor speed
heli_update_rotor_speed_targets();
// update trad heli swash plate movement
heli_update_landing_swash();
#endif
#if GNDEFFECT_COMPENSATION == ENABLED
update_ground_effect_detector();
#endif // GNDEFFECT_COMPENSATION == ENABLED
}
// update_mount - update camera mount position
// should be run at 50hz
void Copter::update_mount()
{
#if MOUNT == ENABLED
// update camera mount's position
camera_mount.update();
#endif
#if CAMERA == ENABLED
camera.trigger_pic_cleanup();
#endif
}
// update_batt_compass - read battery and compass
// should be called at 10hz
void Copter::update_batt_compass(void)
{
// read battery before compass because it may be used for motor interference compensation
read_battery();
if(g.compass_enabled) {
// update compass with throttle value - used for compassmot
compass.set_throttle(motors.get_throttle()/1000.0f);
compass.read();
// log compass information
if (should_log(MASK_LOG_COMPASS)) {
DataFlash.Log_Write_Compass(compass);
}
}
}
// ten_hz_logging_loop
// should be run at 10hz
void Copter::ten_hz_logging_loop()
{
// log attitude data if we're not already logging at the higher rate
if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST)) {
Log_Write_Attitude();
Log_Write_Rate();
if (should_log(MASK_LOG_PID)) {
DataFlash.Log_Write_PID(LOG_PIDR_MSG, g.pid_rate_roll.get_pid_info() );
DataFlash.Log_Write_PID(LOG_PIDP_MSG, g.pid_rate_pitch.get_pid_info() );
DataFlash.Log_Write_PID(LOG_PIDY_MSG, g.pid_rate_yaw.get_pid_info() );
DataFlash.Log_Write_PID(LOG_PIDA_MSG, g.pid_accel_z.get_pid_info() );
}
}
if (should_log(MASK_LOG_MOTBATT)) {
Log_Write_MotBatt();
}
if (should_log(MASK_LOG_RCIN)) {
DataFlash.Log_Write_RCIN();
if (rssi.enabled()) {
DataFlash.Log_Write_RSSI(rssi);
}
}
if (should_log(MASK_LOG_RCOUT)) {
DataFlash.Log_Write_RCOUT();
}
if (should_log(MASK_LOG_NTUN) && (mode_requires_GPS(control_mode) || landing_with_GPS())) {
Log_Write_Nav_Tuning();
}
if (should_log(MASK_LOG_IMU) || should_log(MASK_LOG_IMU_FAST) || should_log(MASK_LOG_IMU_RAW)) {
DataFlash.Log_Write_Vibration(ins);
}
#if FRAME_CONFIG == HELI_FRAME
Log_Write_Heli();
#endif
}
// fifty_hz_logging_loop
// should be run at 50hz
void Copter::fifty_hz_logging_loop()
{
#if HIL_MODE != HIL_MODE_DISABLED
// HIL for a copter needs very fast update of the servo values
gcs_send_message(MSG_RADIO_OUT);
#endif
#if HIL_MODE == HIL_MODE_DISABLED
if (should_log(MASK_LOG_ATTITUDE_FAST)) {
Log_Write_Attitude();
Log_Write_Rate();
if (should_log(MASK_LOG_PID)) {
DataFlash.Log_Write_PID(LOG_PIDR_MSG, g.pid_rate_roll.get_pid_info() );
DataFlash.Log_Write_PID(LOG_PIDP_MSG, g.pid_rate_pitch.get_pid_info() );
DataFlash.Log_Write_PID(LOG_PIDY_MSG, g.pid_rate_yaw.get_pid_info() );
DataFlash.Log_Write_PID(LOG_PIDA_MSG, g.pid_accel_z.get_pid_info() );
}
}
// log IMU data if we're not already logging at the higher rate
if (should_log(MASK_LOG_IMU) && !(should_log(MASK_LOG_IMU_FAST) || should_log(MASK_LOG_IMU_RAW))) {
DataFlash.Log_Write_IMU(ins);
}
#endif
}
// full_rate_logging_loop
// should be run at the MAIN_LOOP_RATE
void Copter::full_rate_logging_loop()
{
if (should_log(MASK_LOG_IMU_FAST) && !should_log(MASK_LOG_IMU_RAW)) {
DataFlash.Log_Write_IMU(ins);
}
if (should_log(MASK_LOG_IMU_FAST) || should_log(MASK_LOG_IMU_RAW)) {
DataFlash.Log_Write_IMUDT(ins);
}
}
void Copter::dataflash_periodic(void)
{
DataFlash.periodic_tasks();
}
// three_hz_loop - 3.3hz loop
void Copter::three_hz_loop()
{
// check if we've lost contact with the ground station
failsafe_gcs_check();
#if AC_FENCE == ENABLED
// check if we have breached a fence
fence_check();
#endif // AC_FENCE_ENABLED
#if SPRAYER == ENABLED
sprayer.update();
#endif
update_events();
// update ch6 in flight tuning
tuning();
}
// one_hz_loop - runs at 1Hz
void Copter::one_hz_loop()
{
if (should_log(MASK_LOG_ANY)) {
Log_Write_Data(DATA_AP_STATE, ap.value);
}
update_arming_checks();
if (!motors.armed()) {
// make it possible to change ahrs orientation at runtime during initial config
ahrs.set_orientation();
update_using_interlock();
#if FRAME_CONFIG != HELI_FRAME
// check the user hasn't updated the frame orientation
motors.set_frame_orientation(g.frame_orientation);
// set all throttle channel settings
motors.set_throttle_range(g.throttle_min, channel_throttle->radio_min, channel_throttle->radio_max);
// set hover throttle
motors.set_hover_throttle(g.throttle_mid);
#endif
}
// update assigned functions and enable auxiliary servos
RC_Channel_aux::enable_aux_servos();
check_usb_mux();
#if AP_TERRAIN_AVAILABLE && AC_TERRAIN
terrain.update();
// tell the rangefinder our height, so it can go into power saving
// mode if available
#if CONFIG_SONAR == ENABLED
float height;
if (terrain.height_above_terrain(height, true)) {
sonar.set_estimated_terrain_height(height);
}
#endif
#endif
// update position controller alt limits
update_poscon_alt_max();
// enable/disable raw gyro/accel logging
ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW));
}
// called at 50hz
void Copter::update_GPS(void)
{
static uint32_t last_gps_reading[GPS_MAX_INSTANCES]; // time of last gps message
bool gps_updated = false;
gps.update();
// log after every gps message
for (uint8_t i=0; i<gps.num_sensors(); i++) {
if (gps.last_message_time_ms(i) != last_gps_reading[i]) {
last_gps_reading[i] = gps.last_message_time_ms(i);
// log GPS message
if (should_log(MASK_LOG_GPS)) {
DataFlash.Log_Write_GPS(gps, i, current_loc.alt);
}
gps_updated = true;
}
}
if (gps_updated) {
// set system time if necessary
set_system_time_from_GPS();
// checks to initialise home and take location based pictures
if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
#if CAMERA == ENABLED
if (camera.update_location(current_loc, copter.ahrs) == true) {
do_take_picture();
}
#endif
}
}
}
void Copter::init_simple_bearing()
{
// capture current cos_yaw and sin_yaw values
simple_cos_yaw = ahrs.cos_yaw();
simple_sin_yaw = ahrs.sin_yaw();
// initialise super simple heading (i.e. heading towards home) to be 180 deg from simple mode heading
super_simple_last_bearing = wrap_360_cd(ahrs.yaw_sensor+18000);
super_simple_cos_yaw = simple_cos_yaw;
super_simple_sin_yaw = simple_sin_yaw;
// log the simple bearing to dataflash
if (should_log(MASK_LOG_ANY)) {
Log_Write_Data(DATA_INIT_SIMPLE_BEARING, ahrs.yaw_sensor);
}
}
// update_simple_mode - rotates pilot input if we are in simple mode
void Copter::update_simple_mode(void)
{
float rollx, pitchx;
// exit immediately if no new radio frame or not in simple mode
if (ap.simple_mode == 0 || !ap.new_radio_frame) {
return;
}
// mark radio frame as consumed
ap.new_radio_frame = false;
if (ap.simple_mode == 1) {
// rotate roll, pitch input by -initial simple heading (i.e. north facing)
rollx = channel_roll->control_in*simple_cos_yaw - channel_pitch->control_in*simple_sin_yaw;
pitchx = channel_roll->control_in*simple_sin_yaw + channel_pitch->control_in*simple_cos_yaw;
}else{
// rotate roll, pitch input by -super simple heading (reverse of heading to home)
rollx = channel_roll->control_in*super_simple_cos_yaw - channel_pitch->control_in*super_simple_sin_yaw;
pitchx = channel_roll->control_in*super_simple_sin_yaw + channel_pitch->control_in*super_simple_cos_yaw;
}
// rotate roll, pitch input from north facing to vehicle's perspective
channel_roll->control_in = rollx*ahrs.cos_yaw() + pitchx*ahrs.sin_yaw();
channel_pitch->control_in = -rollx*ahrs.sin_yaw() + pitchx*ahrs.cos_yaw();
}
// update_super_simple_bearing - adjusts simple bearing based on location
// should be called after home_bearing has been updated
void Copter::update_super_simple_bearing(bool force_update)
{
// check if we are in super simple mode and at least 10m from home
if(force_update || (ap.simple_mode == 2 && home_distance > SUPER_SIMPLE_RADIUS)) {
// check the bearing to home has changed by at least 5 degrees
if (labs(super_simple_last_bearing - home_bearing) > 500) {
super_simple_last_bearing = home_bearing;
float angle_rad = radians((super_simple_last_bearing+18000)/100);
super_simple_cos_yaw = cosf(angle_rad);
super_simple_sin_yaw = sinf(angle_rad);
}
}
}
void Copter::read_AHRS(void)
{
// Perform IMU calculations and get attitude info
//-----------------------------------------------
#if HIL_MODE != HIL_MODE_DISABLED
// update hil before ahrs update
gcs_check_input();
#endif
ahrs.update();
}
// read baro and sonar altitude at 10hz
void Copter::update_altitude()
{
// read in baro altitude
read_barometer();
// read in sonar altitude
sonar_alt = read_sonar();
// write altitude info to dataflash logs
if (should_log(MASK_LOG_CTUN)) {
Log_Write_Control_Tuning();
}
}
AP_HAL_MAIN_CALLBACKS(&copter);

325
ArduSub/Attitude.cpp Normal file
View File

@ -0,0 +1,325 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
// get_smoothing_gain - returns smoothing gain to be passed into attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth
// result is a number from 2 to 12 with 2 being very sluggish and 12 being very crisp
float Copter::get_smoothing_gain()
{
return (2.0f + (float)g.rc_feel_rp/10.0f);
}
// get_pilot_desired_angle - transform pilot's roll or pitch input into a desired lean angle
// returns desired angle in centi-degrees
void Copter::get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &roll_out, float &pitch_out, float angle_max)
{
// sanity check angle max parameter
aparm.angle_max = constrain_int16(aparm.angle_max,1000,8000);
// limit max lean angle
angle_max = constrain_float(angle_max, 1000, aparm.angle_max);
// scale roll_in, pitch_in to ANGLE_MAX parameter range
float scaler = aparm.angle_max/(float)ROLL_PITCH_INPUT_MAX;
roll_in *= scaler;
pitch_in *= scaler;
// do circular limit
float total_in = pythagorous2((float)pitch_in, (float)roll_in);
if (total_in > angle_max) {
float ratio = angle_max / total_in;
roll_in *= ratio;
pitch_in *= ratio;
}
// do lateral tilt to euler roll conversion
roll_in = (18000/M_PI_F) * atanf(cosf(pitch_in*(M_PI_F/18000))*tanf(roll_in*(M_PI_F/18000)));
// return
roll_out = roll_in;
pitch_out = pitch_in;
}
// get_pilot_desired_heading - transform pilot's yaw input into a
// desired yaw rate
// returns desired yaw rate in centi-degrees per second
float Copter::get_pilot_desired_yaw_rate(int16_t stick_angle)
{
// convert pilot input to the desired yaw rate
return stick_angle * g.acro_yaw_p;
}
// check for ekf yaw reset and adjust target heading
void Copter::check_ekf_yaw_reset()
{
float yaw_angle_change_rad = 0.0f;
uint32_t new_ekfYawReset_ms = ahrs.getLastYawResetAngle(yaw_angle_change_rad);
if (new_ekfYawReset_ms != ekfYawReset_ms) {
attitude_control.shift_ef_yaw_target(ToDeg(yaw_angle_change_rad) * 100.0f);
ekfYawReset_ms = new_ekfYawReset_ms;
}
}
/*************************************************************
* yaw controllers
*************************************************************/
// get_roi_yaw - returns heading towards location held in roi_WP
// should be called at 100hz
float Copter::get_roi_yaw()
{
static uint8_t roi_yaw_counter = 0; // used to reduce update rate to 100hz
roi_yaw_counter++;
if (roi_yaw_counter >= 4) {
roi_yaw_counter = 0;
yaw_look_at_WP_bearing = pv_get_bearing_cd(inertial_nav.get_position(), roi_WP);
}
return yaw_look_at_WP_bearing;
}
float Copter::get_look_ahead_yaw()
{
const Vector3f& vel = inertial_nav.get_velocity();
float speed = pythagorous2(vel.x,vel.y);
// Commanded Yaw to automatically look ahead.
if (position_ok() && (speed > YAW_LOOK_AHEAD_MIN_SPEED)) {
yaw_look_ahead_bearing = degrees(atan2f(vel.y,vel.x))*100.0f;
}
return yaw_look_ahead_bearing;
}
/*************************************************************
* throttle control
****************************************************************/
// update_thr_average - update estimated throttle required to hover (if necessary)
// should be called at 100hz
void Copter::update_thr_average()
{
// ensure throttle_average has been initialised
if( is_zero(throttle_average) ) {
throttle_average = g.throttle_mid;
// update position controller
pos_control.set_throttle_hover(throttle_average);
}
// if not armed or landed exit
if (!motors.armed() || ap.land_complete) {
return;
}
// get throttle output
float throttle = motors.get_throttle();
// calc average throttle if we are in a level hover
if (throttle > g.throttle_min && abs(climb_rate) < 60 && labs(ahrs.roll_sensor) < 500 && labs(ahrs.pitch_sensor) < 500) {
throttle_average = throttle_average * 0.99f + throttle * 0.01f;
// update position controller
pos_control.set_throttle_hover(throttle_average);
}
}
// set_throttle_takeoff - allows parents to tell throttle controller we are taking off so I terms can be cleared
void Copter::set_throttle_takeoff()
{
// tell position controller to reset alt target and reset I terms
pos_control.init_takeoff();
// tell motors to do a slow start
motors.slow_start(true);
}
// get_pilot_desired_throttle - transform pilot's throttle input to make cruise throttle mid stick
// used only for manual throttle modes
// returns throttle output 0 to 1000
int16_t Copter::get_pilot_desired_throttle(int16_t throttle_control)
{
int16_t throttle_out;
int16_t mid_stick = channel_throttle->get_control_mid();
// ensure reasonable throttle values
throttle_control = constrain_int16(throttle_control,0,1000);
g.throttle_mid = constrain_int16(g.throttle_mid,g.throttle_min+50,700);
// check throttle is above, below or in the deadband
if (throttle_control < mid_stick) {
// below the deadband
throttle_out = g.throttle_min + ((float)(throttle_control-g.throttle_min))*((float)(g.throttle_mid - g.throttle_min))/((float)(mid_stick-g.throttle_min));
}else if(throttle_control > mid_stick) {
// above the deadband
throttle_out = g.throttle_mid + ((float)(throttle_control-mid_stick)) * (float)(1000-g.throttle_mid) / (float)(1000-mid_stick);
}else{
// must be in the deadband
throttle_out = g.throttle_mid;
}
return throttle_out;
}
// get_pilot_desired_climb_rate - transform pilot's throttle input to
// climb rate in cm/s. we use radio_in instead of control_in to get the full range
// without any deadzone at the bottom
float Copter::get_pilot_desired_climb_rate(float throttle_control)
{
// throttle failsafe check
if( failsafe.radio ) {
return 0.0f;
}
float desired_rate = 0.0f;
float mid_stick = channel_throttle->get_control_mid();
float deadband_top = mid_stick + g.throttle_deadzone;
float deadband_bottom = mid_stick - g.throttle_deadzone;
// ensure a reasonable throttle value
throttle_control = constrain_float(throttle_control,g.throttle_min,1000.0f);
// ensure a reasonable deadzone
g.throttle_deadzone = constrain_int16(g.throttle_deadzone, 0, 400);
// check throttle is above, below or in the deadband
if (throttle_control < deadband_bottom) {
// below the deadband
desired_rate = g.pilot_velocity_z_max * (throttle_control-deadband_bottom) / (deadband_bottom-g.throttle_min);
}else if (throttle_control > deadband_top) {
// above the deadband
desired_rate = g.pilot_velocity_z_max * (throttle_control-deadband_top) / (1000.0f-deadband_top);
}else{
// must be in the deadband
desired_rate = 0.0f;
}
// desired climb rate for logging
desired_climb_rate = desired_rate;
return desired_rate;
}
// get_non_takeoff_throttle - a throttle somewhere between min and mid throttle which should not lead to a takeoff
float Copter::get_non_takeoff_throttle()
{
return (g.throttle_mid / 2.0f);
}
float Copter::get_takeoff_trigger_throttle()
{
return channel_throttle->get_control_mid() + g.takeoff_trigger_dz;
}
// get_throttle_pre_takeoff - convert pilot's input throttle to a throttle output before take-off
// used only for althold, loiter, hybrid flight modes
// returns throttle output 0 to 1000
float Copter::get_throttle_pre_takeoff(float input_thr)
{
// exit immediately if input_thr is zero
if (input_thr <= 0.0f) {
return 0.0f;
}
// TODO: does this parameter sanity check really belong here?
g.throttle_mid = constrain_int16(g.throttle_mid,g.throttle_min+50,700);
float in_min = g.throttle_min;
float in_max = get_takeoff_trigger_throttle();
#if FRAME_CONFIG == HELI_FRAME
// helicopters swash will move from bottom to 1/2 of mid throttle
float out_min = 0;
#else
// multicopters will output between spin-when-armed and 1/2 of mid throttle
float out_min = motors.get_throttle_warn();
#endif
float out_max = get_non_takeoff_throttle();
if ((g.throttle_behavior & THR_BEHAVE_FEEDBACK_FROM_MID_STICK) != 0) {
in_min = channel_throttle->get_control_mid();
}
float input_range = in_max-in_min;
float output_range = out_max-out_min;
// sanity check ranges
if (input_range <= 0.0f || output_range <= 0.0f) {
return 0.0f;
}
return constrain_float(out_min + (input_thr-in_min)*output_range/input_range, out_min, out_max);
}
// get_surface_tracking_climb_rate - hold copter at the desired distance above the ground
// returns climb rate (in cm/s) which should be passed to the position controller
float Copter::get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt)
{
static uint32_t last_call_ms = 0;
float distance_error;
float velocity_correction;
float current_alt = inertial_nav.get_altitude();
uint32_t now = millis();
// reset target altitude if this controller has just been engaged
if (now - last_call_ms > SONAR_TIMEOUT_MS) {
target_sonar_alt = sonar_alt + current_alt_target - current_alt;
}
last_call_ms = now;
// adjust sonar target alt if motors have not hit their limits
if ((target_rate<0 && !motors.limit.throttle_lower) || (target_rate>0 && !motors.limit.throttle_upper)) {
target_sonar_alt += target_rate * dt;
}
// do not let target altitude get too far from current altitude above ground
// Note: the 750cm limit is perhaps too wide but is consistent with the regular althold limits and helps ensure a smooth transition
target_sonar_alt = constrain_float(target_sonar_alt,sonar_alt-pos_control.get_leash_down_z(),sonar_alt+pos_control.get_leash_up_z());
// calc desired velocity correction from target sonar alt vs actual sonar alt (remove the error already passed to Altitude controller to avoid oscillations)
distance_error = (target_sonar_alt - sonar_alt) - (current_alt_target - current_alt);
velocity_correction = distance_error * g.sonar_gain;
velocity_correction = constrain_float(velocity_correction, -THR_SURFACE_TRACKING_VELZ_MAX, THR_SURFACE_TRACKING_VELZ_MAX);
// return combined pilot climb rate + rate to correct sonar alt error
return (target_rate + velocity_correction);
}
// set_accel_throttle_I_from_pilot_throttle - smoothes transition from pilot controlled throttle to autopilot throttle
void Copter::set_accel_throttle_I_from_pilot_throttle(int16_t pilot_throttle)
{
// shift difference between pilot's throttle and hover throttle into accelerometer I
g.pid_accel_z.set_integrator(pilot_throttle-throttle_average);
}
// updates position controller's maximum altitude using fence and EKF limits
void Copter::update_poscon_alt_max()
{
float alt_limit_cm = 0.0f; // interpreted as no limit if left as zero
#if AC_FENCE == ENABLED
// set fence altitude limit in position controller
if ((fence.get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) != 0) {
alt_limit_cm = pv_alt_above_origin(fence.get_safe_alt()*100.0f);
}
#endif
// get alt limit from EKF (limited during optical flow flight)
float ekf_limit_cm = 0.0f;
if (inertial_nav.get_hgt_ctrl_limit(ekf_limit_cm)) {
if ((alt_limit_cm <= 0.0f) || (ekf_limit_cm < alt_limit_cm)) {
alt_limit_cm = ekf_limit_cm;
}
}
// pass limit to pos controller
pos_control.set_alt_max(alt_limit_cm);
}
// rotate vector from vehicle's perspective to North-East frame
void Copter::rotate_body_frame_to_NE(float &x, float &y)
{
float ne_x = x*ahrs.cos_yaw() - y*ahrs.sin_yaw();
float ne_y = x*ahrs.sin_yaw() + y*ahrs.cos_yaw();
x = ne_x;
y = ne_y;
}

135
ArduSub/Copter.cpp Normal file
View File

@ -0,0 +1,135 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
/*
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/>.
*/
/*
constructor for main Copter class
*/
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
Copter::Copter(void) :
flight_modes(&g.flight_mode1),
sonar_enabled(true),
mission(ahrs,
FUNCTOR_BIND_MEMBER(&Copter::start_command, bool, const AP_Mission::Mission_Command &),
FUNCTOR_BIND_MEMBER(&Copter::verify_command_callback, bool, const AP_Mission::Mission_Command &),
FUNCTOR_BIND_MEMBER(&Copter::exit_mission, void)),
control_mode(STABILIZE),
#if FRAME_CONFIG == HELI_FRAME // helicopter constructor requires more arguments
motors(g.rc_7, g.heli_servo_rsc, g.heli_servo_1, g.heli_servo_2, g.heli_servo_3, g.heli_servo_4, MAIN_LOOP_RATE),
#elif FRAME_CONFIG == TRI_FRAME // tri constructor requires additional rc_7 argument to allow tail servo reversing
motors(MAIN_LOOP_RATE),
#elif FRAME_CONFIG == SINGLE_FRAME // single constructor requires extra servos for flaps
motors(g.single_servo_1, g.single_servo_2, g.single_servo_3, g.single_servo_4, MAIN_LOOP_RATE),
#elif FRAME_CONFIG == COAX_FRAME // single constructor requires extra servos for flaps
motors(g.single_servo_1, g.single_servo_2, MAIN_LOOP_RATE),
#else
motors(MAIN_LOOP_RATE),
#endif
scaleLongDown(1),
wp_bearing(0),
home_bearing(0),
home_distance(0),
wp_distance(0),
auto_mode(Auto_TakeOff),
guided_mode(Guided_TakeOff),
rtl_state(RTL_InitialClimb),
rtl_state_complete(false),
rtl_alt(0.0f),
circle_pilot_yaw_override(false),
simple_cos_yaw(1.0f),
simple_sin_yaw(0.0f),
super_simple_last_bearing(0),
super_simple_cos_yaw(1.0),
super_simple_sin_yaw(0.0f),
initial_armed_bearing(0),
throttle_average(0.0f),
desired_climb_rate(0),
loiter_time_max(0),
loiter_time(0),
#if FRSKY_TELEM_ENABLED == ENABLED
frsky_telemetry(ahrs, battery),
#endif
climb_rate(0),
sonar_alt(0),
sonar_alt_health(0),
target_sonar_alt(0.0f),
baro_alt(0),
baro_climbrate(0.0f),
land_accel_ef_filter(LAND_DETECTOR_ACCEL_LPF_CUTOFF),
auto_yaw_mode(AUTO_YAW_LOOK_AT_NEXT_WP),
yaw_look_at_WP_bearing(0.0f),
yaw_look_at_heading(0),
yaw_look_at_heading_slew(0),
yaw_look_ahead_bearing(0.0f),
condition_value(0),
condition_start(0),
G_Dt(0.0025f),
inertial_nav(ahrs),
attitude_control(ahrs, aparm, motors, g.p_stabilize_roll, g.p_stabilize_pitch, g.p_stabilize_yaw,
g.pid_rate_roll, g.pid_rate_pitch, g.pid_rate_yaw),
pos_control(ahrs, inertial_nav, motors, attitude_control,
g.p_alt_hold, g.p_vel_z, g.pid_accel_z,
g.p_pos_xy, g.pi_vel_xy),
wp_nav(inertial_nav, ahrs, pos_control, attitude_control),
circle_nav(inertial_nav, ahrs, pos_control),
pmTest1(0),
fast_loopTimer(0),
mainLoop_count(0),
rtl_loiter_start_time(0),
auto_trim_counter(0),
ServoRelayEvents(relay),
#if CAMERA == ENABLED
camera(&relay),
#endif
#if MOUNT == ENABLED
camera_mount(ahrs, current_loc),
#endif
#if AC_FENCE == ENABLED
fence(inertial_nav),
#endif
#if AC_RALLY == ENABLED
rally(ahrs),
#endif
#if SPRAYER == ENABLED
sprayer(&inertial_nav),
#endif
#if PARACHUTE == ENABLED
parachute(relay),
#endif
#if AP_TERRAIN_AVAILABLE && AC_TERRAIN
terrain(ahrs, mission, rally),
#endif
#if PRECISION_LANDING == ENABLED
precland(ahrs, inertial_nav, g.pi_precland, MAIN_LOOP_SECONDS),
#endif
#if FRAME_CONFIG == HELI_FRAME
// ToDo: Input Manager is only used by Heli for 3.3, but will be used by all frames for 3.4
input_manager(MAIN_LOOP_RATE),
#endif
in_mavlink_delay(false),
gcs_out_of_time(false),
param_loader(var_info)
{
memset(&current_loc, 0, sizeof(current_loc));
// init sensor error logging flags
sensor_health.baro = true;
sensor_health.compass = true;
}
Copter copter;

1045
ArduSub/Copter.h Normal file
View File

File diff suppressed because it is too large Load Diff

2053
ArduSub/GCS_Mavlink.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

869
ArduSub/Log.cpp Normal file
View File

@ -0,0 +1,869 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#if LOGGING_ENABLED == ENABLED
// Code to Write and Read packets from DataFlash log memory
// Code to interact with the user to dump or erase logs
#if CLI_ENABLED == ENABLED
// Creates a constant array of structs representing menu options
// and stores them in Flash memory, not RAM.
// User enters the string in the console to call the functions on the right.
// See class Menu in AP_Coommon for implementation details
static const struct Menu::command log_menu_commands[] = {
{"dump", MENU_FUNC(dump_log)},
{"erase", MENU_FUNC(erase_logs)},
{"enable", MENU_FUNC(select_logs)},
{"disable", MENU_FUNC(select_logs)}
};
// A Macro to create the Menu
MENU2(log_menu, "Log", log_menu_commands, FUNCTOR_BIND(&copter, &Copter::print_log_menu, bool));
bool Copter::print_log_menu(void)
{
cliSerial->printf("logs enabled: ");
if (0 == g.log_bitmask) {
cliSerial->printf("none");
}else{
if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST) cliSerial->printf(" ATTITUDE_FAST");
if (g.log_bitmask & MASK_LOG_ATTITUDE_MED) cliSerial->printf(" ATTITUDE_MED");
if (g.log_bitmask & MASK_LOG_GPS) cliSerial->printf(" GPS");
if (g.log_bitmask & MASK_LOG_PM) cliSerial->printf(" PM");
if (g.log_bitmask & MASK_LOG_CTUN) cliSerial->printf(" CTUN");
if (g.log_bitmask & MASK_LOG_NTUN) cliSerial->printf(" NTUN");
if (g.log_bitmask & MASK_LOG_RCIN) cliSerial->printf(" RCIN");
if (g.log_bitmask & MASK_LOG_IMU) cliSerial->printf(" IMU");
if (g.log_bitmask & MASK_LOG_CMD) cliSerial->printf(" CMD");
if (g.log_bitmask & MASK_LOG_CURRENT) cliSerial->printf(" CURRENT");
if (g.log_bitmask & MASK_LOG_RCOUT) cliSerial->printf(" RCOUT");
if (g.log_bitmask & MASK_LOG_OPTFLOW) cliSerial->printf(" OPTFLOW");
if (g.log_bitmask & MASK_LOG_COMPASS) cliSerial->printf(" COMPASS");
if (g.log_bitmask & MASK_LOG_CAMERA) cliSerial->printf(" CAMERA");
if (g.log_bitmask & MASK_LOG_PID) cliSerial->printf(" PID");
}
cliSerial->println();
DataFlash.ListAvailableLogs(cliSerial);
return(true);
}
#if CLI_ENABLED == ENABLED
int8_t Copter::dump_log(uint8_t argc, const Menu::arg *argv)
{
int16_t dump_log_num;
uint16_t dump_log_start;
uint16_t dump_log_end;
// check that the requested log number can be read
dump_log_num = argv[1].i;
if (dump_log_num == -2) {
DataFlash.DumpPageInfo(cliSerial);
return(-1);
} else if (dump_log_num <= 0) {
cliSerial->printf("dumping all\n");
Log_Read(0, 1, 0);
return(-1);
} else if ((argc != 2) || ((uint16_t)dump_log_num > DataFlash.get_num_logs())) {
cliSerial->printf("bad log number\n");
return(-1);
}
DataFlash.get_log_boundaries(dump_log_num, dump_log_start, dump_log_end);
Log_Read((uint16_t)dump_log_num, dump_log_start, dump_log_end);
return (0);
}
#endif
int8_t Copter::erase_logs(uint8_t argc, const Menu::arg *argv)
{
in_mavlink_delay = true;
do_erase_logs();
in_mavlink_delay = false;
return 0;
}
int8_t Copter::select_logs(uint8_t argc, const Menu::arg *argv)
{
uint16_t bits;
if (argc != 2) {
cliSerial->printf("missing log type\n");
return(-1);
}
bits = 0;
// Macro to make the following code a bit easier on the eye.
// Pass it the capitalised name of the log option, as defined
// in defines.h but without the LOG_ prefix. It will check for
// that name as the argument to the command, and set the bit in
// bits accordingly.
//
if (!strcasecmp(argv[1].str, "all")) {
bits = ~0;
} else {
#define TARG(_s) if (!strcasecmp(argv[1].str, # _s)) bits |= MASK_LOG_ ## _s
TARG(ATTITUDE_FAST);
TARG(ATTITUDE_MED);
TARG(GPS);
TARG(PM);
TARG(CTUN);
TARG(NTUN);
TARG(RCIN);
TARG(IMU);
TARG(CMD);
TARG(CURRENT);
TARG(RCOUT);
TARG(OPTFLOW);
TARG(COMPASS);
TARG(CAMERA);
TARG(PID);
#undef TARG
}
if (!strcasecmp(argv[0].str, "enable")) {
g.log_bitmask.set_and_save(g.log_bitmask | bits);
}else{
g.log_bitmask.set_and_save(g.log_bitmask & ~bits);
}
return(0);
}
int8_t Copter::process_logs(uint8_t argc, const Menu::arg *argv)
{
log_menu.run();
return 0;
}
#endif // CLI_ENABLED
void Copter::do_erase_logs(void)
{
gcs_send_text(MAV_SEVERITY_INFO, "Erasing logs");
DataFlash.EraseAll();
gcs_send_text(MAV_SEVERITY_INFO, "Log erase complete");
}
#if AUTOTUNE_ENABLED == ENABLED
struct PACKED log_AutoTune {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t axis; // roll or pitch
uint8_t tune_step; // tuning PI or D up or down
float meas_target; // target achieved rotation rate
float meas_min; // maximum achieved rotation rate
float meas_max; // maximum achieved rotation rate
float new_gain_rp; // newly calculated gain
float new_gain_rd; // newly calculated gain
float new_gain_sp; // newly calculated gain
float new_ddt; // newly calculated gain
};
// Write an Autotune data packet
void Copter::Log_Write_AutoTune(uint8_t axis, uint8_t tune_step, float meas_target, float meas_min, float meas_max, float new_gain_rp, float new_gain_rd, float new_gain_sp, float new_ddt)
{
struct log_AutoTune pkt = {
LOG_PACKET_HEADER_INIT(LOG_AUTOTUNE_MSG),
time_us : AP_HAL::micros64(),
axis : axis,
tune_step : tune_step,
meas_target : meas_target,
meas_min : meas_min,
meas_max : meas_max,
new_gain_rp : new_gain_rp,
new_gain_rd : new_gain_rd,
new_gain_sp : new_gain_sp,
new_ddt : new_ddt
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
struct PACKED log_AutoTuneDetails {
LOG_PACKET_HEADER;
uint64_t time_us;
float angle_cd; // lean angle in centi-degrees
float rate_cds; // current rotation rate in centi-degrees / second
};
// Write an Autotune data packet
void Copter::Log_Write_AutoTuneDetails(float angle_cd, float rate_cds)
{
struct log_AutoTuneDetails pkt = {
LOG_PACKET_HEADER_INIT(LOG_AUTOTUNEDETAILS_MSG),
time_us : AP_HAL::micros64(),
angle_cd : angle_cd,
rate_cds : rate_cds
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
#endif
// Write a Current data packet
void Copter::Log_Write_Current()
{
DataFlash.Log_Write_Current(battery, (int16_t)(motors.get_throttle()));
// also write power status
DataFlash.Log_Write_Power();
}
struct PACKED log_Optflow {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t surface_quality;
float flow_x;
float flow_y;
float body_x;
float body_y;
};
// Write an optical flow packet
void Copter::Log_Write_Optflow()
{
#if OPTFLOW == ENABLED
// exit immediately if not enabled
if (!optflow.enabled()) {
return;
}
const Vector2f &flowRate = optflow.flowRate();
const Vector2f &bodyRate = optflow.bodyRate();
struct log_Optflow pkt = {
LOG_PACKET_HEADER_INIT(LOG_OPTFLOW_MSG),
time_us : AP_HAL::micros64(),
surface_quality : optflow.quality(),
flow_x : flowRate.x,
flow_y : flowRate.y,
body_x : bodyRate.x,
body_y : bodyRate.y
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
#endif // OPTFLOW == ENABLED
}
struct PACKED log_Nav_Tuning {
LOG_PACKET_HEADER;
uint64_t time_us;
float desired_pos_x;
float desired_pos_y;
float pos_x;
float pos_y;
float desired_vel_x;
float desired_vel_y;
float vel_x;
float vel_y;
float desired_accel_x;
float desired_accel_y;
};
// Write an Nav Tuning packet
void Copter::Log_Write_Nav_Tuning()
{
const Vector3f &pos_target = pos_control.get_pos_target();
const Vector3f &vel_target = pos_control.get_vel_target();
const Vector3f &accel_target = pos_control.get_accel_target();
const Vector3f &position = inertial_nav.get_position();
const Vector3f &velocity = inertial_nav.get_velocity();
struct log_Nav_Tuning pkt = {
LOG_PACKET_HEADER_INIT(LOG_NAV_TUNING_MSG),
time_us : AP_HAL::micros64(),
desired_pos_x : pos_target.x,
desired_pos_y : pos_target.y,
pos_x : position.x,
pos_y : position.y,
desired_vel_x : vel_target.x,
desired_vel_y : vel_target.y,
vel_x : velocity.x,
vel_y : velocity.y,
desired_accel_x : accel_target.x,
desired_accel_y : accel_target.y
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
struct PACKED log_Control_Tuning {
LOG_PACKET_HEADER;
uint64_t time_us;
int16_t throttle_in;
int16_t angle_boost;
float throttle_out;
float desired_alt;
float inav_alt;
int32_t baro_alt;
int16_t desired_sonar_alt;
int16_t sonar_alt;
int16_t desired_climb_rate;
int16_t climb_rate;
};
// Write a control tuning packet
void Copter::Log_Write_Control_Tuning()
{
struct log_Control_Tuning pkt = {
LOG_PACKET_HEADER_INIT(LOG_CONTROL_TUNING_MSG),
time_us : AP_HAL::micros64(),
throttle_in : channel_throttle->control_in,
angle_boost : attitude_control.angle_boost(),
throttle_out : motors.get_throttle(),
desired_alt : pos_control.get_alt_target() / 100.0f,
inav_alt : inertial_nav.get_altitude() / 100.0f,
baro_alt : baro_alt,
desired_sonar_alt : (int16_t)target_sonar_alt,
sonar_alt : sonar_alt,
desired_climb_rate : (int16_t)pos_control.get_vel_target_z(),
climb_rate : climb_rate
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
struct PACKED log_Performance {
LOG_PACKET_HEADER;
uint64_t time_us;
uint16_t num_long_running;
uint16_t num_loops;
uint32_t max_time;
int16_t pm_test;
uint8_t i2c_lockup_count;
uint16_t ins_error_count;
};
// Write a performance monitoring packet
void Copter::Log_Write_Performance()
{
struct log_Performance pkt = {
LOG_PACKET_HEADER_INIT(LOG_PERFORMANCE_MSG),
time_us : AP_HAL::micros64(),
num_long_running : perf_info_get_num_long_running(),
num_loops : perf_info_get_num_loops(),
max_time : perf_info_get_max_time(),
pm_test : pmTest1,
i2c_lockup_count : hal.i2c->lockup_count(),
ins_error_count : ins.error_count()
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
// Write an attitude packet
void Copter::Log_Write_Attitude()
{
Vector3f targets = attitude_control.get_att_target_euler_cd();
targets.z = wrap_360_cd_float(targets.z);
DataFlash.Log_Write_Attitude(ahrs, targets);
#if OPTFLOW == ENABLED
DataFlash.Log_Write_EKF(ahrs,optflow.enabled());
#else
DataFlash.Log_Write_EKF(ahrs,false);
#endif
DataFlash.Log_Write_AHRS2(ahrs);
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
sitl.Log_Write_SIMSTATE(&DataFlash);
#endif
DataFlash.Log_Write_POS(ahrs);
}
struct PACKED log_Rate {
LOG_PACKET_HEADER;
uint64_t time_us;
float control_roll;
float roll;
float roll_out;
float control_pitch;
float pitch;
float pitch_out;
float control_yaw;
float yaw;
float yaw_out;
float control_accel;
float accel;
float accel_out;
};
// Write an rate packet
void Copter::Log_Write_Rate()
{
const Vector3f &rate_targets = attitude_control.rate_bf_targets();
const Vector3f &accel_target = pos_control.get_accel_target();
struct log_Rate pkt_rate = {
LOG_PACKET_HEADER_INIT(LOG_RATE_MSG),
time_us : AP_HAL::micros64(),
control_roll : (float)rate_targets.x,
roll : (float)(ahrs.get_gyro().x * AC_ATTITUDE_CONTROL_DEGX100),
roll_out : motors.get_roll(),
control_pitch : (float)rate_targets.y,
pitch : (float)(ahrs.get_gyro().y * AC_ATTITUDE_CONTROL_DEGX100),
pitch_out : motors.get_pitch(),
control_yaw : (float)rate_targets.z,
yaw : (float)(ahrs.get_gyro().z * AC_ATTITUDE_CONTROL_DEGX100),
yaw_out : motors.get_yaw(),
control_accel : (float)accel_target.z,
accel : (float)(-(ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f),
accel_out : motors.get_throttle()
};
DataFlash.WriteBlock(&pkt_rate, sizeof(pkt_rate));
}
struct PACKED log_MotBatt {
LOG_PACKET_HEADER;
uint64_t time_us;
float lift_max;
float bat_volt;
float bat_res;
float th_limit;
};
// Write an rate packet
void Copter::Log_Write_MotBatt()
{
#if FRAME_CONFIG != HELI_FRAME
struct log_MotBatt pkt_mot = {
LOG_PACKET_HEADER_INIT(LOG_MOTBATT_MSG),
time_us : AP_HAL::micros64(),
lift_max : (float)(motors.get_lift_max()),
bat_volt : (float)(motors.get_batt_voltage_filt()),
bat_res : (float)(motors.get_batt_resistance()),
th_limit : (float)(motors.get_throttle_limit())
};
DataFlash.WriteBlock(&pkt_mot, sizeof(pkt_mot));
#endif
}
struct PACKED log_Startup {
LOG_PACKET_HEADER;
uint64_t time_us;
};
// Write Startup packet
void Copter::Log_Write_Startup()
{
struct log_Startup pkt = {
LOG_PACKET_HEADER_INIT(LOG_STARTUP_MSG),
time_us : AP_HAL::micros64()
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
struct PACKED log_Event {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t id;
};
// Wrote an event packet
void Copter::Log_Write_Event(uint8_t id)
{
if (should_log(MASK_LOG_ANY)) {
struct log_Event pkt = {
LOG_PACKET_HEADER_INIT(LOG_EVENT_MSG),
time_us : AP_HAL::micros64(),
id : id
};
DataFlash.WriteCriticalBlock(&pkt, sizeof(pkt));
}
}
struct PACKED log_Data_Int16t {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t id;
int16_t data_value;
};
// Write an int16_t data packet
UNUSED_FUNCTION
void Copter::Log_Write_Data(uint8_t id, int16_t value)
{
if (should_log(MASK_LOG_ANY)) {
struct log_Data_Int16t pkt = {
LOG_PACKET_HEADER_INIT(LOG_DATA_INT16_MSG),
time_us : AP_HAL::micros64(),
id : id,
data_value : value
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
}
struct PACKED log_Data_UInt16t {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t id;
uint16_t data_value;
};
// Write an uint16_t data packet
UNUSED_FUNCTION
void Copter::Log_Write_Data(uint8_t id, uint16_t value)
{
if (should_log(MASK_LOG_ANY)) {
struct log_Data_UInt16t pkt = {
LOG_PACKET_HEADER_INIT(LOG_DATA_UINT16_MSG),
time_us : AP_HAL::micros64(),
id : id,
data_value : value
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
}
struct PACKED log_Data_Int32t {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t id;
int32_t data_value;
};
// Write an int32_t data packet
void Copter::Log_Write_Data(uint8_t id, int32_t value)
{
if (should_log(MASK_LOG_ANY)) {
struct log_Data_Int32t pkt = {
LOG_PACKET_HEADER_INIT(LOG_DATA_INT32_MSG),
time_us : AP_HAL::micros64(),
id : id,
data_value : value
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
}
struct PACKED log_Data_UInt32t {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t id;
uint32_t data_value;
};
// Write a uint32_t data packet
void Copter::Log_Write_Data(uint8_t id, uint32_t value)
{
if (should_log(MASK_LOG_ANY)) {
struct log_Data_UInt32t pkt = {
LOG_PACKET_HEADER_INIT(LOG_DATA_UINT32_MSG),
time_us : AP_HAL::micros64(),
id : id,
data_value : value
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
}
struct PACKED log_Data_Float {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t id;
float data_value;
};
// Write a float data packet
UNUSED_FUNCTION
void Copter::Log_Write_Data(uint8_t id, float value)
{
if (should_log(MASK_LOG_ANY)) {
struct log_Data_Float pkt = {
LOG_PACKET_HEADER_INIT(LOG_DATA_FLOAT_MSG),
time_us : AP_HAL::micros64(),
id : id,
data_value : value
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
}
struct PACKED log_Error {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t sub_system;
uint8_t error_code;
};
// Write an error packet
void Copter::Log_Write_Error(uint8_t sub_system, uint8_t error_code)
{
struct log_Error pkt = {
LOG_PACKET_HEADER_INIT(LOG_ERROR_MSG),
time_us : AP_HAL::micros64(),
sub_system : sub_system,
error_code : error_code,
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
void Copter::Log_Write_Baro(void)
{
DataFlash.Log_Write_Baro(barometer);
}
struct PACKED log_ParameterTuning {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t parameter; // parameter we are tuning, e.g. 39 is CH6_CIRCLE_RATE
float tuning_value; // normalized value used inside tuning() function
int16_t control_in; // raw tune input value
int16_t tuning_low; // tuning low end value
int16_t tuning_high; // tuning high end value
};
void Copter::Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, int16_t control_in, int16_t tune_low, int16_t tune_high)
{
struct log_ParameterTuning pkt_tune = {
LOG_PACKET_HEADER_INIT(LOG_PARAMTUNE_MSG),
time_us : AP_HAL::micros64(),
parameter : param,
tuning_value : tuning_val,
control_in : control_in,
tuning_low : tune_low,
tuning_high : tune_high
};
DataFlash.WriteBlock(&pkt_tune, sizeof(pkt_tune));
}
// log EKF origin and ahrs home to dataflash
void Copter::Log_Write_Home_And_Origin()
{
// log ekf origin if set
Location ekf_orig;
if (ahrs.get_NavEKF_const().getOriginLLH(ekf_orig)) {
DataFlash.Log_Write_Origin(LogOriginType::ekf_origin, ekf_orig);
}
// log ahrs home if set
if (ap.home_state != HOME_UNSET) {
DataFlash.Log_Write_Origin(LogOriginType::ahrs_home, ahrs.get_home());
}
}
// logs when baro or compass becomes unhealthy
void Copter::Log_Sensor_Health()
{
// check baro
if (sensor_health.baro != barometer.healthy()) {
sensor_health.baro = barometer.healthy();
Log_Write_Error(ERROR_SUBSYSTEM_BARO, (sensor_health.baro ? ERROR_CODE_ERROR_RESOLVED : ERROR_CODE_UNHEALTHY));
}
// check compass
if (sensor_health.compass != compass.healthy()) {
sensor_health.compass = compass.healthy();
Log_Write_Error(ERROR_SUBSYSTEM_COMPASS, (sensor_health.compass ? ERROR_CODE_ERROR_RESOLVED : ERROR_CODE_UNHEALTHY));
}
}
struct PACKED log_Heli {
LOG_PACKET_HEADER;
uint64_t time_us;
int16_t desired_rotor_speed;
int16_t main_rotor_speed;
};
#if FRAME_CONFIG == HELI_FRAME
// Write an helicopter packet
void Copter::Log_Write_Heli()
{
struct log_Heli pkt_heli = {
LOG_PACKET_HEADER_INIT(LOG_HELI_MSG),
time_us : AP_HAL::micros64(),
desired_rotor_speed : motors.get_desired_rotor_speed(),
main_rotor_speed : motors.get_main_rotor_speed(),
};
DataFlash.WriteBlock(&pkt_heli, sizeof(pkt_heli));
}
#endif
// precision landing logging
struct PACKED log_Precland {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t healthy;
float bf_angle_x;
float bf_angle_y;
float ef_angle_x;
float ef_angle_y;
float pos_x;
float pos_y;
};
// Write an optical flow packet
void Copter::Log_Write_Precland()
{
#if PRECISION_LANDING == ENABLED
// exit immediately if not enabled
if (!precland.enabled()) {
return;
}
const Vector2f &bf_angle = precland.last_bf_angle_to_target();
const Vector2f &ef_angle = precland.last_ef_angle_to_target();
const Vector3f &target_pos_ofs = precland.last_target_pos_offset();
struct log_Precland pkt = {
LOG_PACKET_HEADER_INIT(LOG_PRECLAND_MSG),
time_us : AP_HAL::micros64(),
healthy : precland.healthy(),
bf_angle_x : degrees(bf_angle.x),
bf_angle_y : degrees(bf_angle.y),
ef_angle_x : degrees(ef_angle.x),
ef_angle_y : degrees(ef_angle.y),
pos_x : target_pos_ofs.x,
pos_y : target_pos_ofs.y
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
#endif // PRECISION_LANDING == ENABLED
}
const struct LogStructure Copter::log_structure[] = {
LOG_COMMON_STRUCTURES,
#if AUTOTUNE_ENABLED == ENABLED
{ LOG_AUTOTUNE_MSG, sizeof(log_AutoTune),
"ATUN", "QBBfffffff", "TimeUS,Axis,TuneStep,Targ,Min,Max,RP,RD,SP,ddt" },
{ LOG_AUTOTUNEDETAILS_MSG, sizeof(log_AutoTuneDetails),
"ATDE", "Qff", "TimeUS,Angle,Rate" },
#endif
{ LOG_PARAMTUNE_MSG, sizeof(log_ParameterTuning),
"PTUN", "QBfHHH", "TimeUS,Param,TunVal,CtrlIn,TunLo,TunHi" },
{ LOG_OPTFLOW_MSG, sizeof(log_Optflow),
"OF", "QBffff", "TimeUS,Qual,flowX,flowY,bodyX,bodyY" },
{ LOG_NAV_TUNING_MSG, sizeof(log_Nav_Tuning),
"NTUN", "Qffffffffff", "TimeUS,DPosX,DPosY,PosX,PosY,DVelX,DVelY,VelX,VelY,DAccX,DAccY" },
{ LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning),
"CTUN", "Qhhfffecchh", "TimeUS,ThrIn,AngBst,ThrOut,DAlt,Alt,BarAlt,DSAlt,SAlt,DCRt,CRt" },
{ LOG_PERFORMANCE_MSG, sizeof(log_Performance),
"PM", "QHHIhBH", "TimeUS,NLon,NLoop,MaxT,PMT,I2CErr,INSErr" },
{ LOG_RATE_MSG, sizeof(log_Rate),
"RATE", "Qffffffffffff", "TimeUS,RDes,R,ROut,PDes,P,POut,YDes,Y,YOut,ADes,A,AOut" },
{ LOG_MOTBATT_MSG, sizeof(log_MotBatt),
"MOTB", "Qffff", "TimeUS,LiftMax,BatVolt,BatRes,ThLimit" },
{ LOG_STARTUP_MSG, sizeof(log_Startup),
"STRT", "Q", "TimeUS" },
{ LOG_EVENT_MSG, sizeof(log_Event),
"EV", "QB", "TimeUS,Id" },
{ LOG_DATA_INT16_MSG, sizeof(log_Data_Int16t),
"D16", "QBh", "TimeUS,Id,Value" },
{ LOG_DATA_UINT16_MSG, sizeof(log_Data_UInt16t),
"DU16", "QBH", "TimeUS,Id,Value" },
{ LOG_DATA_INT32_MSG, sizeof(log_Data_Int32t),
"D32", "QBi", "TimeUS,Id,Value" },
{ LOG_DATA_UINT32_MSG, sizeof(log_Data_UInt32t),
"DU32", "QBI", "TimeUS,Id,Value" },
{ LOG_DATA_FLOAT_MSG, sizeof(log_Data_Float),
"DFLT", "QBf", "TimeUS,Id,Value" },
{ LOG_ERROR_MSG, sizeof(log_Error),
"ERR", "QBB", "TimeUS,Subsys,ECode" },
{ LOG_HELI_MSG, sizeof(log_Heli),
"HELI", "Qhh", "TimeUS,DRRPM,ERRPM" },
{ LOG_PRECLAND_MSG, sizeof(log_Precland),
"PL", "QBffffff", "TimeUS,Heal,bX,bY,eX,eY,pX,pY" },
};
#if CLI_ENABLED == ENABLED
// Read the DataFlash log memory
void Copter::Log_Read(uint16_t list_entry, uint16_t start_page, uint16_t end_page)
{
cliSerial->printf("\n" FIRMWARE_STRING
"\nFree RAM: %u\n"
"\nFrame: " FRAME_CONFIG_STRING "\n",
(unsigned) hal.util->available_memory());
cliSerial->println(HAL_BOARD_NAME);
DataFlash.LogReadProcess(list_entry, start_page, end_page,
FUNCTOR_BIND_MEMBER(&Copter::print_flight_mode, void, AP_HAL::BetterStream *, uint8_t),
cliSerial);
}
#endif // CLI_ENABLED
void Copter::Log_Write_Vehicle_Startup_Messages()
{
// only 200(?) bytes are guaranteed by DataFlash
DataFlash.Log_Write_Message("Frame: " FRAME_CONFIG_STRING);
DataFlash.Log_Write_Mode(control_mode);
}
// start a new log
void Copter::start_logging()
{
if (g.log_bitmask != 0) {
if (!ap.logging_started) {
ap.logging_started = true;
DataFlash.set_mission(&mission);
DataFlash.setVehicle_Startup_Log_Writer(FUNCTOR_BIND(&copter, &Copter::Log_Write_Vehicle_Startup_Messages, void));
DataFlash.StartNewLog();
}
// enable writes
DataFlash.EnableWrites(true);
}
}
void Copter::log_init(void)
{
DataFlash.Init(log_structure, ARRAY_SIZE(log_structure));
if (!DataFlash.CardInserted()) {
gcs_send_text(MAV_SEVERITY_WARNING, "No dataflash card inserted");
g.log_bitmask.set(0);
} else if (DataFlash.NeedPrep()) {
gcs_send_text(MAV_SEVERITY_INFO, "Preparing log system");
DataFlash.Prep();
gcs_send_text(MAV_SEVERITY_INFO, "Prepared log system");
for (uint8_t i=0; i<num_gcs; i++) {
gcs[i].reset_cli_timeout();
}
}
}
#else // LOGGING_ENABLED
#if CLI_ENABLED == ENABLED
bool Copter::print_log_menu(void) { return true; }
int8_t Copter::dump_log(uint8_t argc, const Menu::arg *argv) { return 0; }
int8_t Copter::erase_logs(uint8_t argc, const Menu::arg *argv) { return 0; }
int8_t Copter::select_logs(uint8_t argc, const Menu::arg *argv) { return 0; }
int8_t Copter::process_logs(uint8_t argc, const Menu::arg *argv) { return 0; }
void Copter::Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page) {}
#endif // CLI_ENABLED == ENABLED
void Copter::do_erase_logs(void) {}
void Copter::Log_Write_AutoTune(uint8_t axis, uint8_t tune_step, float meas_target, \
float meas_min, float meas_max, float new_gain_rp, \
float new_gain_rd, float new_gain_sp, float new_ddt) {}
void Copter::Log_Write_AutoTuneDetails(float angle_cd, float rate_cds) {}
void Copter::Log_Write_Current() {}
void Copter::Log_Write_Nav_Tuning() {}
void Copter::Log_Write_Control_Tuning() {}
void Copter::Log_Write_Performance() {}
void Copter::Log_Write_Attitude(void) {}
void Copter::Log_Write_Rate() {}
void Copter::Log_Write_MotBatt() {}
void Copter::Log_Write_Startup() {}
void Copter::Log_Write_Event(uint8_t id) {}
void Copter::Log_Write_Data(uint8_t id, int32_t value) {}
void Copter::Log_Write_Data(uint8_t id, uint32_t value) {}
void Copter::Log_Write_Data(uint8_t id, int16_t value) {}
void Copter::Log_Write_Data(uint8_t id, uint16_t value) {}
void Copter::Log_Write_Data(uint8_t id, float value) {}
void Copter::Log_Write_Error(uint8_t sub_system, uint8_t error_code) {}
void Copter::Log_Write_Baro(void) {}
void Copter::Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, int16_t control_in, int16_t tune_low, int16_t tune_high) {}
void Copter::Log_Write_Home_And_Origin() {}
void Copter::Log_Sensor_Health() {}
#if FRAME_CONFIG == HELI_FRAME
void Copter::Log_Write_Heli() {}
#endif
#if OPTFLOW == ENABLED
void Copter::Log_Write_Optflow() {}
#endif
void Copter::start_logging() {}
void Copter::log_init(void) {}
#endif // LOGGING_ENABLED

2
ArduSub/Makefile Normal file
View File

@ -0,0 +1,2 @@
include ../mk/apm.mk

2
ArduSub/Makefile.waf Normal file
View File

@ -0,0 +1,2 @@
all:
@$(MAKE) -C ../ -f Makefile.waf copter

1177
ArduSub/Parameters.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

605
ArduSub/Parameters.h Normal file
View File

@ -0,0 +1,605 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#ifndef PARAMETERS_H
#define PARAMETERS_H
#include <AP_Common/AP_Common.h>
// Global parameter class.
//
class Parameters {
public:
// The version of the layout as described by the parameter enum.
//
// When changing the parameter enum in an incompatible fashion, this
// value should be incremented by one.
//
// The increment will prevent old parameters from being used incorrectly
// by newer code.
//
static const uint16_t k_format_version = 120;
// The parameter software_type is set up solely for ground station use
// and identifies the software type (eg ArduPilotMega versus
// ArduCopterMega)
// GCS will interpret values 0-9 as ArduPilotMega. Developers may use
// values within that range to identify different branches.
//
static const uint16_t k_software_type = 10; // 0 for APM
// trunk
// Parameter identities.
//
// The enumeration defined here is used to ensure that every parameter
// or parameter group has a unique ID number. This number is used by
// AP_Param to store and locate parameters in EEPROM.
//
// Note that entries without a number are assigned the next number after
// the entry preceding them. When adding new entries, ensure that they
// don't overlap.
//
// Try to group related variables together, and assign them a set
// range in the enumeration. Place these groups in numerical order
// at the end of the enumeration.
//
// WARNING: Care should be taken when editing this enumeration as the
// AP_Param load/save code depends on the values here to identify
// variables saved in EEPROM.
//
//
enum {
// Layout version number, always key zero.
//
k_param_format_version = 0,
k_param_software_type,
k_param_ins_old, // *** Deprecated, remove with next eeprom number change
k_param_ins, // libraries/AP_InertialSensor variables
k_param_NavEKF2,
// simulation
k_param_sitl = 10,
// barometer object (needed for SITL)
k_param_barometer,
// scheduler object (for debugging)
k_param_scheduler,
// relay object
k_param_relay,
// EPM object
k_param_epm,
// BoardConfig object
k_param_BoardConfig,
// GPS object
k_param_gps,
// Parachute object
k_param_parachute,
// Landing gear object
k_param_landinggear, // 18
// Input Management object
k_param_input_manager, // 19
// Misc
//
k_param_log_bitmask_old = 20, // Deprecated
k_param_log_last_filenumber, // *** Deprecated - remove
// with next eeprom number
// change
k_param_toy_yaw_rate, // deprecated - remove
k_param_crosstrack_min_distance, // deprecated - remove with next eeprom number change
k_param_rssi_pin, // unused, replaced by rssi_ library parameters
k_param_throttle_accel_enabled, // deprecated - remove
k_param_wp_yaw_behavior,
k_param_acro_trainer,
k_param_pilot_velocity_z_max,
k_param_circle_rate, // deprecated - remove
k_param_sonar_gain,
k_param_ch8_option,
k_param_arming_check,
k_param_sprayer,
k_param_angle_max,
k_param_gps_hdop_good,
k_param_battery,
k_param_fs_batt_mah,
k_param_angle_rate_max, // remove
k_param_rssi_range, // unused, replaced by rssi_ library parameters
k_param_rc_feel_rp,
k_param_NavEKF, // Extended Kalman Filter Inertial Navigation Group
k_param_mission, // mission library
k_param_rc_13,
k_param_rc_14,
k_param_rally,
k_param_poshold_brake_rate,
k_param_poshold_brake_angle_max,
k_param_pilot_accel_z,
k_param_serial0_baud, // deprecated - remove
k_param_serial1_baud, // deprecated - remove
k_param_serial2_baud, // deprecated - remove
k_param_land_repositioning,
k_param_sonar, // sonar object
k_param_fs_ekf_thresh,
k_param_terrain,
k_param_acro_expo,
k_param_throttle_deadzone,
k_param_optflow,
k_param_dcmcheck_thresh, // deprecated - remove
k_param_log_bitmask,
k_param_cli_enabled,
k_param_throttle_filt,
k_param_throttle_behavior,
k_param_pilot_takeoff_alt, // 64
// 65: AP_Limits Library
k_param_limits = 65, // deprecated - remove
k_param_gpslock_limit, // deprecated - remove
k_param_geofence_limit, // deprecated - remove
k_param_altitude_limit, // deprecated - remove
k_param_fence,
k_param_gps_glitch, // deprecated
k_param_baro_glitch, // 71 - deprecated
// AP_ADSB Library
k_param_adsb, // 72
// 74: precision landing object
k_param_precland = 74,
//
// 75: Singlecopter, CoaxCopter
//
k_param_single_servo_1 = 75,
k_param_single_servo_2,
k_param_single_servo_3,
k_param_single_servo_4, // 78
//
// 80: Heli
//
k_param_heli_servo_1 = 80,
k_param_heli_servo_2,
k_param_heli_servo_3,
k_param_heli_servo_4,
k_param_heli_pitch_ff, // remove
k_param_heli_roll_ff, // remove
k_param_heli_yaw_ff, // remove
k_param_heli_stab_col_min, // remove
k_param_heli_stab_col_max, // remove
k_param_heli_servo_rsc, // 89 = full!
//
// 90: misc2
//
k_param_motors = 90,
k_param_disarm_delay,
k_param_fs_crash_check,
// 97: RSSI
k_param_rssi = 97,
//
// 100: Inertial Nav
//
k_param_inertial_nav = 100, // deprecated
k_param_wp_nav,
k_param_attitude_control,
k_param_pos_control,
k_param_circle_nav, // 104
// 110: Telemetry control
//
k_param_gcs0 = 110,
k_param_gcs1,
k_param_sysid_this_mav,
k_param_sysid_my_gcs,
k_param_serial1_baud_old, // deprecated
k_param_telem_delay,
k_param_gcs2,
k_param_serial2_baud_old, // deprecated
k_param_serial2_protocol, // deprecated
k_param_serial_manager,
k_param_ch9_option,
k_param_ch10_option,
k_param_ch11_option,
k_param_ch12_option,
k_param_takeoff_trigger_dz,
k_param_gcs3,
k_param_gcs_pid_mask, // 126
//
// 135 : reserved for Solo until features merged with master
//
k_param_rtl_speed_cms = 135,
k_param_fs_batt_curr_rtl, // 136
//
// 140: Sensor parameters
//
k_param_imu = 140, // deprecated - can be deleted
k_param_battery_monitoring = 141, // deprecated - can be deleted
k_param_volt_div_ratio, // deprecated - can be deleted
k_param_curr_amp_per_volt, // deprecated - can be deleted
k_param_input_voltage, // deprecated - can be deleted
k_param_pack_capacity, // deprecated - can be deleted
k_param_compass_enabled,
k_param_compass,
k_param_sonar_enabled_old, // deprecated
k_param_frame_orientation,
k_param_optflow_enabled, // deprecated
k_param_fs_batt_voltage,
k_param_ch7_option,
k_param_auto_slew_rate, // deprecated - can be deleted
k_param_sonar_type_old, // deprecated
k_param_super_simple = 155,
k_param_axis_enabled = 157, // deprecated - remove with next eeprom number change
k_param_copter_leds_mode, // deprecated - remove with next eeprom number change
k_param_ahrs, // AHRS group // 159
//
// 160: Navigation parameters
//
k_param_rtl_altitude = 160,
k_param_crosstrack_gain, // deprecated - remove with next eeprom number change
k_param_rtl_loiter_time,
k_param_rtl_alt_final,
k_param_tilt_comp, //164 deprecated - remove with next eeprom number change
//
// Camera and mount parameters
//
k_param_camera = 165,
k_param_camera_mount,
k_param_camera_mount2, // deprecated
//
// Batery monitoring parameters
//
k_param_battery_volt_pin = 168, // deprecated - can be deleted
k_param_battery_curr_pin, // 169 deprecated - can be deleted
//
// 170: Radio settings
//
k_param_rc_1 = 170,
k_param_rc_2,
k_param_rc_3,
k_param_rc_4,
k_param_rc_5,
k_param_rc_6,
k_param_rc_7,
k_param_rc_8,
k_param_rc_10,
k_param_rc_11,
k_param_throttle_min,
k_param_throttle_max, // remove
k_param_failsafe_throttle,
k_param_throttle_fs_action, // remove
k_param_failsafe_throttle_value,
k_param_throttle_trim, // remove
k_param_esc_calibrate,
k_param_radio_tuning,
k_param_radio_tuning_high,
k_param_radio_tuning_low,
k_param_rc_speed = 192,
k_param_failsafe_battery_enabled,
k_param_throttle_mid,
k_param_failsafe_gps_enabled, // remove
k_param_rc_9,
k_param_rc_12,
k_param_failsafe_gcs,
k_param_rcmap, // 199
//
// 200: flight modes
//
k_param_flight_mode1 = 200,
k_param_flight_mode2,
k_param_flight_mode3,
k_param_flight_mode4,
k_param_flight_mode5,
k_param_flight_mode6,
k_param_simple_modes,
//
// 210: Waypoint data
//
k_param_waypoint_mode = 210, // remove
k_param_command_total, // remove
k_param_command_index, // remove
k_param_command_nav_index, // remove
k_param_waypoint_radius, // remove
k_param_circle_radius, // remove
k_param_waypoint_speed_max, // remove
k_param_land_speed,
k_param_auto_velocity_z_min, // remove
k_param_auto_velocity_z_max, // remove - 219
//
// 220: PI/D Controllers
//
k_param_acro_rp_p = 221,
k_param_axis_lock_p, // remove
k_param_pid_rate_roll,
k_param_pid_rate_pitch,
k_param_pid_rate_yaw,
k_param_p_stabilize_roll,
k_param_p_stabilize_pitch,
k_param_p_stabilize_yaw,
k_param_p_pos_xy,
k_param_p_loiter_lon, // remove
k_param_pid_loiter_rate_lat, // remove
k_param_pid_loiter_rate_lon, // remove
k_param_pid_nav_lat, // remove
k_param_pid_nav_lon, // remove
k_param_p_alt_hold,
k_param_p_vel_z,
k_param_pid_optflow_roll, // remove
k_param_pid_optflow_pitch, // remove
k_param_acro_balance_roll_old, // remove
k_param_acro_balance_pitch_old, // remove
k_param_pid_accel_z,
k_param_acro_balance_roll,
k_param_acro_balance_pitch,
k_param_acro_yaw_p,
k_param_autotune_axis_bitmask,
k_param_autotune_aggressiveness,
k_param_pi_vel_xy,
k_param_fs_ekf_action,
k_param_rtl_climb_min,
k_param_rpm_sensor,
k_param_autotune_min_d, // 251
k_param_pi_precland, // 252
k_param_DataFlash = 253, // 253 - Logging Group
// 254,255: reserved
};
AP_Int16 format_version;
AP_Int8 software_type;
// Telemetry control
//
AP_Int16 sysid_this_mav;
AP_Int16 sysid_my_gcs;
AP_Int8 telem_delay;
#if CLI_ENABLED == ENABLED
AP_Int8 cli_enabled;
#endif
AP_Float throttle_filt;
AP_Int16 throttle_behavior;
AP_Int16 takeoff_trigger_dz;
AP_Float pilot_takeoff_alt;
AP_Int16 rtl_altitude;
AP_Int16 rtl_speed_cms;
AP_Float sonar_gain;
AP_Int8 failsafe_battery_enabled; // battery failsafe enabled
AP_Float fs_batt_voltage; // battery voltage below which failsafe will be triggered
AP_Float fs_batt_mah; // battery capacity (in mah) below which failsafe will be triggered
AP_Int8 failsafe_gcs; // ground station failsafe behavior
AP_Int16 gps_hdop_good; // GPS Hdop value at or below this value represent a good position
AP_Int8 compass_enabled;
AP_Int8 super_simple;
AP_Int16 rtl_alt_final;
AP_Int16 rtl_climb_min; // rtl minimum climb in cm
AP_Int8 wp_yaw_behavior; // controls how the autopilot controls yaw during missions
AP_Int8 rc_feel_rp; // controls vehicle response to user input with 0 being extremely soft and 100 begin extremely crisp
AP_Int16 poshold_brake_rate; // PosHold flight mode's rotation rate during braking in deg/sec
AP_Int16 poshold_brake_angle_max; // PosHold flight mode's max lean angle during braking in centi-degrees
// Waypoints
//
AP_Int32 rtl_loiter_time;
AP_Int16 land_speed;
AP_Int16 pilot_velocity_z_max; // maximum vertical velocity the pilot may request
AP_Int16 pilot_accel_z; // vertical acceleration the pilot may request
// Throttle
//
AP_Int16 throttle_min;
AP_Int8 failsafe_throttle;
AP_Int16 failsafe_throttle_value;
AP_Int16 throttle_mid;
AP_Int16 throttle_deadzone;
// Flight modes
//
AP_Int8 flight_mode1;
AP_Int8 flight_mode2;
AP_Int8 flight_mode3;
AP_Int8 flight_mode4;
AP_Int8 flight_mode5;
AP_Int8 flight_mode6;
AP_Int8 simple_modes;
// Misc
//
AP_Int32 log_bitmask;
AP_Int8 esc_calibrate;
AP_Int8 radio_tuning;
AP_Int16 radio_tuning_high;
AP_Int16 radio_tuning_low;
AP_Int8 frame_orientation;
AP_Int8 ch7_option;
AP_Int8 ch8_option;
AP_Int8 ch9_option;
AP_Int8 ch10_option;
AP_Int8 ch11_option;
AP_Int8 ch12_option;
AP_Int8 arming_check;
AP_Int8 disarm_delay;
AP_Int8 land_repositioning;
AP_Int8 fs_ekf_action;
AP_Int8 fs_crash_check;
AP_Float fs_ekf_thresh;
AP_Int16 gcs_pid_mask;
#if FRAME_CONFIG == HELI_FRAME
// Heli
RC_Channel heli_servo_1, heli_servo_2, heli_servo_3, heli_servo_4; // servos for swash plate and tail
RC_Channel heli_servo_rsc; // servo for rotor speed control output
#endif
#if FRAME_CONFIG == SINGLE_FRAME
// Single
RC_Channel single_servo_1, single_servo_2, single_servo_3, single_servo_4; // servos for four flaps
#endif
#if FRAME_CONFIG == COAX_FRAME
// Coax copter flaps
RC_Channel single_servo_1, single_servo_2; // servos for two flaps
#endif
// RC channels
RC_Channel rc_1;
RC_Channel rc_2;
RC_Channel rc_3;
RC_Channel rc_4;
RC_Channel_aux rc_5;
RC_Channel_aux rc_6;
RC_Channel_aux rc_7;
RC_Channel_aux rc_8;
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
RC_Channel_aux rc_9;
#endif
RC_Channel_aux rc_10;
RC_Channel_aux rc_11;
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
RC_Channel_aux rc_12;
RC_Channel_aux rc_13;
RC_Channel_aux rc_14;
#endif
AP_Int16 rc_speed; // speed of fast RC Channels in Hz
// Acro parameters
AP_Float acro_rp_p;
AP_Float acro_yaw_p;
AP_Float acro_balance_roll;
AP_Float acro_balance_pitch;
AP_Int8 acro_trainer;
AP_Float acro_expo;
// PI/D controllers
#if FRAME_CONFIG == HELI_FRAME
AC_HELI_PID pid_rate_roll;
AC_HELI_PID pid_rate_pitch;
AC_HELI_PID pid_rate_yaw;
#else
AC_PID pid_rate_roll;
AC_PID pid_rate_pitch;
AC_PID pid_rate_yaw;
#endif
AC_PI_2D pi_vel_xy;
AC_P p_vel_z;
AC_PID pid_accel_z;
#if PRECISION_LANDING == ENABLED
AC_PI_2D pi_precland;
#endif
AC_P p_pos_xy;
AC_P p_stabilize_roll;
AC_P p_stabilize_pitch;
AC_P p_stabilize_yaw;
AC_P p_alt_hold;
// Autotune
AP_Int8 autotune_axis_bitmask;
AP_Float autotune_aggressiveness;
AP_Float autotune_min_d;
// Note: keep initializers here in the same order as they are declared
// above.
Parameters() :
#if FRAME_CONFIG == HELI_FRAME
heli_servo_1 (CH_1),
heli_servo_2 (CH_2),
heli_servo_3 (CH_3),
heli_servo_4 (CH_4),
heli_servo_rsc (CH_8),
#endif
#if FRAME_CONFIG == SINGLE_FRAME
single_servo_1 (CH_1),
single_servo_2 (CH_2),
single_servo_3 (CH_3),
single_servo_4 (CH_4),
#endif
#if FRAME_CONFIG == COAX_FRAME
single_servo_1 (CH_1),
single_servo_2 (CH_2),
#endif
rc_1 (CH_1),
rc_2 (CH_2),
rc_3 (CH_3),
rc_4 (CH_4),
rc_5 (CH_5),
rc_6 (CH_6),
rc_7 (CH_7),
rc_8 (CH_8),
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
rc_9 (CH_9),
#endif
rc_10 (CH_10),
rc_11 (CH_11),
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
rc_12 (CH_12),
rc_13 (CH_13),
rc_14 (CH_14),
#endif
// PID controller initial P initial I initial D initial imax initial filt hz pid rate
//---------------------------------------------------------------------------------------------------------------------------------
#if FRAME_CONFIG == HELI_FRAME
pid_rate_roll (RATE_ROLL_P, RATE_ROLL_I, RATE_ROLL_D, RATE_ROLL_IMAX, RATE_ROLL_FILT_HZ, MAIN_LOOP_SECONDS, RATE_ROLL_FF),
pid_rate_pitch (RATE_PITCH_P, RATE_PITCH_I, RATE_PITCH_D, RATE_PITCH_IMAX, RATE_PITCH_FILT_HZ, MAIN_LOOP_SECONDS, RATE_PITCH_FF),
pid_rate_yaw (RATE_YAW_P, RATE_YAW_I, RATE_YAW_D, RATE_YAW_IMAX, RATE_YAW_FILT_HZ, MAIN_LOOP_SECONDS, RATE_YAW_FF),
#else
pid_rate_roll (RATE_ROLL_P, RATE_ROLL_I, RATE_ROLL_D, RATE_ROLL_IMAX, RATE_ROLL_FILT_HZ, MAIN_LOOP_SECONDS),
pid_rate_pitch (RATE_PITCH_P, RATE_PITCH_I, RATE_PITCH_D, RATE_PITCH_IMAX, RATE_PITCH_FILT_HZ, MAIN_LOOP_SECONDS),
pid_rate_yaw (RATE_YAW_P, RATE_YAW_I, RATE_YAW_D, RATE_YAW_IMAX, RATE_YAW_FILT_HZ, MAIN_LOOP_SECONDS),
#endif
pi_vel_xy (VEL_XY_P, VEL_XY_I, VEL_XY_IMAX, VEL_XY_FILT_HZ, WPNAV_LOITER_UPDATE_TIME),
p_vel_z (VEL_Z_P),
pid_accel_z (ACCEL_Z_P, ACCEL_Z_I, ACCEL_Z_D, ACCEL_Z_IMAX, ACCEL_Z_FILT_HZ, MAIN_LOOP_SECONDS),
#if PRECISION_LANDING == ENABLED
pi_precland (PRECLAND_P, PRECLAND_I, PRECLAND_IMAX, VEL_XY_FILT_HZ, PRECLAND_UPDATE_TIME),
#endif
// P controller initial P
//----------------------------------------------------------------------
p_pos_xy (POS_XY_P),
p_stabilize_roll (STABILIZE_ROLL_P),
p_stabilize_pitch (STABILIZE_PITCH_P),
p_stabilize_yaw (STABILIZE_YAW_P),
p_alt_hold (ALT_HOLD_P)
{
}
};
extern const AP_Param::Info var_info[];
#endif // PARAMETERS_H

3
ArduSub/Parameters.pde Normal file
View File

@ -0,0 +1,3 @@
/*
blank file. This is needed to help with upgrades of old versions if MissionPlanner
*/

876
ArduSub/ReleaseNotes.txt Normal file
View File

@ -0,0 +1,876 @@
APM:Copter Release Notes:
------------------------------------------------------------------
Copter 3.3.2 01-Dec-2015 / 3.3.2-rc2 18-Nov-2015
Changes from 3.3.2-rc1
1) Bug fix for desired climb rate initialisation that could lead to drop when entering AltHold, Loiter, PosHold
2) Fix to hard landings when WPNAV_SPEED_DN set high in RTL, Auto (resolved by using non-feedforward alt hold)
3) Reduce Bad AHRS by filtering innovations
4) Allow arming without GPS if using Optical Flow
5) Smoother throttle output in Guided mode's velocity control (z-axis now 400hz)
------------------------------------------------------------------
Copter 3.3.2-rc1 4-Nov-2015
Changes from 3.3.1
1) Helicopter Improvements:
a) Fix Arming race condition
b) Fix servos to move after arming in Stabilize and Acro
c) Implement Pirouette Compensation
d) Add Rate I-Leak-Min functionality
e) Add new Stab Collective and Acro Expo Col functions
f) Add circular swashplate limits (Cyclic Ring)
g) Add new H_SV_Man functions
h) Add Hover Roll Trim function
i) Add Engine Run Enable Aux Channel function
j) Add servo boot test function
h) Add Disarm Delay parameter
------------------------------------------------------------------
Copter 3.3.1 26-Oct-2015 / 3.3.1-rc1 20-Oct-2015
Changes from 3.3
1) Bug fix to prevent potential crash if Follow-Me is used after an aborted takeoff
2) compiler upgraded to 4.9.3 (runs slightly faster than 4.7.2 which was used previously)
------------------------------------------------------------------
Copter 3.3 29-Sep-2015 / 3.3-rc12 22-Sep-2015
Changes from 3.3-rc11
1) EKF recovers from pre-arm "Compass variance" failure if compasses are consistent
------------------------------------------------------------------
Copter 3.3-rc11 10-Sep-2015
Changes from 3.3-rc10
1) PreArm "Need 3D Fix" message replaced with detailed reason from EKF
------------------------------------------------------------------
Copter 3.3-rc10 28-Aug-2015
Changes from 3.3-rc9
1) EKF improvements:
a) simpler optical flow takeoff check
2) Bug Fixes/Minor enhancements:
a) fix INS3_USE parameter eeprom location
b) fix SToRM32 serial protocol driver to work with recent versions
c) increase motor pwm->thrust conversion (aka MOT_THST_EXPO) to 0.65 (was 0.50)
d) Firmware version sent to GCS in AUTOPILOT_VERSION message
3) Safety:
a) pre-arm check of compass variance if arming in Loiter, PosHold, Guided
b) always check GPS before arming in Loiter (previously could be disabled if ARMING_CHECK=0)
c) sanity check locations received from GCS for follow-me, do-set-home, do-set-ROI
d) fix optical flow failsafe (was not always triggering LAND when optical flow failed)
e) failsafe RTL vs LAND decision based on hardcoded 5m from home check (previously used WPNAV_RADIUS parameter)
------------------------------------------------------------------
Copter 3.3-rc9 19-Aug-2015
Changes from 3.3-rc8
1) EKF improvements:
a) IMU weighting based on vibration levels (previously used accel clipping)
b) fix blended acceleration (used for altitude control) in cases where first IMU fails
c) ensure unhealthy barometer values are never consumed
2) TradHeli: remove acceleration feed forward
3) Safety:
a) check accel, gyro and baro are healthy when arming (previously was only checked pre-arm)
b) Guided mode velocity controller timeout (vehicle stops) after 3 seconds with no update from GCS
4) Minor enhancements:
a) fix for AUAV board's usb-connected detection
b) add Lidar-Lite-V2 support
c) MOT_THR_MIN_MAX param added to control prioritisation of throttle vs attitude during dynamic flight
d) RALLY_INCL_HOME param allows always including home when using rally points
e) DO_FLIGHT_TERMINATION message from GCS acts as kill switch
5) Bug Fixes:
a) fix to ensure motors start slowly on 2nd spin-up
b) fix RTL_CLIMB_MIN feature (vehicle climbed too high above home)
------------------------------------------------------------------
Copter 3.3-rc8 25-Jul-2015
Changes from 3.3-rc7
1) EKF improvements:
a) de-weight accelerometers that are clipping to improve resistance to high vibration
b) fix EKF to use primary compass instead of first compass (normally the same)
2) UBlox "HDOP" corrected to actually be hdop (was pdop) which leads to 40% lower value reported
3) TradHeli:
a) Motors library split into "Multicopter" and "TradHeli" so TradHeli does not see multicopter parameters
b) Heading target reset during landing to reduce vehicle fighting to rotate while on the ground
4) Minor enhancements:
a) SToRM32 gimbal can be connected to any serial port
b) log when baro, compass become unhealthy
c) ESC_CALIBRATION parameter can be set to "9" to disable esc calibration startup check
d) Circle rate adjustment with ch6 takes effect immediately
e) log home and origin
f) pre-arm check of battery voltage and fence
g) RTL_CLIMB_MIN parameter forces vehicle to climb at least this many cm when RTL is engaged (default is zero)
5) Bug fixes:
a) fix THR_MIN being incorrectly scaled as pwm value during low-throttle check
b) fence distance calculated from home (was incorrectly calculated from ekf-origin)
c) When flying with joystick and joystick is disconnected, control returns immediately to regular TX
d) dataflash's ATT yaw fixed to report heading as 0 ~ 360
e) fix to mission's first command being run multiple times during mission if it was a do-command
f) ekf-check is enabled only after ekf-origin is set (stops red-yellow flashing led when flying without GPS lock)
g) fix initialisation of mount's mode
h) start-up logging so parameters only logged once, mission always written
6) Linux:
a) bebop support
------------------------------------------------------------------
Copter 3.3-rc7 28-Jun-2015
Changes from 3.3-rc6
1) reduce EKF gyro bias uncertainty that caused attitude estimate errors
2) force 400hz IMU logging on (temporary for release candidate testing)
------------------------------------------------------------------
Copter 3.3-rc6 25-Jun-2015
Changes from 3.3-rc5
1) EKF related changes:
a) reset altitude even when arming without GPS lock
b) fix yaw twitch caused by EKF heading estimate reset
c) fix IMU time scaling bug that caused height estimate to deviate from the baro
2) AutoTune improvements:
a) improved yaw tuning by increasing yaw step magnitude
b) added logging of accelerations
c) improvements to step tests
3) Improved crash check:
a) allow triggering even if pilot doesn't move throttle to zero
b) easier triggering by removing baro check and using angle error instead of absolute tilt angle
4) TradHeli:
a) swash moves while landed in AltHold mode
b) improvements to land detector
c) fixed RSC Runup Time calculation
d) Rate FF Low-pass Filter changed from 5Hz to 10Hz, more responsive
5) support Linux builds for NAVIO+ and Erle-Brain (http://firmware.diydrones.com/Copter/beta/)
6) Other improvements / Bug Fixes:
a) sonar pre-arm checks only enforced if using optical flow
b) fix EKF failsafe bug that would not allow recovery
c) full rate IMU logging for improved vibration analysis (set LOG_BITMASK to All+FullIMU)
d) new VIBE dataflash message records vibration levels
e) default MNT_TYPE to "1" if servo gimbal rc outputs were defined
f) RC_FEEL defaults to medium
g) addition of SToRM32 serial support (supports mount angle feedback to GCS)
h) new tricopter's tail servo parameters (MOT_YAW_SV_MIN, MAX, TRIM, REV)
------------------------------------------------------------------
Copter 3.3-rc5 23-May-2015
Changes from 3.3-rc4
1) Fix AHRS bad gyro health message caused by timing jitter and log IMU health
2) TradHeli:
a) better default rate PIDs
b) Collective pitch output now operates even when disarmed
3) Small changes/fixes:
a) GCS can use MAV_CMD_MISSION_START to start mission in AUTO even without pilot raising throttle
b) GCS can force disarming even in flight by setting param2 to "21196"
c) rc-override timeout reduced from 2 seconds to 1 (applies when using GCS joysticks to control vehicle)
d) do-set-speed fix so it takes effect immediately during missions
e) GCS failsafe disarms vehicle if already landed (previously it could RTL)
------------------------------------------------------------------
Copter 3.3-rc4 17-May-2015
Changes from 3.3-rc3
1) AutoTune:
a) save roll, pitch, yaw rate acceleration limits along with gains
b) more conservative gains
2) Roll, pitch rate control feed-forward now on by default (set ATC_RATE_FF_ENAB to "0" to disable)
3) Serial ports increased to maximum of 4 (set SERIALX_PROTOCOL to 1)
4) MOT_THR_MIX_MIN param to control minimum throttle vs attitude during landing (higher = more attitude control but bumpier landing)
5) EKF fixes/improvements
a) prevent yaw errors during fast spins
b) bug fix preventing external selection of optical flow mode
6) Parachute:
a) servo/relay held open for 2sec when deploying (was 1sec)
b) fix altitude check to be alt-above-home (was alt-above ekf origin which could be slightly different)
7) TradHeli:
a) parameters moved to stop possibility of corruption if board is switched between tradheli and multicopter firmware. Heli users may need to re-setup some heli-specific params.
b) H_COLYAW param can be float
8) Small Improvements / Bug Fixes:
a) reduced spline overshoot after very long track followed by very short track
b) log entire mission to dataflash whenver it's uploaded
c) altitude reported if vehicle takes off before GPS lock
d) high speed logging of IMU
e) STOP flight mode renamed to BRAKE and aux switch option added
------------------------------------------------------------------
Copter 3.3-rc2/rc3 02-May-2015
Changes from 3.3-rc1
1) AutoTune reliability fixes (improved filtering to reduce noise interference)
2) Optical flow improvements:
a) Range Finder pre-arm check - lift vehicle between 50cm ~ 2m before arming. Can be disabled by setting ARMING_CHECK to "Skip Params/Sonar"
b) Vehicle altitude limited to range finder altitude when optical flow is enabled
3) AltHold & Take-off changes:
a) feed-forward controller and jerk limiting should result in slightly snappier performance and smoother take-offs
b) vehicle climbs automatically to PILOT_TKOFF_ALT alt when taking off in Loiter, AltHold, PosHold, Sport (disabled by default, pilot's throttle input overrides takeoff)
c) PILOT_THR_FILT allows enforcing smoother throttle response in manual flight modes (defaults to 0 = off)
d) TX with sprung throttle can set PILOT_THR_BHV to "1" so motor feedback when landed starts from mid-stick instead of bottom of stick
e) GCS can initiate takeoff even in Loiter, AltHold, PosHold and sport by issuing NAV_TAKEOFF mavlink command
4) Stop flight mode - causes vehicle to stop quickly, and does not respond to user input or waypoint movement via MAVLink. Requires GPS, will be renamed to Brake mode.
5) Aux channel features:
a) Emergency Stop - stops all motors immediately and disarms in 5 seconds
b) Motor Interlock - opposite of Emergency Stop, must be on to allow motors to spin motors, must be off to arm
6) Air pressure gain scaling (of roll, pitch, yaw) should mostly remove need to re-tune when flying at very different altitudes
7) landing detector simplified to only check vehicle is not accelerating & motors have hit their lower limit
8) Loiter tuning params to remove "freight train" stops:
raising WPNAV_LOIT_MAXA makes vehicle start and stop faster
raising WPNAV_LOIT_MINA makes vehicle stop more quickly when sticks centered
9) Other items:
a) faster EKF startup
b) Camera control messages sent via MAVLink to smart cameras. Allow control of camera zoom for upcoming IntelEdison/Sony QX1 camera control board
c) Lost Copter Alarm can be triggered by holding throttle down, roll right, pitch back
10) Bug fixes:
a) Home position set to latest arm position (it was being set to previous disarm location or first GPS lock position)
b) bug fix to mission Jump to command zero
------------------------------------------------------------------
Copter 3.3-rc1 11-Apr-2015
Changes from 3.2.1
1) Only support fast CPUs boards (Pixhawk, VRBrain, etc) and drop support for APM1, APM2 (sorry!)
2) AutoTune for yaw
3) Smooth throttle curve which should reduce wobbles during fast climbs and descents
4) ch7/ch8 aux switches expanded to ch9 ~ ch12 (see CH9_OPT ~ CH12_OPT params)
5) PX4Flow support in Loiter mode (still somewhat experimental)
6) Safety features:
a) EKF on by default replacing DCM/InertialNav which should improve robustness
b) increased accelerometer range from 8G to 16G to reduce chance of climb due to high vibrations (requires accel calibration)
7) Landing features:
a) improved landing on slopes
b) retractable landing gear (see LGR_ parameters)
8) Camera Gimbal features:
a) SToRM32 gimbal support (using MAVLink)
b) AlexMos gimbal support (using AlexMos serial interface)
c) do-mount-control commands supported in missions (allows controlling gimbal angle in missions)
9) Battery related features:
a) PID scaling for battery voltage (disabled by default, see MOT_THST_BAT_ parameters)
b) smart battery support
10) Other:
a) support do-set-home command (allows return-to-me and locked home position once GCS enhancements are completed)
b) performance improvements for Pixhawk reduce CPU load from 80% to 15%
c) firmware string name changed from ArduCopter to APM:Copter
------------------------------------------------------------------
ArduCopter 3.2.1 11-Feb-2015 / 3.2.1-rc2 30-Jan-2015
Changes from 3.2.1-rc1
1) Bug Fixes:
a) prevent infinite loop with linked jump commands
b) Pixhawk memory corruption fix when connecting via USB
c) vehicle stops at fence altitude limit in Loiter, AltHold, PosHold
d) protect against multiple arming messages from GCS causing silent gyro calibration failure
------------------------------------------------------------------
ArduCopter 3.2.1-rc1 08-Jan-2015
Changes from 3.2
1) Enhancements:
a) reduced twitch when passing Spline waypoints
b) Faster disarm after landing in Auto, Land, RTL
c) Pixhawk LED turns green before arming only after GPS HDOP falls below 2.3 (only in flight modes requiring GPS)
2) Safety Features:
a) Add desired descent rate check to reduce chance of false-positive on landing check
b) improved MPU6k health monitoring and re-configuration in case of in-flight failure
c) Rally point distance check reduced to 300m (reduces chance of RTL to far away forgotten Rally point)
d) auto-disarm if vehicle is landed for 15seconds even in Auto, Guided, RTL, Circle
e) fence breach while vehicle is landed causes vehicle to disarm (previously did RTL)
3) Bug Fixes:
a) Check flight mode even when arming from GCS (previously it was possible to arm in RTL mode if arming was initiated from GCS)
b) Send vehicle target destination in RTL, Guided (allows GCS to show where vehicle is flying to in these modes)
c) PosHold wind compensation fix
------------------------------------------------------------------
ArduCopter 3.2 07-Nov2014 / 3.2-rc14 31-Oct-2014
Changes from 3.2-rc13
1) Safety Features:
a) fail to arm if second gyro calibration fails (can be disabled with ARMING_CHECK)
2) Bug fixes:
a) DCM-check to require one continuous second of bad heading before triggering LAND
b) I2C bug that could lead to Pixhawk freezing up if I2C bus is noisy
c) reset DCM and EKF gyro bias estimates after gyro calibration (DCM heading could drift after takeoff due to sudden change in gyro values)
d) use primary GPS for LED status (instead of always using first GPS)
------------------------------------------------------------------
ArduCopter 3.2-rc13 23-Oct-2014
Changes from 3.2-rc12
1) DCM check triggers LAND if yaw disagrees with GPS by > 60deg (configure with DCM_CHECK_THRESH param) and in Loiter, PosHold, Auto, etc
2) Safety features:
a) landing detector checks baro climbrate between -1.5 ~ +1.5 m/s
b) sanity check AHRS_RP_P and AHRS_YAW_P are never less than 0.05
c) check set-mode requests from GCS are for this vehicle
3) Bug fixes:
a) fix ch6 tuning of wp-speed (was getting stuck at zero)
b) parachute servo set to off position on startup
c) Auto Takeoff timing bug fix that could cause severe lean on takeoff
d) timer fix for "slow start" of motors on Pixhawk (timer was incorrectly based on 100hz APM2 main loop speed)
4) reduced number of relays from 4 to 2 (saves memory and flash required on APM boards)
5) reduced number of range finders from 2 to 1 (saves memory and flash on APM boards)
6) allow logging from startup when LOG_BITMASK set to "All+DisarmedLogging"
------------------------------------------------------------------
ArduCopter 3.2-rc12 10-Oct-2014
Changes from 3.2-rc11
1) disable sonar on APM1 and TradHeli (APM1 & APM2) to allow code to fit
2) Add pre-arm and health check that gyro calibration succeeded
3) Bug fix to EKF reporting invalid position and velocity when switched on in flight with Ch7/Ch8 switch
------------------------------------------------------------------
ArduCopter 3.2-rc11 06-Oct-2014
Changes from 3.2-rc10
1) reduce lean on take-off in Auto by resetting horizontal position targets
2) TradHeli landing check ignores overall throttle output
3) reduce AHRS bad messages by delaying 20sec after init to allow EKF to settle (Pixhawk only)
4) Bug fixes:
a) fix THR_MIN scaling issue that could cause landing-detector to fail to detect landing when ch3 min~max > 1000 pwm
b) fix Mediatek GPS configuration so update rate is set correctly to 5hz
c) fix to Condition-Yaw mission command to support relative angles
d) EKF bug fixes when recovering from GPS glitches (affects only Pixhawks using EKF)
------------------------------------------------------------------
ArduCopter 3.2-rc10 24-Sep-2014
Changes from 3.2-rc9
1) two-stage land-detector to reduce motor run-up when landing in Loiter, PosHold, RTL, Auto
2) Allow passthrough from input to output of channels 9 ~ 14 (thanks Emile!)
3) Add 4hz filter to vertical velocity error during AltHold
4) Safety Feature:
a) increase Alt Disparity pre-arm check threshold to 2m (was 1m)
b) reset battery failsafe after disarming/arming (thanks AndKe!)
c) EKF only apply centrifugal corrections when GPS has at least 6 satellites (Pixhawk with EKF enabled only)
5) Bug fixes:
a) to default compass devid to zero when no compass connected
b) reduce motor run-up while landing in RTL
------------------------------------------------------------------
ArduCopter 3.2-rc9 11-Sep-2014
Changes from 3.2-rc8
1) FRAM bug fix that could stop Mission or Parameter changes from being saved (Pixhawk, VRBrain only)
------------------------------------------------------------------
ArduCopter 3.2-rc8 11-Sep-2014
Changes from 3.2-rc7
1) EKF reduced ripple to resolve copter motor pulsing
2) Default Param changes:
a) AltHold Rate P reduced from 6 to 5
b) AltHold Accel P reduced from 0.75 to 0.5, I from 1.5 to 1.0
c) EKF check threshold increased from 0.6 to 0.8 to reduce false positives
3) sensor health flags sent to GCS only after initialisation to remove false alerts
4) suppress bad terrain data alerts
5) Bug Fix:
a)PX4 dataflash RAM useage reduced to 8k so it works again
------------------------------------------------------------------
ArduCopter 3.2-rc7 04-Sep-2014
Changes from 3.2-rc6
1) Safety Items:
a) Landing check made more strict (climb rate requirement reduced to 30cm/s, overall throttle < 25%, rotation < 20deg/sec)
b) pre-arm check that accels are consistent (Pixhawk only, must be within 1m/sec/sec of each other)
c) pre-arm check that gyros are consistent (Pixhawk only, must be within 20deg/sec of each other)
d) report health of all accels and gyros (not just primary) to ground station
------------------------------------------------------------------
ArduCopter 3.2-rc6 31-Aug-2014
Changes from 3.2-rc5
1) Spline twitch when passing through a waypoint largely resolved
2) THR_DZ param added to allow user configuration of throttle deadzone during AltHold, Loiter, PosHold
3) Landing check made more strict (climb rate must be -40~40cm/s for 1 full second)
4) LAND_REPOSITION param default set to 1
5) TradHeli with flybar passes through pilot inputs directly to swash when in ACRO mode
6) Safety Items:
a) EKF check disabled when using inertial nav (caused too many false positives)
b) pre-arm check of internal vs external compass direction (must be within 45deg of each other)
7) Bug Fixes:
a) resolve NaN in angle targets when vehicle hits gimbal lock in ACRO mode
b) resolve GPS driver buffer overflow that could lead to missed GPS messages on Pixhawk/PX4 boards
c) resolve false "compass not calibrated" warnings on Pixhawk/PX4 caused by missing device id initialisation
------------------------------------------------------------------
ArduCopter 3.2-rc5 15-Aug-2014
Changes from 3.2-rc4
1) Pixhawk's max num waypoints increased to 718
2) Smoother take-off in AltHold, Loiter, PosHold (including removing initial 20cm jump when taking off)
3) ACRO mode roll, pitch, yaw EXPO added for faster rotation when sticks are at extremes (see ACRO_EXPO parameter)
4) ch7/ch8 relay option replaces ch6 option (ch6 is reserved for tuning not switching things on/off)
5) Safety Items:
a) Baro glitch check relaxed to 5m distance, 15m/s/s max acceleration
b) EKF/INav check relaxed to 80cm/s/s acceleration correct (default remains as 0.6 but this now means 80cm/s/s)
c) When GPS or Baro glitch clears, the inertial nav velocities are *not* reset reducing chance of sudden vehicle lean
d) Baro altitude calculation checked for divide-by-zero and infinity
6) Bug Fixes:
a) AltHold jump bug fixed (altitude target reset when landed)
b) Rally point bug fix so it does not climb excessively before flying to rally point
c) body-frame rate controller z-axis bug fix (fast rotation in z-axis would cause wobble in roll, pitch)
------------------------------------------------------------------
ArduCopter 3.2-rc4 01-Aug-2014
Changes from 3.2-rc3
1) Pre-takeoff throttle feedback in AltHold, Loiter, PosHold
2) Terrain altitude retrieval from ground station (informational purposes only so far, Pixhawk only)
3) Safety Items:
a) "EKF check" will switch to LAND mode if EKF's compass or velocity variance over 0.6 (configurable with EKFCHECK_THRESH param)
When EKF is not used inertial nav's accelerometer corrections are used as a substitute
b) Barometer glitch protection added. BAROGLTCH_DIST and BAROGLTCH_ACCEL parameters control sensitivity similar to GPSGLITCH protection
When glitching occurs barometer values are temporarily ignored
c) Throttle/radio and battery failsafes now disarm vehicle when landed regardless of pilot's throttle position
d) auto-disarm extended to Drift, Sport and OF_Loiter flight modes
e) APM2 buzzer notification added for arming failure
f) APM2 arming buzz made longer (now matches Pixhawk)
g) do-set-servo commands cannot interfere with motor output
4) Bug Fixes:
a) Drift slow yaw response fixed
b) AC3.2-rc3 failsafe bug resolved. In -rc3 the throttle failsafe could be triggered even when disabled or motors armed (although vehicle would not takeoff)
------------------------------------------------------------------
ArduCopter 3.2-rc3 16-Jul-2014
Changes from 3.2-rc2
1) Hybrid renamed to PosHold
2) Sonar (analog, i2c) and PulsedLight Range Finders enabled on Pixhawk (Allyson, Tridge)
3) Landing changes:
a) disable pilot repositioning while landing in RTL, Auto (set LAND_REPOSITION to 1 to re-enable) (JonathanC)
b) delay 4 seconds before landing due to failsafe (JonathanC)
4) Secondary compass calibration enabled, pre-arm check that offsets match current devices (Randy, Tridge, MichaelO)
5) Control improvements:
a) use bias adjusted gyro rates - helps in cases of severe gyro drift (Jonathan)
b) bug-fixes when feed-forward turned off (Leonard)
6) TradHeli improvements (RobL):
a) bug fix to use full collective range in stabilize and acro flight modes
b) filter added to main rotor input (ch8) to ensure momentary blip doesn't affect main rotor speed
7) Safety items:
a) increased default circular Fence radius to 300m to reduce chance of breach when GPS lock first acquired
b) radio failsafe timeout for late frames reduced to 0.5sec for normal receivers or 2.0sec when flying with joystick (Craig)
c) accelerometer pre-arm check for all active accelerometers (previously only checked the primary accelerometer)
8) Other features:
a) ch7/ch8 option to retract mount (svefro)
b) Do-Set-ROI supported in Guided, RTL mode
c) Condition-Yaw accepted in Guided, RTL modes (MoussSS)
d) CAMERA dataflash message includes relative and absolute altitude (Craig)
9) Red Balloon Popper support (Randy, Leonard):
a) Velocity controller added to Guided mode
b) NAV_GUIDED mission command added
10) Bug fixes:
a) bug fix to flip on take-off in stabilize mode when landing flag cleared slowly (JonathanC)
b) allow disarming in AutoTune (JonathanC)
c) bug fix to unpredictable behaviour when two spline points placed directly ontop of each other
------------------------------------------------------------------
ArduCopter 3.2-rc2 27-May-2014
Changes from 3.2-rc1
1) Hybrid mode initialisation bug fix
2) Throttle pulsing bug fix on Pixhawk
3) Parachute enabled on Pixhawk
4) Rally Points enabled on Pixhawk
------------------------------------------------------------------
ArduCopter 3.2-rc1 9-May-2014
Changes from 3.1.4
1) Hybrid mode - position hold mode but with direct response to pilot input during repositioning (JulienD, SandroT)
2) Spline waypoints (created by David Dewey, modified and integrated by Leonard, Randy)
3) Drift mode uses "throttle assist" for altitude control (Jason)
4) Extended Kalman Filter for potentially more reliable attitude and position control (Pixhawk only) (Paul Riseborough). Set AHRS_EKF_USE to 1 to enable or use Ch7/8 switch to enable/disable in flight.
5) Manual flight smoothness:
a) Smoother roll, pitch response using RC_FEEL_RP parameter (100 = crisp, 0 = extremely soft)
b) Adjustable max rotation rate (ATC_RATE_RP_MAX, ATC_RATE_Y_MAX) and acceleration (ATC_ACCEL_RP_MAX, ATC_ACCEL_Y_MAX)
6) Autopilot smoothness:
a) Vertical acceleration in AltHold, Loiter, Hybrid modes can be configured with PILOT_ACCEL_Z parameter (higher = faster acceleration)
b) Maximum roll and pitch angle acceleration in Loiter mode can be configured with WPNAV_LOIT_JERK (higher = more responsive but potentially jerky)
c) Yaw speed can be adjusted with ATC_SLEW_YAW parameter (higher = faster)
d) smoother takeoff with configurable acceleration using WPNAV_ACCEL_Z parameter
e) Twitches removed during guided mode or when entering Loiter or RTL from high speeds
7) Mission improvements:
a) mission will be resumed from last active command when pilot switches out and then back into Auto mode (prev behaviour can be restored by setting MIS_RESTART param to 1)
b) DO_SET_ROI persistent across waypoints. All-zero DO_SET_ROI command restores default yaw behaviour
c) do-jump fixed
d) conditional_distance fixed
e) conditional_delay fixed
f) do-change-speed command takes effect immediately during mission
g) vehicle faces directly at next waypoint (previously it could be about 10deg off)
h) loiter turns fix to ensure it will circle around lat/lon point specified in mission command (previously it could be off by CIRCLE_RADIUS)
8) Safety improvements:
a) After a fence breach, if the pilot re-takes control he/she will be given a minimum of 10 seconds and 20m to recover before the autopilot will invoke RTL or LAND
b) Parachute support including automatic deployment during mechanical failures
9) Other enhancements:
a) V-tail quad support
b) Dual GPS support (secondary GPS output is simply logged, not actually used yet)
c) Electro Permanent Magnet (aka Gripper) support
d) Servo pass through for channels 6 ~ 8 (set RC6_FUNCTION to 1)
e) Remove 80m limit on RTL's return altitude but never let it be above fence's max altitude
10) Other bug fixes:
a) Bug fix for LAND sometimes getting stuck at 10m
b) During missions, vehicle will maintain altitude even if WPNAV_SPEED is set above the vehicle's capabilities
c) when autopilot controls throttle (i.e. Loiter, Auto, etc) vehicle will reach speeds specified in PILOT_VELZ_MAX and WPNAV_SPEED_UP, WPNAV_SPEED_DN parameters
11) CLI removed from APM1/2 to save flash space, critical functions moved to MAVLink:
a) Individual motor tests (see MP's Initial Setup > Optional Hardware > Motor Test)
b) compassmot (see MP's Initial Setup > Optional Hardware > Compass/Motor Calib)
c) parameter reset to factory defautls (see MP's Config/Tuning > Full Parameter List > Reset to Default)
------------------------------------------------------------------
ArduCopter 3.1.5 27-May-2014 / 3.1.5-rc2 20-May-2014
Changes from 3.1.5-rc1
1) Bug Fix to broken loiter (pixhawk only)
2) Workaround to read from FRAM in 128byte chunks to resolve a few users boot issues (Pixhawk only)
------------------------------------------------------------------
ArduCopter 3.1.5-rc1 14-May-2014
Changes from 3.1.4
1) Bug Fix to ignore roll and pitch inputs to loiter controller when in radio failsafe
2) Bug Fix to allow compassmot to work on Pixhawk
------------------------------------------------------------------
ArduCopter 3.1.4 8-May-2014 / 3.1.4-rc1 2-May-2014
Changes from 3.1.3
1) Bug Fix for Pixhawk/PX4 NuttX I2C memory corruption when errors are found on I2C bus
------------------------------------------------------------------
ArduCopter 3.1.3 7-Apr-2014
Changes from 3.1.2
1) Stability patch fix which could cause motors to go to min at full throttle and with large roll/pitch inputs
------------------------------------------------------------------
ArduCopter 3.1.2 13-Feb-2014 / ArduCopter 3.1.2-rc2 12-Feb-2014
Changes from 3.1.2-rc1
1) GPS Glitch detection disabled when connected via USB
2) RC_FEEL_RP param added for adjusting responsiveness to pilot roll/pitch input in Stabilize, Drift, AltHold modes
------------------------------------------------------------------
ArduCopter 3.1.2-rc1 30-Jan-2014
Changes from 3.1.1
1) Pixhawk baro bug fix to SPI communication which could cause large altitude estimate jumps at high temperatures
------------------------------------------------------------------
ArduCopter 3.1.1 26-Jan-2014 / ArduCopter 3.1.1-rc2 21-Jan-2014
Changes from 3.1.1-rc1
1) Pixhawk improvements (available for APM2 when AC3.2 is released):
a) Faster arming
b) AHRS_TRIM fix - reduces movement in loiter when yawing
c) AUX Out 5 & 6 turned into general purpose I/O pins
d) Three more relays added (relays are pins that can be set to 0V or 5V)
e) do-set-servo fix to allow servos to be controlled from ground station
f) Motorsync CLI test
g) PX4 parameters moved from SD card to eeprom
h) additional pre-arm checks for baro & inertial nav altitude and lean angle
------------------------------------------------------------------
ArduCopter 3.1.1-rc1 14-Jan-2014
Changes from 3.1
1) Pixhawk improvements:
a) Telemetry port 2 enabled (for MinimOSD)
b) SD card reliability improvements
c) parameters moved to FRAM
d) faster parameter loading via USB
e) Futaba SBUS receiver support
2) Bug fixes:
a) Loiter initialisation fix (Loiter would act like AltHold until flight mode switch changed position)
b) ROI commands were not returning Lat, Lon, Alt to mission planner when mission was loaded from APM
3) TradHeli only fixes:
a) Drift now uses same (reduced) collective range as stabilize mode
b) AutoTune disabled (for tradheli only)
c) Landing collective (smaller than normal collective) used whenever copter is not moving
------------------------------------------------------------------
ArduCopter 3.1 14-Dec-2013
Changes from 3.1-rc8
1) Pixhawk improvements:
a) switch to use MPU6k as main accel/gyro
b) auto loading of IO-board firmware on startup
2) RTL fixes:
a) initialise waypoint leash length (first RTL stop would be more aggressive than 2nd)
b) reduce projected stopping distance for higher speed stops
------------------------------------------------------------------
ArduCopter 3.1-rc8 9-Dec-2013
Changes from 3.1-rc7
1) add Y6 motor mapping with all top props CW, bottom pros CCW (set FRAME = 10)
2) Safety Changes:
a) ignore yaw input during radio failsafe (previously the copter could return home spinning if yaw was full over at time of failsafe)
b) Reduce GPSGLITCH_RADIUS to 2m (was 5m) to catch glitches faster
3) Bug fixes:
a) Optical flow SPI bus rates
b) TradHeli main rotor ramp up speed fix
------------------------------------------------------------------
ArduCopter 3.1-rc7 22-Nov-2013
Changes from 3.1-rc6
1) MOT_SPIN_ARMED default to 70
2) Smoother inertial nav response to missed GPS messages
3) Safety related changes
a) radio and battery failsafe disarm copter if landed in Loiter or AltHold (previously they would RTL)
b) Pre-Arm check failure warning output to ground station every 30 seconds until they pass
c) INS and inertial nav errors logged to dataflash's PM message
d) pre-arm check for ACRO_BAL_ROLL, ACRO_BAL_PITCH
------------------------------------------------------------------
ArduCopter 3.1-rc6 16-Nov-2013
Improvements over 3.1-rc5
1) Heli improvements:
a) support for direct drive tails (uses TAIL_TYPE and TAIL_SPEED parameters)
b) smooth main rotor ramp-up for those without external govenor (RSC_RAMP_TIME)
c) internal estimate of rotor speed configurable with RSC_RUNUP_TIME parameter to ensure rotor at top speed before starting missions
d) LAND_COL_MIN collective position used when landed (reduces chance copter will push too hard into the ground when landing or before starting missions)
e) reduced collective while in stabilize mode (STAB_COL_MIN, STAB_COL_MAX) for more precise throttle control
f) external gyro parameter range changed from 1000~2000 to 0~1000 (more consistent with other parameters)
g) dynamic flight detector switches on/off leaky-i term depending on copter speed
2) SingleCopter airframe support (contribution from Bill King)
3) Drift mode replaces TOY
4) MPU6k SPI bus speed decreased to 500khz after 4 errors
5) Safety related changes:
a) crash detector cuts motors if copter upside down for more than 2 seconds
b) INS (accel and gyro) health check in pre-arm checks
c) ARMING_CHECK allows turning on/off individual checks for baro, GPS, compass, parameters, board voltage, radio
d) detect Ublox GPS running at less than 5hz and resend configuration
e) GPSGlitch acceptable radius reduced to 5m (stricter detection of glitches)
f) range check roll, pitch input to ensure crazy radio values don't get through to stabilize controller
g) GPS failsafe options to trigger AltHold instead of LAND or to trigger LAND even if in flight mode that does not require GPS
h) Battery failsafe option to trigger RTL instead of LAND
i) MOT_SPIN_ARMED set to zero by default
6) Bug fixes:
a) missing throttle controller initialisation would mean Stabilize mode's throttle could be non-tilt-compensated
b) inertial nav baro and gps delay compensation fix (contribution from Neurocopter)
c) GPS failsafe was invoking LAND mode which still used GPS for horizontal control
------------------------------------------------------------------
ArduCopter 3.1-rc5 22-Oct-2013
Improvements over 3.1-rc4
1) Pixhawk USB reliability improvements
2) AutoTune changes:
a) enabled by default
b) status output to GCS
c) use 2 pos switch only
3) ch7/ch8 LAND
4) Tricopter stability patch improvements [thanks to texlan]
5) safety improvements:
a) slower speed up of motors after arming
b) pre-arm check that copter is moving less than 50cm/s if arming in Loiter or fence enabled
------------------------------------------------------------------
ArduCopter 3.1-rc4 13-Oct-2013
Improvements over 3.1-rc3
1) Performance improvements to resolve APM alt hold issues for Octacopters:
a) SPI bus speed increased from 500khz to 8Mhz
b) Telemetry buffer increased to 512bytes
c) broke up medium and slow loops into individual scheduled tasks and increased priority of alt hold tasks
2) Bug fix for Pixhawk USB connection
3) GPS Glitch improvements:
a) added GPS glitch check to arming check
b) parameters for vehicle max acceleration (GPSGLITCH_ACCEL) and always-ok radius (GPSGLICH_RADIUS)
------------------------------------------------------------------
ArduCopter 3.1-rc3 09-Oct-2013
Improvements over 3.1-rc2
1) GPS Glitch protection - gps positions are compared with the previous gps position. Position accepted if within 10m or copter could have reached the position with max accel of 10m/s/s.
2) Bug fix for pixhawk SPI bus contention that could lead to corrupted accelerometer values on pixhawk resolved
3) Auto Tuning (compile time option only add "#define AUTOTUNE ENABLED" to APM_Config.h and set CH7_Opt or CH8_Opt parameter to 17)
4) CPU Performance improvement when reading from MPU6k for APM
5) SUPER_SIMPLE parameter changed to a bit map to allow some flight modes to use super simpler while others use regular simple (MP release to allow easier selection will go out with AC3.1 official release)
6) Safety changes:
a) safety button must be pushed before arming on pixhawk
b) RGB LED (aka toshiba led) changed so that disarmed flashes, armed is either blue (if no gps lock) or green (if gps lock)
c) sensor health bitmask sent to groundstations
------------------------------------------------------------------
ArduCopter 3.1-rc2 18-Sep-2013
Improvements over 3.1-rc1
1) bug fix for MOT_SPIN_ARMED to allow it to be higher than 127
2) PX4/pixhawk auto-detect internal/external compass so COMPASS_ORIENT should be set to ORIENTATION_NONE if using GPS+compass module
------------------------------------------------------------------
ArduCopter 3.1-rc1 9-Sep-2013
Improvements over 3.0.1
1) Support for Pixhawks board
2) Arm, Disarm, Land and Takeoff in Loiter and AltHold
3) Improved Acro
a) ACRO_RP_P, ACRO_YAW_P parameters added to control speed of rotation
b) ACRO_BAL_ROLL, ACRO_BAL_PITCH controls speed at which copter returns to level
c) ACRO_TRAINER can be set to 0:disable trainer, 1:auto leveling when sticks released, 2:auto leveling and lean angle limited to ANGLE_MAX
d) Ch7 & Ch8 switch to set ACRO_TRAINER options in-flight
4) SPORT mode - equivalent of earth frame Acro with support for simple mode
5) Sonar ground tracking improvements and bug fixes that reduce reaction to bad sonar data
6) Safety improvements
a) motors always spin when armed (speed can be controlled with MOT_SPIN_ARMED, set to 0 to disable)
b) vehicle's maximum lean angle can be reduced (or increased) with the ANGLE_MAX parameter
c) arming check that GPS hdop is > 2.0 (disable by setting GPS_HDOP parameter to 900)
d) slow take-off in AUTO, LOITER, ALTHOLD to reduce chance of motor/esc burn-out on large copters
7) Bug fixes:
a) Optical flow sensor initialisation fix
b) altitude control fix for Loiter_turns mission command (i.e. mission version of CIRCLE mode)
c) DO_SET_ROI fix (do not use "ROI")
8) Distribute Loiter & Navigation calcs over 4 cycles to reduce impact on a single 100hz loop
9) RCMAP_ parameters allow remapping input channels 1 ~ 4
------------------------------------------------------------------
ArduCopter 3.0.1-rc2 / 3.0.1 11-Jul-2013
Improvements over 3.0.1-rc1
1) Rate Roll, Pitch and Yaw I fix when we hit motor limits
2) pre-arm check changes:
a) double flash arming light when pre-arm checks fail
b) relax mag field checks to 35% min, 165% max of expected field
3) loiter and auto changes:
a) reduced Loiter speed to 5 m/s
b) reduced WP_ACCEL to 1 m/s/s (was 2.5 m/s/s)
c) rounding error fix in loiter controller
d) bug fix to stopping point calculation for RTL and loiter during missions
4) Stability Patch fix which was freezing Rate Taw I term and allowing uncommanded Yaw
------------------------------------------------------------------
ArduCopter 3.0.1-rc1 26-Jun-2013
Improvements over 3.0.0
1) bug fix to Fence checking position after GPS lock was lost
2) bug fix to LAND so that it does not attempt to maintain horizontal position without GPS lock
------------------------------------------------------------------
ArduCopter 3.0.0 / 3.0.0-rc6 16-Jun-2013
Improvements over 3.0.0-rc5
1) bug fix to Circle mode's start position (was moving to last loiter target)
2) WP_ACCEL parameter added to allow user to adjust acceleration during missions
3) loiter acceleration set to half of LOIT_SPEED parameter value (was hard-coded)
4) reduce AltHold P to 1.0 (was 2.0)
------------------------------------------------------------------
ArduCopter 3.0.0-rc5 04-Jun-2013
Improvements over 3.0.0-rc4
1) bug fix to LAND flight mode in which it could try to fly to mission's next waypoint location
2) bug fix to Circle mode to allow counter-clockwise rotation
3) bug fix to heading change in Loiter, RTL, Missions when pilot's throttle is zero
4) bug fix for mission sticking at take-off command when pilot's throttle is zero
5) bug fix for parameters not saving when new value is same as default value
6) reduce pre-arm board min voltage check to 4.3V (was 4.5V)
7) remove throttle controller's ability to limit lean angle in loiter, rtl, auto
------------------------------------------------------------------
ArduCopter 3.0.0-rc4 02-Jun-2013
Improvements over 3.0.0-rc3
1) loiter improvements:
i) repositioning enhanced with feed forward
ii) use tan to convert desired accel to lean angle
2) stability patch improvements for high powered copters or those with roll-pitch rate gains set too high
3) auto mode vertical speed fix (it was not reaching the desired speeds)
4) alt hold smoothed by filtering feed forward input
5) circle mode fix to initial position and smoother initialisation
6) RTL returns to initial yaw heading before descending
7) safe features:
i) check for gps lock when entering failsafe
ii) pre-arm check for mag field lenght
iii) pre-arm check for board voltage between 4.5v ~ 5.8V
iv) beep twice during arming
v) GPS failsafe enabled by default (will LAND if loses GPS in Loiter, AUTO, Guided modes)
vi) bug fix for alt-hold mode spinning motors before pilot has raised throttle
8) bug fixes:
i) fixed position mode so it responding to pilot input
ii) baro cli test
iii) moved cli motor test to test sub menu and minor change to throttle output
iv) guided mode yaw control fix
------------------------------------------------------------------
ArduCopter 3.0.0-rc3 22-May-2013
Improvements over 3.0.0-rc2
1) bug fix for dataflash erasing unnecessarily
2) smoother transition to waypoints, loiter:
intermediate point's speed initialised from copter's current speed
3) Ch8 auxiliary function switch (same features as Ch7)
4) safety checks:
Warning to GCS of reason for pre-arm check failure
ARMING_CHECK parameter added to allow disabling pre-arm checks
Added compass health and offset check to pre-arm check
compassmot procedure displays interference as percentage of total mag field
5) WPNAV dataflash message combined into NTUN message
6) allow TriCopters to use ESC calibration
------------------------------------------------------------------
ArduCopter 3.0.0-rc2 13-May-2013
Improvements over 3.0.0-rc1:
1) smoother transition to waypoints, loiter:
reduced loiter max acceleration to smooth waypoints
bug fix to uninitialised roll/pitch when entering RTL, AUTO, LOITER
2) fast waypoints - copter does not stop at waypoints unless delay is specified
3) WPNAV_LOIT_SPEED added to allow faster/slower loiter repositioning
4) removed speed limits on auto missions
5) enhance LAND mission command takes lat/lon coordinates
6) bug fix for RTL not pointing home sometimes
7) centrifugal correction disabled when copter is disarmed to stop HUD moving
8) centrifugal correction disabled when sat count less than 6 (AHRS_GPS_MINSATS)
9) compass calibration reliability improvements when run from mission planner
10) bug fix to allow compassmot to be run from mission planner terminal screen
11) add support for H-quad frame
12) add COMPASS_ORIENT parameter to support external compass in any orientation
------------------------------------------------------------------
ArduCopter 3.0.0-rc1 01-May-2013
Improvements over 2.9.1b:
1) Inertial navigation for X & Y axis (Randy/Leonard/Jonathan)
2) 3D waypoint navigation library (Leonard/Randy)
WPNAV_SPEED, WPNAV_SPEED_UP, WPNAV_SPEED_DN control target speeds during missions and RTL
WP_YAW_BEHAVIOR to allow disabling yaw during missions and RTL
3) PX4 support (some features still not available) (Tridge/Pat/PX4Dev Team)
4) Safety improvements:
Tin-can shaped fence (set FENCE_ENABLED to 1 and copter will RTL if alt > 150m or horizontal distance from home > 300m) (Randy/Tridge/Leonard)
GCS failsafe (set FS_GCS_ENABLED to 1 and if you are using a tablet to fly your copter it will RTL and return control to the radio 3 seconds after losing telemetry) (Randy)
pre-arm checks to ensure accelerometer and radio calibration has been performed before arming (Randy)
5) motor interference compensation for compass (Jonathan/Randy)
6) Circle mode improvements:
set CIRCLE_RADIUS to zero to do panorama shots in circle mode (copter does not move in a circle but instead slowly rotates)
CIRCLE_RATE parameter allows controlling direction and speed of rotation in CIRCLE mode and LOITER_TURNS (can also be adjusted in flight from CH6 knob)
7) SONAR_GAIN parameter add to allow reducing the response to objects sensed by sonar (Randy)
8) support for trapezoidal quads (aka V shaped or FPV quads) (Leonard/Craig)
9) performance improvements to dataflash logging (Tridge)
10) bug-fix to analog read which could cause bad sonar reads when using voltage or current monitor (Tridge)
11) bug-fix to motors going to minimum when throttle is low while switching into Loiter, AUTO, RTL, ALT_HOLD (Jason/Randy)
12) bug-fix for auto disarm sometimes disarming shortly after arming (Jason/SirAlex)
------------------------------------------------------------------
ArduCopter 2.9.1b 30-Feb-2013
Improvements over 2.9.1:
1) reduce INS_MPU6K_FILTER to 20hz
2) reduce InertialNav Z-axis time constant to 5 (was 7)
3) increased max InertialNav accel correction to 3 m/s (was 1m/s)
4) bug fix for alt_hold being passed as int16_t to get_throttle_althold_with_slew
5) bug fix for throttle after acro flip (was being kept at min throttle if pilot switched out of ACRO mode while inverted)
6) reduce yaw_rate P default to 0.20 (was 0.25)
------------------------------------------------------------------
ArduCopter 2.9.1 & 2.9.1-rc2 01-Feb-2013
Improvements over 2.9.1-rc1:
1) small corretion to use of THR_MID to scale lower end of manual throttle between THR_MIN and 500 instead of 0 and 500
2) bug fix for longitude scaling being incorrectly calculated using Next Waypoint instead of home which could lead to scaling being 1
3) ESC calibration change to set update rate to ESCs to 50hz to allow simonk ESC to be calibrated
------------------------------------------------------------------
ArduCopter 2.9.1-rc1 31-Jan-2013
Improvements over 2.9:
1) THR_MID parameter added to allow users to adjust the manual throttle so that vehicle hovers at about mid stick
2) bug fix for autotrim - roll axis was backwards
3) bug fix to set sonar_alt_health to zero when sonar is disabled
4) capture level roll and pitch trims as part of accel calibration
5) bug fix to ppm encoder false positives
------------------------------------------------------------------
ArduCopter 2.9 & 2.9-rc5 14-Jan-2013
Improvements over 2.9-rc4:
1) add constraint to loiter commanded roll and pitch angles
2) relax altitude requirement for take-off command to complete
------------------------------------------------------------------
ArduCopter 2.9-rc4 12-Jan-2013
Improvements over 2.9-rc3:
1) Smoother transition between manual and auto flight modes (Leonard)
2) bug fix for LAND not actually landing when initiated from failsafe (Randy/Craig)
------------------------------------------------------------------
ArduCopter 2.9-rc3 11-Jan-2013
Improvements over 2.9-rc2:
1) alt hold with sonar improvements - now on by default (Leonard/Randy)
2) performance and memory useage improvements (Tridge/Randy)
3) increase APM1 baro pressure read from 5hz to 8.3hz to improve alt hold (Randy)
4) bug fix: altitude error reported to GCS (Randy)
5) limit inertial nav's max accel offset correction to 100cm/s/s to speed up recovery after hard impacts (Randy)_
6) moved rate controllers to run after ins read (Tridge/Randy)
------------------------------------------------------------------
ArduCopter 2.9-rc2 31-Dec-2012
Improvements over 2.9-rc1:
1) increased throttle rate gains from 1.0 to 6.0
2) APM1 fix so it works with inertial nav (5hz update rate of baro was beyond the tolerance set in the inav library)
------------------------------------------------------------------
ArduCopter 2.9-rc1 23-Dec-2012
Improvements over 2.8.1:
1) altitude hold improvements:
a)inertial navigation for vertical axis [Randy/Jonathan/Leonard/Jason]
b)accel based throttle controller [Leonard/Randy]
c)accelerometer calibration routine updated to use gauss-newton method [Randy/Tridge/Rolfe Schmidt]
d)parameters to control climb rate:
AUTO_VELZ_MIN, AUTO_VELZ_MAX - allows you to control the maximum climb and descent rates of the autopilot (in cm/s)
PILOT_VELZ_MAX - allows you to control the maximum climb/descent rate when in alt hold or loiter (in cm/s)
2) landing improvements [Leonard/Randy]
LAND_SPEED - allows you to set the landing speed in cm/s
3) camera related improvements:
a) AP_Relay enabled for APM2 and integrated with AP_Camera [Sandro Benigno]
b) camera trigger with channel 7 switch or DO_DIGICAM_CONTROL mission command [Randy]
c) allow yaw override by pilot or with CONDITIONAL_YAW command during missions [Randy]
YAW_OVR_BEHAVE - Controls when autopilot takes back normal control of yaw after pilot overrides (0=after next wp, 1=never)
4) trad heli improvements [Rob]
a) code tested and brought back into the fold (2.8.1 was never released for trad helis)
b) enabled rate controller (previously only used angle controllers)
c) fix to rotor speed controllers - now operates by switching off channel 8
d) allow wider collective pitch range in acro and alt hold modes vs stabilize mode
e) removed angle boost function because it created more problems than it solved
f) bug fix to allow collective pitch to use the entire range of servos
5) mediatek gps driver improvements [Craig]
a) added support for 1.9 firmware
b) bug fix to start-up routine so sbas can be enabled
6) failsafe improvements (both throttle and battery) [Randy/Craig/John Arne Birkeland]
a) RTL will not trigger if your throttle is zero - reduces risk of accidentally invoking RTL if you switch off your receiver before disarming
b) failsafe triggered in unlikely case of a PPM encoder failure
c) bug fix to resolve motors momentarily reducing to zero after failsafe is triggered
7) mpu6k filtering made configurable and default changed to 42hz for copters [Leonard/Tridge]
8) support ppm sum for transmitters with as few as 5 channels [Randy/John Arne Birkeland]
9) acro trainer - copter will return to be generally upright if you release the sticks in acro mode [Leonard]
ACRO_BAL_ROLL, ACRO_BAL_PITCH - controls rate at which roll returns to level
ACRO_TRAINER - 1 to enable the auto-bring-upright feature
10) other changes and bug fixes:
a) allow >45 degrees when in stabilize mode by adding lines like below to APM_Config (compile time option only) [Jason]
#define MAX_INPUT_ROLL_ANGLE 6000 // 60 degrees
#define MAX_INPUT_PITCH_ANGLE 6000 // 60 degrees
b) bug fix to stop RTL from ever climbing to an unreasonable height (i.e. >80m) [Jason]
c) event and state logging [Jason]
d) allow cli to be used over telemetry link [Tridge]
e) bug fix to allow compass accumulate to run when we have spare cpu cycles [Randy]
f) bug fix so do_set_servo command works [Randy]
g) bug fix to PID controller's so they don't calculate crazy D term on the first call [Tridge]
h) bug fix to initialise navigation parameter to resolve twitch when entering some navigation modes [Jason]
i) performance improvement to copter leds - use DigitalFastWrite and DigitalFastRead instead of native arduino functions [Randy]
j) removed unused stab_d from roll and pitch controller [Jason]
k) bug fix for guided mode not reaching target altitude if it reaches horizontal target first [Randy]
l) code clean-up, parameter documentation improvements [Randy/Jason/Rob/others]
------------------------------------------------------------------
ArduCopter 2.8.1 22-Oct-2012
Improvements over 2.8:
- 430 bytes of RAM freed up to resolve APM1 level issue and reduce chance of memory corruption on both APM1 and APM2
Improvements over 2.7.3:
- Improved ACRO mode (Leonard Hall)
- Improved stability patch to reduce "climb-on-yaw" problem (Leonard, Rob Lefebvre, Randy)
- Rate controller targets moved to body frames (yaw control now works properly when copter is inverted) (Leonard/Randy)
- Less bouncy Stabilize yaw control (Leonard)
- OpticalFlow sensor support for APM2.5 (Randy)
- DMP works again by adding "#define DMP_ENABLED ENABLED" to APM_Config.h You can also log DMP vs DCM to the dataflash by adding "#define SECONDARY_DMP_ENABLED ENABLED" (Randy)
- Watch dog added to shutdown motors if main loop feezes for 2 seconds (Randy)
- Thrust curve added to linearize pwm->thrust. Removes deadzone found above 90% throttle in most ESC/motors (Randy)
- More timing improvements (main loop is now tied to MPU6000s interrupt) (Randy)
- GPS NMEA bug fix (Alexey Kozin)
- Logging improvements (log I terms, dump all settings at head of dataflash log) (Jason)
Bug Fixes / Parameter changes:
- fixed skipping of last waypoint (Jason)
- resolved twitching when no GPS attached (Tridge)
- fixed loss of altitude if alt hold is engaged before first GPS lock (Randy/Jason)
- moved Roll-Pitch I terms from Stabilize controllers to Rate controllers
- TILT_COMPENSATION param tuned for TradHeli (Rob)
Code Cleanup:
- HAL changes for platform portability (Pat Hickey)
- Removed INSTANT_PWM (Randy)
------------------------------------------------------------------

46
ArduSub/UserCode.cpp Normal file
View File

@ -0,0 +1,46 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#ifdef USERHOOK_INIT
void Copter::userhook_init()
{
// put your initialisation code here
// this will be called once at start-up
}
#endif
#ifdef USERHOOK_FASTLOOP
void Copter::userhook_FastLoop()
{
// put your 100Hz code here
}
#endif
#ifdef USERHOOK_50HZLOOP
void Copter::userhook_50Hz()
{
// put your 50Hz code here
}
#endif
#ifdef USERHOOK_MEDIUMLOOP
void Copter::userhook_MediumLoop()
{
// put your 10Hz code here
}
#endif
#ifdef USERHOOK_SLOWLOOP
void Copter::userhook_SlowLoop()
{
// put your 3.3Hz code here
}
#endif
#ifdef USERHOOK_SUPERSLOWLOOP
void Copter::userhook_SuperSlowLoop()
{
// put your 1Hz code here
}
#endif

19
ArduSub/UserVariables.h Normal file
View File

@ -0,0 +1,19 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// user defined variables
// example variables used in Wii camera testing - replace with your own
// variables
#ifdef USERHOOK_VARIABLES
#if WII_CAMERA == 1
WiiCamera ircam;
int WiiRange=0;
int WiiRotation=0;
int WiiDisplacementX=0;
int WiiDisplacementY=0;
#endif // WII_CAMERA
#endif // USERHOOK_VARIABLES

62
ArduSub/adsb.cpp Normal file
View File

@ -0,0 +1,62 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
* adsb.cpp
* Copyright (C) Tom Pittenger 2015
*
* This file 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 file 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/>.
*/
#include "Copter.h"
#if ADSB_ENABLED == ENABLED
/*
* this module deals with ADS-B handling for ArduPilot
* ADS-B is an RF based collision avoidance protocol to tell nearby aircraft your location
* https://en.wikipedia.org/wiki/Automatic_dependent_surveillance_%E2%80%93_broadcast
*
*/
/*
handle periodic adsb database maintenance and handle threats
*/
void Copter::adsb_update(void)
{
adsb.update();
adsb_handle_vehicle_threats();
}
/*
* Handle ADS-B based threats which are platform dependent
*/
void Copter::adsb_handle_vehicle_threats(void)
{
// handle clearing of threat
if (adsb.get_is_evading_threat() && !adsb.get_another_vehicle_within_radius()) {
adsb.set_is_evading_threat(false);
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_ADSB, ERROR_CODE_FAILSAFE_RESOLVED);
gcs_send_text(MAV_SEVERITY_CRITICAL, "ADS-B threat cleared");
return;
}
// handle new threat
if (!adsb.get_is_evading_threat() && adsb.get_another_vehicle_within_radius()) {
adsb.set_is_evading_threat(true);
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_ADSB, ERROR_CODE_FAILSAFE_OCCURRED);
gcs_send_text(MAV_SEVERITY_CRITICAL, "ADS-B threat!");
return;
}
}
#endif // #ADSB_ENABLED

674
ArduSub/arming_checks.cpp Normal file
View File

@ -0,0 +1,674 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
// performs pre-arm checks. expects to be called at 1hz.
void Copter::update_arming_checks(void)
{
// perform pre-arm checks & display failures every 30 seconds
static uint8_t pre_arm_display_counter = PREARM_DISPLAY_PERIOD/2;
pre_arm_display_counter++;
bool display_fail = false;
if (pre_arm_display_counter >= PREARM_DISPLAY_PERIOD) {
display_fail = true;
pre_arm_display_counter = 0;
}
if (pre_arm_checks(display_fail)) {
set_pre_arm_check(true);
}
}
// performs pre-arm checks and arming checks
bool Copter::all_arming_checks_passing(bool arming_from_gcs)
{
if (pre_arm_checks(true)) {
set_pre_arm_check(true);
}
return ap.pre_arm_check && arm_checks(true, arming_from_gcs);
}
// perform pre-arm checks and set ap.pre_arm_check flag
// return true if the checks pass successfully
bool Copter::pre_arm_checks(bool display_failure)
{
// exit immediately if already armed
if (motors.armed()) {
return true;
}
// check if motor interlock and Emergency Stop aux switches are used
// at the same time. This cannot be allowed.
if (check_if_auxsw_mode_used(AUXSW_MOTOR_INTERLOCK) && check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP)){
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Interlock/E-Stop Conflict");
}
return false;
}
// check if motor interlock aux switch is in use
// if it is, switch needs to be in disabled position to arm
// otherwise exit immediately. This check to be repeated,
// as state can change at any time.
if (ap.using_interlock && motors.get_interlock()){
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Motor Interlock Enabled");
}
return false;
}
// exit immediately if we've already successfully performed the pre-arm check
if (ap.pre_arm_check) {
// run gps checks because results may change and affect LED colour
// no need to display failures because arm_checks will do that if the pilot tries to arm
pre_arm_gps_checks(false);
return true;
}
// succeed if pre arm checks are disabled
if (g.arming_check == ARMING_CHECK_NONE) {
set_pre_arm_check(true);
set_pre_arm_rc_check(true);
return true;
}
// pre-arm rc checks a prerequisite
pre_arm_rc_checks();
if (!ap.pre_arm_rc_check) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: RC not calibrated");
}
return false;
}
// check Baro
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_BARO)) {
// barometer health check
if (!barometer.all_healthy()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Barometer not healthy");
}
return false;
}
// Check baro & inav alt are within 1m if EKF is operating in an absolute position mode.
// Do not check if intending to operate in a ground relative height mode as EKF will output a ground relative height
// that may differ from the baro height due to baro drift.
nav_filter_status filt_status = inertial_nav.get_filter_status();
bool using_baro_ref = (!filt_status.flags.pred_horiz_pos_rel && filt_status.flags.pred_horiz_pos_abs);
if (using_baro_ref) {
if (fabsf(inertial_nav.get_altitude() - baro_alt) > PREARM_MAX_ALT_DISPARITY_CM) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Altitude disparity");
}
return false;
}
}
}
// check Compass
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_COMPASS)) {
// check the primary compass is healthy
if (!compass.healthy()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Compass not healthy");
}
return false;
}
// check compass learning is on or offsets have been set
if (!compass.configured()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Compass not calibrated");
}
return false;
}
// check for unreasonable compass offsets
Vector3f offsets = compass.get_offsets();
if (offsets.length() > COMPASS_OFFSETS_MAX) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Compass offsets too high");
}
return false;
}
// check for unreasonable mag field length
float mag_field = compass.get_field().length();
if (mag_field > COMPASS_MAGFIELD_EXPECTED*1.65f || mag_field < COMPASS_MAGFIELD_EXPECTED*0.35f) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check mag field");
}
return false;
}
// check all compasses point in roughly same direction
if (!compass.consistent()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: inconsistent compasses");
}
return false;
}
}
// check GPS
if (!pre_arm_gps_checks(display_failure)) {
return false;
}
#if AC_FENCE == ENABLED
// check fence is initialised
if (!fence.pre_arm_check()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: check fence");
}
return false;
}
#endif
// check INS
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_INS)) {
// check accelerometers have been calibrated
if (!ins.accel_calibrated_ok_all()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Accels not calibrated");
}
return false;
}
// check accels are healthy
if (!ins.get_accel_health_all()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Accelerometers not healthy");
}
return false;
}
//check if accelerometers have calibrated and require reboot
if (ins.accel_cal_requires_reboot()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL, "PreArm: Accelerometers calibrated requires reboot");
}
return false;
}
// check all accelerometers point in roughly same direction
if (ins.get_accel_count() > 1) {
const Vector3f &prime_accel_vec = ins.get_accel();
for(uint8_t i=0; i<ins.get_accel_count(); i++) {
// get next accel vector
const Vector3f &accel_vec = ins.get_accel(i);
Vector3f vec_diff = accel_vec - prime_accel_vec;
float threshold = PREARM_MAX_ACCEL_VECTOR_DIFF;
if (i >= 2) {
/*
* for boards with 3 IMUs we only use the first two
* in the EKF. Allow for larger accel discrepancy
* for IMU3 as it may be running at a different temperature
*/
threshold *= 2;
}
if (vec_diff.length() > threshold) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: inconsistent Accelerometers");
}
return false;
}
}
}
// check gyros are healthy
if (!ins.get_gyro_health_all()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Gyros not healthy");
}
return false;
}
// check all gyros are consistent
if (ins.get_gyro_count() > 1) {
for(uint8_t i=0; i<ins.get_gyro_count(); i++) {
// get rotation rate difference between gyro #i and primary gyro
Vector3f vec_diff = ins.get_gyro(i) - ins.get_gyro();
if (vec_diff.length() > PREARM_MAX_GYRO_VECTOR_DIFF) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: inconsistent Gyros");
}
return false;
}
}
}
// get ekf attitude (if bad, it's usually the gyro biases)
if (!pre_arm_ekf_attitude_check()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: gyros still settling");
}
return false;
}
}
#if CONFIG_HAL_BOARD != HAL_BOARD_VRBRAIN
#ifndef CONFIG_ARCH_BOARD_PX4FMU_V1
// check board voltage
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_VOLTAGE)) {
if (hal.analogin->board_voltage() < BOARD_VOLTAGE_MIN || hal.analogin->board_voltage() > BOARD_VOLTAGE_MAX) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check Board Voltage");
}
return false;
}
}
#endif
#endif
// check battery voltage
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_VOLTAGE)) {
if (failsafe.battery || (!ap.usb_connected && battery.exhausted(g.fs_batt_voltage, g.fs_batt_mah))) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check Battery");
}
return false;
}
}
// check various parameter values
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_PARAMETERS)) {
// ensure ch7 and ch8 have different functions
if (check_duplicate_auxsw()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Duplicate Aux Switch Options");
}
return false;
}
// failsafe parameter checks
if (g.failsafe_throttle) {
// check throttle min is above throttle failsafe trigger and that the trigger is above ppm encoder's loss-of-signal value of 900
if (channel_throttle->radio_min <= g.failsafe_throttle_value+10 || g.failsafe_throttle_value < 910) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check FS_THR_VALUE");
}
return false;
}
}
// lean angle parameter check
if (aparm.angle_max < 1000 || aparm.angle_max > 8000) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check ANGLE_MAX");
}
return false;
}
// acro balance parameter check
if ((g.acro_balance_roll > g.p_stabilize_roll.kP()) || (g.acro_balance_pitch > g.p_stabilize_pitch.kP())) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: ACRO_BAL_ROLL/PITCH");
}
return false;
}
#if CONFIG_SONAR == ENABLED && OPTFLOW == ENABLED
// check range finder if optflow enabled
if (optflow.enabled() && !sonar.pre_arm_check()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: check range finder");
}
return false;
}
#endif
#if FRAME_CONFIG == HELI_FRAME
// check helicopter parameters
if (!motors.parameter_check(display_failure)) {
return false;
}
#endif // HELI_FRAME
}
// check throttle is above failsafe throttle
// this is near the bottom to allow other failures to be displayed before checking pilot throttle
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_RC)) {
if (g.failsafe_throttle != FS_THR_DISABLED && channel_throttle->radio_in < g.failsafe_throttle_value) {
if (display_failure) {
#if FRAME_CONFIG == HELI_FRAME
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Collective below Failsafe");
#else
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Throttle below Failsafe");
#endif
}
return false;
}
}
return true;
}
// perform pre_arm_rc_checks checks and set ap.pre_arm_rc_check flag
void Copter::pre_arm_rc_checks()
{
// exit immediately if we've already successfully performed the pre-arm rc check
if (ap.pre_arm_rc_check) {
return;
}
// set rc-checks to success if RC checks are disabled
if ((g.arming_check != ARMING_CHECK_ALL) && !(g.arming_check & ARMING_CHECK_RC)) {
set_pre_arm_rc_check(true);
return;
}
// check if radio has been calibrated
if (!channel_throttle->radio_min.configured() && !channel_throttle->radio_max.configured()) {
return;
}
// check channels 1 & 2 have min <= 1300 and max >= 1700
if (channel_roll->radio_min > 1300 || channel_roll->radio_max < 1700 || channel_pitch->radio_min > 1300 || channel_pitch->radio_max < 1700) {
return;
}
// check channels 3 & 4 have min <= 1300 and max >= 1700
if (channel_throttle->radio_min > 1300 || channel_throttle->radio_max < 1700 || channel_yaw->radio_min > 1300 || channel_yaw->radio_max < 1700) {
return;
}
// check channels 1 & 2 have trim >= 1300 and <= 1700
if (channel_roll->radio_trim < 1300 || channel_roll->radio_trim > 1700 || channel_pitch->radio_trim < 1300 || channel_pitch->radio_trim > 1700) {
return;
}
// check channel 4 has trim >= 1300 and <= 1700
if (channel_yaw->radio_trim < 1300 || channel_yaw->radio_trim > 1700) {
return;
}
// if we've gotten this far rc is ok
set_pre_arm_rc_check(true);
}
// performs pre_arm gps related checks and returns true if passed
bool Copter::pre_arm_gps_checks(bool display_failure)
{
// always check if inertial nav has started and is ready
if (!ahrs.healthy()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Waiting for Nav Checks");
}
return false;
}
// check if flight mode requires GPS
bool gps_required = mode_requires_GPS(control_mode);
#if AC_FENCE == ENABLED
// if circular fence is enabled we need GPS
if ((fence.get_enabled_fences() & AC_FENCE_TYPE_CIRCLE) != 0) {
gps_required = true;
}
#endif
// return true if GPS is not required
if (!gps_required) {
AP_Notify::flags.pre_arm_gps_check = true;
return true;
}
// ensure GPS is ok
if (!position_ok()) {
if (display_failure) {
const char *reason = ahrs.prearm_failure_reason();
if (reason) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: %s", reason);
} else {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Need 3D Fix");
}
}
AP_Notify::flags.pre_arm_gps_check = false;
return false;
}
// check EKF compass variance is below failsafe threshold
float vel_variance, pos_variance, hgt_variance, tas_variance;
Vector3f mag_variance;
Vector2f offset;
ahrs.get_variances(vel_variance, pos_variance, hgt_variance, mag_variance, tas_variance, offset);
if (mag_variance.length() >= g.fs_ekf_thresh) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: EKF compass variance");
}
return false;
}
// check home and EKF origin are not too far
if (far_from_EKF_origin(ahrs.get_home())) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: EKF-home variance");
}
AP_Notify::flags.pre_arm_gps_check = false;
return false;
}
// return true immediately if gps check is disabled
if (!(g.arming_check == ARMING_CHECK_ALL || g.arming_check & ARMING_CHECK_GPS)) {
AP_Notify::flags.pre_arm_gps_check = true;
return true;
}
// warn about hdop separately - to prevent user confusion with no gps lock
if (gps.get_hdop() > g.gps_hdop_good) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: High GPS HDOP");
}
AP_Notify::flags.pre_arm_gps_check = false;
return false;
}
// if we got here all must be ok
AP_Notify::flags.pre_arm_gps_check = true;
return true;
}
// check ekf attitude is acceptable
bool Copter::pre_arm_ekf_attitude_check()
{
// get ekf filter status
nav_filter_status filt_status = inertial_nav.get_filter_status();
return filt_status.flags.attitude;
}
// arm_checks - perform final checks before arming
// always called just before arming. Return true if ok to arm
// has side-effect that logging is started
bool Copter::arm_checks(bool display_failure, bool arming_from_gcs)
{
#if LOGGING_ENABLED == ENABLED
// start dataflash
start_logging();
#endif
// check accels and gyro are healthy
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_INS)) {
//check if accelerometers have calibrated and require reboot
if (ins.accel_cal_requires_reboot()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL, "PreArm: Accelerometers calibrated requires reboot");
}
return false;
}
if (!ins.get_accel_health_all()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Accelerometers not healthy");
}
return false;
}
if (!ins.get_gyro_health_all()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Gyros not healthy");
}
return false;
}
// get ekf attitude (if bad, it's usually the gyro biases)
if (!pre_arm_ekf_attitude_check()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: gyros still settling");
}
return false;
}
}
// always check if inertial nav has started and is ready
if (!ahrs.healthy()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Waiting for Nav Checks");
}
return false;
}
if (compass.is_calibrating()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Compass calibration running");
}
return false;
}
// always check if the current mode allows arming
if (!mode_allows_arming(control_mode, arming_from_gcs)) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Mode not armable");
}
return false;
}
// always check gps
if (!pre_arm_gps_checks(display_failure)) {
return false;
}
// if we are using motor interlock switch and it's enabled, fail to arm
if (ap.using_interlock && motors.get_interlock()){
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Motor Interlock Enabled");
return false;
}
// if we are not using Emergency Stop switch option, force Estop false to ensure motors
// can run normally
if (!check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP)){
set_motor_emergency_stop(false);
// if we are using motor Estop switch, it must not be in Estop position
} else if (check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP) && ap.motor_emergency_stop){
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Motor Emergency Stopped");
return false;
}
// succeed if arming checks are disabled
if (g.arming_check == ARMING_CHECK_NONE) {
return true;
}
// baro checks
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_BARO)) {
// baro health check
if (!barometer.all_healthy()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Barometer not healthy");
}
return false;
}
// Check baro & inav alt are within 1m if EKF is operating in an absolute position mode.
// Do not check if intending to operate in a ground relative height mode as EKF will output a ground relative height
// that may differ from the baro height due to baro drift.
nav_filter_status filt_status = inertial_nav.get_filter_status();
bool using_baro_ref = (!filt_status.flags.pred_horiz_pos_rel && filt_status.flags.pred_horiz_pos_abs);
if (using_baro_ref && (fabsf(inertial_nav.get_altitude() - baro_alt) > PREARM_MAX_ALT_DISPARITY_CM)) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Altitude disparity");
}
return false;
}
}
#if AC_FENCE == ENABLED
// check vehicle is within fence
if (!fence.pre_arm_check()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: check fence");
}
return false;
}
#endif
// check lean angle
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_INS)) {
if (degrees(acosf(ahrs.cos_roll()*ahrs.cos_pitch()))*100.0f > aparm.angle_max) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Leaning");
}
return false;
}
}
// check battery voltage
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_VOLTAGE)) {
if (failsafe.battery || (!ap.usb_connected && battery.exhausted(g.fs_batt_voltage, g.fs_batt_mah))) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Check Battery");
}
return false;
}
}
// check throttle
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_RC)) {
// check throttle is not too low - must be above failsafe throttle
if (g.failsafe_throttle != FS_THR_DISABLED && channel_throttle->radio_in < g.failsafe_throttle_value) {
if (display_failure) {
#if FRAME_CONFIG == HELI_FRAME
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Collective below Failsafe");
#else
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Throttle below Failsafe");
#endif
}
return false;
}
// check throttle is not too high - skips checks if arming from GCS in Guided
if (!(arming_from_gcs && control_mode == GUIDED)) {
// above top of deadband is too always high
if (channel_throttle->control_in > get_takeoff_trigger_throttle()) {
if (display_failure) {
#if FRAME_CONFIG == HELI_FRAME
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Collective too high");
#else
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Throttle too high");
#endif
}
return false;
}
// in manual modes throttle must be at zero
if ((mode_has_manual_throttle(control_mode) || control_mode == DRIFT) && channel_throttle->control_in > 0) {
if (display_failure) {
#if FRAME_CONFIG == HELI_FRAME
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Collective too high");
#else
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Throttle too high");
#endif
}
return false;
}
}
}
// check if safety switch has been pushed
if (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Safety Switch");
}
return false;
}
// if we've gotten this far all is ok
return true;
}

72
ArduSub/baro_ground_effect.cpp Normal file
View File

@ -0,0 +1,72 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#if GNDEFFECT_COMPENSATION == ENABLED
void Copter::update_ground_effect_detector(void)
{
if(!motors.armed()) {
// disarmed - disable ground effect and return
gndeffect_state.takeoff_expected = false;
gndeffect_state.touchdown_expected = false;
ahrs.setTakeoffExpected(gndeffect_state.takeoff_expected);
ahrs.setTouchdownExpected(gndeffect_state.touchdown_expected);
return;
}
// variable initialization
uint32_t tnow_ms = millis();
float xy_des_speed_cms = 0.0f;
float xy_speed_cms = 0.0f;
float des_climb_rate_cms = pos_control.get_desired_velocity().z;
if (pos_control.is_active_xy()) {
Vector3f vel_target = pos_control.get_vel_target();
vel_target.z = 0.0f;
xy_des_speed_cms = vel_target.length();
}
if (position_ok() || optflow_position_ok()) {
Vector3f vel = inertial_nav.get_velocity();
vel.z = 0.0f;
xy_speed_cms = vel.length();
}
// takeoff logic
// if we are armed and haven't yet taken off
if (motors.armed() && ap.land_complete && !gndeffect_state.takeoff_expected) {
gndeffect_state.takeoff_expected = true;
}
// if we aren't taking off yet, reset the takeoff timer, altitude and complete flag
bool throttle_up = mode_has_manual_throttle(control_mode) && g.rc_3.control_in > 0;
if (!throttle_up && ap.land_complete) {
gndeffect_state.takeoff_time_ms = tnow_ms;
gndeffect_state.takeoff_alt_cm = inertial_nav.get_altitude();
}
// if we are in takeoff_expected and we meet the conditions for having taken off
// end the takeoff_expected state
if (gndeffect_state.takeoff_expected && (tnow_ms-gndeffect_state.takeoff_time_ms > 5000 || inertial_nav.get_altitude()-gndeffect_state.takeoff_alt_cm > 50.0f)) {
gndeffect_state.takeoff_expected = false;
}
// landing logic
Vector3f angle_target_rad = attitude_control.get_att_target_euler_cd() * radians(0.01f);
bool small_angle_request = cosf(angle_target_rad.x)*cosf(angle_target_rad.y) > cosf(radians(7.5f));
bool xy_speed_low = (position_ok() || optflow_position_ok()) && xy_speed_cms <= 125.0f;
bool xy_speed_demand_low = pos_control.is_active_xy() && xy_des_speed_cms <= 125.0f;
bool slow_horizontal = xy_speed_demand_low || (xy_speed_low && !pos_control.is_active_xy()) || (control_mode == ALT_HOLD && small_angle_request);
bool descent_demanded = pos_control.is_active_z() && des_climb_rate_cms < 0.0f;
bool slow_descent_demanded = descent_demanded && des_climb_rate_cms >= -100.0f;
bool z_speed_low = abs(inertial_nav.get_velocity_z()) <= 60.0f;
bool slow_descent = (slow_descent_demanded || (z_speed_low && descent_demanded));
gndeffect_state.touchdown_expected = slow_horizontal && slow_descent;
// Prepare the EKF for ground effect if either takeoff or touchdown is expected.
ahrs.setTakeoffExpected(gndeffect_state.takeoff_expected);
ahrs.setTouchdownExpected(gndeffect_state.touchdown_expected);
}
#endif // GNDEFFECT_COMPENSATION == ENABLED

13
ArduSub/capabilities.cpp Normal file
View File

@ -0,0 +1,13 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
void Copter::init_capabilities(void)
{
hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT);
hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT);
hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED);
hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_GLOBAL_INT);
hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_FLIGHT_TERMINATION);
hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET);
}

143
ArduSub/commands.cpp Normal file
View File

@ -0,0 +1,143 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
/*
* the home_state has a number of possible values (see enum HomeState in defines.h's)
* HOME_UNSET = home is not set, no GPS positions yet received
* HOME_SET_NOT_LOCKED = home is set to EKF origin or armed location (can be moved)
* HOME_SET_AND_LOCKED = home has been set by user, cannot be moved except by user initiated do-set-home command
*/
// checks if we should update ahrs/RTL home position from the EKF
void Copter::update_home_from_EKF()
{
// exit immediately if home already set
if (ap.home_state != HOME_UNSET) {
return;
}
// special logic if home is set in-flight
if (motors.armed()) {
set_home_to_current_location_inflight();
} else {
// move home to current ekf location (this will set home_state to HOME_SET)
set_home_to_current_location();
}
}
// set_home_to_current_location_inflight - set home to current GPS location (horizontally) and EKF origin vertically
void Copter::set_home_to_current_location_inflight() {
// get current location from EKF
Location temp_loc;
if (inertial_nav.get_location(temp_loc)) {
const struct Location &ekf_origin = inertial_nav.get_origin();
temp_loc.alt = ekf_origin.alt;
set_home(temp_loc);
}
}
// set_home_to_current_location - set home to current GPS location
bool Copter::set_home_to_current_location() {
// get current location from EKF
Location temp_loc;
if (inertial_nav.get_location(temp_loc)) {
return set_home(temp_loc);
}
return false;
}
// set_home_to_current_location_and_lock - set home to current location and lock so it cannot be moved
bool Copter::set_home_to_current_location_and_lock()
{
if (set_home_to_current_location()) {
set_home_state(HOME_SET_AND_LOCKED);
return true;
}
return false;
}
// set_home_and_lock - sets ahrs home (used for RTL) to specified location and locks so it cannot be moved
// unless this function is called again
bool Copter::set_home_and_lock(const Location& loc)
{
if (set_home(loc)) {
set_home_state(HOME_SET_AND_LOCKED);
return true;
}
return false;
}
// set_home - sets ahrs home (used for RTL) to specified location
// initialises inertial nav and compass on first call
// returns true if home location set successfully
bool Copter::set_home(const Location& loc)
{
// check location is valid
if (loc.lat == 0 && loc.lng == 0) {
return false;
}
// set ahrs home (used for RTL)
ahrs.set_home(loc);
// init inav and compass declination
if (ap.home_state == HOME_UNSET) {
// Set compass declination automatically
if (g.compass_enabled) {
compass.set_initial_location(gps.location().lat, gps.location().lng);
}
// update navigation scalers. used to offset the shrinking longitude as we go towards the poles
scaleLongDown = longitude_scale(loc);
// record home is set
set_home_state(HOME_SET_NOT_LOCKED);
// log new home position which mission library will pull from ahrs
if (should_log(MASK_LOG_CMD)) {
AP_Mission::Mission_Command temp_cmd;
if (mission.read_cmd_from_storage(0, temp_cmd)) {
DataFlash.Log_Write_Mission_Cmd(mission, temp_cmd);
}
}
}
// log ahrs home and ekf origin dataflash
Log_Write_Home_And_Origin();
// send new home location to GCS
GCS_MAVLINK::send_home_all(loc);
// return success
return true;
}
// far_from_EKF_origin - checks if a location is too far from the EKF origin
// returns true if too far
bool Copter::far_from_EKF_origin(const Location& loc)
{
// check distance to EKF origin
const struct Location &ekf_origin = inertial_nav.get_origin();
if (get_distance(ekf_origin, loc) > EKF_ORIGIN_MAX_DIST_M) {
return true;
}
// close enough to origin
return false;
}
// checks if we should update ahrs/RTL home position from GPS
void Copter::set_system_time_from_GPS()
{
// exit immediately if system time already set
if (ap.system_time_set) {
return;
}
// if we have a 3d lock and valid location
if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
// set system clock for log timestamps
hal.util->set_system_clock(gps.time_epoch_usec());
ap.system_time_set = true;
Log_Write_Event(DATA_SYSTEM_TIME_SET);
}
}

910
ArduSub/commands_logic.cpp Normal file
View File

@ -0,0 +1,910 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
// start_command - this function will be called when the ap_mission lib wishes to start a new command
bool Copter::start_command(const AP_Mission::Mission_Command& cmd)
{
// To-Do: logging when new commands start/end
if (should_log(MASK_LOG_CMD)) {
DataFlash.Log_Write_Mission_Cmd(mission, cmd);
}
switch(cmd.id) {
///
/// navigation commands
///
case MAV_CMD_NAV_TAKEOFF: // 22
do_takeoff(cmd);
break;
case MAV_CMD_NAV_WAYPOINT: // 16 Navigate to Waypoint
do_nav_wp(cmd);
break;
case MAV_CMD_NAV_LAND: // 21 LAND to Waypoint
do_land(cmd);
break;
case MAV_CMD_NAV_LOITER_UNLIM: // 17 Loiter indefinitely
do_loiter_unlimited(cmd);
break;
case MAV_CMD_NAV_LOITER_TURNS: //18 Loiter N Times
do_circle(cmd);
break;
case MAV_CMD_NAV_LOITER_TIME: // 19
do_loiter_time(cmd);
break;
case MAV_CMD_NAV_RETURN_TO_LAUNCH: //20
do_RTL();
break;
case MAV_CMD_NAV_SPLINE_WAYPOINT: // 82 Navigate to Waypoint using spline
do_spline_wp(cmd);
break;
#if NAV_GUIDED == ENABLED
case MAV_CMD_NAV_GUIDED_ENABLE: // 92 accept navigation commands from external nav computer
do_nav_guided_enable(cmd);
break;
#endif
//
// conditional commands
//
case MAV_CMD_CONDITION_DELAY: // 112
do_wait_delay(cmd);
break;
case MAV_CMD_CONDITION_DISTANCE: // 114
do_within_distance(cmd);
break;
case MAV_CMD_CONDITION_CHANGE_ALT: // 113
do_change_alt(cmd);
break;
case MAV_CMD_CONDITION_YAW: // 115
do_yaw(cmd);
break;
///
/// do commands
///
case MAV_CMD_DO_CHANGE_SPEED: // 178
do_change_speed(cmd);
break;
case MAV_CMD_DO_SET_HOME: // 179
do_set_home(cmd);
break;
case MAV_CMD_DO_SET_SERVO:
ServoRelayEvents.do_set_servo(cmd.content.servo.channel, cmd.content.servo.pwm);
break;
case MAV_CMD_DO_SET_RELAY:
ServoRelayEvents.do_set_relay(cmd.content.relay.num, cmd.content.relay.state);
break;
case MAV_CMD_DO_REPEAT_SERVO:
ServoRelayEvents.do_repeat_servo(cmd.content.repeat_servo.channel, cmd.content.repeat_servo.pwm,
cmd.content.repeat_servo.repeat_count, cmd.content.repeat_servo.cycle_time * 1000.0f);
break;
case MAV_CMD_DO_REPEAT_RELAY:
ServoRelayEvents.do_repeat_relay(cmd.content.repeat_relay.num, cmd.content.repeat_relay.repeat_count,
cmd.content.repeat_relay.cycle_time * 1000.0f);
break;
case MAV_CMD_DO_SET_ROI: // 201
// point the copter and camera at a region of interest (ROI)
do_roi(cmd);
break;
case MAV_CMD_DO_MOUNT_CONTROL: // 205
// point the camera to a specified angle
do_mount_control(cmd);
break;
#if CAMERA == ENABLED
case MAV_CMD_DO_CONTROL_VIDEO: // Control on-board camera capturing. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty|
break;
case MAV_CMD_DO_DIGICAM_CONFIGURE: // Mission command to configure an on-board camera controller system. |Modes: P, TV, AV, M, Etc| Shutter speed: Divisor number for one second| Aperture: F stop number| ISO number e.g. 80, 100, 200, Etc| Exposure type enumerator| Command Identity| Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)|
do_digicam_configure(cmd);
break;
case MAV_CMD_DO_DIGICAM_CONTROL: // Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty|
do_digicam_control(cmd);
break;
case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
camera.set_trigger_distance(cmd.content.cam_trigg_dist.meters);
break;
#endif
#if PARACHUTE == ENABLED
case MAV_CMD_DO_PARACHUTE: // Mission command to configure or release parachute
do_parachute(cmd);
break;
#endif
#if EPM_ENABLED == ENABLED
case MAV_CMD_DO_GRIPPER: // Mission command to control EPM gripper
do_gripper(cmd);
break;
#endif
#if NAV_GUIDED == ENABLED
case MAV_CMD_DO_GUIDED_LIMITS: // 220 accept guided mode limits
do_guided_limits(cmd);
break;
#endif
default:
// do nothing with unrecognized MAVLink messages
break;
}
// always return success
return true;
}
/********************************************************************************/
// Verify command Handlers
/********************************************************************************/
// verify_command_callback - callback function called from ap-mission at 10hz or higher when a command is being run
// we double check that the flight mode is AUTO to avoid the possibility of ap-mission triggering actions while we're not in AUTO mode
bool Copter::verify_command_callback(const AP_Mission::Mission_Command& cmd)
{
if (control_mode == AUTO) {
bool cmd_complete = verify_command(cmd);
// send message to GCS
if (cmd_complete) {
gcs_send_mission_item_reached_message(cmd.index);
}
return cmd_complete;
}
return false;
}
// verify_command - this will be called repeatedly by ap_mission lib to ensure the active commands are progressing
// should return true once the active navigation command completes successfully
// called at 10hz or higher
bool Copter::verify_command(const AP_Mission::Mission_Command& cmd)
{
switch(cmd.id) {
//
// navigation commands
//
case MAV_CMD_NAV_TAKEOFF:
return verify_takeoff();
case MAV_CMD_NAV_WAYPOINT:
return verify_nav_wp(cmd);
case MAV_CMD_NAV_LAND:
return verify_land();
case MAV_CMD_NAV_LOITER_UNLIM:
return verify_loiter_unlimited();
case MAV_CMD_NAV_LOITER_TURNS:
return verify_circle(cmd);
case MAV_CMD_NAV_LOITER_TIME:
return verify_loiter_time();
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
return verify_RTL();
case MAV_CMD_NAV_SPLINE_WAYPOINT:
return verify_spline_wp(cmd);
#if NAV_GUIDED == ENABLED
case MAV_CMD_NAV_GUIDED_ENABLE:
return verify_nav_guided_enable(cmd);
#endif
///
/// conditional commands
///
case MAV_CMD_CONDITION_DELAY:
return verify_wait_delay();
case MAV_CMD_CONDITION_DISTANCE:
return verify_within_distance();
case MAV_CMD_CONDITION_CHANGE_ALT:
return verify_change_alt();
case MAV_CMD_CONDITION_YAW:
return verify_yaw();
case MAV_CMD_DO_PARACHUTE:
// assume parachute was released successfully
return true;
default:
// return true if we do not recognize the command so that we move on to the next command
return true;
}
}
// exit_mission - function that is called once the mission completes
void Copter::exit_mission()
{
// play a tone
AP_Notify::events.mission_complete = 1;
// if we are not on the ground switch to loiter or land
if(!ap.land_complete) {
// try to enter loiter but if that fails land
if(!auto_loiter_start()) {
set_mode(LAND);
}
}else{
#if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED
// disarm when the landing detector says we've landed and throttle is at minimum
if (ap.throttle_zero || failsafe.radio) {
init_disarm_motors();
}
#else
// if we've landed it's safe to disarm
init_disarm_motors();
#endif
}
}
/********************************************************************************/
//
/********************************************************************************/
// do_RTL - start Return-to-Launch
void Copter::do_RTL(void)
{
// start rtl in auto flight mode
auto_rtl_start();
}
/********************************************************************************/
// Nav (Must) commands
/********************************************************************************/
// do_takeoff - initiate takeoff navigation command
void Copter::do_takeoff(const AP_Mission::Mission_Command& cmd)
{
// Set wp navigation target to safe altitude above current position
float takeoff_alt = cmd.content.location.alt;
takeoff_alt = MAX(takeoff_alt,current_loc.alt);
takeoff_alt = MAX(takeoff_alt,100.0f);
auto_takeoff_start(takeoff_alt);
}
// do_nav_wp - initiate move to next waypoint
void Copter::do_nav_wp(const AP_Mission::Mission_Command& cmd)
{
const Vector3f &curr_pos = inertial_nav.get_position();
const Vector3f local_pos = pv_location_to_vector_with_default(cmd.content.location, curr_pos);
// this will be used to remember the time in millis after we reach or pass the WP.
loiter_time = 0;
// this is the delay, stored in seconds
loiter_time_max = abs(cmd.p1);
// Set wp navigation target
auto_wp_start(local_pos);
// if no delay set the waypoint as "fast"
if (loiter_time_max == 0 ) {
wp_nav.set_fast_waypoint(true);
}
}
// do_land - initiate landing procedure
void Copter::do_land(const AP_Mission::Mission_Command& cmd)
{
// To-Do: check if we have already landed
// if location provided we fly to that location at current altitude
if (cmd.content.location.lat != 0 || cmd.content.location.lng != 0) {
// set state to fly to location
land_state = LAND_STATE_FLY_TO_LOCATION;
// calculate and set desired location above landing target
Vector3f pos = pv_location_to_vector(cmd.content.location);
pos.z = inertial_nav.get_altitude();
auto_wp_start(pos);
}else{
// set landing state
land_state = LAND_STATE_DESCENDING;
// initialise landing controller
auto_land_start();
}
}
// do_loiter_unlimited - start loitering with no end conditions
// note: caller should set yaw_mode
void Copter::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
{
Vector3f target_pos;
// get current position
Vector3f curr_pos = inertial_nav.get_position();
// default to use position provided
target_pos = pv_location_to_vector(cmd.content.location);
// use current location if not provided
if(cmd.content.location.lat == 0 && cmd.content.location.lng == 0) {
wp_nav.get_wp_stopping_point_xy(target_pos);
}
// use current altitude if not provided
// To-Do: use z-axis stopping point instead of current alt
if( cmd.content.location.alt == 0 ) {
target_pos.z = curr_pos.z;
}
// start way point navigator and provide it the desired location
auto_wp_start(target_pos);
}
// do_circle - initiate moving in a circle
void Copter::do_circle(const AP_Mission::Mission_Command& cmd)
{
Vector3f curr_pos = inertial_nav.get_position();
Vector3f circle_center = pv_location_to_vector(cmd.content.location);
uint8_t circle_radius_m = HIGHBYTE(cmd.p1); // circle radius held in high byte of p1
bool move_to_edge_required = false;
// set target altitude if not provided
if (cmd.content.location.alt == 0) {
circle_center.z = curr_pos.z;
} else {
move_to_edge_required = true;
}
// set lat/lon position if not provided
// To-Do: use previous command's destination if it was a straight line or spline waypoint command
if (cmd.content.location.lat == 0 && cmd.content.location.lng == 0) {
circle_center.x = curr_pos.x;
circle_center.y = curr_pos.y;
} else {
move_to_edge_required = true;
}
// set circle controller's center
circle_nav.set_center(circle_center);
// set circle radius
if (circle_radius_m != 0) {
circle_nav.set_radius((float)circle_radius_m * 100.0f);
}
// check if we need to move to edge of circle
if (move_to_edge_required) {
// move to edge of circle (verify_circle) will ensure we begin circling once we reach the edge
auto_circle_movetoedge_start();
} else {
// start circling
auto_circle_start();
}
}
// do_loiter_time - initiate loitering at a point for a given time period
// note: caller should set yaw_mode
void Copter::do_loiter_time(const AP_Mission::Mission_Command& cmd)
{
Vector3f target_pos;
// get current position
Vector3f curr_pos = inertial_nav.get_position();
// default to use position provided
target_pos = pv_location_to_vector(cmd.content.location);
// use current location if not provided
if(cmd.content.location.lat == 0 && cmd.content.location.lng == 0) {
wp_nav.get_wp_stopping_point_xy(target_pos);
}
// use current altitude if not provided
if( cmd.content.location.alt == 0 ) {
target_pos.z = curr_pos.z;
}
// start way point navigator and provide it the desired location
auto_wp_start(target_pos);
// setup loiter timer
loiter_time = 0;
loiter_time_max = cmd.p1; // units are (seconds)
}
// do_spline_wp - initiate move to next waypoint
void Copter::do_spline_wp(const AP_Mission::Mission_Command& cmd)
{
const Vector3f& curr_pos = inertial_nav.get_position();
Vector3f local_pos = pv_location_to_vector_with_default(cmd.content.location, curr_pos);
// this will be used to remember the time in millis after we reach or pass the WP.
loiter_time = 0;
// this is the delay, stored in seconds
loiter_time_max = abs(cmd.p1);
// determine segment start and end type
bool stopped_at_start = true;
AC_WPNav::spline_segment_end_type seg_end_type = AC_WPNav::SEGMENT_END_STOP;
AP_Mission::Mission_Command temp_cmd;
Vector3f next_destination; // end of next segment
// if previous command was a wp_nav command with no delay set stopped_at_start to false
// To-Do: move processing of delay into wp-nav controller to allow it to determine the stopped_at_start value itself?
uint16_t prev_cmd_idx = mission.get_prev_nav_cmd_index();
if (prev_cmd_idx != AP_MISSION_CMD_INDEX_NONE) {
if (mission.read_cmd_from_storage(prev_cmd_idx, temp_cmd)) {
if ((temp_cmd.id == MAV_CMD_NAV_WAYPOINT || temp_cmd.id == MAV_CMD_NAV_SPLINE_WAYPOINT) && temp_cmd.p1 == 0) {
stopped_at_start = false;
}
}
}
// if there is no delay at the end of this segment get next nav command
if (cmd.p1 == 0 && mission.get_next_nav_cmd(cmd.index+1, temp_cmd)) {
// if the next nav command is a waypoint set end type to spline or straight
if (temp_cmd.id == MAV_CMD_NAV_WAYPOINT) {
seg_end_type = AC_WPNav::SEGMENT_END_STRAIGHT;
next_destination = pv_location_to_vector_with_default(temp_cmd.content.location, local_pos);
}else if (temp_cmd.id == MAV_CMD_NAV_SPLINE_WAYPOINT) {
seg_end_type = AC_WPNav::SEGMENT_END_SPLINE;
next_destination = pv_location_to_vector_with_default(temp_cmd.content.location, local_pos);
}
}
// set spline navigation target
auto_spline_start(local_pos, stopped_at_start, seg_end_type, next_destination);
}
#if NAV_GUIDED == ENABLED
// do_nav_guided_enable - initiate accepting commands from external nav computer
void Copter::do_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
{
if (cmd.p1 > 0) {
// initialise guided limits
guided_limit_init_time_and_pos();
// set spline navigation target
auto_nav_guided_start();
}
}
#endif // NAV_GUIDED
#if PARACHUTE == ENABLED
// do_parachute - configure or release parachute
void Copter::do_parachute(const AP_Mission::Mission_Command& cmd)
{
switch (cmd.p1) {
case PARACHUTE_DISABLE:
parachute.enabled(false);
Log_Write_Event(DATA_PARACHUTE_DISABLED);
break;
case PARACHUTE_ENABLE:
parachute.enabled(true);
Log_Write_Event(DATA_PARACHUTE_ENABLED);
break;
case PARACHUTE_RELEASE:
parachute_release();
break;
default:
// do nothing
break;
}
}
#endif
#if EPM_ENABLED == ENABLED
// do_gripper - control EPM gripper
void Copter::do_gripper(const AP_Mission::Mission_Command& cmd)
{
// Note: we ignore the gripper num parameter because we only support one gripper
switch (cmd.content.gripper.action) {
case GRIPPER_ACTION_RELEASE:
epm.release();
Log_Write_Event(DATA_EPM_RELEASE);
break;
case GRIPPER_ACTION_GRAB:
epm.grab();
Log_Write_Event(DATA_EPM_GRAB);
break;
default:
// do nothing
break;
}
}
#endif
#if NAV_GUIDED == ENABLED
// do_guided_limits - pass guided limits to guided controller
void Copter::do_guided_limits(const AP_Mission::Mission_Command& cmd)
{
guided_limit_set(cmd.p1 * 1000, // convert seconds to ms
cmd.content.guided_limits.alt_min * 100.0f, // convert meters to cm
cmd.content.guided_limits.alt_max * 100.0f, // convert meters to cm
cmd.content.guided_limits.horiz_max * 100.0f); // convert meters to cm
}
#endif
/********************************************************************************/
// Verify Nav (Must) commands
/********************************************************************************/
// verify_takeoff - check if we have completed the takeoff
bool Copter::verify_takeoff()
{
// have we reached our target altitude?
return wp_nav.reached_wp_destination();
}
// verify_land - returns true if landing has been completed
bool Copter::verify_land()
{
bool retval = false;
switch( land_state ) {
case LAND_STATE_FLY_TO_LOCATION:
// check if we've reached the location
if (wp_nav.reached_wp_destination()) {
// get destination so we can use it for loiter target
Vector3f dest = wp_nav.get_wp_destination();
// initialise landing controller
auto_land_start(dest);
// advance to next state
land_state = LAND_STATE_DESCENDING;
}
break;
case LAND_STATE_DESCENDING:
// rely on THROTTLE_LAND mode to correctly update landing status
retval = ap.land_complete;
break;
default:
// this should never happen
// TO-DO: log an error
retval = true;
break;
}
// true is returned if we've successfully landed
return retval;
}
// verify_nav_wp - check if we have reached the next way point
bool Copter::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
{
// check if we have reached the waypoint
if( !wp_nav.reached_wp_destination() ) {
return false;
}
// play a tone
AP_Notify::events.waypoint_complete = 1;
// start timer if necessary
if(loiter_time == 0) {
loiter_time = millis();
}
// check if timer has run out
if (((millis() - loiter_time) / 1000) >= loiter_time_max) {
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Reached command #%i",cmd.index);
return true;
}else{
return false;
}
}
bool Copter::verify_loiter_unlimited()
{
return false;
}
// verify_loiter_time - check if we have loitered long enough
bool Copter::verify_loiter_time()
{
// return immediately if we haven't reached our destination
if (!wp_nav.reached_wp_destination()) {
return false;
}
// start our loiter timer
if( loiter_time == 0 ) {
loiter_time = millis();
}
// check if loiter timer has run out
return (((millis() - loiter_time) / 1000) >= loiter_time_max);
}
// verify_circle - check if we have circled the point enough
bool Copter::verify_circle(const AP_Mission::Mission_Command& cmd)
{
// check if we've reached the edge
if (auto_mode == Auto_CircleMoveToEdge) {
if (wp_nav.reached_wp_destination()) {
Vector3f curr_pos = inertial_nav.get_position();
Vector3f circle_center = pv_location_to_vector(cmd.content.location);
// set target altitude if not provided
if (is_zero(circle_center.z)) {
circle_center.z = curr_pos.z;
}
// set lat/lon position if not provided
if (cmd.content.location.lat == 0 && cmd.content.location.lng == 0) {
circle_center.x = curr_pos.x;
circle_center.y = curr_pos.y;
}
// start circling
auto_circle_start();
}
return false;
}
// check if we have completed circling
return fabsf(circle_nav.get_angle_total()/M_2PI_F) >= LOWBYTE(cmd.p1);
}
// verify_RTL - handles any state changes required to implement RTL
// do_RTL should have been called once first to initialise all variables
// returns true with RTL has completed successfully
bool Copter::verify_RTL()
{
return (rtl_state_complete && (rtl_state == RTL_FinalDescent || rtl_state == RTL_Land));
}
// verify_spline_wp - check if we have reached the next way point using spline
bool Copter::verify_spline_wp(const AP_Mission::Mission_Command& cmd)
{
// check if we have reached the waypoint
if( !wp_nav.reached_wp_destination() ) {
return false;
}
// start timer if necessary
if(loiter_time == 0) {
loiter_time = millis();
}
// check if timer has run out
if (((millis() - loiter_time) / 1000) >= loiter_time_max) {
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Reached command #%i",cmd.index);
return true;
}else{
return false;
}
}
#if NAV_GUIDED == ENABLED
// verify_nav_guided - check if we have breached any limits
bool Copter::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
{
// if disabling guided mode then immediately return true so we move to next command
if (cmd.p1 == 0) {
return true;
}
// check time and position limits
return guided_limit_check();
}
#endif // NAV_GUIDED
/********************************************************************************/
// Condition (May) commands
/********************************************************************************/
void Copter::do_wait_delay(const AP_Mission::Mission_Command& cmd)
{
condition_start = millis();
condition_value = cmd.content.delay.seconds * 1000; // convert seconds to milliseconds
}
void Copter::do_change_alt(const AP_Mission::Mission_Command& cmd)
{
// To-Do: store desired altitude in a variable so that it can be verified later
}
void Copter::do_within_distance(const AP_Mission::Mission_Command& cmd)
{
condition_value = cmd.content.distance.meters * 100;
}
void Copter::do_yaw(const AP_Mission::Mission_Command& cmd)
{
set_auto_yaw_look_at_heading(
cmd.content.yaw.angle_deg,
cmd.content.yaw.turn_rate_dps,
cmd.content.yaw.direction,
cmd.content.yaw.relative_angle);
}
/********************************************************************************/
// Verify Condition (May) commands
/********************************************************************************/
bool Copter::verify_wait_delay()
{
if (millis() - condition_start > (uint32_t)MAX(condition_value,0)) {
condition_value = 0;
return true;
}
return false;
}
bool Copter::verify_change_alt()
{
// To-Do: use recorded target altitude to verify we have reached the target
return true;
}
bool Copter::verify_within_distance()
{
// update distance calculation
calc_wp_distance();
if (wp_distance < (uint32_t)MAX(condition_value,0)) {
condition_value = 0;
return true;
}
return false;
}
// verify_yaw - return true if we have reached the desired heading
bool Copter::verify_yaw()
{
// set yaw mode if it has been changed (the waypoint controller often retakes control of yaw as it executes a new waypoint command)
if (auto_yaw_mode != AUTO_YAW_LOOK_AT_HEADING) {
set_auto_yaw_mode(AUTO_YAW_LOOK_AT_HEADING);
}
// check if we are within 2 degrees of the target heading
if (labs(wrap_180_cd(ahrs.yaw_sensor-yaw_look_at_heading)) <= 200) {
return true;
}else{
return false;
}
}
/********************************************************************************/
// Do (Now) commands
/********************************************************************************/
// do_guided - start guided mode
bool Copter::do_guided(const AP_Mission::Mission_Command& cmd)
{
Vector3f pos_or_vel; // target location or velocity
// only process guided waypoint if we are in guided mode
if (control_mode != GUIDED && !(control_mode == AUTO && auto_mode == Auto_NavGuided)) {
return false;
}
// switch to handle different commands
switch (cmd.id) {
case MAV_CMD_NAV_WAYPOINT:
// set wp_nav's destination
pos_or_vel = pv_location_to_vector(cmd.content.location);
guided_set_destination(pos_or_vel);
return true;
break;
case MAV_CMD_CONDITION_YAW:
do_yaw(cmd);
return true;
break;
default:
// reject unrecognised command
return false;
break;
}
return true;
}
void Copter::do_change_speed(const AP_Mission::Mission_Command& cmd)
{
if (cmd.content.speed.target_ms > 0) {
wp_nav.set_speed_xy(cmd.content.speed.target_ms * 100.0f);
}
}
void Copter::do_set_home(const AP_Mission::Mission_Command& cmd)
{
if(cmd.p1 == 1 || (cmd.content.location.lat == 0 && cmd.content.location.lng == 0 && cmd.content.location.alt == 0)) {
set_home_to_current_location();
} else {
if (!far_from_EKF_origin(cmd.content.location)) {
set_home(cmd.content.location);
}
}
}
// do_roi - starts actions required by MAV_CMD_NAV_ROI
// this involves either moving the camera to point at the ROI (region of interest)
// and possibly rotating the copter to point at the ROI if our mount type does not support a yaw feature
// TO-DO: add support for other features of MAV_CMD_DO_SET_ROI including pointing at a given waypoint
void Copter::do_roi(const AP_Mission::Mission_Command& cmd)
{
set_auto_yaw_roi(cmd.content.location);
}
// do_digicam_configure Send Digicam Configure message with the camera library
void Copter::do_digicam_configure(const AP_Mission::Mission_Command& cmd)
{
#if CAMERA == ENABLED
camera.configure(cmd.content.digicam_configure.shooting_mode,
cmd.content.digicam_configure.shutter_speed,
cmd.content.digicam_configure.aperture,
cmd.content.digicam_configure.ISO,
cmd.content.digicam_configure.exposure_type,
cmd.content.digicam_configure.cmd_id,
cmd.content.digicam_configure.engine_cutoff_time);
#endif
}
// do_digicam_control Send Digicam Control message with the camera library
void Copter::do_digicam_control(const AP_Mission::Mission_Command& cmd)
{
#if CAMERA == ENABLED
camera.control(cmd.content.digicam_control.session,
cmd.content.digicam_control.zoom_pos,
cmd.content.digicam_control.zoom_step,
cmd.content.digicam_control.focus_lock,
cmd.content.digicam_control.shooting_cmd,
cmd.content.digicam_control.cmd_id);
log_picture();
#endif
}
// do_take_picture - take a picture with the camera library
void Copter::do_take_picture()
{
#if CAMERA == ENABLED
camera.trigger_pic(true);
log_picture();
#endif
}
// log_picture - log picture taken and send feedback to GCS
void Copter::log_picture()
{
gcs_send_message(MSG_CAMERA_FEEDBACK);
if (should_log(MASK_LOG_CAMERA)) {
DataFlash.Log_Write_Camera(ahrs, gps, current_loc);
}
}
// point the camera to a specified angle
void Copter::do_mount_control(const AP_Mission::Mission_Command& cmd)
{
#if MOUNT == ENABLED
camera_mount.set_angle_targets(cmd.content.mount_control.roll, cmd.content.mount_control.pitch, cmd.content.mount_control.yaw);
#endif
}

274
ArduSub/compassmot.cpp Normal file
View File

@ -0,0 +1,274 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
/*
compass/motor interference calibration
*/
// setup_compassmot - sets compass's motor interference parameters
uint8_t Copter::mavlink_compassmot(mavlink_channel_t chan)
{
#if FRAME_CONFIG == HELI_FRAME
// compassmot not implemented for tradheli
return 1;
#else
int8_t comp_type; // throttle or current based compensation
Vector3f compass_base[COMPASS_MAX_INSTANCES]; // compass vector when throttle is zero
Vector3f motor_impact[COMPASS_MAX_INSTANCES]; // impact of motors on compass vector
Vector3f motor_impact_scaled[COMPASS_MAX_INSTANCES]; // impact of motors on compass vector scaled with throttle
Vector3f motor_compensation[COMPASS_MAX_INSTANCES]; // final compensation to be stored to eeprom
float throttle_pct; // throttle as a percentage 0.0 ~ 1.0
float throttle_pct_max = 0.0f; // maximum throttle reached (as a percentage 0~1.0)
float current_amps_max = 0.0f; // maximum current reached
float interference_pct[COMPASS_MAX_INSTANCES]; // interference as a percentage of total mag field (for reporting purposes only)
uint32_t last_run_time;
uint32_t last_send_time;
bool updated = false; // have we updated the compensation vector at least once
uint8_t command_ack_start = command_ack_counter;
// exit immediately if we are already in compassmot
if (ap.compass_mot) {
// ignore restart messages
return 1;
}else{
ap.compass_mot = true;
}
// check compass is enabled
if (!g.compass_enabled) {
gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL, "Compass disabled");
ap.compass_mot = false;
return 1;
}
// check compass health
compass.read();
for (uint8_t i=0; i<compass.get_count(); i++) {
if (!compass.healthy(i)) {
gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL, "Check compass");
ap.compass_mot = false;
return 1;
}
}
// check if radio is calibrated
pre_arm_rc_checks();
if (!ap.pre_arm_rc_check) {
gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL, "RC not calibrated");
ap.compass_mot = false;
return 1;
}
// check throttle is at zero
read_radio();
if (channel_throttle->control_in != 0) {
gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL, "Throttle not zero");
ap.compass_mot = false;
return 1;
}
// check we are landed
if (!ap.land_complete) {
gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL, "Not landed");
ap.compass_mot = false;
return 1;
}
// disable cpu failsafe
failsafe_disable();
// initialise compass
init_compass();
// default compensation type to use current if possible
if (battery.has_current()) {
comp_type = AP_COMPASS_MOT_COMP_CURRENT;
}else{
comp_type = AP_COMPASS_MOT_COMP_THROTTLE;
}
// send back initial ACK
mavlink_msg_command_ack_send(chan, MAV_CMD_PREFLIGHT_CALIBRATION,0);
// flash leds
AP_Notify::flags.esc_calibration = true;
// warn user we are starting calibration
gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_INFO, "Starting calibration");
// inform what type of compensation we are attempting
if (comp_type == AP_COMPASS_MOT_COMP_CURRENT) {
gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_INFO, "Current");
} else{
gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_INFO, "Throttle");
}
// disable throttle and battery failsafe
g.failsafe_throttle = FS_THR_DISABLED;
g.failsafe_battery_enabled = FS_BATT_DISABLED;
// disable motor compensation
compass.motor_compensation_type(AP_COMPASS_MOT_COMP_DISABLED);
for (uint8_t i=0; i<compass.get_count(); i++) {
compass.set_motor_compensation(i, Vector3f(0,0,0));
}
// get initial compass readings
last_run_time = millis();
while ( millis() - last_run_time < 500 ) {
compass.accumulate();
}
compass.read();
// store initial x,y,z compass values
// initialise interference percentage
for (uint8_t i=0; i<compass.get_count(); i++) {
compass_base[i] = compass.get_field(i);
interference_pct[i] = 0.0f;
}
// enable motors and pass through throttle
init_rc_out();
enable_motor_output();
motors.armed(true);
// initialise run time
last_run_time = millis();
last_send_time = millis();
// main run while there is no user input and the compass is healthy
while (command_ack_start == command_ack_counter && compass.healthy(compass.get_primary()) && motors.armed()) {
// 50hz loop
if (millis() - last_run_time < 20) {
// grab some compass values
compass.accumulate();
hal.scheduler->delay(5);
continue;
}
last_run_time = millis();
// read radio input
read_radio();
// pass through throttle to motors
motors.throttle_pass_through(channel_throttle->radio_in);
// read some compass values
compass.read();
// read current
read_battery();
// calculate scaling for throttle
throttle_pct = (float)channel_throttle->control_in / 1000.0f;
throttle_pct = constrain_float(throttle_pct,0.0f,1.0f);
// if throttle is near zero, update base x,y,z values
if (throttle_pct <= 0.0f) {
for (uint8_t i=0; i<compass.get_count(); i++) {
compass_base[i] = compass_base[i] * 0.99f + compass.get_field(i) * 0.01f;
}
// causing printing to happen as soon as throttle is lifted
} else {
// calculate diff from compass base and scale with throttle
for (uint8_t i=0; i<compass.get_count(); i++) {
motor_impact[i] = compass.get_field(i) - compass_base[i];
}
// throttle based compensation
if (comp_type == AP_COMPASS_MOT_COMP_THROTTLE) {
// for each compass
for (uint8_t i=0; i<compass.get_count(); i++) {
// scale by throttle
motor_impact_scaled[i] = motor_impact[i] / throttle_pct;
// adjust the motor compensation to negate the impact
motor_compensation[i] = motor_compensation[i] * 0.99f - motor_impact_scaled[i] * 0.01f;
}
updated = true;
} else {
// for each compass
for (uint8_t i=0; i<compass.get_count(); i++) {
// current based compensation if more than 3amps being drawn
motor_impact_scaled[i] = motor_impact[i] / battery.current_amps();
// adjust the motor compensation to negate the impact if drawing over 3amps
if (battery.current_amps() >= 3.0f) {
motor_compensation[i] = motor_compensation[i] * 0.99f - motor_impact_scaled[i] * 0.01f;
updated = true;
}
}
}
// calculate interference percentage at full throttle as % of total mag field
if (comp_type == AP_COMPASS_MOT_COMP_THROTTLE) {
for (uint8_t i=0; i<compass.get_count(); i++) {
// interference is impact@fullthrottle / mag field * 100
interference_pct[i] = motor_compensation[i].length() / (float)COMPASS_MAGFIELD_EXPECTED * 100.0f;
}
}else{
for (uint8_t i=0; i<compass.get_count(); i++) {
// interference is impact/amp * (max current seen / max throttle seen) / mag field * 100
interference_pct[i] = motor_compensation[i].length() * (current_amps_max/throttle_pct_max) / (float)COMPASS_MAGFIELD_EXPECTED * 100.0f;
}
}
// record maximum throttle and current
throttle_pct_max = MAX(throttle_pct_max, throttle_pct);
current_amps_max = MAX(current_amps_max, battery.current_amps());
}
if (AP_HAL::millis() - last_send_time > 500) {
last_send_time = AP_HAL::millis();
mavlink_msg_compassmot_status_send(chan,
channel_throttle->control_in,
battery.current_amps(),
interference_pct[compass.get_primary()],
motor_compensation[compass.get_primary()].x,
motor_compensation[compass.get_primary()].y,
motor_compensation[compass.get_primary()].z);
}
}
// stop motors
motors.output_min();
motors.armed(false);
// set and save motor compensation
if (updated) {
compass.motor_compensation_type(comp_type);
for (uint8_t i=0; i<compass.get_count(); i++) {
compass.set_motor_compensation(i, motor_compensation[i]);
}
compass.save_motor_compensation();
// display success message
gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_INFO, "Calibration successful");
} else {
// compensation vector never updated, report failure
gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_NOTICE, "Failed");
compass.motor_compensation_type(AP_COMPASS_MOT_COMP_DISABLED);
}
// display new motor offsets and save
report_compass();
// turn off notify leds
AP_Notify::flags.esc_calibration = false;
// re-enable cpu failsafe
failsafe_enable();
// re-enable failsafes
g.failsafe_throttle.load();
g.failsafe_battery_enabled.load();
// flag we have completed
ap.compass_mot = false;
return 0;
#endif // FRAME_CONFIG != HELI_FRAME
}

6
ArduSub/compat.cpp Normal file
View File

@ -0,0 +1,6 @@
#include "Copter.h"
void Copter::delay(uint32_t ms)
{
hal.scheduler->delay(ms);
}

762
ArduSub/config.h Normal file
View File

@ -0,0 +1,762 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
//
#ifndef __ARDUCOPTER_CONFIG_H__
#define __ARDUCOPTER_CONFIG_H__
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//
// WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING
//
// DO NOT EDIT this file to adjust your configuration. Create your own
// APM_Config.h and use APM_Config.h.example as a reference.
//
// WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING
///
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//
// Default and automatic configuration details.
//
// Notes for maintainers:
//
// - Try to keep this file organised in the same order as APM_Config.h.example
//
#include "defines.h"
///
/// DO NOT EDIT THIS INCLUDE - if you want to make a local change, make that
/// change in your local copy of APM_Config.h.
///
#ifdef USE_CMAKE_APM_CONFIG
#include "APM_Config_cmake.h" // <== Prefer cmake config if it exists
#else
#include "APM_Config.h" // <== THIS INCLUDE, DO NOT EDIT IT. EVER.
#endif
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// HARDWARE CONFIGURATION AND CONNECTIONS
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
#ifndef CONFIG_HAL_BOARD
#error CONFIG_HAL_BOARD must be defined to build ArduCopter
#endif
//////////////////////////////////////////////////////////////////////////////
// HIL_MODE OPTIONAL
#ifndef HIL_MODE
#define HIL_MODE HIL_MODE_DISABLED
#endif
#if HIL_MODE != HIL_MODE_DISABLED // we are in HIL mode
#undef CONFIG_SONAR
#define CONFIG_SONAR DISABLED
#endif
#define MAGNETOMETER ENABLED
// run at 400Hz on all systems
# define MAIN_LOOP_RATE 400
# define MAIN_LOOP_SECONDS 0.0025f
# define MAIN_LOOP_MICROS 2500
//////////////////////////////////////////////////////////////////////////////
// FRAME_CONFIG
//
#ifndef FRAME_CONFIG
# define FRAME_CONFIG QUAD_FRAME
#endif
#if FRAME_CONFIG == QUAD_FRAME
# define FRAME_CONFIG_STRING "QUAD"
#elif FRAME_CONFIG == TRI_FRAME
# define FRAME_CONFIG_STRING "TRI"
#elif FRAME_CONFIG == HEXA_FRAME
# define FRAME_CONFIG_STRING "HEXA"
#elif FRAME_CONFIG == Y6_FRAME
# define FRAME_CONFIG_STRING "Y6"
#elif FRAME_CONFIG == OCTA_FRAME
# define FRAME_CONFIG_STRING "OCTA"
#elif FRAME_CONFIG == OCTA_QUAD_FRAME
# define FRAME_CONFIG_STRING "OCTA_QUAD"
#elif FRAME_CONFIG == HELI_FRAME
# define FRAME_CONFIG_STRING "HELI"
#elif FRAME_CONFIG == SINGLE_FRAME
# define FRAME_CONFIG_STRING "SINGLE"
#elif FRAME_CONFIG == COAX_FRAME
# define FRAME_CONFIG_STRING "COAX"
#else
# define FRAME_CONFIG_STRING "UNKNOWN"
#endif
/////////////////////////////////////////////////////////////////////////////////
// TradHeli defaults
#if FRAME_CONFIG == HELI_FRAME
# define RC_FAST_SPEED 125
# define WP_YAW_BEHAVIOR_DEFAULT WP_YAW_BEHAVIOR_LOOK_AHEAD
# define RATE_ROLL_P 0.02
# define RATE_ROLL_I 0.5
# define RATE_ROLL_D 0.001
# define RATE_ROLL_IMAX 4500
# define RATE_ROLL_FF 0.05
# define RATE_ROLL_FILT_HZ 20.0f
# define RATE_PITCH_P 0.02
# define RATE_PITCH_I 0.5
# define RATE_PITCH_D 0.001
# define RATE_PITCH_IMAX 4500
# define RATE_PITCH_FF 0.05
# define RATE_PITCH_FILT_HZ 20.0f
# define RATE_YAW_P 0.15
# define RATE_YAW_I 0.100
# define RATE_YAW_D 0.003
# define RATE_YAW_IMAX 4500
# define RATE_YAW_FF 0.02
# define RATE_YAW_FILT_HZ 20.0f
# define THR_MIN_DEFAULT 0
# define AUTOTUNE_ENABLED DISABLED
#endif
/////////////////////////////////////////////////////////////////////////////////
// Y6 defaults
#if FRAME_CONFIG == Y6_FRAME
# define RATE_ROLL_P 0.1f
# define RATE_ROLL_D 0.006f
# define RATE_PITCH_P 0.1f
# define RATE_PITCH_D 0.006f
# define RATE_YAW_P 0.150f
# define RATE_YAW_I 0.015f
#endif
/////////////////////////////////////////////////////////////////////////////////
// Tri defaults
#if FRAME_CONFIG == TRI_FRAME
# define RATE_YAW_FILT_HZ 100.0f
#endif
//////////////////////////////////////////////////////////////////////////////
// PWM control
// default RC speed in Hz
#ifndef RC_FAST_SPEED
# define RC_FAST_SPEED 490
#endif
//////////////////////////////////////////////////////////////////////////////
// Sonar
//
#ifndef CONFIG_SONAR
# define CONFIG_SONAR ENABLED
#endif
#ifndef SONAR_ALT_HEALTH_MAX
# define SONAR_ALT_HEALTH_MAX 3 // number of good reads that indicates a healthy sonar
#endif
#ifndef SONAR_RELIABLE_DISTANCE_PCT
# define SONAR_RELIABLE_DISTANCE_PCT 0.60f // we trust the sonar out to 60% of it's maximum range
#endif
#ifndef SONAR_GAIN_DEFAULT
# define SONAR_GAIN_DEFAULT 0.8f // gain for controlling how quickly sonar range adjusts target altitude (lower means slower reaction)
#endif
#ifndef THR_SURFACE_TRACKING_VELZ_MAX
# define THR_SURFACE_TRACKING_VELZ_MAX 150 // max vertical speed change while surface tracking with sonar
#endif
#ifndef SONAR_TIMEOUT_MS
# define SONAR_TIMEOUT_MS 1000 // desired sonar alt will reset to current sonar alt after this many milliseconds without a good sonar alt
#endif
#ifndef SONAR_TILT_CORRECTION // by disable tilt correction for use of range finder data by EKF
# define SONAR_TILT_CORRECTION DISABLED
#endif
#ifndef MAV_SYSTEM_ID
# define MAV_SYSTEM_ID 1
#endif
//////////////////////////////////////////////////////////////////////////////
// Battery monitoring
//
#ifndef FS_BATT_VOLTAGE_DEFAULT
# define FS_BATT_VOLTAGE_DEFAULT 10.5f // default battery voltage below which failsafe will be triggered
#endif
#ifndef FS_BATT_MAH_DEFAULT
# define FS_BATT_MAH_DEFAULT 0 // default battery capacity (in mah) below which failsafe will be triggered
#endif
#ifndef BOARD_VOLTAGE_MIN
# define BOARD_VOLTAGE_MIN 4.3f // min board voltage in volts for pre-arm checks
#endif
#ifndef BOARD_VOLTAGE_MAX
# define BOARD_VOLTAGE_MAX 5.8f // max board voltage in volts for pre-arm checks
#endif
// prearm GPS hdop check
#ifndef GPS_HDOP_GOOD_DEFAULT
# define GPS_HDOP_GOOD_DEFAULT 140 // minimum hdop that represents a good position. used during pre-arm checks if fence is enabled
#endif
// GCS failsafe
#ifndef FS_GCS
# define FS_GCS DISABLED
#endif
#ifndef FS_GCS_TIMEOUT_MS
# define FS_GCS_TIMEOUT_MS 5000 // gcs failsafe triggers after 5 seconds with no GCS heartbeat
#endif
#ifndef GNDEFFECT_COMPENSATION
# define GNDEFFECT_COMPENSATION DISABLED
#endif
// possible values for FS_GCS parameter
#define FS_GCS_DISABLED 0
#define FS_GCS_ENABLED_ALWAYS_RTL 1
#define FS_GCS_ENABLED_CONTINUE_MISSION 2
// Radio failsafe while using RC_override
#ifndef FS_RADIO_RC_OVERRIDE_TIMEOUT_MS
# define FS_RADIO_RC_OVERRIDE_TIMEOUT_MS 1000 // RC Radio failsafe triggers after 1 second while using RC_override from ground station
#endif
// Radio failsafe
#ifndef FS_RADIO_TIMEOUT_MS
#define FS_RADIO_TIMEOUT_MS 500 // RC Radio Failsafe triggers after 500 miliseconds with No RC Input
#endif
#ifndef FS_CLOSE_TO_HOME_CM
# define FS_CLOSE_TO_HOME_CM 500 // if vehicle within 5m of home, vehicle will LAND instead of RTL during some failsafes
#endif
#ifndef PREARM_DISPLAY_PERIOD
# define PREARM_DISPLAY_PERIOD 30
#endif
// pre-arm baro vs inertial nav max alt disparity
#ifndef PREARM_MAX_ALT_DISPARITY_CM
# define PREARM_MAX_ALT_DISPARITY_CM 100 // barometer and inertial nav altitude must be within this many centimeters
#endif
// arming check's maximum acceptable accelerometer vector difference (in m/s/s) between primary and backup accelerometers
#ifndef PREARM_MAX_ACCEL_VECTOR_DIFF
#define PREARM_MAX_ACCEL_VECTOR_DIFF 0.70f // pre arm accel check will fail if primary and backup accelerometer vectors differ by 0.7m/s/s
#endif
// arming check's maximum acceptable rotation rate difference (in rad/sec) between primary and backup gyros
#ifndef PREARM_MAX_GYRO_VECTOR_DIFF
#define PREARM_MAX_GYRO_VECTOR_DIFF 0.0873f // pre arm gyro check will fail if primary and backup gyro vectors differ by 0.0873 rad/sec (=5deg/sec)
#endif
//////////////////////////////////////////////////////////////////////////////
// EKF Failsafe
#ifndef FS_EKF_ACTION_DEFAULT
# define FS_EKF_ACTION_DEFAULT FS_EKF_ACTION_LAND // EKF failsafe triggers land by default
#endif
#ifndef FS_EKF_THRESHOLD_DEFAULT
# define FS_EKF_THRESHOLD_DEFAULT 0.8f // EKF failsafe's default compass and velocity variance threshold above which the EKF failsafe will be triggered
#endif
#ifndef EKF_ORIGIN_MAX_DIST_M
# define EKF_ORIGIN_MAX_DIST_M 50000 // EKF origin and waypoints (including home) must be within 50km
#endif
//////////////////////////////////////////////////////////////////////////////
// MAGNETOMETER
#ifndef MAGNETOMETER
# define MAGNETOMETER ENABLED
#endif
// expected magnetic field strength. pre-arm checks will fail if 50% higher or lower than this value
#ifndef COMPASS_MAGFIELD_EXPECTED
#define COMPASS_MAGFIELD_EXPECTED 530 // pre arm will fail if mag field > 874 or < 185
#endif
// max compass offset length (i.e. sqrt(offs_x^2+offs_y^2+offs_Z^2))
#ifndef CONFIG_ARCH_BOARD_PX4FMU_V1
#ifndef COMPASS_OFFSETS_MAX
# define COMPASS_OFFSETS_MAX 600 // PX4 onboard compass has high offsets
#endif
#else // SITL, FLYMAPLE, etc
#ifndef COMPASS_OFFSETS_MAX
# define COMPASS_OFFSETS_MAX 500
#endif
#endif
//////////////////////////////////////////////////////////////////////////////
// OPTICAL_FLOW
#ifndef OPTFLOW
# define OPTFLOW ENABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// Auto Tuning
#ifndef AUTOTUNE_ENABLED
# define AUTOTUNE_ENABLED ENABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// Crop Sprayer
#ifndef SPRAYER
# define SPRAYER DISABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// Precision Landing with companion computer or IRLock sensor
#ifndef PRECISION_LANDING
# define PRECISION_LANDING DISABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// EPM cargo gripper
#ifndef EPM_ENABLED
# define EPM_ENABLED ENABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// Parachute release
#ifndef PARACHUTE
# define PARACHUTE ENABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// ADSB support
#ifndef ADSB_ENABLED
# define ADSB_ENABLED ENABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// Nav-Guided - allows external nav computer to control vehicle
#ifndef NAV_GUIDED
# define NAV_GUIDED ENABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// RADIO CONFIGURATION
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// FLIGHT_MODE
//
#ifndef FLIGHT_MODE_1
# define FLIGHT_MODE_1 STABILIZE
#endif
#ifndef FLIGHT_MODE_2
# define FLIGHT_MODE_2 STABILIZE
#endif
#ifndef FLIGHT_MODE_3
# define FLIGHT_MODE_3 STABILIZE
#endif
#ifndef FLIGHT_MODE_4
# define FLIGHT_MODE_4 STABILIZE
#endif
#ifndef FLIGHT_MODE_5
# define FLIGHT_MODE_5 STABILIZE
#endif
#ifndef FLIGHT_MODE_6
# define FLIGHT_MODE_6 STABILIZE
#endif
//////////////////////////////////////////////////////////////////////////////
// Throttle Failsafe
//
#ifndef FS_THR_VALUE_DEFAULT
# define FS_THR_VALUE_DEFAULT 975
#endif
//////////////////////////////////////////////////////////////////////////////
// Takeoff
//
#ifndef PILOT_TKOFF_ALT_DEFAULT
# define PILOT_TKOFF_ALT_DEFAULT 0 // default final alt above home for pilot initiated takeoff
#endif
//////////////////////////////////////////////////////////////////////////////
// Landing
//
#ifndef LAND_SPEED
# define LAND_SPEED 50 // the descent speed for the final stage of landing in cm/s
#endif
#ifndef LAND_START_ALT
# define LAND_START_ALT 1000 // altitude in cm where land controller switches to slow rate of descent
#endif
#ifndef LAND_REQUIRE_MIN_THROTTLE_TO_DISARM
# define LAND_REQUIRE_MIN_THROTTLE_TO_DISARM DISABLED // we do not require pilot to reduce throttle to minimum before vehicle will disarm in AUTO, LAND or RTL
#endif
#ifndef LAND_REPOSITION_DEFAULT
# define LAND_REPOSITION_DEFAULT 1 // by default the pilot can override roll/pitch during landing
#endif
#ifndef LAND_WITH_DELAY_MS
# define LAND_WITH_DELAY_MS 4000 // default delay (in milliseconds) when a land-with-delay is triggered during a failsafe event
#endif
//////////////////////////////////////////////////////////////////////////////
// Landing Detector
//
#ifndef LAND_DETECTOR_TRIGGER_SEC
# define LAND_DETECTOR_TRIGGER_SEC 1.0f // number of seconds to detect a landing
#endif
#ifndef LAND_DETECTOR_MAYBE_TRIGGER_SEC
# define LAND_DETECTOR_MAYBE_TRIGGER_SEC 0.2f // number of seconds that means we might be landed (used to reset horizontal position targets to prevent tipping over)
#endif
#ifndef LAND_DETECTOR_ACCEL_LPF_CUTOFF
# define LAND_DETECTOR_ACCEL_LPF_CUTOFF 1.0f // frequency cutoff of land detector accelerometer filter
#endif
#ifndef LAND_DETECTOR_ACCEL_MAX
# define LAND_DETECTOR_ACCEL_MAX 1.0f // vehicle acceleration must be under 1m/s/s
#endif
//////////////////////////////////////////////////////////////////////////////
// CAMERA TRIGGER AND CONTROL
//
#ifndef CAMERA
# define CAMERA ENABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// MOUNT (ANTENNA OR CAMERA)
//
#ifndef MOUNT
# define MOUNT ENABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// Flight mode definitions
//
// Acro Mode
#ifndef ACRO_RP_P
# define ACRO_RP_P 4.5f
#endif
#ifndef ACRO_YAW_P
# define ACRO_YAW_P 4.5f
#endif
#ifndef ACRO_LEVEL_MAX_ANGLE
# define ACRO_LEVEL_MAX_ANGLE 3000
#endif
#ifndef ACRO_BALANCE_ROLL
#define ACRO_BALANCE_ROLL 1.0f
#endif
#ifndef ACRO_BALANCE_PITCH
#define ACRO_BALANCE_PITCH 1.0f
#endif
#ifndef ACRO_EXPO_DEFAULT
#define ACRO_EXPO_DEFAULT 0.3f
#endif
// Stabilize (angle controller) gains
#ifndef STABILIZE_ROLL_P
# define STABILIZE_ROLL_P 4.5f
#endif
#ifndef STABILIZE_PITCH_P
# define STABILIZE_PITCH_P 4.5f
#endif
#ifndef STABILIZE_YAW_P
# define STABILIZE_YAW_P 4.5f
#endif
// RTL Mode
#ifndef RTL_ALT_FINAL
# define RTL_ALT_FINAL 0 // the altitude the vehicle will move to as the final stage of Returning to Launch. Set to zero to land.
#endif
#ifndef RTL_ALT
# define RTL_ALT 1500 // default alt to return to home in cm, 0 = Maintain current altitude
#endif
#ifndef RTL_ALT_MIN
# define RTL_ALT_MIN 200 // min height above ground for RTL (i.e 2m)
#endif
#ifndef RTL_CLIMB_MIN_DEFAULT
# define RTL_CLIMB_MIN_DEFAULT 0 // vehicle will always climb this many cm as first stage of RTL
#endif
#ifndef RTL_LOITER_TIME
# define RTL_LOITER_TIME 5000 // Time (in milliseconds) to loiter above home before begining final descent
#endif
// AUTO Mode
#ifndef WP_YAW_BEHAVIOR_DEFAULT
# define WP_YAW_BEHAVIOR_DEFAULT WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL
#endif
#ifndef AUTO_YAW_SLEW_RATE
# define AUTO_YAW_SLEW_RATE 60 // degrees/sec
#endif
#ifndef YAW_LOOK_AHEAD_MIN_SPEED
# define YAW_LOOK_AHEAD_MIN_SPEED 100 // minimum ground speed in cm/s required before copter is aimed at ground course
#endif
// Super Simple mode
#ifndef SUPER_SIMPLE_RADIUS
# define SUPER_SIMPLE_RADIUS 1000
#endif
//////////////////////////////////////////////////////////////////////////////
// Stabilize Rate Control
//
#ifndef ROLL_PITCH_INPUT_MAX
# define ROLL_PITCH_INPUT_MAX 4500 // roll, pitch input range
#endif
#ifndef DEFAULT_ANGLE_MAX
# define DEFAULT_ANGLE_MAX 4500 // ANGLE_MAX parameters default value
#endif
#ifndef ANGLE_RATE_MAX
# define ANGLE_RATE_MAX 18000 // default maximum rotation rate in roll/pitch axis requested by angle controller used in stabilize, loiter, rtl, auto flight modes
#endif
//////////////////////////////////////////////////////////////////////////////
// Rate controller gains
//
#ifndef RATE_ROLL_P
# define RATE_ROLL_P 0.150f
#endif
#ifndef RATE_ROLL_I
# define RATE_ROLL_I 0.100f
#endif
#ifndef RATE_ROLL_D
# define RATE_ROLL_D 0.004f
#endif
#ifndef RATE_ROLL_IMAX
# define RATE_ROLL_IMAX 2000
#endif
#ifndef RATE_ROLL_FILT_HZ
# define RATE_ROLL_FILT_HZ 20.0f
#endif
#ifndef RATE_PITCH_P
# define RATE_PITCH_P 0.150f
#endif
#ifndef RATE_PITCH_I
# define RATE_PITCH_I 0.100f
#endif
#ifndef RATE_PITCH_D
# define RATE_PITCH_D 0.004f
#endif
#ifndef RATE_PITCH_IMAX
# define RATE_PITCH_IMAX 2000
#endif
#ifndef RATE_PITCH_FILT_HZ
# define RATE_PITCH_FILT_HZ 20.0f
#endif
#ifndef RATE_YAW_P
# define RATE_YAW_P 0.200f
#endif
#ifndef RATE_YAW_I
# define RATE_YAW_I 0.020f
#endif
#ifndef RATE_YAW_D
# define RATE_YAW_D 0.000f
#endif
#ifndef RATE_YAW_IMAX
# define RATE_YAW_IMAX 1000
#endif
#ifndef RATE_YAW_FILT_HZ
# define RATE_YAW_FILT_HZ 5.0f
#endif
//////////////////////////////////////////////////////////////////////////////
// Loiter position control gains
//
#ifndef POS_XY_P
# define POS_XY_P 1.0f
#endif
//////////////////////////////////////////////////////////////////////////////
// Stop mode defaults
//
#ifndef BRAKE_MODE_SPEED_Z
# define BRAKE_MODE_SPEED_Z 250 // z-axis speed in cm/s in Brake Mode
#endif
#ifndef BRAKE_MODE_DECEL_RATE
# define BRAKE_MODE_DECEL_RATE 750 // acceleration rate in cm/s/s in Brake Mode
#endif
//////////////////////////////////////////////////////////////////////////////
// Velocity (horizontal) gains
//
#ifndef VEL_XY_P
# define VEL_XY_P 1.0f
#endif
#ifndef VEL_XY_I
# define VEL_XY_I 0.5f
#endif
#ifndef VEL_XY_IMAX
# define VEL_XY_IMAX 1000
#endif
#ifndef VEL_XY_FILT_HZ
# define VEL_XY_FILT_HZ 5.0f
#endif
//////////////////////////////////////////////////////////////////////////////
// PosHold parameter defaults
//
#ifndef POSHOLD_ENABLED
# define POSHOLD_ENABLED ENABLED // PosHold flight mode enabled by default
#endif
#ifndef POSHOLD_BRAKE_RATE_DEFAULT
# define POSHOLD_BRAKE_RATE_DEFAULT 8 // default POSHOLD_BRAKE_RATE param value. Rotation rate during braking in deg/sec
#endif
#ifndef POSHOLD_BRAKE_ANGLE_DEFAULT
# define POSHOLD_BRAKE_ANGLE_DEFAULT 3000 // default POSHOLD_BRAKE_ANGLE param value. Max lean angle during braking in centi-degrees
#endif
//////////////////////////////////////////////////////////////////////////////
// Throttle control gains
//
#ifndef THR_MID_DEFAULT
# define THR_MID_DEFAULT 500 // Throttle output (0 ~ 1000) when throttle stick is in mid position
#endif
#ifndef THR_MIN_DEFAULT
# define THR_MIN_DEFAULT 130 // minimum throttle sent to the motors when armed and pilot throttle above zero
#endif
#define THR_MAX 1000 // maximum throttle input and output sent to the motors
#ifndef THR_DZ_DEFAULT
# define THR_DZ_DEFAULT 100 // the deadzone above and below mid throttle while in althold or loiter
#endif
#ifndef ALT_HOLD_P
# define ALT_HOLD_P 1.0f
#endif
// Velocity (vertical) control gains
#ifndef VEL_Z_P
# define VEL_Z_P 5.0f
#endif
// Accel (vertical) control gains
#ifndef ACCEL_Z_P
# define ACCEL_Z_P 0.50f
#endif
#ifndef ACCEL_Z_I
# define ACCEL_Z_I 1.00f
#endif
#ifndef ACCEL_Z_D
# define ACCEL_Z_D 0.0f
#endif
#ifndef ACCEL_Z_IMAX
# define ACCEL_Z_IMAX 800
#endif
#ifndef ACCEL_Z_FILT_HZ
# define ACCEL_Z_FILT_HZ 20.0f
#endif
// default maximum vertical velocity and acceleration the pilot may request
#ifndef PILOT_VELZ_MAX
# define PILOT_VELZ_MAX 250 // maximum vertical velocity in cm/s
#endif
#ifndef PILOT_ACCEL_Z_DEFAULT
# define PILOT_ACCEL_Z_DEFAULT 250 // vertical acceleration in cm/s/s while altitude is under pilot control
#endif
// max distance in cm above or below current location that will be used for the alt target when transitioning to alt-hold mode
#ifndef ALT_HOLD_INIT_MAX_OVERSHOOT
# define ALT_HOLD_INIT_MAX_OVERSHOOT 200
#endif
// the acceleration used to define the distance-velocity curve
#ifndef ALT_HOLD_ACCEL_MAX
# define ALT_HOLD_ACCEL_MAX 250 // if you change this you must also update the duplicate declaration in AC_WPNav.h
#endif
#ifndef AUTO_DISARMING_DELAY
# define AUTO_DISARMING_DELAY 10
#endif
//////////////////////////////////////////////////////////////////////////////
// Dataflash logging control
//
#ifndef LOGGING_ENABLED
# define LOGGING_ENABLED ENABLED
#endif
// Default logging bitmask
#ifndef DEFAULT_LOG_BITMASK
# define DEFAULT_LOG_BITMASK \
MASK_LOG_ATTITUDE_MED | \
MASK_LOG_GPS | \
MASK_LOG_PM | \
MASK_LOG_CTUN | \
MASK_LOG_NTUN | \
MASK_LOG_RCIN | \
MASK_LOG_IMU | \
MASK_LOG_CMD | \
MASK_LOG_CURRENT | \
MASK_LOG_RCOUT | \
MASK_LOG_OPTFLOW | \
MASK_LOG_COMPASS | \
MASK_LOG_CAMERA | \
MASK_LOG_MOTBATT
#endif
//////////////////////////////////////////////////////////////////////////////
// AP_Limits Defaults
//
// Enable/disable AP_Limits
#ifndef AC_FENCE
#define AC_FENCE ENABLED
#endif
#ifndef AC_RALLY
#define AC_RALLY ENABLED
#endif
#ifndef AC_TERRAIN
#define AC_TERRAIN ENABLED
#if !AC_RALLY
#error Terrain relies on Rally which is disabled
#endif
#endif
//////////////////////////////////////////////////////////////////////////////
// Developer Items
//
// use this to completely disable the CLI
#ifndef CLI_ENABLED
# define CLI_ENABLED ENABLED
#endif
//use this to completely disable FRSKY TELEM
#ifndef FRSKY_TELEM_ENABLED
# define FRSKY_TELEM_ENABLED ENABLED
#endif
/*
build a firmware version string.
GIT_VERSION comes from Makefile builds
*/
#ifndef GIT_VERSION
#define FIRMWARE_STRING THISFIRMWARE
#else
#define FIRMWARE_STRING THISFIRMWARE " (" GIT_VERSION ")"
#endif
#endif // __ARDUCOPTER_CONFIG_H__

22
ArduSub/config_channels.h Normal file
View File

@ -0,0 +1,22 @@
#ifndef __ARDUCOPTER_CONFIG_MOTORS_H__
#define __ARDUCOPTER_CONFIG_MOTORS_H__
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//
// WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING
//
// DO NOT EDIT this file to adjust your configuration. Create your own
// APM_Config.h and use APM_Config.h.example as a reference.
//
// WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING
///
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//
#include "config.h" // Parent Config File
#include "APM_Config.h" // User Overrides
#endif // __ARDUCOPTER_CONFIG_MOTORS_H__

162
ArduSub/control_acro.cpp Normal file
View File

@ -0,0 +1,162 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
/*
* control_acro.pde - init and run calls for acro flight mode
*/
// acro_init - initialise acro controller
bool Copter::acro_init(bool ignore_checks)
{
// if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high
if (motors.armed() && ap.land_complete && !mode_has_manual_throttle(control_mode) && (g.rc_3.control_in > get_non_takeoff_throttle())) {
return false;
}
// set target altitude to zero for reporting
pos_control.set_alt_target(0);
return true;
}
// acro_run - runs the acro controller
// should be called at 100hz or more
void Copter::acro_run()
{
float target_roll, target_pitch, target_yaw;
int16_t pilot_throttle_scaled;
// if motors not running reset angle targets
if(!motors.armed() || ap.throttle_zero) {
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
// slow start if landed
if (ap.land_complete) {
motors.slow_start(true);
}
return;
}
// convert the input to the desired body frame rate
get_pilot_desired_angle_rates(channel_roll->control_in, channel_pitch->control_in, channel_yaw->control_in, target_roll, target_pitch, target_yaw);
// get pilot's desired throttle
pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->control_in);
// run attitude controller
attitude_control.input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw);
// output pilot's throttle without angle boost
attitude_control.set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt);
}
// get_pilot_desired_angle_rates - transform pilot's roll pitch and yaw input into a desired lean angle rates
// returns desired angle rates in centi-degrees-per-second
void Copter::get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out)
{
float rate_limit;
Vector3f rate_ef_level, rate_bf_level, rate_bf_request;
// apply circular limit to pitch and roll inputs
float total_in = pythagorous2((float)pitch_in, (float)roll_in);
if (total_in > ROLL_PITCH_INPUT_MAX) {
float ratio = (float)ROLL_PITCH_INPUT_MAX / total_in;
roll_in *= ratio;
pitch_in *= ratio;
}
// calculate roll, pitch rate requests
if (g.acro_expo <= 0) {
rate_bf_request.x = roll_in * g.acro_rp_p;
rate_bf_request.y = pitch_in * g.acro_rp_p;
} else {
// expo variables
float rp_in, rp_in3, rp_out;
// range check expo
if (g.acro_expo > 1.0f) {
g.acro_expo = 1.0f;
}
// roll expo
rp_in = float(roll_in)/ROLL_PITCH_INPUT_MAX;
rp_in3 = rp_in*rp_in*rp_in;
rp_out = (g.acro_expo * rp_in3) + ((1 - g.acro_expo) * rp_in);
rate_bf_request.x = ROLL_PITCH_INPUT_MAX * rp_out * g.acro_rp_p;
// pitch expo
rp_in = float(pitch_in)/ROLL_PITCH_INPUT_MAX;
rp_in3 = rp_in*rp_in*rp_in;
rp_out = (g.acro_expo * rp_in3) + ((1 - g.acro_expo) * rp_in);
rate_bf_request.y = ROLL_PITCH_INPUT_MAX * rp_out * g.acro_rp_p;
}
// calculate yaw rate request
rate_bf_request.z = yaw_in * g.acro_yaw_p;
// calculate earth frame rate corrections to pull the copter back to level while in ACRO mode
if (g.acro_trainer != ACRO_TRAINER_DISABLED) {
// Calculate trainer mode earth frame rate command for roll
int32_t roll_angle = wrap_180_cd(ahrs.roll_sensor);
rate_ef_level.x = -constrain_int32(roll_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_roll;
// Calculate trainer mode earth frame rate command for pitch
int32_t pitch_angle = wrap_180_cd(ahrs.pitch_sensor);
rate_ef_level.y = -constrain_int32(pitch_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_pitch;
// Calculate trainer mode earth frame rate command for yaw
rate_ef_level.z = 0;
// Calculate angle limiting earth frame rate commands
if (g.acro_trainer == ACRO_TRAINER_LIMITED) {
if (roll_angle > aparm.angle_max){
rate_ef_level.x -= g.acro_balance_roll*(roll_angle-aparm.angle_max);
}else if (roll_angle < -aparm.angle_max) {
rate_ef_level.x -= g.acro_balance_roll*(roll_angle+aparm.angle_max);
}
if (pitch_angle > aparm.angle_max){
rate_ef_level.y -= g.acro_balance_pitch*(pitch_angle-aparm.angle_max);
}else if (pitch_angle < -aparm.angle_max) {
rate_ef_level.y -= g.acro_balance_pitch*(pitch_angle+aparm.angle_max);
}
}
// convert earth-frame level rates to body-frame level rates
attitude_control.euler_rate_to_ang_vel(attitude_control.get_att_target_euler_cd()*radians(0.01f), rate_ef_level, rate_bf_level);
// combine earth frame rate corrections with rate requests
if (g.acro_trainer == ACRO_TRAINER_LIMITED) {
rate_bf_request.x += rate_bf_level.x;
rate_bf_request.y += rate_bf_level.y;
rate_bf_request.z += rate_bf_level.z;
}else{
float acro_level_mix = constrain_float(1-MAX(MAX(abs(roll_in), abs(pitch_in)), abs(yaw_in))/4500.0, 0, 1)*ahrs.cos_pitch();
// Scale leveling rates by stick input
rate_bf_level = rate_bf_level*acro_level_mix;
// Calculate rate limit to prevent change of rate through inverted
rate_limit = fabsf(fabsf(rate_bf_request.x)-fabsf(rate_bf_level.x));
rate_bf_request.x += rate_bf_level.x;
rate_bf_request.x = constrain_float(rate_bf_request.x, -rate_limit, rate_limit);
// Calculate rate limit to prevent change of rate through inverted
rate_limit = fabsf(fabsf(rate_bf_request.y)-fabsf(rate_bf_level.y));
rate_bf_request.y += rate_bf_level.y;
rate_bf_request.y = constrain_float(rate_bf_request.y, -rate_limit, rate_limit);
// Calculate rate limit to prevent change of rate through inverted
rate_limit = fabsf(fabsf(rate_bf_request.z)-fabsf(rate_bf_level.z));
rate_bf_request.z += rate_bf_level.z;
rate_bf_request.z = constrain_float(rate_bf_request.z, -rate_limit, rate_limit);
}
}
// hand back rate request
roll_out = rate_bf_request.x;
pitch_out = rate_bf_request.y;
yaw_out = rate_bf_request.z;
}

161
ArduSub/control_althold.cpp Normal file
View File

@ -0,0 +1,161 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
/*
* control_althold.pde - init and run calls for althold, flight mode
*/
// althold_init - initialise althold controller
bool Copter::althold_init(bool ignore_checks)
{
#if FRAME_CONFIG == HELI_FRAME
// do not allow helis to enter Alt Hold if the Rotor Runup is not complete
if (!ignore_checks && !motors.rotor_runup_complete()){
return false;
}
#endif
// initialize vertical speeds and leash lengths
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
pos_control.set_accel_z(g.pilot_accel_z);
// initialise position and desired velocity
pos_control.set_alt_target(inertial_nav.get_altitude());
pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
// stop takeoff if running
takeoff_stop();
return true;
}
// althold_run - runs the althold controller
// should be called at 100hz or more
void Copter::althold_run()
{
AltHoldModeState althold_state;
float takeoff_climb_rate = 0.0f;
// initialize vertical speeds and acceleration
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
pos_control.set_accel_z(g.pilot_accel_z);
// apply SIMPLE mode transform to pilot inputs
update_simple_mode();
// get pilot desired lean angles
float target_roll, target_pitch;
get_pilot_desired_lean_angles(channel_roll->control_in, channel_pitch->control_in, target_roll, target_pitch, attitude_control.get_althold_lean_angle_max());
// get pilot's desired yaw rate
float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);
// get pilot desired climb rate
float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->control_in);
target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max, g.pilot_velocity_z_max);
#if FRAME_CONFIG == HELI_FRAME
// helicopters are held on the ground until rotor speed runup has finished
bool takeoff_triggered = (ap.land_complete && (channel_throttle->control_in > get_takeoff_trigger_throttle()) && motors.rotor_runup_complete());
#else
bool takeoff_triggered = (ap.land_complete && (channel_throttle->control_in > get_takeoff_trigger_throttle()));
#endif
// Alt Hold State Machine Determination
if(!ap.auto_armed) {
althold_state = AltHold_Disarmed;
} else if (!motors.get_interlock()){
althold_state = AltHold_MotorStop;
} else if (takeoff_state.running || takeoff_triggered){
althold_state = AltHold_Takeoff;
} else if (ap.land_complete){
althold_state = AltHold_Landed;
} else {
althold_state = AltHold_Flying;
}
// Alt Hold State Machine
switch (althold_state) {
case AltHold_Disarmed:
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
attitude_control.set_yaw_target_to_current_heading();
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // Multicopter do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif // HELI_FRAME
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);
break;
case AltHold_MotorStop:
#if FRAME_CONFIG == HELI_FRAME
// helicopters are capable of flying even with the motor stopped, therefore we will attempt to keep flying
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
// force descent rate and call position controller
pos_control.set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false);
pos_control.update_z_controller();
#else // Multicopter do not stabilize roll/pitch/yaw when motor are stopped
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);
#endif // HELI_FRAME
break;
case AltHold_Takeoff:
// initiate take-off
if (!takeoff_state.running) {
takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));
// indicate we are taking off
set_land_complete(false);
// clear i terms
set_throttle_takeoff();
}
// get take-off adjusted pilot and takeoff climb rates
takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate);
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
// call position controller
pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
pos_control.add_takeoff_climb_rate(takeoff_climb_rate, G_Dt);
pos_control.update_z_controller();
break;
case AltHold_Landed:
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
attitude_control.set_yaw_target_to_current_heading();
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
attitude_control.set_throttle_out(get_throttle_pre_takeoff(channel_throttle->control_in),false,g.throttle_filt);
#else // Multicopter do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(get_throttle_pre_takeoff(channel_throttle->control_in),true,g.throttle_filt);
#endif
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);
break;
case AltHold_Flying:
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
// call throttle controller
if (sonar_enabled && (sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) {
// if sonar is ok, use surface tracking
target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control.get_alt_target(), G_Dt);
}
// call position controller
pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
pos_control.update_z_controller();
break;
}
}

672
ArduSub/control_auto.cpp Normal file
View File

@ -0,0 +1,672 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
/*
* control_auto.pde - init and run calls for auto flight mode
*
* This file contains the implementation for Land, Waypoint navigation and Takeoff from Auto mode
* Command execution code (i.e. command_logic.pde) should:
* a) switch to Auto flight mode with set_mode() function. This will cause auto_init to be called
* b) call one of the three auto initialisation functions: auto_wp_start(), auto_takeoff_start(), auto_land_start()
* c) call one of the verify functions auto_wp_verify(), auto_takeoff_verify, auto_land_verify repeated to check if the command has completed
* The main loop (i.e. fast loop) will call update_flight_modes() which will in turn call auto_run() which, based upon the auto_mode variable will call
* correct auto_wp_run, auto_takeoff_run or auto_land_run to actually implement the feature
*/
/*
* While in the auto flight mode, navigation or do/now commands can be run.
* Code in this file implements the navigation commands
*/
// auto_init - initialise auto controller
bool Copter::auto_init(bool ignore_checks)
{
if ((position_ok() && mission.num_commands() > 1) || ignore_checks) {
auto_mode = Auto_Loiter;
// stop ROI from carrying over from previous runs of the mission
// To-Do: reset the yaw as part of auto_wp_start when the previous command was not a wp command to remove the need for this special ROI check
if (auto_yaw_mode == AUTO_YAW_ROI) {
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
// initialise waypoint and spline controller
wp_nav.wp_and_spline_init();
// clear guided limits
guided_limit_clear();
// start/resume the mission (based on MIS_RESTART parameter)
mission.start_or_resume();
return true;
}else{
return false;
}
}
// auto_run - runs the auto controller
// should be called at 100hz or more
// relies on run_autopilot being called at 10hz which handles decision making and non-navigation related commands
void Copter::auto_run()
{
// call the correct auto controller
switch (auto_mode) {
case Auto_TakeOff:
auto_takeoff_run();
break;
case Auto_WP:
case Auto_CircleMoveToEdge:
auto_wp_run();
break;
case Auto_Land:
auto_land_run();
break;
case Auto_RTL:
auto_rtl_run();
break;
case Auto_Circle:
auto_circle_run();
break;
case Auto_Spline:
auto_spline_run();
break;
case Auto_NavGuided:
#if NAV_GUIDED == ENABLED
auto_nav_guided_run();
#endif
break;
case Auto_Loiter:
auto_loiter_run();
break;
}
}
// auto_takeoff_start - initialises waypoint controller to implement take-off
void Copter::auto_takeoff_start(float final_alt_above_home)
{
auto_mode = Auto_TakeOff;
// initialise wpnav destination
Vector3f target_pos = inertial_nav.get_position();
target_pos.z = pv_alt_above_origin(final_alt_above_home);
wp_nav.set_wp_destination(target_pos);
// initialise yaw
set_auto_yaw_mode(AUTO_YAW_HOLD);
// clear i term when we're taking off
set_throttle_takeoff();
}
// auto_takeoff_run - takeoff in auto mode
// called by auto_run at 100hz or more
void Copter::auto_takeoff_run()
{
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || !motors.get_interlock()) {
// initialise wpnav targets
wp_nav.shift_wp_origin_to_current_pos();
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
// reset attitude control targets
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
// clear i term when we're taking off
set_throttle_takeoff();
return;
}
// process pilot's yaw input
float target_yaw_rate = 0;
if (!failsafe.radio) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);
}
// run waypoint controller
wp_nav.update_wpnav();
// call z-axis position controller (wpnav should have already updated it's alt target)
pos_control.update_z_controller();
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
}
// auto_wp_start - initialises waypoint controller to implement flying to a particular destination
void Copter::auto_wp_start(const Vector3f& destination)
{
auto_mode = Auto_WP;
// initialise wpnav
wp_nav.set_wp_destination(destination);
// initialise yaw
// To-Do: reset the yaw only when the previous navigation command is not a WP. this would allow removing the special check for ROI
if (auto_yaw_mode != AUTO_YAW_ROI) {
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
}
}
// auto_wp_run - runs the auto waypoint controller
// called by auto_run at 100hz or more
void Copter::auto_wp_run()
{
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || !motors.get_interlock()) {
// To-Do: reset waypoint origin to current location because copter is probably on the ground so we don't want it lurching left or right on take-off
// (of course it would be better if people just used take-off)
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
// clear i term when we're taking off
set_throttle_takeoff();
return;
}
// process pilot's yaw input
float target_yaw_rate = 0;
if (!failsafe.radio) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);
if (!is_zero(target_yaw_rate)) {
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
}
// run waypoint controller
wp_nav.update_wpnav();
// call z-axis position controller (wpnav should have already updated it's alt target)
pos_control.update_z_controller();
// call attitude controller
if (auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
}else{
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true);
}
}
// auto_spline_start - initialises waypoint controller to implement flying to a particular destination using the spline controller
// seg_end_type can be SEGMENT_END_STOP, SEGMENT_END_STRAIGHT or SEGMENT_END_SPLINE. If Straight or Spline the next_destination should be provided
void Copter::auto_spline_start(const Vector3f& destination, bool stopped_at_start,
AC_WPNav::spline_segment_end_type seg_end_type,
const Vector3f& next_destination)
{
auto_mode = Auto_Spline;
// initialise wpnav
wp_nav.set_spline_destination(destination, stopped_at_start, seg_end_type, next_destination);
// initialise yaw
// To-Do: reset the yaw only when the previous navigation command is not a WP. this would allow removing the special check for ROI
if (auto_yaw_mode != AUTO_YAW_ROI) {
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
}
}
// auto_spline_run - runs the auto spline controller
// called by auto_run at 100hz or more
void Copter::auto_spline_run()
{
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || !motors.get_interlock()) {
// To-Do: reset waypoint origin to current location because copter is probably on the ground so we don't want it lurching left or right on take-off
// (of course it would be better if people just used take-off)
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
// clear i term when we're taking off
set_throttle_takeoff();
return;
}
// process pilot's yaw input
float target_yaw_rate = 0;
if (!failsafe.radio) {
// get pilot's desired yaw rat
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);
if (!is_zero(target_yaw_rate)) {
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
}
// run waypoint controller
wp_nav.update_spline();
// call z-axis position controller (wpnav should have already updated it's alt target)
pos_control.update_z_controller();
// call attitude controller
if (auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
}else{
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(), true);
}
}
// auto_land_start - initialises controller to implement a landing
void Copter::auto_land_start()
{
// set target to stopping point
Vector3f stopping_point;
wp_nav.get_loiter_stopping_point_xy(stopping_point);
// call location specific land start function
auto_land_start(stopping_point);
}
// auto_land_start - initialises controller to implement a landing
void Copter::auto_land_start(const Vector3f& destination)
{
auto_mode = Auto_Land;
// initialise loiter target destination
wp_nav.init_loiter_target(destination);
// initialise altitude target to stopping point
pos_control.set_target_to_stopping_point_z();
// initialise yaw
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
// auto_land_run - lands in auto mode
// called by auto_run at 100hz or more
void Copter::auto_land_run()
{
int16_t roll_control = 0, pitch_control = 0;
float target_yaw_rate = 0;
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || ap.land_complete) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
// set target to current position
wp_nav.init_loiter_target();
return;
}
// relax loiter targets if we might be landed
if (ap.land_complete_maybe) {
wp_nav.loiter_soften_for_landing();
}
// process pilot's input
if (!failsafe.radio) {
if (g.land_repositioning) {
// apply SIMPLE mode transform to pilot inputs
update_simple_mode();
// process pilot's roll and pitch input
roll_control = channel_roll->control_in;
pitch_control = channel_pitch->control_in;
}
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);
}
// process roll, pitch inputs
wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control);
// run loiter controller
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
// call z-axis position controller
float cmb_rate = get_land_descent_speed();
pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt, true);
pos_control.update_z_controller();
// record desired climb rate for logging
desired_climb_rate = cmb_rate;
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
}
// auto_rtl_start - initialises RTL in AUTO flight mode
void Copter::auto_rtl_start()
{
auto_mode = Auto_RTL;
// call regular rtl flight mode initialisation and ask it to ignore checks
rtl_init(true);
}
// auto_rtl_run - rtl in AUTO flight mode
// called by auto_run at 100hz or more
void Copter::auto_rtl_run()
{
// call regular rtl flight mode run function
rtl_run();
}
// auto_circle_movetoedge_start - initialise waypoint controller to move to edge of a circle with it's center at the specified location
// we assume the caller has set the circle's circle with circle_nav.set_center()
// we assume the caller has performed all required GPS_ok checks
void Copter::auto_circle_movetoedge_start()
{
// check our distance from edge of circle
Vector3f circle_edge;
circle_nav.get_closest_point_on_circle(circle_edge);
// set the state to move to the edge of the circle
auto_mode = Auto_CircleMoveToEdge;
// initialise wpnav to move to edge of circle
wp_nav.set_wp_destination(circle_edge);
// if we are outside the circle, point at the edge, otherwise hold yaw
const Vector3f &curr_pos = inertial_nav.get_position();
const Vector3f &circle_center = circle_nav.get_center();
float dist_to_center = pythagorous2(circle_center.x - curr_pos.x, circle_center.y - curr_pos.y);
if (dist_to_center > circle_nav.get_radius() && dist_to_center > 500) {
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
} else {
// vehicle is within circle so hold yaw to avoid spinning as we move to edge of circle
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
}
// auto_circle_start - initialises controller to fly a circle in AUTO flight mode
void Copter::auto_circle_start()
{
auto_mode = Auto_Circle;
// initialise circle controller
// center was set in do_circle so initialise with current center
circle_nav.init(circle_nav.get_center());
}
// auto_circle_run - circle in AUTO flight mode
// called by auto_run at 100hz or more
void Copter::auto_circle_run()
{
// call circle controller
circle_nav.update();
// call z-axis position controller
pos_control.update_z_controller();
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_yaw(circle_nav.get_roll(), circle_nav.get_pitch(), circle_nav.get_yaw(),true);
}
#if NAV_GUIDED == ENABLED
// auto_nav_guided_start - hand over control to external navigation controller in AUTO mode
void Copter::auto_nav_guided_start()
{
auto_mode = Auto_NavGuided;
// call regular guided flight mode initialisation
guided_init(true);
// initialise guided start time and position as reference for limit checking
guided_limit_init_time_and_pos();
}
// auto_nav_guided_run - allows control by external navigation controller
// called by auto_run at 100hz or more
void Copter::auto_nav_guided_run()
{
// call regular guided flight mode run function
guided_run();
}
#endif // NAV_GUIDED
// auto_loiter_start - initialises loitering in auto mode
// returns success/failure because this can be called by exit_mission
bool Copter::auto_loiter_start()
{
// return failure if GPS is bad
if (!position_ok()) {
return false;
}
auto_mode = Auto_Loiter;
Vector3f origin = inertial_nav.get_position();
// calculate stopping point
Vector3f stopping_point;
pos_control.get_stopping_point_xy(stopping_point);
pos_control.get_stopping_point_z(stopping_point);
// initialise waypoint controller target to stopping point
wp_nav.set_wp_origin_and_destination(origin, stopping_point);
// hold yaw at current heading
set_auto_yaw_mode(AUTO_YAW_HOLD);
return true;
}
// auto_loiter_run - loiter in AUTO flight mode
// called by auto_run at 100hz or more
void Copter::auto_loiter_run()
{
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
return;
}
// accept pilot input of yaw
float target_yaw_rate = 0;
if(!failsafe.radio) {
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);
}
// run waypoint and z-axis postion controller
wp_nav.update_wpnav();
pos_control.update_z_controller();
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
}
// get_default_auto_yaw_mode - returns auto_yaw_mode based on WP_YAW_BEHAVIOR parameter
// set rtl parameter to true if this is during an RTL
uint8_t Copter::get_default_auto_yaw_mode(bool rtl)
{
switch (g.wp_yaw_behavior) {
case WP_YAW_BEHAVIOR_NONE:
return AUTO_YAW_HOLD;
break;
case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL:
if (rtl) {
return AUTO_YAW_HOLD;
}else{
return AUTO_YAW_LOOK_AT_NEXT_WP;
}
break;
case WP_YAW_BEHAVIOR_LOOK_AHEAD:
return AUTO_YAW_LOOK_AHEAD;
break;
case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP:
default:
return AUTO_YAW_LOOK_AT_NEXT_WP;
break;
}
}
// set_auto_yaw_mode - sets the yaw mode for auto
void Copter::set_auto_yaw_mode(uint8_t yaw_mode)
{
// return immediately if no change
if (auto_yaw_mode == yaw_mode) {
return;
}
auto_yaw_mode = yaw_mode;
// perform initialisation
switch (auto_yaw_mode) {
case AUTO_YAW_LOOK_AT_NEXT_WP:
// wpnav will initialise heading when wpnav's set_destination method is called
break;
case AUTO_YAW_ROI:
// point towards a location held in yaw_look_at_WP
yaw_look_at_WP_bearing = ahrs.yaw_sensor;
break;
case AUTO_YAW_LOOK_AT_HEADING:
// keep heading pointing in the direction held in yaw_look_at_heading
// caller should set the yaw_look_at_heading
break;
case AUTO_YAW_LOOK_AHEAD:
// Commanded Yaw to automatically look ahead.
yaw_look_ahead_bearing = ahrs.yaw_sensor;
break;
case AUTO_YAW_RESETTOARMEDYAW:
// initial_armed_bearing will be set during arming so no init required
break;
}
}
// set_auto_yaw_look_at_heading - sets the yaw look at heading for auto mode
void Copter::set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, uint8_t relative_angle)
{
// get current yaw target
int32_t curr_yaw_target = attitude_control.get_att_target_euler_cd().z;
// get final angle, 1 = Relative, 0 = Absolute
if (relative_angle == 0) {
// absolute angle
yaw_look_at_heading = wrap_360_cd(angle_deg * 100);
} else {
// relative angle
if (direction < 0) {
angle_deg = -angle_deg;
}
yaw_look_at_heading = wrap_360_cd((angle_deg*100+curr_yaw_target));
}
// get turn speed
if (is_zero(turn_rate_dps)) {
// default to regular auto slew rate
yaw_look_at_heading_slew = AUTO_YAW_SLEW_RATE;
}else{
int32_t turn_rate = (wrap_180_cd(yaw_look_at_heading - curr_yaw_target) / 100) / turn_rate_dps;
yaw_look_at_heading_slew = constrain_int32(turn_rate, 1, 360); // deg / sec
}
// set yaw mode
set_auto_yaw_mode(AUTO_YAW_LOOK_AT_HEADING);
// TO-DO: restore support for clockwise and counter clockwise rotation held in cmd.content.yaw.direction. 1 = clockwise, -1 = counterclockwise
}
// set_auto_yaw_roi - sets the yaw to look at roi for auto mode
void Copter::set_auto_yaw_roi(const Location &roi_location)
{
// if location is zero lat, lon and altitude turn off ROI
if (roi_location.alt == 0 && roi_location.lat == 0 && roi_location.lng == 0) {
// set auto yaw mode back to default assuming the active command is a waypoint command. A more sophisticated method is required to ensure we return to the proper yaw control for the active command
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
#if MOUNT == ENABLED
// switch off the camera tracking if enabled
if (camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) {
camera_mount.set_mode_to_default();
}
#endif // MOUNT == ENABLED
}else{
#if MOUNT == ENABLED
// check if mount type requires us to rotate the quad
if(!camera_mount.has_pan_control()) {
roi_WP = pv_location_to_vector(roi_location);
set_auto_yaw_mode(AUTO_YAW_ROI);
}
// send the command to the camera mount
camera_mount.set_roi_target(roi_location);
// TO-DO: expand handling of the do_nav_roi to support all modes of the MAVLink. Currently we only handle mode 4 (see below)
// 0: do nothing
// 1: point at next waypoint
// 2: point at a waypoint taken from WP# parameter (2nd parameter?)
// 3: point at a location given by alt, lon, lat parameters
// 4: point at a target given a target id (can't be implemented)
#else
// if we have no camera mount aim the quad at the location
roi_WP = pv_location_to_vector(roi_location);
set_auto_yaw_mode(AUTO_YAW_ROI);
#endif // MOUNT == ENABLED
}
}
// get_auto_heading - returns target heading depending upon auto_yaw_mode
// 100hz update rate
float Copter::get_auto_heading(void)
{
switch(auto_yaw_mode) {
case AUTO_YAW_ROI:
// point towards a location held in roi_WP
return get_roi_yaw();
break;
case AUTO_YAW_LOOK_AT_HEADING:
// keep heading pointing in the direction held in yaw_look_at_heading with no pilot input allowed
return yaw_look_at_heading;
break;
case AUTO_YAW_LOOK_AHEAD:
// Commanded Yaw to automatically look ahead.
return get_look_ahead_yaw();
break;
case AUTO_YAW_RESETTOARMEDYAW:
// changes yaw to be same as when quad was armed
return initial_armed_bearing;
break;
case AUTO_YAW_LOOK_AT_NEXT_WP:
default:
// point towards next waypoint.
// we don't use wp_bearing because we don't want the copter to turn too much during flight
return wp_nav.get_yaw();
break;
}
}

1311
ArduSub/control_autotune.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

73
ArduSub/control_brake.cpp Normal file
View File

@ -0,0 +1,73 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
/*
* control_brake.pde - init and run calls for brake flight mode
*/
// brake_init - initialise brake controller
bool Copter::brake_init(bool ignore_checks)
{
if (position_ok() || ignore_checks) {
// set desired acceleration to zero
wp_nav.clear_pilot_desired_acceleration();
// set target to current position
wp_nav.init_brake_target(BRAKE_MODE_DECEL_RATE);
// initialize vertical speed and acceleration
pos_control.set_speed_z(BRAKE_MODE_SPEED_Z, BRAKE_MODE_SPEED_Z);
pos_control.set_accel_z(BRAKE_MODE_DECEL_RATE);
// initialise position and desired velocity
pos_control.set_alt_target(inertial_nav.get_altitude());
pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
return true;
}else{
return false;
}
}
// brake_run - runs the brake controller
// should be called at 100hz or more
void Copter::brake_run()
{
// if not auto armed set throttle to zero and exit immediately
if(!ap.auto_armed) {
wp_nav.init_brake_target(BRAKE_MODE_DECEL_RATE);
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(0)-throttle_average);
return;
}
// relax stop target if we might be landed
if (ap.land_complete_maybe) {
wp_nav.loiter_soften_for_landing();
}
// if landed immediately disarm
if (ap.land_complete) {
init_disarm_motors();
}
// run brake controller
wp_nav.update_brake(ekfGndSpdLimit, ekfNavVelGainScaler);
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), 0.0f);
// body-frame rate controller is run directly from 100hz loop
// update altitude target and call position controller
pos_control.set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false);
pos_control.update_z_controller();
}

96
ArduSub/control_circle.cpp Normal file
View File

@ -0,0 +1,96 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
/*
* control_circle.pde - init and run calls for circle flight mode
*/
// circle_init - initialise circle controller flight mode
bool Copter::circle_init(bool ignore_checks)
{
if (position_ok() || ignore_checks) {
circle_pilot_yaw_override = false;
// initialize speeds and accelerations
pos_control.set_speed_xy(wp_nav.get_speed_xy());
pos_control.set_accel_xy(wp_nav.get_wp_acceleration());
pos_control.set_jerk_xy_to_default();
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
pos_control.set_accel_z(g.pilot_accel_z);
// initialise circle controller including setting the circle center based on vehicle speed
circle_nav.init();
return true;
}else{
return false;
}
}
// circle_run - runs the circle flight mode
// should be called at 100hz or more
void Copter::circle_run()
{
float target_yaw_rate = 0;
float target_climb_rate = 0;
// initialize speeds and accelerations
pos_control.set_speed_xy(wp_nav.get_speed_xy());
pos_control.set_accel_xy(wp_nav.get_wp_acceleration());
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
pos_control.set_accel_z(g.pilot_accel_z);
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
// To-Do: add some initialisation of position controllers
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
pos_control.set_alt_target_to_current_alt();
return;
}
// process pilot inputs
if (!failsafe.radio) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);
if (!is_zero(target_yaw_rate)) {
circle_pilot_yaw_override = true;
}
// get pilot desired climb rate
target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->control_in);
// check for pilot requested take-off
if (ap.land_complete && target_climb_rate > 0) {
// indicate we are taking off
set_land_complete(false);
// clear i term when we're taking off
set_throttle_takeoff();
}
}
// run circle controller
circle_nav.update();
// call attitude controller
if (circle_pilot_yaw_override) {
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(circle_nav.get_roll(), circle_nav.get_pitch(), target_yaw_rate);
}else{
attitude_control.input_euler_angle_roll_pitch_yaw(circle_nav.get_roll(), circle_nav.get_pitch(), circle_nav.get_yaw(),true);
}
// run altitude controller
if (sonar_enabled && (sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) {
// if sonar is ok, use surface tracking
target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control.get_alt_target(), G_Dt);
}
// update altitude target and call position controller
pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false);
pos_control.update_z_controller();
}

124
ArduSub/control_drift.cpp Normal file
View File

@ -0,0 +1,124 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
/*
* control_drift.pde - init and run calls for drift flight mode
*/
#ifndef DRIFT_SPEEDGAIN
# define DRIFT_SPEEDGAIN 8.0f
#endif
#ifndef DRIFT_SPEEDLIMIT
# define DRIFT_SPEEDLIMIT 560.0f
#endif
#ifndef DRIFT_THR_ASSIST_GAIN
# define DRIFT_THR_ASSIST_GAIN 1.8f // gain controlling amount of throttle assistance
#endif
#ifndef DRIFT_THR_ASSIST_MAX
# define DRIFT_THR_ASSIST_MAX 300.0f // maximum assistance throttle assist will provide
#endif
#ifndef DRIFT_THR_MIN
# define DRIFT_THR_MIN 213 // throttle assist will be active when pilot's throttle is above this value
#endif
#ifndef DRIFT_THR_MAX
# define DRIFT_THR_MAX 787 // throttle assist will be active when pilot's throttle is below this value
#endif
// drift_init - initialise drift controller
bool Copter::drift_init(bool ignore_checks)
{
if (position_ok() || ignore_checks) {
return true;
}else{
return false;
}
}
// drift_run - runs the drift controller
// should be called at 100hz or more
void Copter::drift_run()
{
static float breaker = 0.0f;
static float roll_input = 0.0f;
float target_roll, target_pitch;
float target_yaw_rate;
int16_t pilot_throttle_scaled;
// if not armed or motor interlock not enabled or landed and throttle at zero, set throttle to zero and exit immediately
if(!motors.armed() || !motors.get_interlock() || (ap.land_complete && ap.throttle_zero)) {
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
return;
}
// convert pilot input to lean angles
get_pilot_desired_lean_angles(channel_roll->control_in, channel_pitch->control_in, target_roll, target_pitch, aparm.angle_max);
// get pilot's desired throttle
pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->control_in);
// Grab inertial velocity
const Vector3f& vel = inertial_nav.get_velocity();
// rotate roll, pitch input from north facing to vehicle's perspective
float roll_vel = vel.y * ahrs.cos_yaw() - vel.x * ahrs.sin_yaw(); // body roll vel
float pitch_vel = vel.y * ahrs.sin_yaw() + vel.x * ahrs.cos_yaw(); // body pitch vel
// gain sceduling for Yaw
float pitch_vel2 = MIN(fabsf(pitch_vel), 2000);
target_yaw_rate = ((float)target_roll/1.0f) * (1.0f - (pitch_vel2 / 5000.0f)) * g.acro_yaw_p;
roll_vel = constrain_float(roll_vel, -DRIFT_SPEEDLIMIT, DRIFT_SPEEDLIMIT);
pitch_vel = constrain_float(pitch_vel, -DRIFT_SPEEDLIMIT, DRIFT_SPEEDLIMIT);
roll_input = roll_input * .96f + (float)channel_yaw->control_in * .04f;
//convert user input into desired roll velocity
float roll_vel_error = roll_vel - (roll_input / DRIFT_SPEEDGAIN);
// Roll velocity is feed into roll acceleration to minimize slip
target_roll = roll_vel_error * -DRIFT_SPEEDGAIN;
target_roll = constrain_int16(target_roll, -4500, 4500);
// If we let go of sticks, bring us to a stop
if(is_zero(target_pitch)){
// .14/ (.03 * 100) = 4.6 seconds till full breaking
breaker += .03f;
breaker = MIN(breaker, DRIFT_SPEEDGAIN);
target_pitch = pitch_vel * breaker;
}else{
breaker = 0.0f;
}
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
// output pilot's throttle with angle boost
attitude_control.set_throttle_out(get_throttle_assist(vel.z, pilot_throttle_scaled), true, g.throttle_filt);
}
// get_throttle_assist - return throttle output (range 0 ~ 1000) based on pilot input and z-axis velocity
int16_t Copter::get_throttle_assist(float velz, int16_t pilot_throttle_scaled)
{
// throttle assist - adjusts throttle to slow the vehicle's vertical velocity
// Only active when pilot's throttle is between 213 ~ 787
// Assistance is strongest when throttle is at mid, drops linearly to no assistance at 213 and 787
float thr_assist = 0.0f;
if (pilot_throttle_scaled > g.throttle_min && pilot_throttle_scaled < THR_MAX &&
pilot_throttle_scaled > DRIFT_THR_MIN && pilot_throttle_scaled < DRIFT_THR_MAX) {
// calculate throttle assist gain
thr_assist = 1.2f - ((float)abs(pilot_throttle_scaled - 500) / 240.0f);
thr_assist = constrain_float(thr_assist, 0.0f, 1.0f) * -DRIFT_THR_ASSIST_GAIN * velz;
// ensure throttle assist never adjusts the throttle by more than 300 pwm
thr_assist = constrain_float(thr_assist, -DRIFT_THR_ASSIST_MAX, DRIFT_THR_ASSIST_MAX);
// ensure throttle assist never pushes throttle below throttle_min or above throttle_max
thr_assist = constrain_float(thr_assist, g.throttle_min - pilot_throttle_scaled, THR_MAX - pilot_throttle_scaled);
}
return pilot_throttle_scaled + thr_assist;
}

228
ArduSub/control_flip.cpp Normal file
View File

@ -0,0 +1,228 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
/*
* control_flip.pde - init and run calls for flip flight mode
* original implementation in 2010 by Jose Julio
* Adapted and updated for AC2 in 2011 by Jason Short
*
* Controls:
* CH7_OPT - CH12_OPT parameter must be set to "Flip" (AUXSW_FLIP) which is "2"
* Pilot switches to Stabilize, Acro or AltHold flight mode and puts ch7/ch8 switch to ON position
* Vehicle will Roll right by default but if roll or pitch stick is held slightly left, forward or back it will flip in that direction
* Vehicle should complete the roll within 2.5sec and will then return to the original flight mode it was in before flip was triggered
* Pilot may manually exit flip by switching off ch7/ch8 or by moving roll stick to >40deg left or right
*
* State machine approach:
* Flip_Start (while copter is leaning <45deg) : roll right at 400deg/sec, increase throttle
* Flip_Roll (while copter is between +45deg ~ -90) : roll right at 400deg/sec, reduce throttle
* Flip_Recover (while copter is between -90deg and original target angle) : use earth frame angle controller to return vehicle to original attitude
*/
#define FLIP_THR_INC 200 // throttle increase during Flip_Start stage (under 45deg lean angle)
#define FLIP_THR_DEC 240 // throttle decrease during Flip_Roll stage (between 45deg ~ -90deg roll)
#define FLIP_ROTATION_RATE 40000 // rotation rate request in centi-degrees / sec (i.e. 400 deg/sec)
#define FLIP_TIMEOUT_MS 2500 // timeout after 2.5sec. Vehicle will switch back to original flight mode
#define FLIP_RECOVERY_ANGLE 500 // consider successful recovery when roll is back within 5 degrees of original
#define FLIP_ROLL_RIGHT 1 // used to set flip_dir
#define FLIP_ROLL_LEFT -1 // used to set flip_dir
#define FLIP_PITCH_BACK 1 // used to set flip_dir
#define FLIP_PITCH_FORWARD -1 // used to set flip_dir
FlipState flip_state; // current state of flip
uint8_t flip_orig_control_mode; // flight mode when flip was initated
uint32_t flip_start_time; // time since flip began
int8_t flip_roll_dir; // roll direction (-1 = roll left, 1 = roll right)
int8_t flip_pitch_dir; // pitch direction (-1 = pitch forward, 1 = pitch back)
// flip_init - initialise flip controller
bool Copter::flip_init(bool ignore_checks)
{
// only allow flip from ACRO, Stabilize, AltHold or Drift flight modes
if (control_mode != ACRO && control_mode != STABILIZE && control_mode != ALT_HOLD) {
return false;
}
// if in acro or stabilize ensure throttle is above zero
if (ap.throttle_zero && (control_mode == ACRO || control_mode == STABILIZE)) {
return false;
}
// ensure roll input is less than 40deg
if (abs(channel_roll->control_in) >= 4000) {
return false;
}
// only allow flip when flying
if (!motors.armed() || ap.land_complete) {
return false;
}
// capture original flight mode so that we can return to it after completion
flip_orig_control_mode = control_mode;
// initialise state
flip_state = Flip_Start;
flip_start_time = millis();
flip_roll_dir = flip_pitch_dir = 0;
// choose direction based on pilot's roll and pitch sticks
if (channel_pitch->control_in > 300) {
flip_pitch_dir = FLIP_PITCH_BACK;
}else if(channel_pitch->control_in < -300) {
flip_pitch_dir = FLIP_PITCH_FORWARD;
}else if (channel_roll->control_in >= 0) {
flip_roll_dir = FLIP_ROLL_RIGHT;
}else{
flip_roll_dir = FLIP_ROLL_LEFT;
}
// log start of flip
Log_Write_Event(DATA_FLIP_START);
// capture current attitude which will be used during the Flip_Recovery stage
flip_orig_attitude.x = constrain_float(ahrs.roll_sensor, -aparm.angle_max, aparm.angle_max);
flip_orig_attitude.y = constrain_float(ahrs.pitch_sensor, -aparm.angle_max, aparm.angle_max);
flip_orig_attitude.z = ahrs.yaw_sensor;
return true;
}
// flip_run - runs the flip controller
// should be called at 100hz or more
void Copter::flip_run()
{
int16_t throttle_out;
float recovery_angle;
// if pilot inputs roll > 40deg or timeout occurs abandon flip
if (!motors.armed() || (abs(channel_roll->control_in) >= 4000) || (abs(channel_pitch->control_in) >= 4000) || ((millis() - flip_start_time) > FLIP_TIMEOUT_MS)) {
flip_state = Flip_Abandon;
}
// get pilot's desired throttle
throttle_out = get_pilot_desired_throttle(channel_throttle->control_in);
// get corrected angle based on direction and axis of rotation
// we flip the sign of flip_angle to minimize the code repetition
int32_t flip_angle;
if (flip_roll_dir != 0) {
flip_angle = ahrs.roll_sensor * flip_roll_dir;
} else {
flip_angle = ahrs.pitch_sensor * flip_pitch_dir;
}
// state machine
switch (flip_state) {
case Flip_Start:
// under 45 degrees request 400deg/sec roll or pitch
attitude_control.input_rate_bf_roll_pitch_yaw(FLIP_ROTATION_RATE * flip_roll_dir, FLIP_ROTATION_RATE * flip_pitch_dir, 0.0);
// increase throttle
throttle_out += FLIP_THR_INC;
// beyond 45deg lean angle move to next stage
if (flip_angle >= 4500) {
if (flip_roll_dir != 0) {
// we are rolling
flip_state = Flip_Roll;
} else {
// we are pitching
flip_state = Flip_Pitch_A;
}
}
break;
case Flip_Roll:
// between 45deg ~ -90deg request 400deg/sec roll
attitude_control.input_rate_bf_roll_pitch_yaw(FLIP_ROTATION_RATE * flip_roll_dir, 0.0, 0.0);
// decrease throttle
if (throttle_out >= g.throttle_min) {
throttle_out = MAX(throttle_out - FLIP_THR_DEC, g.throttle_min);
}
// beyond -90deg move on to recovery
if ((flip_angle < 4500) && (flip_angle > -9000)) {
flip_state = Flip_Recover;
}
break;
case Flip_Pitch_A:
// between 45deg ~ -90deg request 400deg/sec pitch
attitude_control.input_rate_bf_roll_pitch_yaw(0.0, FLIP_ROTATION_RATE * flip_pitch_dir, 0.0);
// decrease throttle
if (throttle_out >= g.throttle_min) {
throttle_out = MAX(throttle_out - FLIP_THR_DEC, g.throttle_min);
}
// check roll for inversion
if ((labs(ahrs.roll_sensor) > 9000) && (flip_angle > 4500)) {
flip_state = Flip_Pitch_B;
}
break;
case Flip_Pitch_B:
// between 45deg ~ -90deg request 400deg/sec pitch
attitude_control.input_rate_bf_roll_pitch_yaw(0.0, FLIP_ROTATION_RATE * flip_pitch_dir, 0.0);
// decrease throttle
if (throttle_out >= g.throttle_min) {
throttle_out = MAX(throttle_out - FLIP_THR_DEC, g.throttle_min);
}
// check roll for inversion
if ((labs(ahrs.roll_sensor) < 9000) && (flip_angle > -4500)) {
flip_state = Flip_Recover;
}
break;
case Flip_Recover:
// use originally captured earth-frame angle targets to recover
attitude_control.input_euler_angle_roll_pitch_yaw(flip_orig_attitude.x, flip_orig_attitude.y, flip_orig_attitude.z, false);
// increase throttle to gain any lost altitude
throttle_out += FLIP_THR_INC;
if (flip_roll_dir != 0) {
// we are rolling
recovery_angle = fabsf(flip_orig_attitude.x - (float)ahrs.roll_sensor);
} else {
// we are pitching
recovery_angle = fabsf(flip_orig_attitude.y - (float)ahrs.pitch_sensor);
}
// check for successful recovery
if (fabsf(recovery_angle) <= FLIP_RECOVERY_ANGLE) {
// restore original flight mode
if (!set_mode(flip_orig_control_mode)) {
// this should never happen but just in case
set_mode(STABILIZE);
}
// log successful completion
Log_Write_Event(DATA_FLIP_END);
}
break;
case Flip_Abandon:
// restore original flight mode
if (!set_mode(flip_orig_control_mode)) {
// this should never happen but just in case
set_mode(STABILIZE);
}
// log abandoning flip
Log_Write_Error(ERROR_SUBSYSTEM_FLIP,ERROR_CODE_FLIP_ABANDONED);
break;
}
// output pilot's throttle without angle boost
if (throttle_out == 0) {
attitude_control.set_throttle_out_unstabilized(0,false,g.throttle_filt);
} else {
attitude_control.set_throttle_out(throttle_out, false, g.throttle_filt);
}
}

554
ArduSub/control_guided.cpp Normal file
View File

@ -0,0 +1,554 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
/*
* control_guided.pde - init and run calls for guided flight mode
*/
#ifndef GUIDED_LOOK_AT_TARGET_MIN_DISTANCE_CM
# define GUIDED_LOOK_AT_TARGET_MIN_DISTANCE_CM 500 // point nose at target if it is more than 5m away
#endif
#define GUIDED_POSVEL_TIMEOUT_MS 3000 // guided mode's position-velocity controller times out after 3seconds with no new updates
#define GUIDED_ATTITUDE_TIMEOUT_MS 1000 // guided mode's attitude controller times out after 1 second with no new updates
static Vector3f posvel_pos_target_cm;
static Vector3f posvel_vel_target_cms;
static uint32_t posvel_update_time_ms;
static uint32_t vel_update_time_ms;
struct {
uint32_t update_time_ms;
float roll_cd;
float pitch_cd;
float yaw_cd;
float climb_rate_cms;
} static guided_angle_state = {0,0.0f, 0.0f, 0.0f, 0.0f};
struct Guided_Limit {
uint32_t timeout_ms; // timeout (in seconds) from the time that guided is invoked
float alt_min_cm; // lower altitude limit in cm above home (0 = no limit)
float alt_max_cm; // upper altitude limit in cm above home (0 = no limit)
float horiz_max_cm; // horizontal position limit in cm from where guided mode was initiated (0 = no limit)
uint32_t start_time;// system time in milliseconds that control was handed to the external computer
Vector3f start_pos; // start position as a distance from home in cm. used for checking horiz_max limit
} guided_limit;
// guided_init - initialise guided controller
bool Copter::guided_init(bool ignore_checks)
{
if (position_ok() || ignore_checks) {
// initialise yaw
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
// start in position control mode
guided_pos_control_start();
return true;
}else{
return false;
}
}
// guided_takeoff_start - initialises waypoint controller to implement take-off
void Copter::guided_takeoff_start(float final_alt_above_home)
{
guided_mode = Guided_TakeOff;
// initialise wpnav destination
Vector3f target_pos = inertial_nav.get_position();
target_pos.z = pv_alt_above_origin(final_alt_above_home);
wp_nav.set_wp_destination(target_pos);
// initialise yaw
set_auto_yaw_mode(AUTO_YAW_HOLD);
// clear i term when we're taking off
set_throttle_takeoff();
}
// initialise guided mode's position controller
void Copter::guided_pos_control_start()
{
// set to position control mode
guided_mode = Guided_WP;
// initialise waypoint and spline controller
wp_nav.wp_and_spline_init();
// initialise wpnav to stopping point at current altitude
// To-Do: set to current location if disarmed?
// To-Do: set to stopping point altitude?
Vector3f stopping_point;
stopping_point.z = inertial_nav.get_altitude();
wp_nav.get_wp_stopping_point_xy(stopping_point);
wp_nav.set_wp_destination(stopping_point);
// initialise yaw
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
}
// initialise guided mode's velocity controller
void Copter::guided_vel_control_start()
{
// set guided_mode to velocity controller
guided_mode = Guided_Velocity;
// initialize vertical speeds and leash lengths
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
pos_control.set_accel_z(g.pilot_accel_z);
// initialise velocity controller
pos_control.init_vel_controller_xyz();
}
// initialise guided mode's posvel controller
void Copter::guided_posvel_control_start()
{
// set guided_mode to velocity controller
guided_mode = Guided_PosVel;
pos_control.init_xy_controller();
// set speed and acceleration from wpnav's speed and acceleration
pos_control.set_speed_xy(wp_nav.get_speed_xy());
pos_control.set_accel_xy(wp_nav.get_wp_acceleration());
pos_control.set_jerk_xy_to_default();
const Vector3f& curr_pos = inertial_nav.get_position();
const Vector3f& curr_vel = inertial_nav.get_velocity();
// set target position and velocity to current position and velocity
pos_control.set_xy_target(curr_pos.x, curr_pos.y);
pos_control.set_desired_velocity_xy(curr_vel.x, curr_vel.y);
// set vertical speed and acceleration
pos_control.set_speed_z(wp_nav.get_speed_down(), wp_nav.get_speed_up());
pos_control.set_accel_z(wp_nav.get_accel_z());
// pilot always controls yaw
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
// initialise guided mode's angle controller
void Copter::guided_angle_control_start()
{
// set guided_mode to velocity controller
guided_mode = Guided_Angle;
// set vertical speed and acceleration
pos_control.set_speed_z(wp_nav.get_speed_down(), wp_nav.get_speed_up());
pos_control.set_accel_z(wp_nav.get_accel_z());
// initialise position and desired velocity
pos_control.set_alt_target(inertial_nav.get_altitude());
pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
// initialise targets
guided_angle_state.update_time_ms = millis();
guided_angle_state.roll_cd = ahrs.roll_sensor;
guided_angle_state.pitch_cd = ahrs.pitch_sensor;
guided_angle_state.yaw_cd = ahrs.yaw_sensor;
guided_angle_state.climb_rate_cms = 0.0f;
// pilot always controls yaw
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
// guided_set_destination - sets guided mode's target destination
void Copter::guided_set_destination(const Vector3f& destination)
{
// ensure we are in position control mode
if (guided_mode != Guided_WP) {
guided_pos_control_start();
}
wp_nav.set_wp_destination(destination);
}
// guided_set_velocity - sets guided mode's target velocity
void Copter::guided_set_velocity(const Vector3f& velocity)
{
// check we are in velocity control mode
if (guided_mode != Guided_Velocity) {
guided_vel_control_start();
}
vel_update_time_ms = millis();
// set position controller velocity target
pos_control.set_desired_velocity(velocity);
}
// set guided mode posvel target
void Copter::guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity) {
// check we are in velocity control mode
if (guided_mode != Guided_PosVel) {
guided_posvel_control_start();
}
posvel_update_time_ms = millis();
posvel_pos_target_cm = destination;
posvel_vel_target_cms = velocity;
pos_control.set_pos_target(posvel_pos_target_cm);
}
// set guided mode angle target
void Copter::guided_set_angle(const Quaternion &q, float climb_rate_cms)
{
// check we are in velocity control mode
if (guided_mode != Guided_Angle) {
guided_angle_control_start();
}
// convert quaternion to euler angles
q.to_euler(guided_angle_state.roll_cd, guided_angle_state.pitch_cd, guided_angle_state.yaw_cd);
guided_angle_state.roll_cd = ToDeg(guided_angle_state.roll_cd) * 100.0f;
guided_angle_state.pitch_cd = ToDeg(guided_angle_state.pitch_cd) * 100.0f;
guided_angle_state.yaw_cd = wrap_180_cd_float(ToDeg(guided_angle_state.yaw_cd) * 100.0f);
guided_angle_state.climb_rate_cms = climb_rate_cms;
guided_angle_state.update_time_ms = millis();
}
// guided_run - runs the guided controller
// should be called at 100hz or more
void Copter::guided_run()
{
// call the correct auto controller
switch (guided_mode) {
case Guided_TakeOff:
// run takeoff controller
guided_takeoff_run();
break;
case Guided_WP:
// run position controller
guided_pos_control_run();
break;
case Guided_Velocity:
// run velocity controller
guided_vel_control_run();
break;
case Guided_PosVel:
// run position-velocity controller
guided_posvel_control_run();
break;
case Guided_Angle:
// run angle controller
guided_angle_control_run();
break;
}
}
// guided_takeoff_run - takeoff in guided mode
// called by guided_run at 100hz or more
void Copter::guided_takeoff_run()
{
// if not auto armed or motors not enabled set throttle to zero and exit immediately
if (!ap.auto_armed || !motors.get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
return;
}
// process pilot's yaw input
float target_yaw_rate = 0;
if (!failsafe.radio) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);
}
// run waypoint controller
wp_nav.update_wpnav();
// call z-axis position controller (wpnav should have already updated it's alt target)
pos_control.update_z_controller();
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
}
// guided_pos_control_run - runs the guided position controller
// called from guided_run
void Copter::guided_pos_control_run()
{
// if not auto armed or motors not enabled set throttle to zero and exit immediately
if (!ap.auto_armed || !motors.get_interlock() || ap.land_complete) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
return;
}
// process pilot's yaw input
float target_yaw_rate = 0;
if (!failsafe.radio) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);
if (!is_zero(target_yaw_rate)) {
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
}
// run waypoint controller
wp_nav.update_wpnav();
// call z-axis position controller (wpnav should have already updated it's alt target)
pos_control.update_z_controller();
// call attitude controller
if (auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
}else{
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(), true);
}
}
// guided_vel_control_run - runs the guided velocity controller
// called from guided_run
void Copter::guided_vel_control_run()
{
// if not auto armed or motors not enabled set throttle to zero and exit immediately
if (!ap.auto_armed || !motors.get_interlock() || ap.land_complete) {
// initialise velocity controller
pos_control.init_vel_controller_xyz();
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
return;
}
// process pilot's yaw input
float target_yaw_rate = 0;
if (!failsafe.radio) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);
if (!is_zero(target_yaw_rate)) {
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
}
// set velocity to zero if no updates received for 3 seconds
uint32_t tnow = millis();
if (tnow - vel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS && !pos_control.get_desired_velocity().is_zero()) {
pos_control.set_desired_velocity(Vector3f(0,0,0));
}
// call velocity controller which includes z axis controller
pos_control.update_vel_controller_xyz(ekfNavVelGainScaler);
// call attitude controller
if (auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(pos_control.get_roll(), pos_control.get_pitch(), target_yaw_rate);
}else{
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control.input_euler_angle_roll_pitch_yaw(pos_control.get_roll(), pos_control.get_pitch(), get_auto_heading(), true);
}
}
// guided_posvel_control_run - runs the guided spline controller
// called from guided_run
void Copter::guided_posvel_control_run()
{
// if not auto armed or motors not enabled set throttle to zero and exit immediately
if (!ap.auto_armed || !motors.get_interlock() || ap.land_complete) {
// set target position and velocity to current position and velocity
pos_control.set_pos_target(inertial_nav.get_position());
pos_control.set_desired_velocity(Vector3f(0,0,0));
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
return;
}
// process pilot's yaw input
float target_yaw_rate = 0;
if (!failsafe.radio) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);
if (!is_zero(target_yaw_rate)) {
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
}
// set velocity to zero if no updates received for 3 seconds
uint32_t tnow = millis();
if (tnow - posvel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS && !posvel_vel_target_cms.is_zero()) {
posvel_vel_target_cms.zero();
}
// calculate dt
float dt = pos_control.time_since_last_xy_update();
// update at poscontrol update rate
if (dt >= pos_control.get_dt_xy()) {
// sanity check dt
if (dt >= 0.2f) {
dt = 0.0f;
}
// advance position target using velocity target
posvel_pos_target_cm += posvel_vel_target_cms * dt;
// send position and velocity targets to position controller
pos_control.set_pos_target(posvel_pos_target_cm);
pos_control.set_desired_velocity_xy(posvel_vel_target_cms.x, posvel_vel_target_cms.y);
// run position controller
pos_control.update_xy_controller(AC_PosControl::XY_MODE_POS_AND_VEL_FF, ekfNavVelGainScaler, false);
}
pos_control.update_z_controller();
// call attitude controller
if (auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(pos_control.get_roll(), pos_control.get_pitch(), target_yaw_rate);
}else{
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control.input_euler_angle_roll_pitch_yaw(pos_control.get_roll(), pos_control.get_pitch(), get_auto_heading(), true);
}
}
// guided_angle_control_run - runs the guided angle controller
// called from guided_run
void Copter::guided_angle_control_run()
{
// if not auto armed or motors not enabled set throttle to zero and exit immediately
if (!ap.auto_armed || !motors.get_interlock() || ap.land_complete) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.set_yaw_target_to_current_heading();
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0.0f, 0.0f, 0.0f, get_smoothing_gain());
attitude_control.set_throttle_out(0.0f,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0.0f,true,g.throttle_filt);
#endif
pos_control.relax_alt_hold_controllers(0.0f);
return;
}
// constrain desired lean angles
float roll_in = guided_angle_state.roll_cd;
float pitch_in = guided_angle_state.pitch_cd;
float total_in = pythagorous2(roll_in, pitch_in);
float angle_max = MIN(attitude_control.get_althold_lean_angle_max(), aparm.angle_max);
if (total_in > angle_max) {
float ratio = angle_max / total_in;
roll_in *= ratio;
pitch_in *= ratio;
}
// wrap yaw request
float yaw_in = wrap_180_cd_float(guided_angle_state.yaw_cd);
// constrain climb rate
float climb_rate_cms = constrain_float(guided_angle_state.climb_rate_cms, -fabs(wp_nav.get_speed_down()), wp_nav.get_speed_up());
// check for timeout - set lean angles and climb rate to zero if no updates received for 3 seconds
uint32_t tnow = millis();
if (tnow - guided_angle_state.update_time_ms > GUIDED_ATTITUDE_TIMEOUT_MS) {
roll_in = 0.0f;
pitch_in = 0.0f;
climb_rate_cms = 0.0f;
}
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_yaw(roll_in, pitch_in, yaw_in, true);
// call position controller
pos_control.set_alt_target_from_climb_rate_ff(climb_rate_cms, G_Dt, false);
pos_control.update_z_controller();
}
// Guided Limit code
// guided_limit_clear - clear/turn off guided limits
void Copter::guided_limit_clear()
{
guided_limit.timeout_ms = 0;
guided_limit.alt_min_cm = 0.0f;
guided_limit.alt_max_cm = 0.0f;
guided_limit.horiz_max_cm = 0.0f;
}
// guided_limit_set - set guided timeout and movement limits
void Copter::guided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm)
{
guided_limit.timeout_ms = timeout_ms;
guided_limit.alt_min_cm = alt_min_cm;
guided_limit.alt_max_cm = alt_max_cm;
guided_limit.horiz_max_cm = horiz_max_cm;
}
// guided_limit_init_time_and_pos - initialise guided start time and position as reference for limit checking
// only called from AUTO mode's auto_nav_guided_start function
void Copter::guided_limit_init_time_and_pos()
{
// initialise start time
guided_limit.start_time = AP_HAL::millis();
// initialise start position from current position
guided_limit.start_pos = inertial_nav.get_position();
}
// guided_limit_check - returns true if guided mode has breached a limit
// used when guided is invoked from the NAV_GUIDED_ENABLE mission command
bool Copter::guided_limit_check()
{
// check if we have passed the timeout
if ((guided_limit.timeout_ms > 0) && (millis() - guided_limit.start_time >= guided_limit.timeout_ms)) {
return true;
}
// get current location
const Vector3f& curr_pos = inertial_nav.get_position();
// check if we have gone below min alt
if (!is_zero(guided_limit.alt_min_cm) && (curr_pos.z < guided_limit.alt_min_cm)) {
return true;
}
// check if we have gone above max alt
if (!is_zero(guided_limit.alt_max_cm) && (curr_pos.z > guided_limit.alt_max_cm)) {
return true;
}
// check if we have gone beyond horizontal limit
if (guided_limit.horiz_max_cm > 0.0f) {
float horiz_move = pv_get_horizontal_distance_cm(guided_limit.start_pos, curr_pos);
if (horiz_move > guided_limit.horiz_max_cm) {
return true;
}
}
// if we got this far we must be within limits
return false;
}

247
ArduSub/control_land.cpp Normal file
View File

@ -0,0 +1,247 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
static bool land_with_gps;
static uint32_t land_start_time;
static bool land_pause;
// land_init - initialise land controller
bool Copter::land_init(bool ignore_checks)
{
// check if we have GPS and decide which LAND we're going to do
land_with_gps = position_ok();
if (land_with_gps) {
// set target to stopping point
Vector3f stopping_point;
wp_nav.get_loiter_stopping_point_xy(stopping_point);
wp_nav.init_loiter_target(stopping_point);
}
// initialize vertical speeds and leash lengths
pos_control.set_speed_z(wp_nav.get_speed_down(), wp_nav.get_speed_up());
pos_control.set_accel_z(wp_nav.get_accel_z());
// initialise altitude target to stopping point
pos_control.set_target_to_stopping_point_z();
land_start_time = millis();
land_pause = false;
// reset flag indicating if pilot has applied roll or pitch inputs during landing
ap.land_repo_active = false;
return true;
}
// land_run - runs the land controller
// should be called at 100hz or more
void Copter::land_run()
{
if (land_with_gps) {
land_gps_run();
}else{
land_nogps_run();
}
}
// land_run - runs the land controller
// horizontal position controlled with loiter controller
// should be called at 100hz or more
void Copter::land_gps_run()
{
int16_t roll_control = 0, pitch_control = 0;
float target_yaw_rate = 0;
// if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
wp_nav.init_loiter_target();
#if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED
// disarm when the landing detector says we've landed and throttle is at minimum
if (ap.land_complete && (ap.throttle_zero || failsafe.radio)) {
init_disarm_motors();
}
#else
// disarm when the landing detector says we've landed
if (ap.land_complete) {
init_disarm_motors();
}
#endif
return;
}
// relax loiter target if we might be landed
if (ap.land_complete_maybe) {
wp_nav.loiter_soften_for_landing();
}
// process pilot inputs
if (!failsafe.radio) {
if (g.land_repositioning) {
// apply SIMPLE mode transform to pilot inputs
update_simple_mode();
// process pilot's roll and pitch input
roll_control = channel_roll->control_in;
pitch_control = channel_pitch->control_in;
// record if pilot has overriden roll or pitch
if (roll_control != 0 || pitch_control != 0) {
ap.land_repo_active = true;
}
}
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);
}
// process roll, pitch inputs
wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control);
#if PRECISION_LANDING == ENABLED
// run precision landing
if (!ap.land_repo_active) {
wp_nav.shift_loiter_target(precland.get_target_shift(wp_nav.get_loiter_target()));
}
#endif
// run loiter controller
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
// pause 4 seconds before beginning land descent
float cmb_rate;
if(land_pause && millis()-land_start_time < 4000) {
cmb_rate = 0;
} else {
land_pause = false;
cmb_rate = get_land_descent_speed();
}
// record desired climb rate for logging
desired_climb_rate = cmb_rate;
// update altitude target and call position controller
pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt, true);
pos_control.update_z_controller();
}
// land_nogps_run - runs the land controller
// pilot controls roll and pitch angles
// should be called at 100hz or more
void Copter::land_nogps_run()
{
float target_roll = 0.0f, target_pitch = 0.0f;
float target_yaw_rate = 0;
// process pilot inputs
if (!failsafe.radio) {
if (g.land_repositioning) {
// apply SIMPLE mode transform to pilot inputs
update_simple_mode();
// get pilot desired lean angles
get_pilot_desired_lean_angles(channel_roll->control_in, channel_pitch->control_in, target_roll, target_pitch, aparm.angle_max);
}
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);
}
// if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
#if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED
// disarm when the landing detector says we've landed and throttle is at minimum
if (ap.land_complete && (ap.throttle_zero || failsafe.radio)) {
init_disarm_motors();
}
#else
// disarm when the landing detector says we've landed
if (ap.land_complete) {
init_disarm_motors();
}
#endif
return;
}
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
// pause 4 seconds before beginning land descent
float cmb_rate;
if(land_pause && millis()-land_start_time < LAND_WITH_DELAY_MS) {
cmb_rate = 0;
} else {
land_pause = false;
cmb_rate = get_land_descent_speed();
}
// record desired climb rate for logging
desired_climb_rate = cmb_rate;
// call position controller
pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt, true);
pos_control.update_z_controller();
}
// get_land_descent_speed - high level landing logic
// returns climb rate (in cm/s) which should be passed to the position controller
// should be called at 100hz or higher
float Copter::get_land_descent_speed()
{
#if CONFIG_SONAR == ENABLED
bool sonar_ok = sonar_enabled && (sonar.status() == RangeFinder::RangeFinder_Good);
#else
bool sonar_ok = false;
#endif
// if we are above 10m and the sonar does not sense anything perform regular alt hold descent
if (pos_control.get_pos_target().z >= pv_alt_above_origin(LAND_START_ALT) && !(sonar_ok && sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) {
return pos_control.get_speed_down();
}else{
return -abs(g.land_speed);
}
}
// land_do_not_use_GPS - forces land-mode to not use the GPS but instead rely on pilot input for roll and pitch
// called during GPS failsafe to ensure that if we were already in LAND mode that we do not use the GPS
// has no effect if we are not already in LAND mode
void Copter::land_do_not_use_GPS()
{
land_with_gps = false;
}
// set_mode_land_with_pause - sets mode to LAND and triggers 4 second delay before descent starts
// this is always called from a failsafe so we trigger notification to pilot
void Copter::set_mode_land_with_pause()
{
set_mode(LAND);
land_pause = true;
// alert pilot to mode change
AP_Notify::events.failsafe_mode_change = 1;
}
// landing_with_GPS - returns true if vehicle is landing using GPS
bool Copter::landing_with_GPS() {
return (control_mode == LAND && land_with_gps);
}

183
ArduSub/control_loiter.cpp Normal file
View File

@ -0,0 +1,183 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
/*
* control_loiter.pde - init and run calls for loiter flight mode
*/
// loiter_init - initialise loiter controller
bool Copter::loiter_init(bool ignore_checks)
{
#if FRAME_CONFIG == HELI_FRAME
// do not allow helis to enter Loiter if the Rotor Runup is not complete
if (!ignore_checks && !motors.rotor_runup_complete()){
return false;
}
#endif
if (position_ok() || ignore_checks) {
// set target to current position
wp_nav.init_loiter_target();
// initialize vertical speed and acceleration
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
pos_control.set_accel_z(g.pilot_accel_z);
// initialise position and desired velocity
pos_control.set_alt_target(inertial_nav.get_altitude());
pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
return true;
}else{
return false;
}
}
// loiter_run - runs the loiter controller
// should be called at 100hz or more
void Copter::loiter_run()
{
LoiterModeState loiter_state;
float target_yaw_rate = 0.0f;
float target_climb_rate = 0.0f;
float takeoff_climb_rate = 0.0f;
// initialize vertical speed and acceleration
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
pos_control.set_accel_z(g.pilot_accel_z);
// process pilot inputs unless we are in radio failsafe
if (!failsafe.radio) {
// apply SIMPLE mode transform to pilot inputs
update_simple_mode();
// process pilot's roll and pitch input
wp_nav.set_pilot_desired_acceleration(channel_roll->control_in, channel_pitch->control_in);
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);
// get pilot desired climb rate
target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->control_in);
target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max, g.pilot_velocity_z_max);
} else {
// clear out pilot desired acceleration in case radio failsafe event occurs and we do not switch to RTL for some reason
wp_nav.clear_pilot_desired_acceleration();
}
// relax loiter target if we might be landed
if (ap.land_complete_maybe) {
wp_nav.loiter_soften_for_landing();
}
// Loiter State Machine Determination
if(!ap.auto_armed) {
loiter_state = Loiter_Disarmed;
} else if (!motors.get_interlock()){
loiter_state = Loiter_MotorStop;
} else if (takeoff_state.running || (ap.land_complete && (channel_throttle->control_in > get_takeoff_trigger_throttle()))){
loiter_state = Loiter_Takeoff;
} else if (ap.land_complete){
loiter_state = Loiter_Landed;
} else {
loiter_state = Loiter_Flying;
}
// Loiter State Machine
switch (loiter_state) {
case Loiter_Disarmed:
wp_nav.init_loiter_target();
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif // HELI_FRAME
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);
break;
case Loiter_MotorStop:
#if FRAME_CONFIG == HELI_FRAME
// helicopters are capable of flying even with the motor stopped, therefore we will attempt to keep flying
// run loiter controller
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
// force descent rate and call position controller
pos_control.set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false);
pos_control.update_z_controller();
#else
wp_nav.init_loiter_target();
// multicopters do not stabilize roll/pitch/yaw when motors are stopped
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);
#endif // HELI_FRAME
break;
case Loiter_Takeoff:
if (!takeoff_state.running) {
takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));
// indicate we are taking off
set_land_complete(false);
// clear i term when we're taking off
set_throttle_takeoff();
}
// get takeoff adjusted pilot and takeoff climb rates
takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate);
// run loiter controller
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
// update altitude target and call position controller
pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
pos_control.add_takeoff_climb_rate(takeoff_climb_rate, G_Dt);
pos_control.update_z_controller();
break;
case Loiter_Landed:
wp_nav.init_loiter_target();
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(get_throttle_pre_takeoff(channel_throttle->control_in),false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
// move throttle to between minimum and non-takeoff-throttle to keep us on the ground
attitude_control.set_throttle_out_unstabilized(get_throttle_pre_takeoff(channel_throttle->control_in),true,g.throttle_filt);
#endif
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);
break;
case Loiter_Flying:
// run loiter controller
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
// run altitude controller
if (sonar_enabled && (sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) {
// if sonar is ok, use surface tracking
target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control.get_alt_target(), G_Dt);
}
// update altitude target and call position controller
pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
pos_control.update_z_controller();
break;
}
}

654
ArduSub/control_poshold.cpp Normal file
View File

@ -0,0 +1,654 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#if POSHOLD_ENABLED == ENABLED
/*
* control_poshold.pde - init and run calls for PosHold flight mode
* PosHold tries to improve upon regular loiter by mixing the pilot input with the loiter controller
*/
#define POSHOLD_SPEED_0 10 // speed below which it is always safe to switch to loiter
// 400hz loop update rate
#define POSHOLD_BRAKE_TIME_ESTIMATE_MAX (600*4) // max number of cycles the brake will be applied before we switch to loiter
#define POSHOLD_BRAKE_TO_LOITER_TIMER (150*4) // Number of cycles to transition from brake mode to loiter mode. Must be lower than POSHOLD_LOITER_STAB_TIMER
#define POSHOLD_WIND_COMP_START_TIMER (150*4) // Number of cycles to start wind compensation update after loiter is engaged
#define POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER (50*4) // Set it from 100 to 200, the number of centiseconds loiter and manual commands are mixed to make a smooth transition.
#define POSHOLD_SMOOTH_RATE_FACTOR 0.0125f // filter applied to pilot's roll/pitch input as it returns to center. A lower number will cause the roll/pitch to return to zero more slowly if the brake_rate is also low.
#define POSHOLD_WIND_COMP_TIMER_10HZ 40 // counter value used to reduce wind compensation to 10hz
#define LOOP_RATE_FACTOR 4 // used to adapt PosHold params to loop_rate
#define TC_WIND_COMP 0.0025f // Time constant for poshold_update_wind_comp_estimate()
// definitions that are independent of main loop rate
#define POSHOLD_STICK_RELEASE_SMOOTH_ANGLE 1800 // max angle required (in centi-degrees) after which the smooth stick release effect is applied
#define POSHOLD_WIND_COMP_ESTIMATE_SPEED_MAX 10 // wind compensation estimates will only run when velocity is at or below this speed in cm/s
// mission state enumeration
enum poshold_rp_mode {
POSHOLD_PILOT_OVERRIDE=0, // pilot is controlling this axis (i.e. roll or pitch)
POSHOLD_BRAKE, // this axis is braking towards zero
POSHOLD_BRAKE_READY_TO_LOITER, // this axis has completed braking and is ready to enter loiter mode (both modes must be this value before moving to next stage)
POSHOLD_BRAKE_TO_LOITER, // both vehicle's axis (roll and pitch) are transitioning from braking to loiter mode (braking and loiter controls are mixed)
POSHOLD_LOITER, // both vehicle axis are holding position
POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE // pilot has input controls on this axis and this axis is transitioning to pilot override (other axis will transition to brake if no pilot input)
};
static struct {
poshold_rp_mode roll_mode : 3; // roll mode: pilot override, brake or loiter
poshold_rp_mode pitch_mode : 3; // pitch mode: pilot override, brake or loiter
uint8_t braking_time_updated_roll : 1; // true once we have re-estimated the braking time. This is done once as the vehicle begins to flatten out after braking
uint8_t braking_time_updated_pitch : 1; // true once we have re-estimated the braking time. This is done once as the vehicle begins to flatten out after braking
uint8_t loiter_reset_I : 1; // true the very first time PosHold enters loiter, thereafter we trust the i terms loiter has
// pilot input related variables
float pilot_roll; // pilot requested roll angle (filtered to slow returns to zero)
float pilot_pitch; // pilot requested roll angle (filtered to slow returns to zero)
// braking related variables
float brake_gain; // gain used during conversion of vehicle's velocity to lean angle during braking (calculated from brake_rate)
int16_t brake_roll; // target roll angle during braking periods
int16_t brake_pitch; // target pitch angle during braking periods
int16_t brake_timeout_roll; // number of cycles allowed for the braking to complete, this timeout will be updated at half-braking
int16_t brake_timeout_pitch; // number of cycles allowed for the braking to complete, this timeout will be updated at half-braking
int16_t brake_angle_max_roll; // maximum lean angle achieved during braking. Used to determine when the vehicle has begun to flatten out so that we can re-estimate the braking time
int16_t brake_angle_max_pitch; // maximum lean angle achieved during braking Used to determine when the vehicle has begun to flatten out so that we can re-estimate the braking time
int16_t brake_to_loiter_timer; // cycles to mix brake and loiter controls in POSHOLD_BRAKE_TO_LOITER
// loiter related variables
int16_t controller_to_pilot_timer_roll; // cycles to mix controller and pilot controls in POSHOLD_CONTROLLER_TO_PILOT
int16_t controller_to_pilot_timer_pitch; // cycles to mix controller and pilot controls in POSHOLD_CONTROLLER_TO_PILOT
int16_t controller_final_roll; // final roll angle from controller as we exit brake or loiter mode (used for mixing with pilot input)
int16_t controller_final_pitch; // final pitch angle from controller as we exit brake or loiter mode (used for mixing with pilot input)
// wind compensation related variables
Vector2f wind_comp_ef; // wind compensation in earth frame, filtered lean angles from position controller
int16_t wind_comp_roll; // roll angle to compensate for wind
int16_t wind_comp_pitch; // pitch angle to compensate for wind
uint16_t wind_comp_start_timer; // counter to delay start of wind compensation for a short time after loiter is engaged
int8_t wind_comp_timer; // counter to reduce wind comp roll/pitch lean angle calcs to 10hz
// final output
int16_t roll; // final roll angle sent to attitude controller
int16_t pitch; // final pitch angle sent to attitude controller
} poshold;
// poshold_init - initialise PosHold controller
bool Copter::poshold_init(bool ignore_checks)
{
// fail to initialise PosHold mode if no GPS lock
if (!position_ok() && !ignore_checks) {
return false;
}
// initialize vertical speeds and acceleration
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
pos_control.set_accel_z(g.pilot_accel_z);
// initialise position and desired velocity
pos_control.set_alt_target(inertial_nav.get_altitude());
pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
// initialise lean angles to current attitude
poshold.pilot_roll = 0;
poshold.pilot_pitch = 0;
// compute brake_gain
poshold.brake_gain = (15.0f * (float)g.poshold_brake_rate + 95.0f) / 100.0f;
if (ap.land_complete) {
// if landed begin in loiter mode
poshold.roll_mode = POSHOLD_LOITER;
poshold.pitch_mode = POSHOLD_LOITER;
// set target to current position
// only init here as we can switch to PosHold in flight with a velocity <> 0 that will be used as _last_vel in PosControl and never updated again as we inhibit Reset_I
wp_nav.init_loiter_target();
}else{
// if not landed start in pilot override to avoid hard twitch
poshold.roll_mode = POSHOLD_PILOT_OVERRIDE;
poshold.pitch_mode = POSHOLD_PILOT_OVERRIDE;
}
// loiter's I terms should be reset the first time only
poshold.loiter_reset_I = true;
// initialise wind_comp each time PosHold is switched on
poshold.wind_comp_ef.zero();
poshold.wind_comp_roll = 0;
poshold.wind_comp_pitch = 0;
poshold.wind_comp_timer = 0;
return true;
}
// poshold_run - runs the PosHold controller
// should be called at 100hz or more
void Copter::poshold_run()
{
float target_roll, target_pitch; // pilot's roll and pitch angle inputs
float target_yaw_rate = 0; // pilot desired yaw rate in centi-degrees/sec
float target_climb_rate = 0; // pilot desired climb rate in centimeters/sec
float takeoff_climb_rate = 0.0f; // takeoff induced climb rate
float brake_to_loiter_mix; // mix of brake and loiter controls. 0 = fully brake controls, 1 = fully loiter controls
float controller_to_pilot_roll_mix; // mix of controller and pilot controls. 0 = fully last controller controls, 1 = fully pilot controls
float controller_to_pilot_pitch_mix; // mix of controller and pilot controls. 0 = fully last controller controls, 1 = fully pilot controls
float vel_fw, vel_right; // vehicle's current velocity in body-frame forward and right directions
const Vector3f& vel = inertial_nav.get_velocity();
// initialize vertical speeds and acceleration
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
pos_control.set_accel_z(g.pilot_accel_z);
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || !motors.get_interlock()) {
wp_nav.init_loiter_target();
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);
return;
}
// process pilot inputs
if (!failsafe.radio) {
// apply SIMPLE mode transform to pilot inputs
update_simple_mode();
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);
// get pilot desired climb rate (for alt-hold mode and take-off)
target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->control_in);
target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max, g.pilot_velocity_z_max);
// get takeoff adjusted pilot and takeoff climb rates
takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate);
// check for take-off
if (ap.land_complete && (takeoff_state.running || channel_throttle->control_in > get_takeoff_trigger_throttle())) {
if (!takeoff_state.running) {
takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));
}
// indicate we are taking off
set_land_complete(false);
// clear i term when we're taking off
set_throttle_takeoff();
}
}
// relax loiter target if we might be landed
if (ap.land_complete_maybe) {
wp_nav.loiter_soften_for_landing();
}
// if landed initialise loiter targets, set throttle to zero and exit
if (ap.land_complete) {
wp_nav.init_loiter_target();
// move throttle to between minimum and non-takeoff-throttle to keep us on the ground
attitude_control.set_throttle_out_unstabilized(get_throttle_pre_takeoff(channel_throttle->control_in),true,g.throttle_filt);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);
return;
}else{
// convert pilot input to lean angles
get_pilot_desired_lean_angles(channel_roll->control_in, channel_pitch->control_in, target_roll, target_pitch, aparm.angle_max);
// convert inertial nav earth-frame velocities to body-frame
// To-Do: move this to AP_Math (or perhaps we already have a function to do this)
vel_fw = vel.x*ahrs.cos_yaw() + vel.y*ahrs.sin_yaw();
vel_right = -vel.x*ahrs.sin_yaw() + vel.y*ahrs.cos_yaw();
// If not in LOITER, retrieve latest wind compensation lean angles related to current yaw
if (poshold.roll_mode != POSHOLD_LOITER || poshold.pitch_mode != POSHOLD_LOITER)
poshold_get_wind_comp_lean_angles(poshold.wind_comp_roll, poshold.wind_comp_pitch);
// Roll state machine
// Each state (aka mode) is responsible for:
// 1. dealing with pilot input
// 2. calculating the final roll output to the attitude controller
// 3. checking if the state (aka mode) should be changed and if 'yes' perform any required initialisation for the new state
switch (poshold.roll_mode) {
case POSHOLD_PILOT_OVERRIDE:
// update pilot desired roll angle using latest radio input
// this filters the input so that it returns to zero no faster than the brake-rate
poshold_update_pilot_lean_angle(poshold.pilot_roll, target_roll);
// switch to BRAKE mode for next iteration if no pilot input
if (is_zero(target_roll) && (fabsf(poshold.pilot_roll) < 2 * g.poshold_brake_rate)) {
// initialise BRAKE mode
poshold.roll_mode = POSHOLD_BRAKE; // Set brake roll mode
poshold.brake_roll = 0; // initialise braking angle to zero
poshold.brake_angle_max_roll = 0; // reset brake_angle_max so we can detect when vehicle begins to flatten out during braking
poshold.brake_timeout_roll = POSHOLD_BRAKE_TIME_ESTIMATE_MAX; // number of cycles the brake will be applied, updated during braking mode.
poshold.braking_time_updated_roll = false; // flag the braking time can be re-estimated
}
// final lean angle should be pilot input plus wind compensation
poshold.roll = poshold.pilot_roll + poshold.wind_comp_roll;
break;
case POSHOLD_BRAKE:
case POSHOLD_BRAKE_READY_TO_LOITER:
// calculate brake_roll angle to counter-act velocity
poshold_update_brake_angle_from_velocity(poshold.brake_roll, vel_right);
// update braking time estimate
if (!poshold.braking_time_updated_roll) {
// check if brake angle is increasing
if (abs(poshold.brake_roll) >= poshold.brake_angle_max_roll) {
poshold.brake_angle_max_roll = abs(poshold.brake_roll);
} else {
// braking angle has started decreasing so re-estimate braking time
poshold.brake_timeout_roll = 1+(uint16_t)(LOOP_RATE_FACTOR*15L*(int32_t)(abs(poshold.brake_roll))/(10L*(int32_t)g.poshold_brake_rate)); // the 1.2 (12/10) factor has to be tuned in flight, here it means 120% of the "normal" time.
poshold.braking_time_updated_roll = true;
}
}
// if velocity is very low reduce braking time to 0.5seconds
if ((fabsf(vel_right) <= POSHOLD_SPEED_0) && (poshold.brake_timeout_roll > 50*LOOP_RATE_FACTOR)) {
poshold.brake_timeout_roll = 50*LOOP_RATE_FACTOR;
}
// reduce braking timer
if (poshold.brake_timeout_roll > 0) {
poshold.brake_timeout_roll--;
} else {
// indicate that we are ready to move to Loiter.
// Loiter will only actually be engaged once both roll_mode and pitch_mode are changed to POSHOLD_BRAKE_READY_TO_LOITER
// logic for engaging loiter is handled below the roll and pitch mode switch statements
poshold.roll_mode = POSHOLD_BRAKE_READY_TO_LOITER;
}
// final lean angle is braking angle + wind compensation angle
poshold.roll = poshold.brake_roll + poshold.wind_comp_roll;
// check for pilot input
if (!is_zero(target_roll)) {
// init transition to pilot override
poshold_roll_controller_to_pilot_override();
}
break;
case POSHOLD_BRAKE_TO_LOITER:
case POSHOLD_LOITER:
// these modes are combined roll-pitch modes and are handled below
break;
case POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE:
// update pilot desired roll angle using latest radio input
// this filters the input so that it returns to zero no faster than the brake-rate
poshold_update_pilot_lean_angle(poshold.pilot_roll, target_roll);
// count-down loiter to pilot timer
if (poshold.controller_to_pilot_timer_roll > 0) {
poshold.controller_to_pilot_timer_roll--;
} else {
// when timer runs out switch to full pilot override for next iteration
poshold.roll_mode = POSHOLD_PILOT_OVERRIDE;
}
// calculate controller_to_pilot mix ratio
controller_to_pilot_roll_mix = (float)poshold.controller_to_pilot_timer_roll / (float)POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER;
// mix final loiter lean angle and pilot desired lean angles
poshold.roll = poshold_mix_controls(controller_to_pilot_roll_mix, poshold.controller_final_roll, poshold.pilot_roll + poshold.wind_comp_roll);
break;
}
// Pitch state machine
// Each state (aka mode) is responsible for:
// 1. dealing with pilot input
// 2. calculating the final pitch output to the attitude contpitcher
// 3. checking if the state (aka mode) should be changed and if 'yes' perform any required initialisation for the new state
switch (poshold.pitch_mode) {
case POSHOLD_PILOT_OVERRIDE:
// update pilot desired pitch angle using latest radio input
// this filters the input so that it returns to zero no faster than the brake-rate
poshold_update_pilot_lean_angle(poshold.pilot_pitch, target_pitch);
// switch to BRAKE mode for next iteration if no pilot input
if (is_zero(target_pitch) && (fabsf(poshold.pilot_pitch) < 2 * g.poshold_brake_rate)) {
// initialise BRAKE mode
poshold.pitch_mode = POSHOLD_BRAKE; // set brake pitch mode
poshold.brake_pitch = 0; // initialise braking angle to zero
poshold.brake_angle_max_pitch = 0; // reset brake_angle_max so we can detect when vehicle begins to flatten out during braking
poshold.brake_timeout_pitch = POSHOLD_BRAKE_TIME_ESTIMATE_MAX; // number of cycles the brake will be applied, updated during braking mode.
poshold.braking_time_updated_pitch = false; // flag the braking time can be re-estimated
}
// final lean angle should be pilot input plus wind compensation
poshold.pitch = poshold.pilot_pitch + poshold.wind_comp_pitch;
break;
case POSHOLD_BRAKE:
case POSHOLD_BRAKE_READY_TO_LOITER:
// calculate brake_pitch angle to counter-act velocity
poshold_update_brake_angle_from_velocity(poshold.brake_pitch, -vel_fw);
// update braking time estimate
if (!poshold.braking_time_updated_pitch) {
// check if brake angle is increasing
if (abs(poshold.brake_pitch) >= poshold.brake_angle_max_pitch) {
poshold.brake_angle_max_pitch = abs(poshold.brake_pitch);
} else {
// braking angle has started decreasing so re-estimate braking time
poshold.brake_timeout_pitch = 1+(uint16_t)(LOOP_RATE_FACTOR*15L*(int32_t)(abs(poshold.brake_pitch))/(10L*(int32_t)g.poshold_brake_rate)); // the 1.2 (12/10) factor has to be tuned in flight, here it means 120% of the "normal" time.
poshold.braking_time_updated_pitch = true;
}
}
// if velocity is very low reduce braking time to 0.5seconds
if ((fabsf(vel_fw) <= POSHOLD_SPEED_0) && (poshold.brake_timeout_pitch > 50*LOOP_RATE_FACTOR)) {
poshold.brake_timeout_pitch = 50*LOOP_RATE_FACTOR;
}
// reduce braking timer
if (poshold.brake_timeout_pitch > 0) {
poshold.brake_timeout_pitch--;
} else {
// indicate that we are ready to move to Loiter.
// Loiter will only actually be engaged once both pitch_mode and pitch_mode are changed to POSHOLD_BRAKE_READY_TO_LOITER
// logic for engaging loiter is handled below the pitch and pitch mode switch statements
poshold.pitch_mode = POSHOLD_BRAKE_READY_TO_LOITER;
}
// final lean angle is braking angle + wind compensation angle
poshold.pitch = poshold.brake_pitch + poshold.wind_comp_pitch;
// check for pilot input
if (!is_zero(target_pitch)) {
// init transition to pilot override
poshold_pitch_controller_to_pilot_override();
}
break;
case POSHOLD_BRAKE_TO_LOITER:
case POSHOLD_LOITER:
// these modes are combined pitch-pitch modes and are handled below
break;
case POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE:
// update pilot desired pitch angle using latest radio input
// this filters the input so that it returns to zero no faster than the brake-rate
poshold_update_pilot_lean_angle(poshold.pilot_pitch, target_pitch);
// count-down loiter to pilot timer
if (poshold.controller_to_pilot_timer_pitch > 0) {
poshold.controller_to_pilot_timer_pitch--;
} else {
// when timer runs out switch to full pilot override for next iteration
poshold.pitch_mode = POSHOLD_PILOT_OVERRIDE;
}
// calculate controller_to_pilot mix ratio
controller_to_pilot_pitch_mix = (float)poshold.controller_to_pilot_timer_pitch / (float)POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER;
// mix final loiter lean angle and pilot desired lean angles
poshold.pitch = poshold_mix_controls(controller_to_pilot_pitch_mix, poshold.controller_final_pitch, poshold.pilot_pitch + poshold.wind_comp_pitch);
break;
}
//
// Shared roll & pitch states (POSHOLD_BRAKE_TO_LOITER and POSHOLD_LOITER)
//
// switch into LOITER mode when both roll and pitch are ready
if (poshold.roll_mode == POSHOLD_BRAKE_READY_TO_LOITER && poshold.pitch_mode == POSHOLD_BRAKE_READY_TO_LOITER) {
poshold.roll_mode = POSHOLD_BRAKE_TO_LOITER;
poshold.pitch_mode = POSHOLD_BRAKE_TO_LOITER;
poshold.brake_to_loiter_timer = POSHOLD_BRAKE_TO_LOITER_TIMER;
// init loiter controller
wp_nav.init_loiter_target(inertial_nav.get_position(), poshold.loiter_reset_I); // (false) to avoid I_term reset. In original code, velocity(0,0,0) was used instead of current velocity: wp_nav.init_loiter_target(inertial_nav.get_position(), Vector3f(0,0,0));
// at this stage, we are going to run update_loiter that will reset I_term once. From now, we ensure next time that we will enter loiter and update it, I_term won't be reset anymore
poshold.loiter_reset_I = false;
// set delay to start of wind compensation estimate updates
poshold.wind_comp_start_timer = POSHOLD_WIND_COMP_START_TIMER;
}
// roll-mode is used as the combined roll+pitch mode when in BRAKE_TO_LOITER or LOITER modes
if (poshold.roll_mode == POSHOLD_BRAKE_TO_LOITER || poshold.roll_mode == POSHOLD_LOITER) {
// force pitch mode to be same as roll_mode just to keep it consistent (it's not actually used in these states)
poshold.pitch_mode = poshold.roll_mode;
// handle combined roll+pitch mode
switch (poshold.roll_mode) {
case POSHOLD_BRAKE_TO_LOITER:
// reduce brake_to_loiter timer
if (poshold.brake_to_loiter_timer > 0) {
poshold.brake_to_loiter_timer--;
} else {
// progress to full loiter on next iteration
poshold.roll_mode = POSHOLD_LOITER;
poshold.pitch_mode = POSHOLD_LOITER;
}
// calculate percentage mix of loiter and brake control
brake_to_loiter_mix = (float)poshold.brake_to_loiter_timer / (float)POSHOLD_BRAKE_TO_LOITER_TIMER;
// calculate brake_roll and pitch angles to counter-act velocity
poshold_update_brake_angle_from_velocity(poshold.brake_roll, vel_right);
poshold_update_brake_angle_from_velocity(poshold.brake_pitch, -vel_fw);
// run loiter controller
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
// calculate final roll and pitch output by mixing loiter and brake controls
poshold.roll = poshold_mix_controls(brake_to_loiter_mix, poshold.brake_roll + poshold.wind_comp_roll, wp_nav.get_roll());
poshold.pitch = poshold_mix_controls(brake_to_loiter_mix, poshold.brake_pitch + poshold.wind_comp_pitch, wp_nav.get_pitch());
// check for pilot input
if (!is_zero(target_roll) || !is_zero(target_pitch)) {
// if roll input switch to pilot override for roll
if (!is_zero(target_roll)) {
// init transition to pilot override
poshold_roll_controller_to_pilot_override();
// switch pitch-mode to brake (but ready to go back to loiter anytime)
// no need to reset poshold.brake_pitch here as wind comp has not been updated since last brake_pitch computation
poshold.pitch_mode = POSHOLD_BRAKE_READY_TO_LOITER;
}
// if pitch input switch to pilot override for pitch
if (!is_zero(target_pitch)) {
// init transition to pilot override
poshold_pitch_controller_to_pilot_override();
if (is_zero(target_roll)) {
// switch roll-mode to brake (but ready to go back to loiter anytime)
// no need to reset poshold.brake_roll here as wind comp has not been updated since last brake_roll computation
poshold.roll_mode = POSHOLD_BRAKE_READY_TO_LOITER;
}
}
}
break;
case POSHOLD_LOITER:
// run loiter controller
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
// set roll angle based on loiter controller outputs
poshold.roll = wp_nav.get_roll();
poshold.pitch = wp_nav.get_pitch();
// update wind compensation estimate
poshold_update_wind_comp_estimate();
// check for pilot input
if (!is_zero(target_roll) || !is_zero(target_pitch)) {
// if roll input switch to pilot override for roll
if (!is_zero(target_roll)) {
// init transition to pilot override
poshold_roll_controller_to_pilot_override();
// switch pitch-mode to brake (but ready to go back to loiter anytime)
poshold.pitch_mode = POSHOLD_BRAKE_READY_TO_LOITER;
// reset brake_pitch because wind_comp is now different and should give the compensation of the whole previous loiter angle
poshold.brake_pitch = 0;
}
// if pitch input switch to pilot override for pitch
if (!is_zero(target_pitch)) {
// init transition to pilot override
poshold_pitch_controller_to_pilot_override();
// if roll not overriden switch roll-mode to brake (but be ready to go back to loiter any time)
if (is_zero(target_roll)) {
poshold.roll_mode = POSHOLD_BRAKE_READY_TO_LOITER;
poshold.brake_roll = 0;
}
}
}
break;
default:
// do nothing for uncombined roll and pitch modes
break;
}
}
// constrain target pitch/roll angles
poshold.roll = constrain_int16(poshold.roll, -aparm.angle_max, aparm.angle_max);
poshold.pitch = constrain_int16(poshold.pitch, -aparm.angle_max, aparm.angle_max);
// update attitude controller targets
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(poshold.roll, poshold.pitch, target_yaw_rate);
// throttle control
if (sonar_enabled && (sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) {
// if sonar is ok, use surface tracking
target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control.get_alt_target(), G_Dt);
}
// update altitude target and call position controller
pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
pos_control.add_takeoff_climb_rate(takeoff_climb_rate, G_Dt);
pos_control.update_z_controller();
}
}
// poshold_update_pilot_lean_angle - update the pilot's filtered lean angle with the latest raw input received
void Copter::poshold_update_pilot_lean_angle(float &lean_angle_filtered, float &lean_angle_raw)
{
// if raw input is large or reversing the vehicle's lean angle immediately set the fitlered angle to the new raw angle
if ((lean_angle_filtered > 0 && lean_angle_raw < 0) || (lean_angle_filtered < 0 && lean_angle_raw > 0) || (fabsf(lean_angle_raw) > POSHOLD_STICK_RELEASE_SMOOTH_ANGLE)) {
lean_angle_filtered = lean_angle_raw;
} else {
// lean_angle_raw must be pulling lean_angle_filtered towards zero, smooth the decrease
if (lean_angle_filtered > 0) {
// reduce the filtered lean angle at 5% or the brake rate (whichever is faster).
lean_angle_filtered -= MAX((float)lean_angle_filtered * POSHOLD_SMOOTH_RATE_FACTOR, MAX(1, g.poshold_brake_rate/LOOP_RATE_FACTOR));
// do not let the filtered angle fall below the pilot's input lean angle.
// the above line pulls the filtered angle down and the below line acts as a catch
lean_angle_filtered = MAX(lean_angle_filtered, lean_angle_raw);
}else{
lean_angle_filtered += MAX(-(float)lean_angle_filtered * POSHOLD_SMOOTH_RATE_FACTOR, MAX(1, g.poshold_brake_rate/LOOP_RATE_FACTOR));
lean_angle_filtered = MIN(lean_angle_filtered, lean_angle_raw);
}
}
}
// poshold_mix_controls - mixes two controls based on the mix_ratio
// mix_ratio of 1 = use first_control completely, 0 = use second_control completely, 0.5 = mix evenly
int16_t Copter::poshold_mix_controls(float mix_ratio, int16_t first_control, int16_t second_control)
{
mix_ratio = constrain_float(mix_ratio, 0.0f, 1.0f);
return (int16_t)((mix_ratio * first_control) + ((1.0f-mix_ratio)*second_control));
}
// poshold_update_brake_angle_from_velocity - updates the brake_angle based on the vehicle's velocity and brake_gain
// brake_angle is slewed with the wpnav.poshold_brake_rate and constrained by the wpnav.poshold_braking_angle_max
// velocity is assumed to be in the same direction as lean angle so for pitch you should provide the velocity backwards (i.e. -ve forward velocity)
void Copter::poshold_update_brake_angle_from_velocity(int16_t &brake_angle, float velocity)
{
float lean_angle;
int16_t brake_rate = g.poshold_brake_rate;
brake_rate /= 4;
if (brake_rate <= 0) {
brake_rate = 1;
}
// calculate velocity-only based lean angle
if (velocity >= 0) {
lean_angle = -poshold.brake_gain * velocity * (1.0f+500.0f/(velocity+60.0f));
} else {
lean_angle = -poshold.brake_gain * velocity * (1.0f+500.0f/(-velocity+60.0f));
}
// do not let lean_angle be too far from brake_angle
brake_angle = constrain_int16((int16_t)lean_angle, brake_angle - brake_rate, brake_angle + brake_rate);
// constrain final brake_angle
brake_angle = constrain_int16(brake_angle, -g.poshold_brake_angle_max, g.poshold_brake_angle_max);
}
// poshold_update_wind_comp_estimate - updates wind compensation estimate
// should be called at the maximum loop rate when loiter is engaged
void Copter::poshold_update_wind_comp_estimate()
{
// check wind estimate start has not been delayed
if (poshold.wind_comp_start_timer > 0) {
poshold.wind_comp_start_timer--;
return;
}
// check horizontal velocity is low
if (inertial_nav.get_velocity_xy() > POSHOLD_WIND_COMP_ESTIMATE_SPEED_MAX) {
return;
}
// get position controller accel target
// To-Do: clean this up by using accessor in loiter controller (or move entire PosHold controller to a library shared with loiter)
const Vector3f& accel_target = pos_control.get_accel_target();
// update wind compensation in earth-frame lean angles
if (is_zero(poshold.wind_comp_ef.x)) {
// if wind compensation has not been initialised set it immediately to the pos controller's desired accel in north direction
poshold.wind_comp_ef.x = accel_target.x;
} else {
// low pass filter the position controller's lean angle output
poshold.wind_comp_ef.x = (1.0f-TC_WIND_COMP)*poshold.wind_comp_ef.x + TC_WIND_COMP*accel_target.x;
}
if (is_zero(poshold.wind_comp_ef.y)) {
// if wind compensation has not been initialised set it immediately to the pos controller's desired accel in north direction
poshold.wind_comp_ef.y = accel_target.y;
} else {
// low pass filter the position controller's lean angle output
poshold.wind_comp_ef.y = (1.0f-TC_WIND_COMP)*poshold.wind_comp_ef.y + TC_WIND_COMP*accel_target.y;
}
}
// poshold_get_wind_comp_lean_angles - retrieve wind compensation angles in body frame roll and pitch angles
// should be called at the maximum loop rate
void Copter::poshold_get_wind_comp_lean_angles(int16_t &roll_angle, int16_t &pitch_angle)
{
// reduce rate to 10hz
poshold.wind_comp_timer++;
if (poshold.wind_comp_timer < POSHOLD_WIND_COMP_TIMER_10HZ) {
return;
}
poshold.wind_comp_timer = 0;
// convert earth frame desired accelerations to body frame roll and pitch lean angles
roll_angle = atanf((-poshold.wind_comp_ef.x*ahrs.sin_yaw() + poshold.wind_comp_ef.y*ahrs.cos_yaw())/981)*(18000/M_PI_F);
pitch_angle = atanf(-(poshold.wind_comp_ef.x*ahrs.cos_yaw() + poshold.wind_comp_ef.y*ahrs.sin_yaw())/981)*(18000/M_PI_F);
}
// poshold_roll_controller_to_pilot_override - initialises transition from a controller submode (brake or loiter) to a pilot override on roll axis
void Copter::poshold_roll_controller_to_pilot_override()
{
poshold.roll_mode = POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE;
poshold.controller_to_pilot_timer_roll = POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER;
// initialise pilot_roll to 0, wind_comp will be updated to compensate and poshold_update_pilot_lean_angle function shall not smooth this transition at next iteration. so 0 is the right value
poshold.pilot_roll = 0;
// store final controller output for mixing with pilot input
poshold.controller_final_roll = poshold.roll;
}
// poshold_pitch_controller_to_pilot_override - initialises transition from a controller submode (brake or loiter) to a pilot override on roll axis
void Copter::poshold_pitch_controller_to_pilot_override()
{
poshold.pitch_mode = POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE;
poshold.controller_to_pilot_timer_pitch = POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER;
// initialise pilot_pitch to 0, wind_comp will be updated to compensate and poshold_update_pilot_lean_angle function shall not smooth this transition at next iteration. so 0 is the right value
poshold.pilot_pitch = 0;
// store final loiter outputs for mixing with pilot input
poshold.controller_final_pitch = poshold.pitch;
}
#endif // POSHOLD_ENABLED == ENABLED

435
ArduSub/control_rtl.cpp Normal file
View File

@ -0,0 +1,435 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
/*
* control_rtl.pde - init and run calls for RTL flight mode
*
* There are two parts to RTL, the high level decision making which controls which state we are in
* and the lower implementation of the waypoint or landing controllers within those states
*/
// rtl_init - initialise rtl controller
bool Copter::rtl_init(bool ignore_checks)
{
if (position_ok() || ignore_checks) {
rtl_climb_start();
return true;
}else{
return false;
}
}
// rtl_run - runs the return-to-launch controller
// should be called at 100hz or more
void Copter::rtl_run()
{
// check if we need to move to next state
if (rtl_state_complete) {
switch (rtl_state) {
case RTL_InitialClimb:
rtl_return_start();
break;
case RTL_ReturnHome:
rtl_loiterathome_start();
break;
case RTL_LoiterAtHome:
if (g.rtl_alt_final > 0 && !failsafe.radio) {
rtl_descent_start();
}else{
rtl_land_start();
}
break;
case RTL_FinalDescent:
// do nothing
break;
case RTL_Land:
// do nothing - rtl_land_run will take care of disarming motors
break;
}
}
// call the correct run function
switch (rtl_state) {
case RTL_InitialClimb:
rtl_climb_return_run();
break;
case RTL_ReturnHome:
rtl_climb_return_run();
break;
case RTL_LoiterAtHome:
rtl_loiterathome_run();
break;
case RTL_FinalDescent:
rtl_descent_run();
break;
case RTL_Land:
rtl_land_run();
break;
}
}
// rtl_climb_start - initialise climb to RTL altitude
void Copter::rtl_climb_start()
{
rtl_state = RTL_InitialClimb;
rtl_state_complete = false;
rtl_alt = get_RTL_alt();
// initialise waypoint and spline controller
wp_nav.wp_and_spline_init();
// RTL_SPEED == 0 means use WPNAV_SPEED
if (!is_zero(g.rtl_speed_cms)) {
wp_nav.set_speed_xy(g.rtl_speed_cms);
}
// get horizontal stopping point
Vector3f destination;
wp_nav.get_wp_stopping_point_xy(destination);
#if AC_RALLY == ENABLED
// rally_point.alt will be the altitude of the nearest rally point or the RTL_ALT. uses absolute altitudes
Location rally_point = rally.calc_best_rally_or_home_location(current_loc, rtl_alt+ahrs.get_home().alt);
rally_point.alt -= ahrs.get_home().alt; // convert to altitude above home
rally_point.alt = MAX(rally_point.alt, current_loc.alt); // ensure we do not descend before reaching home
destination.z = pv_alt_above_origin(rally_point.alt);
#else
destination.z = pv_alt_above_origin(rtl_alt);
#endif
// set the destination
wp_nav.set_wp_destination(destination);
wp_nav.set_fast_waypoint(true);
// hold current yaw during initial climb
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
// rtl_return_start - initialise return to home
void Copter::rtl_return_start()
{
rtl_state = RTL_ReturnHome;
rtl_state_complete = false;
// set target to above home/rally point
#if AC_RALLY == ENABLED
// rally_point will be the nearest rally point or home. uses absolute altitudes
Location rally_point = rally.calc_best_rally_or_home_location(current_loc, rtl_alt+ahrs.get_home().alt);
rally_point.alt -= ahrs.get_home().alt; // convert to altitude above home
rally_point.alt = MAX(rally_point.alt, current_loc.alt); // ensure we do not descend before reaching home
Vector3f destination = pv_location_to_vector(rally_point);
#else
Vector3f destination = pv_location_to_vector(ahrs.get_home());
destination.z = pv_alt_above_origin(rtl_alt);
#endif
wp_nav.set_wp_destination(destination);
// initialise yaw to point home (maybe)
set_auto_yaw_mode(get_default_auto_yaw_mode(true));
}
// rtl_climb_return_run - implements the initial climb, return home and descent portions of RTL which all rely on the wp controller
// called by rtl_run at 100hz or more
void Copter::rtl_climb_return_run()
{
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || !motors.get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
// reset attitude control targets
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
// To-Do: re-initialise wpnav targets
return;
}
// process pilot's yaw input
float target_yaw_rate = 0;
if (!failsafe.radio) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);
if (!is_zero(target_yaw_rate)) {
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
}
// run waypoint controller
wp_nav.update_wpnav();
// call z-axis position controller (wpnav should have already updated it's alt target)
pos_control.update_z_controller();
// call attitude controller
if (auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
}else{
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true);
}
// check if we've completed this stage of RTL
rtl_state_complete = wp_nav.reached_wp_destination();
}
// rtl_return_start - initialise return to home
void Copter::rtl_loiterathome_start()
{
rtl_state = RTL_LoiterAtHome;
rtl_state_complete = false;
rtl_loiter_start_time = millis();
// yaw back to initial take-off heading yaw unless pilot has already overridden yaw
if(get_default_auto_yaw_mode(true) != AUTO_YAW_HOLD) {
set_auto_yaw_mode(AUTO_YAW_RESETTOARMEDYAW);
} else {
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
}
// rtl_climb_return_descent_run - implements the initial climb, return home and descent portions of RTL which all rely on the wp controller
// called by rtl_run at 100hz or more
void Copter::rtl_loiterathome_run()
{
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || !motors.get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
// reset attitude control targets
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
// To-Do: re-initialise wpnav targets
return;
}
// process pilot's yaw input
float target_yaw_rate = 0;
if (!failsafe.radio) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);
if (!is_zero(target_yaw_rate)) {
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
}
// run waypoint controller
wp_nav.update_wpnav();
// call z-axis position controller (wpnav should have already updated it's alt target)
pos_control.update_z_controller();
// call attitude controller
if (auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
}else{
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true);
}
// check if we've completed this stage of RTL
if ((millis() - rtl_loiter_start_time) >= (uint32_t)g.rtl_loiter_time.get()) {
if (auto_yaw_mode == AUTO_YAW_RESETTOARMEDYAW) {
// check if heading is within 2 degrees of heading when vehicle was armed
if (labs(wrap_180_cd(ahrs.yaw_sensor-initial_armed_bearing)) <= 200) {
rtl_state_complete = true;
}
} else {
// we have loitered long enough
rtl_state_complete = true;
}
}
}
// rtl_descent_start - initialise descent to final alt
void Copter::rtl_descent_start()
{
rtl_state = RTL_FinalDescent;
rtl_state_complete = false;
// Set wp navigation target to above home
wp_nav.init_loiter_target(wp_nav.get_wp_destination());
// initialise altitude target to stopping point
pos_control.set_target_to_stopping_point_z();
// initialise yaw
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
// rtl_descent_run - implements the final descent to the RTL_ALT
// called by rtl_run at 100hz or more
void Copter::rtl_descent_run()
{
int16_t roll_control = 0, pitch_control = 0;
float target_yaw_rate = 0;
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || !motors.get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
// set target to current position
wp_nav.init_loiter_target();
return;
}
// process pilot's input
if (!failsafe.radio) {
if (g.land_repositioning) {
// apply SIMPLE mode transform to pilot inputs
update_simple_mode();
// process pilot's roll and pitch input
roll_control = channel_roll->control_in;
pitch_control = channel_pitch->control_in;
}
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);
}
// process roll, pitch inputs
wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control);
// run loiter controller
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
// call z-axis position controller
pos_control.set_alt_target_with_slew(pv_alt_above_origin(g.rtl_alt_final), G_Dt);
pos_control.update_z_controller();
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
// check if we've reached within 20cm of final altitude
rtl_state_complete = fabsf(pv_alt_above_origin(g.rtl_alt_final) - inertial_nav.get_altitude()) < 20.0f;
}
// rtl_loiterathome_start - initialise controllers to loiter over home
void Copter::rtl_land_start()
{
rtl_state = RTL_Land;
rtl_state_complete = false;
// Set wp navigation target to above home
wp_nav.init_loiter_target(wp_nav.get_wp_destination());
// initialise altitude target to stopping point
pos_control.set_target_to_stopping_point_z();
// initialise yaw
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
// rtl_returnhome_run - return home
// called by rtl_run at 100hz or more
void Copter::rtl_land_run()
{
int16_t roll_control = 0, pitch_control = 0;
float target_yaw_rate = 0;
// if not auto armed or landing completed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
// set target to current position
wp_nav.init_loiter_target();
#if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED
// disarm when the landing detector says we've landed and throttle is at minimum
if (ap.land_complete && (ap.throttle_zero || failsafe.radio)) {
init_disarm_motors();
}
#else
// disarm when the landing detector says we've landed
if (ap.land_complete) {
init_disarm_motors();
}
#endif
// check if we've completed this stage of RTL
rtl_state_complete = ap.land_complete;
return;
}
// relax loiter target if we might be landed
if (ap.land_complete_maybe) {
wp_nav.loiter_soften_for_landing();
}
// process pilot's input
if (!failsafe.radio) {
if (g.land_repositioning) {
// apply SIMPLE mode transform to pilot inputs
update_simple_mode();
// process pilot's roll and pitch input
roll_control = channel_roll->control_in;
pitch_control = channel_pitch->control_in;
}
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);
}
// process pilot's roll and pitch input
wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control);
// run loiter controller
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
// call z-axis position controller
float cmb_rate = get_land_descent_speed();
pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt, true);
pos_control.update_z_controller();
// record desired climb rate for logging
desired_climb_rate = cmb_rate;
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
// check if we've completed this stage of RTL
rtl_state_complete = ap.land_complete;
}
// get_RTL_alt - return altitude which vehicle should return home at
// altitude is in cm above home
float Copter::get_RTL_alt()
{
// maximum of current altitude + climb_min and rtl altitude
float ret = MAX(current_loc.alt + MAX(0, g.rtl_climb_min), g.rtl_altitude);
ret = MAX(ret, RTL_ALT_MIN);
#if AC_FENCE == ENABLED
// ensure not above fence altitude if alt fence is enabled
if ((fence.get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) != 0) {
ret = MIN(ret, fence.get_safe_alt()*100.0f);
}
#endif
return ret;
}

113
ArduSub/control_sport.cpp Normal file
View File

@ -0,0 +1,113 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
/*
* control_sport.pde - init and run calls for sport flight mode
*/
// sport_init - initialise sport controller
bool Copter::sport_init(bool ignore_checks)
{
// initialize vertical speed and acceleration
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
pos_control.set_accel_z(g.pilot_accel_z);
// initialise position and desired velocity
pos_control.set_alt_target(inertial_nav.get_altitude());
pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
return true;
}
// sport_run - runs the sport controller
// should be called at 100hz or more
void Copter::sport_run()
{
float target_roll_rate, target_pitch_rate, target_yaw_rate;
float target_climb_rate = 0;
float takeoff_climb_rate = 0.0f;
// initialize vertical speed and acceleration
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
pos_control.set_accel_z(g.pilot_accel_z);
// if not armed or throttle at zero, set throttle to zero and exit immediately
if(!motors.armed() || ap.throttle_zero) {
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);
return;
}
// apply SIMPLE mode transform
update_simple_mode();
// get pilot's desired roll and pitch rates
// calculate rate requests
target_roll_rate = channel_roll->control_in * g.acro_rp_p;
target_pitch_rate = channel_pitch->control_in * g.acro_rp_p;
int32_t roll_angle = wrap_180_cd(ahrs.roll_sensor);
target_roll_rate -= constrain_int32(roll_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_roll;
// Calculate trainer mode earth frame rate command for pitch
int32_t pitch_angle = wrap_180_cd(ahrs.pitch_sensor);
target_pitch_rate -= constrain_int32(pitch_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_pitch;
if (roll_angle > aparm.angle_max){
target_roll_rate -= g.acro_rp_p*(roll_angle-aparm.angle_max);
}else if (roll_angle < -aparm.angle_max) {
target_roll_rate -= g.acro_rp_p*(roll_angle+aparm.angle_max);
}
if (pitch_angle > aparm.angle_max){
target_pitch_rate -= g.acro_rp_p*(pitch_angle-aparm.angle_max);
}else if (pitch_angle < -aparm.angle_max) {
target_pitch_rate -= g.acro_rp_p*(pitch_angle+aparm.angle_max);
}
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);
// get pilot desired climb rate
target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->control_in);
target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max, g.pilot_velocity_z_max);
// get takeoff adjusted pilot and takeoff climb rates
takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate);
// check for take-off
if (ap.land_complete && (takeoff_state.running || (channel_throttle->control_in > get_takeoff_trigger_throttle()))) {
if (!takeoff_state.running) {
takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));
}
// indicate we are taking off
set_land_complete(false);
// clear i term when we're taking off
set_throttle_takeoff();
}
// reset target lean angles and heading while landed
if (ap.land_complete) {
// move throttle to between minimum and non-takeoff-throttle to keep us on the ground
attitude_control.set_throttle_out_unstabilized(get_throttle_pre_takeoff(channel_throttle->control_in),true,g.throttle_filt);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);
}else{
// call attitude controller
attitude_control.input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate);
// call throttle controller
if (sonar_enabled && (sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) {
// if sonar is ok, use surface tracking
target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control.get_alt_target(), G_Dt);
}
// call position controller
pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
pos_control.add_takeoff_climb_rate(takeoff_climb_rate, G_Dt);
pos_control.update_z_controller();
}
}

60
ArduSub/control_stabilize.cpp Normal file
View File

@ -0,0 +1,60 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
/*
* control_stabilize.pde - init and run calls for stabilize flight mode
*/
// stabilize_init - initialise stabilize controller
bool Copter::stabilize_init(bool ignore_checks)
{
// if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high
if (motors.armed() && ap.land_complete && !mode_has_manual_throttle(control_mode) && (g.rc_3.control_in > get_non_takeoff_throttle())) {
return false;
}
// set target altitude to zero for reporting
pos_control.set_alt_target(0);
return true;
}
// stabilize_run - runs the main stabilize controller
// should be called at 100hz or more
void Copter::stabilize_run()
{
float target_roll, target_pitch;
float target_yaw_rate;
int16_t pilot_throttle_scaled;
// if not armed or throttle at zero, set throttle to zero and exit immediately
if(!motors.armed() || ap.throttle_zero) {
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
// slow start if landed
if (ap.land_complete) {
motors.slow_start(true);
}
return;
}
// apply SIMPLE mode transform to pilot inputs
update_simple_mode();
// convert pilot input to lean angles
// To-Do: convert get_pilot_desired_lean_angles to return angles as floats
get_pilot_desired_lean_angles(channel_roll->control_in, channel_pitch->control_in, target_roll, target_pitch, aparm.angle_max);
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);
// get pilot's desired throttle
pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->control_in);
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
// body-frame rate controller is run directly from 100hz loop
// output pilot's throttle
attitude_control.set_throttle_out(pilot_throttle_scaled, true, g.throttle_filt);
}

181
ArduSub/crash_check.cpp Normal file
View File

@ -0,0 +1,181 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
// Code to detect a crash main ArduCopter code
#define CRASH_CHECK_TRIGGER_SEC 2 // 2 seconds inverted indicates a crash
#define CRASH_CHECK_ANGLE_DEVIATION_CD 3000.0f // 30 degrees beyond angle max is signal we are inverted
#define CRASH_CHECK_ACCEL_MAX 3.0f // vehicle must be accelerating less than 3m/s/s to be considered crashed
// crash_check - disarms motors if a crash has been detected
// crashes are detected by the vehicle being more than 20 degrees beyond it's angle limits continuously for more than 1 second
// called at MAIN_LOOP_RATE
void Copter::crash_check()
{
static uint16_t crash_counter; // number of iterations vehicle may have been crashed
// return immediately if disarmed, or crash checking disabled
if (!motors.armed() || ap.land_complete || g.fs_crash_check == 0) {
crash_counter = 0;
return;
}
// return immediately if we are not in an angle stabilize flight mode or we are flipping
if (control_mode == ACRO || control_mode == FLIP) {
crash_counter = 0;
return;
}
// vehicle not crashed if 1hz filtered acceleration is more than 3m/s (1G on Z-axis has been subtracted)
if (land_accel_ef_filter.get().length() >= CRASH_CHECK_ACCEL_MAX) {
crash_counter = 0;
return;
}
// check for angle error over 30 degrees
const Vector3f angle_error = attitude_control.get_att_error_rot_vec_cd();
if (pythagorous2(angle_error.x, angle_error.y) <= CRASH_CHECK_ANGLE_DEVIATION_CD) {
crash_counter = 0;
return;
}
// we may be crashing
crash_counter++;
// check if crashing for 2 seconds
if (crash_counter >= (CRASH_CHECK_TRIGGER_SEC * MAIN_LOOP_RATE)) {
// log an error in the dataflash
Log_Write_Error(ERROR_SUBSYSTEM_CRASH_CHECK, ERROR_CODE_CRASH_CHECK_CRASH);
// send message to gcs
gcs_send_text(MAV_SEVERITY_EMERGENCY,"Crash: Disarming");
// disarm motors
init_disarm_motors();
}
}
#if PARACHUTE == ENABLED
// Code to detect a crash main ArduCopter code
#define PARACHUTE_CHECK_TRIGGER_SEC 1 // 1 second of loss of control triggers the parachute
#define PARACHUTE_CHECK_ANGLE_DEVIATION_CD 3000 // 30 degrees off from target indicates a loss of control
// parachute_check - disarms motors and triggers the parachute if serious loss of control has been detected
// vehicle is considered to have a "serious loss of control" by the vehicle being more than 30 degrees off from the target roll and pitch angles continuously for 1 second
// called at MAIN_LOOP_RATE
void Copter::parachute_check()
{
static uint16_t control_loss_count; // number of iterations we have been out of control
static int32_t baro_alt_start;
// exit immediately if parachute is not enabled
if (!parachute.enabled()) {
return;
}
// call update to give parachute a chance to move servo or relay back to off position
parachute.update();
// return immediately if motors are not armed or pilot's throttle is above zero
if (!motors.armed()) {
control_loss_count = 0;
return;
}
// return immediately if we are not in an angle stabilize flight mode or we are flipping
if (control_mode == ACRO || control_mode == FLIP) {
control_loss_count = 0;
return;
}
// ensure we are flying
if (ap.land_complete) {
control_loss_count = 0;
return;
}
// ensure the first control_loss event is from above the min altitude
if (control_loss_count == 0 && parachute.alt_min() != 0 && (current_loc.alt < (int32_t)parachute.alt_min() * 100)) {
return;
}
// check for angle error over 30 degrees
const Vector3f angle_error = attitude_control.get_att_error_rot_vec_cd();
if (pythagorous2(angle_error.x, angle_error.y) <= CRASH_CHECK_ANGLE_DEVIATION_CD) {
control_loss_count = 0;
return;
}
// increment counter
if (control_loss_count < (PARACHUTE_CHECK_TRIGGER_SEC*MAIN_LOOP_RATE)) {
control_loss_count++;
}
// record baro alt if we have just started losing control
if (control_loss_count == 1) {
baro_alt_start = baro_alt;
// exit if baro altitude change indicates we are not falling
} else if (baro_alt >= baro_alt_start) {
control_loss_count = 0;
return;
// To-Do: add check that the vehicle is actually falling
// check if loss of control for at least 1 second
} else if (control_loss_count >= (PARACHUTE_CHECK_TRIGGER_SEC*MAIN_LOOP_RATE)) {
// reset control loss counter
control_loss_count = 0;
// log an error in the dataflash
Log_Write_Error(ERROR_SUBSYSTEM_CRASH_CHECK, ERROR_CODE_CRASH_CHECK_LOSS_OF_CONTROL);
// release parachute
parachute_release();
}
}
// parachute_release - trigger the release of the parachute, disarm the motors and notify the user
void Copter::parachute_release()
{
// send message to gcs and dataflash
gcs_send_text(MAV_SEVERITY_INFO,"Parachute: Released");
Log_Write_Event(DATA_PARACHUTE_RELEASED);
// disarm motors
init_disarm_motors();
// release parachute
parachute.release();
}
// parachute_manual_release - trigger the release of the parachute, after performing some checks for pilot error
// checks if the vehicle is landed
void Copter::parachute_manual_release()
{
// exit immediately if parachute is not enabled
if (!parachute.enabled()) {
return;
}
// do not release if vehicle is landed
// do not release if we are landed or below the minimum altitude above home
if (ap.land_complete) {
// warn user of reason for failure
gcs_send_text(MAV_SEVERITY_INFO,"Parachute: Landed");
// log an error in the dataflash
Log_Write_Error(ERROR_SUBSYSTEM_PARACHUTE, ERROR_CODE_PARACHUTE_LANDED);
return;
}
// do not release if we are landed or below the minimum altitude above home
if ((parachute.alt_min() != 0 && (current_loc.alt < (int32_t)parachute.alt_min() * 100))) {
// warn user of reason for failure
gcs_send_text(MAV_SEVERITY_ALERT,"Parachute: Too low");
// log an error in the dataflash
Log_Write_Error(ERROR_SUBSYSTEM_PARACHUTE, ERROR_CODE_PARACHUTE_TOO_LOW);
return;
}
// if we get this far release parachute
parachute_release();
}
#endif // PARACHUTE == ENABLED

424
ArduSub/defines.h Normal file
View File

@ -0,0 +1,424 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#ifndef _DEFINES_H
#define _DEFINES_H
#include <AP_HAL/AP_HAL_Boards.h>
// Just so that it's completely clear...
#define ENABLED 1
#define DISABLED 0
// this avoids a very common config error
#define ENABLE ENABLED
#define DISABLE DISABLED
// Autopilot Yaw Mode enumeration
enum autopilot_yaw_mode {
AUTO_YAW_HOLD = 0, // pilot controls the heading
AUTO_YAW_LOOK_AT_NEXT_WP = 1, // point towards next waypoint (no pilot input accepted)
AUTO_YAW_ROI = 2, // point towards a location held in roi_WP (no pilot input accepted)
AUTO_YAW_LOOK_AT_HEADING = 3, // point towards a particular angle (not pilot input accepted)
AUTO_YAW_LOOK_AHEAD = 4, // point in the direction the copter is moving
AUTO_YAW_RESETTOARMEDYAW = 5, // point towards heading at time motors were armed
};
// Ch6... Ch12 aux switch control
#define AUX_SWITCH_PWM_TRIGGER_HIGH 1800 // pwm value above which the ch7 or ch8 option will be invoked
#define AUX_SWITCH_PWM_TRIGGER_LOW 1200 // pwm value below which the ch7 or ch8 option will be disabled
#define CH6_PWM_TRIGGER_HIGH 1800
#define CH6_PWM_TRIGGER_LOW 1200
// values used by the ap.ch7_opt and ap.ch8_opt flags
#define AUX_SWITCH_LOW 0 // indicates auxiliary switch is in the low position (pwm <1200)
#define AUX_SWITCH_MIDDLE 1 // indicates auxiliary switch is in the middle position (pwm >1200, <1800)
#define AUX_SWITCH_HIGH 2 // indicates auxiliary switch is in the high position (pwm >1800)
// Aux Switch enumeration
enum aux_sw_func {
AUXSW_DO_NOTHING = 0, // aux switch disabled
AUXSW_FLIP = 2, // flip
AUXSW_SIMPLE_MODE = 3, // change to simple mode
AUXSW_RTL = 4, // change to RTL flight mode
AUXSW_SAVE_TRIM = 5, // save current position as level
AUXSW_SAVE_WP = 7, // save mission waypoint or RTL if in auto mode
AUXSW_CAMERA_TRIGGER = 9, // trigger camera servo or relay
AUXSW_SONAR = 10, // allow enabling or disabling sonar in flight which helps avoid surface tracking when you are far above the ground
AUXSW_FENCE = 11, // allow enabling or disabling fence in flight
AUXSW_RESETTOARMEDYAW = 12, // changes yaw to be same as when quad was armed
AUXSW_SUPERSIMPLE_MODE = 13, // change to simple mode in middle, super simple at top
AUXSW_ACRO_TRAINER = 14, // low = disabled, middle = leveled, high = leveled and limited
AUXSW_SPRAYER = 15, // enable/disable the crop sprayer
AUXSW_AUTO = 16, // change to auto flight mode
AUXSW_AUTOTUNE = 17, // auto tune
AUXSW_LAND = 18, // change to LAND flight mode
AUXSW_EPM = 19, // Operate the EPM cargo gripper low=off, middle=neutral, high=on
AUXSW_PARACHUTE_ENABLE = 21, // Parachute enable/disable
AUXSW_PARACHUTE_RELEASE = 22, // Parachute release
AUXSW_PARACHUTE_3POS = 23, // Parachute disable, enable, release with 3 position switch
AUXSW_MISSION_RESET = 24, // Reset auto mission to start from first command
AUXSW_ATTCON_FEEDFWD = 25, // enable/disable the roll and pitch rate feed forward
AUXSW_ATTCON_ACCEL_LIM = 26, // enable/disable the roll, pitch and yaw accel limiting
AUXSW_RETRACT_MOUNT = 27, // Retract Mount
AUXSW_RELAY = 28, // Relay pin on/off (only supports first relay)
AUXSW_LANDING_GEAR = 29, // Landing gear controller
AUXSW_LOST_COPTER_SOUND = 30, // Play lost copter sound
AUXSW_MOTOR_ESTOP = 31, // Emergency Stop Switch
AUXSW_MOTOR_INTERLOCK = 32, // Motor On/Off switch
AUXSW_BRAKE = 33 // Brake flight mode
};
// Frame types
#define UNDEFINED_FRAME 0
#define QUAD_FRAME 1
#define TRI_FRAME 2
#define HEXA_FRAME 3
#define Y6_FRAME 4
#define OCTA_FRAME 5
#define HELI_FRAME 6
#define OCTA_QUAD_FRAME 7
#define SINGLE_FRAME 8
#define COAX_FRAME 9
// HIL enumerations
#define HIL_MODE_DISABLED 0
#define HIL_MODE_SENSORS 1
// Auto Pilot Modes enumeration
enum autopilot_modes {
STABILIZE = 0, // manual airframe angle with manual throttle
ACRO = 1, // manual body-frame angular rate with manual throttle
ALT_HOLD = 2, // manual airframe angle with automatic throttle
AUTO = 3, // fully automatic waypoint control using mission commands
GUIDED = 4, // fully automatic fly to coordinate or fly at velocity/direction using GCS immediate commands
LOITER = 5, // automatic horizontal acceleration with automatic throttle
RTL = 6, // automatic return to launching point
CIRCLE = 7, // automatic circular flight with automatic throttle
LAND = 9, // automatic landing with horizontal position control
OF_LOITER = 10, // deprecated
DRIFT = 11, // semi-automous position, yaw and throttle control
SPORT = 13, // manual earth-frame angular rate control with manual throttle
FLIP = 14, // automatically flip the vehicle on the roll axis
AUTOTUNE = 15, // automatically tune the vehicle's roll and pitch gains
POSHOLD = 16, // automatic position hold with manual override, with automatic throttle
BRAKE = 17 // full-brake using inertial/GPS system, no pilot input
};
// Tuning enumeration
enum tuning_func {
TUNING_NONE = 0, //
TUNING_STABILIZE_ROLL_PITCH_KP = 1, // stabilize roll/pitch angle controller's P term
TUNING_STABILIZE_YAW_KP = 3, // stabilize yaw heading controller's P term
TUNING_RATE_ROLL_PITCH_KP = 4, // body frame roll/pitch rate controller's P term
TUNING_RATE_ROLL_PITCH_KI = 5, // body frame roll/pitch rate controller's I term
TUNING_YAW_RATE_KP = 6, // body frame yaw rate controller's P term
TUNING_THROTTLE_RATE_KP = 7, // throttle rate controller's P term (desired rate to acceleration or motor output)
TUNING_WP_SPEED = 10, // maximum speed to next way point (0 to 10m/s)
TUNING_LOITER_POSITION_KP = 12, // loiter distance controller's P term (position error to speed)
TUNING_HELI_EXTERNAL_GYRO = 13, // TradHeli specific external tail gyro gain
TUNING_ALTITUDE_HOLD_KP = 14, // altitude hold controller's P term (alt error to desired rate)
TUNING_RATE_ROLL_PITCH_KD = 21, // body frame roll/pitch rate controller's D term
TUNING_VEL_XY_KP = 22, // loiter rate controller's P term (speed error to tilt angle)
TUNING_ACRO_RP_KP = 25, // acro controller's P term. converts pilot input to a desired roll, pitch or yaw rate
TUNING_YAW_RATE_KD = 26, // body frame yaw rate controller's D term
TUNING_VEL_XY_KI = 28, // loiter rate controller's I term (speed error to tilt angle)
TUNING_AHRS_YAW_KP = 30, // ahrs's compass effect on yaw angle (0 = very low, 1 = very high)
TUNING_AHRS_KP = 31, // accelerometer effect on roll/pitch angle (0=low)
TUNING_ACCEL_Z_KP = 34, // accel based throttle controller's P term
TUNING_ACCEL_Z_KI = 35, // accel based throttle controller's I term
TUNING_ACCEL_Z_KD = 36, // accel based throttle controller's D term
TUNING_DECLINATION = 38, // compass declination in radians
TUNING_CIRCLE_RATE = 39, // circle turn rate in degrees (hard coded to about 45 degrees in either direction)
TUNING_ACRO_YAW_KP = 40, // acro controller's P term. converts pilot input to a desired roll, pitch or yaw rate
TUNING_SONAR_GAIN = 41, // sonar gain
TUNING_EKF_VERTICAL_POS = 42, // EKF's baro vs accel (higher rely on accels more, baro impact is reduced). Range should be 0.2 ~ 4.0? 2.0 is default
TUNING_EKF_HORIZONTAL_POS = 43, // EKF's gps vs accel (higher rely on accels more, gps impact is reduced). Range should be 1.0 ~ 3.0? 1.5 is default
TUNING_EKF_ACCEL_NOISE = 44, // EKF's accel noise (lower means trust accels more, gps & baro less). Range should be 0.02 ~ 0.5 0.5 is default (but very robust at that level)
TUNING_RC_FEEL_RP = 45, // roll-pitch input smoothing
TUNING_RATE_PITCH_KP = 46, // body frame pitch rate controller's P term
TUNING_RATE_PITCH_KI = 47, // body frame pitch rate controller's I term
TUNING_RATE_PITCH_KD = 48, // body frame pitch rate controller's D term
TUNING_RATE_ROLL_KP = 49, // body frame roll rate controller's P term
TUNING_RATE_ROLL_KI = 50, // body frame roll rate controller's I term
TUNING_RATE_ROLL_KD = 51, // body frame roll rate controller's D term
TUNING_RATE_PITCH_FF = 52, // body frame pitch rate controller FF term
TUNING_RATE_ROLL_FF = 53, // body frame roll rate controller FF term
TUNING_RATE_YAW_FF = 54, // body frame yaw rate controller FF term
TUNING_RATE_MOT_YAW_HEADROOM = 55, // motors yaw headroom minimum
TUNING_RATE_YAW_FILT = 56 // yaw rate input filter
};
// Acro Trainer types
#define ACRO_TRAINER_DISABLED 0
#define ACRO_TRAINER_LEVELING 1
#define ACRO_TRAINER_LIMITED 2
// RC Feel roll/pitch definitions
#define RC_FEEL_RP_VERY_SOFT 0
#define RC_FEEL_RP_SOFT 25
#define RC_FEEL_RP_MEDIUM 50
#define RC_FEEL_RP_CRISP 75
#define RC_FEEL_RP_VERY_CRISP 100
// Yaw behaviours during missions - possible values for WP_YAW_BEHAVIOR parameter
#define WP_YAW_BEHAVIOR_NONE 0 // auto pilot will never control yaw during missions or rtl (except for DO_CONDITIONAL_YAW command received)
#define WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP 1 // auto pilot will face next waypoint or home during rtl
#define WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL 2 // auto pilot will face next waypoint except when doing RTL at which time it will stay in it's last
#define WP_YAW_BEHAVIOR_LOOK_AHEAD 3 // auto pilot will look ahead during missions and rtl (primarily meant for traditional helicotpers)
// Auto modes
enum AutoMode {
Auto_TakeOff,
Auto_WP,
Auto_Land,
Auto_RTL,
Auto_CircleMoveToEdge,
Auto_Circle,
Auto_Spline,
Auto_NavGuided,
Auto_Loiter
};
// Guided modes
enum GuidedMode {
Guided_TakeOff,
Guided_WP,
Guided_Velocity,
Guided_PosVel,
Guided_Angle,
};
// RTL states
enum RTLState {
RTL_InitialClimb,
RTL_ReturnHome,
RTL_LoiterAtHome,
RTL_FinalDescent,
RTL_Land
};
// Alt_Hold states
enum AltHoldModeState {
AltHold_Disarmed,
AltHold_MotorStop,
AltHold_Takeoff,
AltHold_Flying,
AltHold_Landed
};
// Loiter states
enum LoiterModeState {
Loiter_Disarmed,
Loiter_MotorStop,
Loiter_Takeoff,
Loiter_Flying,
Loiter_Landed
};
// Flip states
enum FlipState {
Flip_Start,
Flip_Roll,
Flip_Pitch_A,
Flip_Pitch_B,
Flip_Recover,
Flip_Abandon
};
// LAND state
#define LAND_STATE_FLY_TO_LOCATION 0
#define LAND_STATE_DESCENDING 1
// Logging parameters
#define TYPE_AIRSTART_MSG 0x00
#define TYPE_GROUNDSTART_MSG 0x01
#define LOG_CONTROL_TUNING_MSG 0x04
#define LOG_NAV_TUNING_MSG 0x05
#define LOG_PERFORMANCE_MSG 0x06
#define LOG_STARTUP_MSG 0x0A
#define LOG_OPTFLOW_MSG 0x0C
#define LOG_EVENT_MSG 0x0D
#define LOG_PID_MSG 0x0E // deprecated
#define LOG_INAV_MSG 0x11 // deprecated
#define LOG_CAMERA_MSG_DEPRECATED 0x12 // deprecated
#define LOG_ERROR_MSG 0x13
#define LOG_DATA_INT16_MSG 0x14
#define LOG_DATA_UINT16_MSG 0x15
#define LOG_DATA_INT32_MSG 0x16
#define LOG_DATA_UINT32_MSG 0x17
#define LOG_DATA_FLOAT_MSG 0x18
#define LOG_AUTOTUNE_MSG 0x19
#define LOG_AUTOTUNEDETAILS_MSG 0x1A
#define LOG_RATE_MSG 0x1D
#define LOG_MOTBATT_MSG 0x1E
#define LOG_PARAMTUNE_MSG 0x1F
#define LOG_HELI_MSG 0x20
#define LOG_PRECLAND_MSG 0x21
#define MASK_LOG_ATTITUDE_FAST (1<<0)
#define MASK_LOG_ATTITUDE_MED (1<<1)
#define MASK_LOG_GPS (1<<2)
#define MASK_LOG_PM (1<<3)
#define MASK_LOG_CTUN (1<<4)
#define MASK_LOG_NTUN (1<<5)
#define MASK_LOG_RCIN (1<<6)
#define MASK_LOG_IMU (1<<7)
#define MASK_LOG_CMD (1<<8)
#define MASK_LOG_CURRENT (1<<9)
#define MASK_LOG_RCOUT (1<<10)
#define MASK_LOG_OPTFLOW (1<<11)
#define MASK_LOG_PID (1<<12)
#define MASK_LOG_COMPASS (1<<13)
#define MASK_LOG_INAV (1<<14) // deprecated
#define MASK_LOG_CAMERA (1<<15)
#define MASK_LOG_WHEN_DISARMED (1UL<<16)
#define MASK_LOG_MOTBATT (1UL<<17)
#define MASK_LOG_IMU_FAST (1UL<<18)
#define MASK_LOG_IMU_RAW (1UL<<19)
#define MASK_LOG_ANY 0xFFFF
// DATA - event logging
#define DATA_MAVLINK_FLOAT 1
#define DATA_MAVLINK_INT32 2
#define DATA_MAVLINK_INT16 3
#define DATA_MAVLINK_INT8 4
#define DATA_AP_STATE 7
#define DATA_SYSTEM_TIME_SET 8
#define DATA_INIT_SIMPLE_BEARING 9
#define DATA_ARMED 10
#define DATA_DISARMED 11
#define DATA_AUTO_ARMED 15
#define DATA_TAKEOFF 16
#define DATA_LAND_COMPLETE_MAYBE 17
#define DATA_LAND_COMPLETE 18
#define DATA_NOT_LANDED 28
#define DATA_LOST_GPS 19
#define DATA_FLIP_START 21
#define DATA_FLIP_END 22
#define DATA_SET_HOME 25
#define DATA_SET_SIMPLE_ON 26
#define DATA_SET_SIMPLE_OFF 27
#define DATA_SET_SUPERSIMPLE_ON 29
#define DATA_AUTOTUNE_INITIALISED 30
#define DATA_AUTOTUNE_OFF 31
#define DATA_AUTOTUNE_RESTART 32
#define DATA_AUTOTUNE_SUCCESS 33
#define DATA_AUTOTUNE_FAILED 34
#define DATA_AUTOTUNE_REACHED_LIMIT 35
#define DATA_AUTOTUNE_PILOT_TESTING 36
#define DATA_AUTOTUNE_SAVEDGAINS 37
#define DATA_SAVE_TRIM 38
#define DATA_SAVEWP_ADD_WP 39
#define DATA_SAVEWP_CLEAR_MISSION_RTL 40
#define DATA_FENCE_ENABLE 41
#define DATA_FENCE_DISABLE 42
#define DATA_ACRO_TRAINER_DISABLED 43
#define DATA_ACRO_TRAINER_LEVELING 44
#define DATA_ACRO_TRAINER_LIMITED 45
#define DATA_EPM_GRAB 46
#define DATA_EPM_RELEASE 47
#define DATA_EPM_NEUTRAL 48 // deprecated
#define DATA_PARACHUTE_DISABLED 49
#define DATA_PARACHUTE_ENABLED 50
#define DATA_PARACHUTE_RELEASED 51
#define DATA_LANDING_GEAR_DEPLOYED 52
#define DATA_LANDING_GEAR_RETRACTED 53
#define DATA_MOTORS_EMERGENCY_STOPPED 54
#define DATA_MOTORS_EMERGENCY_STOP_CLEARED 55
#define DATA_MOTORS_INTERLOCK_DISABLED 56
#define DATA_MOTORS_INTERLOCK_ENABLED 57
#define DATA_ROTOR_RUNUP_COMPLETE 58 // Heli only
#define DATA_ROTOR_SPEED_BELOW_CRITICAL 59 // Heli only
#define DATA_EKF_ALT_RESET 60
// Centi-degrees to radians
#define DEGX100 5729.57795f
// Error message sub systems and error codes
#define ERROR_SUBSYSTEM_MAIN 1
#define ERROR_SUBSYSTEM_RADIO 2
#define ERROR_SUBSYSTEM_COMPASS 3
#define ERROR_SUBSYSTEM_OPTFLOW 4
#define ERROR_SUBSYSTEM_FAILSAFE_RADIO 5
#define ERROR_SUBSYSTEM_FAILSAFE_BATT 6
#define ERROR_SUBSYSTEM_FAILSAFE_GPS 7 // not used
#define ERROR_SUBSYSTEM_FAILSAFE_GCS 8
#define ERROR_SUBSYSTEM_FAILSAFE_FENCE 9
#define ERROR_SUBSYSTEM_FLIGHT_MODE 10
#define ERROR_SUBSYSTEM_GPS 11 // not used
#define ERROR_SUBSYSTEM_CRASH_CHECK 12
#define ERROR_SUBSYSTEM_FLIP 13
#define ERROR_SUBSYSTEM_AUTOTUNE 14
#define ERROR_SUBSYSTEM_PARACHUTE 15
#define ERROR_SUBSYSTEM_EKFCHECK 16
#define ERROR_SUBSYSTEM_FAILSAFE_EKFINAV 17
#define ERROR_SUBSYSTEM_BARO 18
#define ERROR_SUBSYSTEM_CPU 19
#define ERROR_SUBSYSTEM_FAILSAFE_ADSB 20
// general error codes
#define ERROR_CODE_ERROR_RESOLVED 0
#define ERROR_CODE_FAILED_TO_INITIALISE 1
#define ERROR_CODE_UNHEALTHY 4
// subsystem specific error codes -- radio
#define ERROR_CODE_RADIO_LATE_FRAME 2
// subsystem specific error codes -- failsafe_thr, batt, gps
#define ERROR_CODE_FAILSAFE_RESOLVED 0
#define ERROR_CODE_FAILSAFE_OCCURRED 1
// subsystem specific error codes -- compass
#define ERROR_CODE_COMPASS_FAILED_TO_READ 2
// subsystem specific error codes -- main
#define ERROR_CODE_MAIN_INS_DELAY 1
// subsystem specific error codes -- crash checker
#define ERROR_CODE_CRASH_CHECK_CRASH 1
#define ERROR_CODE_CRASH_CHECK_LOSS_OF_CONTROL 2
// subsystem specific error codes -- flip
#define ERROR_CODE_FLIP_ABANDONED 2
// parachute failed to deploy because of low altitude or landed
#define ERROR_CODE_PARACHUTE_TOO_LOW 2
#define ERROR_CODE_PARACHUTE_LANDED 3
// EKF check definitions
#define ERROR_CODE_EKFCHECK_BAD_VARIANCE 2
#define ERROR_CODE_EKFCHECK_VARIANCE_CLEARED 0
// Baro specific error codes
#define ERROR_CODE_BARO_GLITCH 2
// Arming Check Enable/Disable bits
#define ARMING_CHECK_NONE 0x00
#define ARMING_CHECK_ALL 0x01
#define ARMING_CHECK_BARO 0x02
#define ARMING_CHECK_COMPASS 0x04
#define ARMING_CHECK_GPS 0x08
#define ARMING_CHECK_INS 0x10
#define ARMING_CHECK_PARAMETERS 0x20
#define ARMING_CHECK_RC 0x40
#define ARMING_CHECK_VOLTAGE 0x80
// Radio failsafe definitions (FS_THR parameter)
#define FS_THR_DISABLED 0
#define FS_THR_ENABLED_ALWAYS_RTL 1
#define FS_THR_ENABLED_CONTINUE_MISSION 2
#define FS_THR_ENABLED_ALWAYS_LAND 3
// Battery failsafe definitions (FS_BATT_ENABLE parameter)
#define FS_BATT_DISABLED 0 // battery failsafe disabled
#define FS_BATT_LAND 1 // switch to LAND mode on battery failsafe
#define FS_BATT_RTL 2 // switch to RTL mode on battery failsafe
// EKF failsafe definitions (FS_EKF_ACTION parameter)
#define FS_EKF_ACTION_LAND 1 // switch to LAND mode on EKF failsafe
#define FS_EKF_ACTION_ALTHOLD 2 // switch to ALTHOLD mode on EKF failsafe
#define FS_EKF_ACTION_LAND_EVEN_STABILIZE 3 // switch to Land mode on EKF failsafe even if in a manual flight mode like stabilize
// for mavlink SET_POSITION_TARGET messages
#define MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE ((1<<0) | (1<<1) | (1<<2))
#define MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE ((1<<3) | (1<<4) | (1<<5))
#define MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE ((1<<6) | (1<<7) | (1<<8))
#define MAVLINK_SET_POS_TYPE_MASK_FORCE (1<<9)
#define MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE (1<<10)
#define MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE (1<<11)
// for PILOT_THR_BHV parameter
#define THR_BEHAVE_FEEDBACK_FROM_MID_STICK (1<<0)
#endif // _DEFINES_H

170
ArduSub/ekf_check.cpp Normal file
View File

@ -0,0 +1,170 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
/**
*
* ekf_check.pde - detects failures of the ekf or inertial nav system
* triggers an alert to the pilot and helps take countermeasures
*
*/
#ifndef EKF_CHECK_ITERATIONS_MAX
# define EKF_CHECK_ITERATIONS_MAX 10 // 1 second (ie. 10 iterations at 10hz) of bad variances signals a failure
#endif
#ifndef EKF_CHECK_WARNING_TIME
# define EKF_CHECK_WARNING_TIME (30*1000) // warning text messages are sent to ground no more than every 30 seconds
#endif
////////////////////////////////////////////////////////////////////////////////
// EKF_check strucutre
////////////////////////////////////////////////////////////////////////////////
static struct {
uint8_t fail_count; // number of iterations ekf or dcm have been out of tolerances
uint8_t bad_variance : 1; // true if ekf should be considered untrusted (fail_count has exceeded EKF_CHECK_ITERATIONS_MAX)
uint32_t last_warn_time; // system time of last warning in milliseconds. Used to throttle text warnings sent to GCS
} ekf_check_state;
// ekf_check - detects if ekf variance are out of tolerance and triggers failsafe
// should be called at 10hz
void Copter::ekf_check()
{
// exit immediately if ekf has no origin yet - this assumes the origin can never become unset
Location temp_loc;
if (!ahrs.get_origin(temp_loc)) {
return;
}
// return immediately if motors are not armed, ekf check is disabled, not using ekf or usb is connected
if (!motors.armed() || ap.usb_connected || (g.fs_ekf_thresh <= 0.0f)) {
ekf_check_state.fail_count = 0;
ekf_check_state.bad_variance = false;
AP_Notify::flags.ekf_bad = ekf_check_state.bad_variance;
failsafe_ekf_off_event(); // clear failsafe
return;
}
// compare compass and velocity variance vs threshold
if (ekf_over_threshold()) {
// if compass is not yet flagged as bad
if (!ekf_check_state.bad_variance) {
// increase counter
ekf_check_state.fail_count++;
// if counter above max then trigger failsafe
if (ekf_check_state.fail_count >= EKF_CHECK_ITERATIONS_MAX) {
// limit count from climbing too high
ekf_check_state.fail_count = EKF_CHECK_ITERATIONS_MAX;
ekf_check_state.bad_variance = true;
// log an error in the dataflash
Log_Write_Error(ERROR_SUBSYSTEM_EKFCHECK, ERROR_CODE_EKFCHECK_BAD_VARIANCE);
// send message to gcs
if ((AP_HAL::millis() - ekf_check_state.last_warn_time) > EKF_CHECK_WARNING_TIME) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"EKF variance");
ekf_check_state.last_warn_time = AP_HAL::millis();
}
failsafe_ekf_event();
}
}
} else {
// reduce counter
if (ekf_check_state.fail_count > 0) {
ekf_check_state.fail_count--;
// if compass is flagged as bad and the counter reaches zero then clear flag
if (ekf_check_state.bad_variance && ekf_check_state.fail_count == 0) {
ekf_check_state.bad_variance = false;
// log recovery in the dataflash
Log_Write_Error(ERROR_SUBSYSTEM_EKFCHECK, ERROR_CODE_EKFCHECK_VARIANCE_CLEARED);
// clear failsafe
failsafe_ekf_off_event();
}
}
}
// set AP_Notify flags
AP_Notify::flags.ekf_bad = ekf_check_state.bad_variance;
// To-Do: add ekf variances to extended status
}
// ekf_over_threshold - returns true if the ekf's variance are over the tolerance
bool Copter::ekf_over_threshold()
{
// return false immediately if disabled
if (g.fs_ekf_thresh <= 0.0f) {
return false;
}
// return true immediately if position is bad
if (!ekf_position_ok() && !optflow_position_ok()) {
return true;
}
// use EKF to get variance
float posVar, hgtVar, tasVar;
Vector3f magVar;
Vector2f offset;
float compass_variance;
float vel_variance;
ahrs.get_variances(vel_variance, posVar, hgtVar, magVar, tasVar, offset);
compass_variance = magVar.length();
// return true if compass and velocity variance over the threshold
return (compass_variance >= g.fs_ekf_thresh && vel_variance >= g.fs_ekf_thresh);
}
// failsafe_ekf_event - perform ekf failsafe
void Copter::failsafe_ekf_event()
{
// return immediately if ekf failsafe already triggered
if (failsafe.ekf) {
return;
}
// do nothing if motors disarmed
if (!motors.armed()) {
return;
}
// do nothing if not in GPS flight mode and ekf-action is not land-even-stabilize
if (!mode_requires_GPS(control_mode) && (g.fs_ekf_action != FS_EKF_ACTION_LAND_EVEN_STABILIZE)) {
return;
}
// EKF failsafe event has occurred
failsafe.ekf = true;
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_EKFINAV, ERROR_CODE_FAILSAFE_OCCURRED);
// take action based on fs_ekf_action parameter
switch (g.fs_ekf_action) {
case FS_EKF_ACTION_ALTHOLD:
// AltHold
if (failsafe.radio || !set_mode(ALT_HOLD)) {
set_mode_land_with_pause();
}
break;
default:
set_mode_land_with_pause();
break;
}
// if flight mode is already LAND ensure it's not the GPS controlled LAND
if (control_mode == LAND) {
land_do_not_use_GPS();
}
}
// failsafe_ekf_off_event - actions to take when EKF failsafe is cleared
void Copter::failsafe_ekf_off_event(void)
{
// return immediately if not in ekf failsafe
if (!failsafe.ekf) {
return;
}
// clear flag and log recovery
failsafe.ekf = false;
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_EKFINAV, ERROR_CODE_FAILSAFE_RESOLVED);
}

152
ArduSub/esc_calibration.cpp Normal file
View File

@ -0,0 +1,152 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
/*****************************************************************************
* esc_calibration.pde : functions to check and perform ESC calibration
*****************************************************************************/
#define ESC_CALIBRATION_HIGH_THROTTLE 950
// enum for ESC CALIBRATION
enum ESCCalibrationModes {
ESCCAL_NONE = 0,
ESCCAL_PASSTHROUGH_IF_THROTTLE_HIGH = 1,
ESCCAL_PASSTHROUGH_ALWAYS = 2,
ESCCAL_AUTO = 3,
ESCCAL_DISABLED = 9,
};
// check if we should enter esc calibration mode
void Copter::esc_calibration_startup_check()
{
#if FRAME_CONFIG != HELI_FRAME
// exit immediately if pre-arm rc checks fail
pre_arm_rc_checks();
if (!ap.pre_arm_rc_check) {
// clear esc flag for next time
if ((g.esc_calibrate != ESCCAL_NONE) && (g.esc_calibrate != ESCCAL_DISABLED)) {
g.esc_calibrate.set_and_save(ESCCAL_NONE);
}
return;
}
// check ESC parameter
switch (g.esc_calibrate) {
case ESCCAL_NONE:
// check if throttle is high
if (channel_throttle->control_in >= ESC_CALIBRATION_HIGH_THROTTLE) {
// we will enter esc_calibrate mode on next reboot
g.esc_calibrate.set_and_save(ESCCAL_PASSTHROUGH_IF_THROTTLE_HIGH);
// send message to gcs
gcs_send_text(MAV_SEVERITY_CRITICAL,"ESC calibration: Restart board");
// turn on esc calibration notification
AP_Notify::flags.esc_calibration = true;
// block until we restart
while(1) { delay(5); }
}
break;
case ESCCAL_PASSTHROUGH_IF_THROTTLE_HIGH:
// check if throttle is high
if (channel_throttle->control_in >= ESC_CALIBRATION_HIGH_THROTTLE) {
// pass through pilot throttle to escs
esc_calibration_passthrough();
}
break;
case ESCCAL_PASSTHROUGH_ALWAYS:
// pass through pilot throttle to escs
esc_calibration_passthrough();
break;
case ESCCAL_AUTO:
// perform automatic ESC calibration
esc_calibration_auto();
break;
case ESCCAL_DISABLED:
default:
// do nothing
break;
}
// clear esc flag for next time
if (g.esc_calibrate != ESCCAL_DISABLED) {
g.esc_calibrate.set_and_save(ESCCAL_NONE);
}
#endif // FRAME_CONFIG != HELI_FRAME
}
// esc_calibration_passthrough - pass through pilot throttle to escs
void Copter::esc_calibration_passthrough()
{
#if FRAME_CONFIG != HELI_FRAME
// clear esc flag for next time
g.esc_calibrate.set_and_save(ESCCAL_NONE);
// reduce update rate to motors to 50Hz
motors.set_update_rate(50);
// send message to GCS
gcs_send_text(MAV_SEVERITY_INFO,"ESC calibration: Passing pilot throttle to ESCs");
while(1) {
// arm motors
motors.armed(true);
motors.enable();
// flash LEDS
AP_Notify::flags.esc_calibration = true;
// read pilot input
read_radio();
delay(10);
// pass through to motors
motors.throttle_pass_through(channel_throttle->radio_in);
}
#endif // FRAME_CONFIG != HELI_FRAME
}
// esc_calibration_auto - calibrate the ESCs automatically using a timer and no pilot input
void Copter::esc_calibration_auto()
{
#if FRAME_CONFIG != HELI_FRAME
bool printed_msg = false;
// reduce update rate to motors to 50Hz
motors.set_update_rate(50);
// send message to GCS
gcs_send_text(MAV_SEVERITY_INFO,"ESC calibration: Auto calibration");
// arm and enable motors
motors.armed(true);
motors.enable();
// flash LEDS
AP_Notify::flags.esc_calibration = true;
// raise throttle to maximum
delay(10);
motors.throttle_pass_through(channel_throttle->radio_max);
// wait for safety switch to be pressed
while (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) {
if (!printed_msg) {
gcs_send_text(MAV_SEVERITY_INFO,"ESC calibration: Push safety switch");
printed_msg = true;
}
delay(10);
}
// delay for 5 seconds
delay(5000);
// reduce throttle to minimum
motors.throttle_pass_through(channel_throttle->radio_min);
// clear esc parameter
g.esc_calibrate.set_and_save(ESCCAL_NONE);
// block until we restart
while(1) { delay(5); }
#endif // FRAME_CONFIG != HELI_FRAME
}

280
ArduSub/events.cpp Normal file
View File

@ -0,0 +1,280 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
/*
* This event will be called when the failsafe changes
* boolean failsafe reflects the current state
*/
void Copter::failsafe_radio_on_event()
{
// if motors are not armed there is nothing to do
if( !motors.armed() ) {
return;
}
// This is how to handle a failsafe.
switch(control_mode) {
case STABILIZE:
case ACRO:
// if throttle is zero OR vehicle is landed disarm motors
if (ap.throttle_zero || ap.land_complete) {
init_disarm_motors();
// if failsafe_throttle is FS_THR_ENABLED_ALWAYS_LAND then land immediately
}else if(g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND) {
set_mode_land_with_pause();
// if far from home then RTL
} else if(home_distance > FS_CLOSE_TO_HOME_CM) {
// switch to RTL or if that fails, LAND
set_mode_RTL_or_land_with_pause();
// We have no GPS or are very close to home so we will land
}else{
set_mode_land_with_pause();
}
break;
case AUTO:
// if mission has not started AND vehicle is landed, disarm motors
if (!ap.auto_armed && ap.land_complete) {
init_disarm_motors();
// if failsafe_throttle is FS_THR_ENABLED_ALWAYS_LAND then land immediately
} else if(g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND) {
set_mode_land_with_pause();
// if failsafe_throttle is FS_THR_ENABLED_ALWAYS_RTL do RTL
} else if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_RTL) {
if (home_distance > FS_CLOSE_TO_HOME_CM) {
// switch to RTL or if that fails, LAND
set_mode_RTL_or_land_with_pause();
}else{
// We are very close to home so we will land
set_mode_land_with_pause();
}
}
// failsafe_throttle must be FS_THR_ENABLED_CONTINUE_MISSION so no need to do anything
break;
case LAND:
// continue to land if battery failsafe is also active otherwise fall through to default handling
if (g.failsafe_battery_enabled == FS_BATT_LAND && failsafe.battery) {
break;
}
// no break
default:
// used for AltHold, Guided, Loiter, RTL, Circle, Drift, Sport, Flip, Autotune, PosHold
// if landed disarm
if (ap.land_complete) {
init_disarm_motors();
// if failsafe_throttle is FS_THR_ENABLED_ALWAYS_LAND then land immediately
} else if(g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND) {
set_mode_land_with_pause();
// if far from home then RTL
} else if(home_distance > FS_CLOSE_TO_HOME_CM) {
// switch to RTL or if that fails, LAND
set_mode_RTL_or_land_with_pause();
}else{
// We have no GPS or are very close to home so we will land
set_mode_land_with_pause();
}
break;
}
// log the error to the dataflash
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_RADIO, ERROR_CODE_FAILSAFE_OCCURRED);
}
// failsafe_off_event - respond to radio contact being regained
// we must be in AUTO, LAND or RTL modes
// or Stabilize or ACRO mode but with motors disarmed
void Copter::failsafe_radio_off_event()
{
// no need to do anything except log the error as resolved
// user can now override roll, pitch, yaw and throttle and even use flight mode switch to restore previous flight mode
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_RADIO, ERROR_CODE_FAILSAFE_RESOLVED);
}
void Copter::failsafe_battery_event(void)
{
// return immediately if low battery event has already been triggered
if (failsafe.battery) {
return;
}
// failsafe check
if (g.failsafe_battery_enabled != FS_BATT_DISABLED && motors.armed()) {
switch(control_mode) {
case STABILIZE:
case ACRO:
// if throttle is zero OR vehicle is landed disarm motors
if (ap.throttle_zero || ap.land_complete) {
init_disarm_motors();
}else{
// set mode to RTL or LAND
if (g.failsafe_battery_enabled == FS_BATT_RTL && home_distance > FS_CLOSE_TO_HOME_CM) {
// switch to RTL or if that fails, LAND
set_mode_RTL_or_land_with_pause();
}else{
set_mode_land_with_pause();
}
}
break;
case AUTO:
// if mission has not started AND vehicle is landed, disarm motors
if (!ap.auto_armed && ap.land_complete) {
init_disarm_motors();
// set mode to RTL or LAND
} else if (home_distance > FS_CLOSE_TO_HOME_CM) {
// switch to RTL or if that fails, LAND
set_mode_RTL_or_land_with_pause();
} else {
set_mode_land_with_pause();
}
break;
default:
// used for AltHold, Guided, Loiter, RTL, Circle, Drift, Sport, Flip, Autotune, PosHold
// if landed disarm
if (ap.land_complete) {
init_disarm_motors();
// set mode to RTL or LAND
} else if (g.failsafe_battery_enabled == FS_BATT_RTL && home_distance > FS_CLOSE_TO_HOME_CM) {
// switch to RTL or if that fails, LAND
set_mode_RTL_or_land_with_pause();
} else {
set_mode_land_with_pause();
}
break;
}
}
// set the low battery flag
set_failsafe_battery(true);
// warn the ground station and log to dataflash
gcs_send_text(MAV_SEVERITY_WARNING,"Low battery");
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_BATT, ERROR_CODE_FAILSAFE_OCCURRED);
}
// failsafe_gcs_check - check for ground station failsafe
void Copter::failsafe_gcs_check()
{
uint32_t last_gcs_update_ms;
// return immediately if gcs failsafe is disabled, gcs has never been connected or we are not overriding rc controls from the gcs and we are not in guided mode
// this also checks to see if we have a GCS failsafe active, if we do, then must continue to process the logic for recovery from this state.
if ((!failsafe.gcs)&&(g.failsafe_gcs == FS_GCS_DISABLED || failsafe.last_heartbeat_ms == 0 || (!failsafe.rc_override_active && control_mode != GUIDED))) {
return;
}
// calc time since last gcs update
// note: this only looks at the heartbeat from the device id set by g.sysid_my_gcs
last_gcs_update_ms = millis() - failsafe.last_heartbeat_ms;
// check if all is well
if (last_gcs_update_ms < FS_GCS_TIMEOUT_MS) {
// check for recovery from gcs failsafe
if (failsafe.gcs) {
failsafe_gcs_off_event();
set_failsafe_gcs(false);
}
return;
}
// do nothing if gcs failsafe already triggered or motors disarmed
if (failsafe.gcs || !motors.armed()) {
return;
}
// GCS failsafe event has occured
// update state, log to dataflash
set_failsafe_gcs(true);
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_GCS, ERROR_CODE_FAILSAFE_OCCURRED);
// clear overrides so that RC control can be regained with radio.
hal.rcin->clear_overrides();
failsafe.rc_override_active = false;
// This is how to handle a failsafe.
// use the throttle failsafe setting to decide what to do
switch(control_mode) {
case STABILIZE:
case ACRO:
case SPORT:
// if throttle is zero disarm motors
if (ap.throttle_zero || ap.land_complete) {
init_disarm_motors();
}else if(home_distance > FS_CLOSE_TO_HOME_CM) {
// switch to RTL or if that fails, LAND
set_mode_RTL_or_land_with_pause();
}else{
// We have no GPS or are very close to home so we will land
set_mode_land_with_pause();
}
break;
case AUTO:
// if mission has not started AND vehicle is landed, disarm motors
if (!ap.auto_armed && ap.land_complete) {
init_disarm_motors();
// if g.failsafe_gcs is 1 do RTL, 2 means continue with the mission
} else if (g.failsafe_gcs == FS_GCS_ENABLED_ALWAYS_RTL) {
if (home_distance > FS_CLOSE_TO_HOME_CM) {
// switch to RTL or if that fails, LAND
set_mode_RTL_or_land_with_pause();
}else{
// We are very close to home so we will land
set_mode_land_with_pause();
}
}
// if failsafe_throttle is 2 (i.e. FS_THR_ENABLED_CONTINUE_MISSION) no need to do anything
break;
default:
// used for AltHold, Guided, Loiter, RTL, Circle, Drift, Sport, Flip, Autotune, PosHold
// if landed disarm
if (ap.land_complete) {
init_disarm_motors();
} else if (home_distance > FS_CLOSE_TO_HOME_CM) {
// switch to RTL or if that fails, LAND
set_mode_RTL_or_land_with_pause();
}else{
// We have no GPS or are very close to home so we will land
set_mode_land_with_pause();
}
break;
}
}
// failsafe_gcs_off_event - actions to take when GCS contact is restored
void Copter::failsafe_gcs_off_event(void)
{
// log recovery of GCS in logs?
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_GCS, ERROR_CODE_FAILSAFE_RESOLVED);
}
// set_mode_RTL_or_land_with_pause - sets mode to RTL if possible or LAND with 4 second delay before descent starts
// this is always called from a failsafe so we trigger notification to pilot
void Copter::set_mode_RTL_or_land_with_pause()
{
// attempt to switch to RTL, if this fails then switch to Land
if (!set_mode(RTL)) {
// set mode to land will trigger mode change notification to pilot
set_mode_land_with_pause();
} else {
// alert pilot to mode change
AP_Notify::events.failsafe_mode_change = 1;
}
}
void Copter::update_events()
{
ServoRelayEvents.update_events();
}

73
ArduSub/failsafe.cpp Normal file
View File

@ -0,0 +1,73 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
//
// failsafe support
// Andrew Tridgell, December 2011
//
// our failsafe strategy is to detect main loop lockup and disarm the motors
//
static bool failsafe_enabled = false;
static uint16_t failsafe_last_mainLoop_count;
static uint32_t failsafe_last_timestamp;
static bool in_failsafe;
//
// failsafe_enable - enable failsafe
//
void Copter::failsafe_enable()
{
failsafe_enabled = true;
failsafe_last_timestamp = micros();
}
//
// failsafe_disable - used when we know we are going to delay the mainloop significantly
//
void Copter::failsafe_disable()
{
failsafe_enabled = false;
}
//
// failsafe_check - this function is called from the core timer interrupt at 1kHz.
//
void Copter::failsafe_check()
{
uint32_t tnow = AP_HAL::micros();
if (mainLoop_count != failsafe_last_mainLoop_count) {
// the main loop is running, all is OK
failsafe_last_mainLoop_count = mainLoop_count;
failsafe_last_timestamp = tnow;
if (in_failsafe) {
in_failsafe = false;
Log_Write_Error(ERROR_SUBSYSTEM_CPU,ERROR_CODE_FAILSAFE_RESOLVED);
}
return;
}
if (!in_failsafe && failsafe_enabled && tnow - failsafe_last_timestamp > 2000000) {
// motors are running but we have gone 2 second since the
// main loop ran. That means we're in trouble and should
// disarm the motors.
in_failsafe = true;
// reduce motors to minimum (we do not immediately disarm because we want to log the failure)
if (motors.armed()) {
motors.output_min();
}
// log an error
Log_Write_Error(ERROR_SUBSYSTEM_CPU,ERROR_CODE_FAILSAFE_OCCURRED);
}
if (failsafe_enabled && in_failsafe && tnow - failsafe_last_timestamp > 1000000) {
// disarm motors every second
failsafe_last_timestamp = tnow;
if(motors.armed()) {
motors.armed(false);
motors.output();
}
}
}

83
ArduSub/fence.cpp Normal file
View File

@ -0,0 +1,83 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
// Code to integrate AC_Fence library with main ArduCopter code
#if AC_FENCE == ENABLED
// fence_check - ask fence library to check for breaches and initiate the response
// called at 1hz
void Copter::fence_check()
{
uint8_t new_breaches; // the type of fence that has been breached
uint8_t orig_breaches = fence.get_breaches();
// give fence library our current distance from home in meters
fence.set_home_distance(home_distance*0.01f);
// check for a breach
new_breaches = fence.check_fence(current_loc.alt/100.0f);
// return immediately if motors are not armed
if(!motors.armed()) {
return;
}
// if there is a new breach take action
if( new_breaches != AC_FENCE_TYPE_NONE ) {
// if the user wants some kind of response and motors are armed
if(fence.get_action() != AC_FENCE_ACTION_REPORT_ONLY ) {
// disarm immediately if we think we are on the ground or in a manual flight mode with zero throttle
// don't disarm if the high-altitude fence has been broken because it's likely the user has pulled their throttle to zero to bring it down
if (ap.land_complete || (mode_has_manual_throttle(control_mode) && ap.throttle_zero && !failsafe.radio && ((fence.get_breaches() & AC_FENCE_TYPE_ALT_MAX)== 0))){
init_disarm_motors();
}else{
// if we are within 100m of the fence, RTL
if (fence.get_breach_distance(new_breaches) <= AC_FENCE_GIVE_UP_DISTANCE) {
if (!set_mode(RTL)) {
set_mode(LAND);
}
}else{
// if more than 100m outside the fence just force a land
set_mode(LAND);
}
}
}
// log an error in the dataflash
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_FENCE, new_breaches);
}
// record clearing of breach
if(orig_breaches != AC_FENCE_TYPE_NONE && fence.get_breaches() == AC_FENCE_TYPE_NONE) {
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_FENCE, ERROR_CODE_ERROR_RESOLVED);
}
}
// fence_send_mavlink_status - send fence status to ground station
void Copter::fence_send_mavlink_status(mavlink_channel_t chan)
{
if (fence.enabled()) {
// traslate fence library breach types to mavlink breach types
uint8_t mavlink_breach_type = FENCE_BREACH_NONE;
uint8_t breaches = fence.get_breaches();
if ((breaches & AC_FENCE_TYPE_ALT_MAX) != 0) {
mavlink_breach_type = FENCE_BREACH_MAXALT;
}
if ((breaches & AC_FENCE_TYPE_CIRCLE) != 0) {
mavlink_breach_type = FENCE_BREACH_BOUNDARY;
}
// send status
mavlink_msg_fence_status_send(chan,
(int8_t)(fence.get_breaches()!=0),
fence.get_breach_count(),
mavlink_breach_type,
fence.get_breach_time());
}
}
#endif

381
ArduSub/flight_mode.cpp Normal file
View File

@ -0,0 +1,381 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
/*
* flight.pde - high level calls to set and update flight modes
* logic for individual flight modes is in control_acro.pde, control_stabilize.pde, etc
*/
// set_mode - change flight mode and perform any necessary initialisation
// optional force parameter used to force the flight mode change (used only first time mode is set)
// returns true if mode was succesfully set
// ACRO, STABILIZE, ALTHOLD, LAND, DRIFT and SPORT can always be set successfully but the return state of other flight modes should be checked and the caller should deal with failures appropriately
bool Copter::set_mode(uint8_t mode)
{
// boolean to record if flight mode could be set
bool success = false;
bool ignore_checks = !motors.armed(); // allow switching to any mode if disarmed. We rely on the arming check to perform
// return immediately if we are already in the desired mode
if (mode == control_mode) {
return true;
}
switch(mode) {
case ACRO:
#if FRAME_CONFIG == HELI_FRAME
success = heli_acro_init(ignore_checks);
#else
success = acro_init(ignore_checks);
#endif
break;
case STABILIZE:
#if FRAME_CONFIG == HELI_FRAME
success = heli_stabilize_init(ignore_checks);
#else
success = stabilize_init(ignore_checks);
#endif
break;
case ALT_HOLD:
success = althold_init(ignore_checks);
break;
case AUTO:
success = auto_init(ignore_checks);
break;
case CIRCLE:
success = circle_init(ignore_checks);
break;
case LOITER:
success = loiter_init(ignore_checks);
break;
case GUIDED:
success = guided_init(ignore_checks);
break;
case LAND:
success = land_init(ignore_checks);
break;
case RTL:
success = rtl_init(ignore_checks);
break;
case DRIFT:
success = drift_init(ignore_checks);
break;
case SPORT:
success = sport_init(ignore_checks);
break;
case FLIP:
success = flip_init(ignore_checks);
break;
#if AUTOTUNE_ENABLED == ENABLED
case AUTOTUNE:
success = autotune_init(ignore_checks);
break;
#endif
#if POSHOLD_ENABLED == ENABLED
case POSHOLD:
success = poshold_init(ignore_checks);
break;
#endif
case BRAKE:
success = brake_init(ignore_checks);
break;
default:
success = false;
break;
}
// update flight mode
if (success) {
// perform any cleanup required by previous flight mode
exit_mode(control_mode, mode);
control_mode = mode;
DataFlash.Log_Write_Mode(control_mode);
#if AC_FENCE == ENABLED
// pilot requested flight mode change during a fence breach indicates pilot is attempting to manually recover
// this flight mode change could be automatic (i.e. fence, battery, GPS or GCS failsafe)
// but it should be harmless to disable the fence temporarily in these situations as well
fence.manual_recovery_start();
#endif
}else{
// Log error that we failed to enter desired flight mode
Log_Write_Error(ERROR_SUBSYSTEM_FLIGHT_MODE,mode);
}
// update notify object
if (success) {
notify_flight_mode(control_mode);
}
// return success or failure
return success;
}
// update_flight_mode - calls the appropriate attitude controllers based on flight mode
// called at 100hz or more
void Copter::update_flight_mode()
{
// Update EKF speed limit - used to limit speed when we are using optical flow
ahrs.getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler);
switch (control_mode) {
case ACRO:
#if FRAME_CONFIG == HELI_FRAME
heli_acro_run();
#else
acro_run();
#endif
break;
case STABILIZE:
#if FRAME_CONFIG == HELI_FRAME
heli_stabilize_run();
#else
stabilize_run();
#endif
break;
case ALT_HOLD:
althold_run();
break;
case AUTO:
auto_run();
break;
case CIRCLE:
circle_run();
break;
case LOITER:
loiter_run();
break;
case GUIDED:
guided_run();
break;
case LAND:
land_run();
break;
case RTL:
rtl_run();
break;
case DRIFT:
drift_run();
break;
case SPORT:
sport_run();
break;
case FLIP:
flip_run();
break;
#if AUTOTUNE_ENABLED == ENABLED
case AUTOTUNE:
autotune_run();
break;
#endif
#if POSHOLD_ENABLED == ENABLED
case POSHOLD:
poshold_run();
break;
#endif
case BRAKE:
brake_run();
break;
}
}
// exit_mode - high level call to organise cleanup as a flight mode is exited
void Copter::exit_mode(uint8_t old_control_mode, uint8_t new_control_mode)
{
#if AUTOTUNE_ENABLED == ENABLED
if (old_control_mode == AUTOTUNE) {
autotune_stop();
}
#endif
// stop mission when we leave auto mode
if (old_control_mode == AUTO) {
if (mission.state() == AP_Mission::MISSION_RUNNING) {
mission.stop();
}
#if MOUNT == ENABLED
camera_mount.set_mode_to_default();
#endif // MOUNT == ENABLED
}
// smooth throttle transition when switching from manual to automatic flight modes
if (mode_has_manual_throttle(old_control_mode) && !mode_has_manual_throttle(new_control_mode) && motors.armed() && !ap.land_complete) {
// this assumes all manual flight modes use get_pilot_desired_throttle to translate pilot input to output throttle
set_accel_throttle_I_from_pilot_throttle(get_pilot_desired_throttle(channel_throttle->control_in));
}
// cancel any takeoffs in progress
takeoff_stop();
#if FRAME_CONFIG == HELI_FRAME
// firmly reset the flybar passthrough to false when exiting acro mode.
if (old_control_mode == ACRO) {
attitude_control.use_flybar_passthrough(false, false);
motors.set_acro_tail(false);
}
// if we are changing from a mode that did not use manual throttle,
// stab col ramp value should be pre-loaded to the correct value to avoid a twitch
// heli_stab_col_ramp should really only be active switching between Stabilize and Acro modes
if (!mode_has_manual_throttle(old_control_mode)){
if (new_control_mode == STABILIZE){
input_manager.set_stab_col_ramp(1.0);
} else if (new_control_mode == ACRO){
input_manager.set_stab_col_ramp(0.0);
}
}
// reset RC Passthrough to motors
motors.reset_radio_passthrough();
#endif //HELI_FRAME
}
// returns true or false whether mode requires GPS
bool Copter::mode_requires_GPS(uint8_t mode) {
switch(mode) {
case AUTO:
case GUIDED:
case LOITER:
case RTL:
case CIRCLE:
case DRIFT:
case POSHOLD:
case BRAKE:
return true;
default:
return false;
}
return false;
}
// mode_has_manual_throttle - returns true if the flight mode has a manual throttle (i.e. pilot directly controls throttle)
bool Copter::mode_has_manual_throttle(uint8_t mode) {
switch(mode) {
case ACRO:
case STABILIZE:
return true;
default:
return false;
}
return false;
}
// mode_allows_arming - returns true if vehicle can be armed in the specified mode
// arming_from_gcs should be set to true if the arming request comes from the ground station
bool Copter::mode_allows_arming(uint8_t mode, bool arming_from_gcs) {
if (mode_has_manual_throttle(mode) || mode == LOITER || mode == ALT_HOLD || mode == POSHOLD || (arming_from_gcs && mode == GUIDED)) {
return true;
}
return false;
}
// notify_flight_mode - sets notify object based on flight mode. Only used for OreoLED notify device
void Copter::notify_flight_mode(uint8_t mode) {
switch(mode) {
case AUTO:
case GUIDED:
case RTL:
case CIRCLE:
case LAND:
// autopilot modes
AP_Notify::flags.autopilot_mode = true;
break;
default:
// all other are manual flight modes
AP_Notify::flags.autopilot_mode = false;
break;
}
}
//
// print_flight_mode - prints flight mode to serial port.
//
void Copter::print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
{
switch (mode) {
case STABILIZE:
port->print("STABILIZE");
break;
case ACRO:
port->print("ACRO");
break;
case ALT_HOLD:
port->print("ALT_HOLD");
break;
case AUTO:
port->print("AUTO");
break;
case GUIDED:
port->print("GUIDED");
break;
case LOITER:
port->print("LOITER");
break;
case RTL:
port->print("RTL");
break;
case CIRCLE:
port->print("CIRCLE");
break;
case LAND:
port->print("LAND");
break;
case OF_LOITER:
port->print("OF_LOITER");
break;
case DRIFT:
port->print("DRIFT");
break;
case SPORT:
port->print("SPORT");
break;
case FLIP:
port->print("FLIP");
break;
case AUTOTUNE:
port->print("AUTOTUNE");
break;
case POSHOLD:
port->print("POSHOLD");
break;
case BRAKE:
port->print("BRAKE");
break;
default:
port->printf("Mode(%u)", (unsigned)mode);
break;
}
}

203
ArduSub/heli.cpp Normal file
View File

@ -0,0 +1,203 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
// Traditional helicopter variables and functions
#if FRAME_CONFIG == HELI_FRAME
#ifndef HELI_DYNAMIC_FLIGHT_SPEED_MIN
#define HELI_DYNAMIC_FLIGHT_SPEED_MIN 500 // we are in "dynamic flight" when the speed is over 1m/s for 2 seconds
#endif
// counter to control dynamic flight profile
static int8_t heli_dynamic_flight_counter;
// heli_init - perform any special initialisation required for the tradheli
void Copter::heli_init()
{
/*
automatically set H_RSC_MIN and H_RSC_MAX from RC8_MIN and
RC8_MAX so that when users upgrade from tradheli version 3.3 to
3.4 they get the same throttle range as in previous versions of
the code
*/
if (!g.heli_servo_rsc.radio_min.configured()) {
g.heli_servo_rsc.radio_min.set_and_save(g.rc_8.radio_min.get());
}
if (!g.heli_servo_rsc.radio_max.configured()) {
g.heli_servo_rsc.radio_max.set_and_save(g.rc_8.radio_max.get());
}
// pre-load stab col values as mode is initialized as Stabilize, but stabilize_init() function is not run on start-up.
input_manager.set_use_stab_col(true);
input_manager.set_stab_col_ramp(1.0);
}
// heli_check_dynamic_flight - updates the dynamic_flight flag based on our horizontal velocity
// should be called at 50hz
void Copter::check_dynamic_flight(void)
{
if (!motors.armed() || !motors.rotor_runup_complete() ||
control_mode == LAND || (control_mode==RTL && rtl_state == RTL_Land) || (control_mode == AUTO && auto_mode == Auto_Land)) {
heli_dynamic_flight_counter = 0;
heli_flags.dynamic_flight = false;
return;
}
bool moving = false;
// with GPS lock use inertial nav to determine if we are moving
if (position_ok()) {
// get horizontal velocity
float velocity = inertial_nav.get_velocity_xy();
moving = (velocity >= HELI_DYNAMIC_FLIGHT_SPEED_MIN);
}else{
// with no GPS lock base it on throttle and forward lean angle
moving = (motors.get_throttle() > 800.0f || ahrs.pitch_sensor < -1500);
}
if (!moving && sonar_enabled && sonar.status() == RangeFinder::RangeFinder_Good) {
// when we are more than 2m from the ground with good
// rangefinder lock consider it to be dynamic flight
moving = (sonar.distance_cm() > 200);
}
if (moving) {
// if moving for 2 seconds, set the dynamic flight flag
if (!heli_flags.dynamic_flight) {
heli_dynamic_flight_counter++;
if (heli_dynamic_flight_counter >= 100) {
heli_flags.dynamic_flight = true;
heli_dynamic_flight_counter = 100;
}
}
}else{
// if not moving for 2 seconds, clear the dynamic flight flag
if (heli_flags.dynamic_flight) {
if (heli_dynamic_flight_counter > 0) {
heli_dynamic_flight_counter--;
}else{
heli_flags.dynamic_flight = false;
}
}
}
}
// update_heli_control_dynamics - pushes several important factors up into AP_MotorsHeli.
// should be run between the rate controller and the servo updates.
void Copter::update_heli_control_dynamics(void)
{
static int16_t hover_roll_trim_scalar_slew = 0;
// Use Leaky_I if we are not moving fast
attitude_control.use_leaky_i(!heli_flags.dynamic_flight);
if (ap.land_complete || (motors.get_desired_rotor_speed() == 0)){
// if we are landed or there is no rotor power demanded, decrement slew scalar
hover_roll_trim_scalar_slew--;
} else {
// if we are not landed and motor power is demanded, increment slew scalar
hover_roll_trim_scalar_slew++;
}
hover_roll_trim_scalar_slew = constrain_int16(hover_roll_trim_scalar_slew, 0, MAIN_LOOP_RATE);
// set hover roll trim scalar, will ramp from 0 to 1 over 1 second after we think helicopter has taken off
attitude_control.set_hover_roll_trim_scalar((float)(hover_roll_trim_scalar_slew/MAIN_LOOP_RATE));
}
// heli_update_landing_swash - sets swash plate flag so higher minimum is used when landed or landing
// should be called soon after update_land_detector in main code
void Copter::heli_update_landing_swash()
{
switch(control_mode) {
case ACRO:
case STABILIZE:
case DRIFT:
case SPORT:
// manual modes always uses full swash range
motors.set_collective_for_landing(false);
break;
case LAND:
// landing always uses limit swash range
motors.set_collective_for_landing(true);
break;
case RTL:
if (rtl_state == RTL_Land) {
motors.set_collective_for_landing(true);
}else{
motors.set_collective_for_landing(!heli_flags.dynamic_flight || ap.land_complete || !ap.auto_armed);
}
break;
case AUTO:
if (auto_mode == Auto_Land) {
motors.set_collective_for_landing(true);
}else{
motors.set_collective_for_landing(!heli_flags.dynamic_flight || ap.land_complete || !ap.auto_armed);
}
break;
default:
// auto and hold use limited swash when landed
motors.set_collective_for_landing(!heli_flags.dynamic_flight || ap.land_complete || !ap.auto_armed);
break;
}
}
// heli_update_rotor_speed_targets - reads pilot input and passes new rotor speed targets to heli motors object
void Copter::heli_update_rotor_speed_targets()
{
static bool rotor_runup_complete_last = false;
// get rotor control method
uint8_t rsc_control_mode = motors.get_rsc_mode();
rsc_control_deglitched = rotor_speed_deglitch_filter.apply(g.rc_8.control_in);
switch (rsc_control_mode) {
case ROTOR_CONTROL_MODE_SPEED_PASSTHROUGH:
// pass through pilot desired rotor speed if control input is higher than 10, creating a deadband at the bottom
if (rsc_control_deglitched > 10) {
motors.set_interlock(true);
motors.set_desired_rotor_speed(rsc_control_deglitched);
} else {
motors.set_interlock(false);
motors.set_desired_rotor_speed(0);
}
break;
case ROTOR_CONTROL_MODE_SPEED_SETPOINT:
case ROTOR_CONTROL_MODE_OPEN_LOOP_POWER_OUTPUT:
case ROTOR_CONTROL_MODE_CLOSED_LOOP_POWER_OUTPUT:
// pass setpoint through as desired rotor speed, this is almost pointless as the Setpoint serves no function in this mode
// other than being used to create a crude estimate of rotor speed
if (rsc_control_deglitched > 0) {
motors.set_interlock(true);
motors.set_desired_rotor_speed(motors.get_rsc_setpoint());
}else{
motors.set_interlock(false);
motors.set_desired_rotor_speed(0);
}
break;
}
// when rotor_runup_complete changes to true, log event
if (!rotor_runup_complete_last && motors.rotor_runup_complete()){
Log_Write_Event(DATA_ROTOR_RUNUP_COMPLETE);
} else if (rotor_runup_complete_last && !motors.rotor_runup_complete()){
Log_Write_Event(DATA_ROTOR_SPEED_BELOW_CRITICAL);
}
rotor_runup_complete_last = motors.rotor_runup_complete();
}
// heli_radio_passthrough send RC inputs direct into motors library for use during manual passthrough for helicopter setup
void Copter::heli_radio_passthrough()
{
motors.set_radio_passthrough(channel_roll->control_in, channel_pitch->control_in, channel_throttle->control_in, channel_yaw->control_in);
}
#endif // FRAME_CONFIG == HELI_FRAME

98
ArduSub/heli_control_acro.cpp Normal file
View File

@ -0,0 +1,98 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#if FRAME_CONFIG == HELI_FRAME
/*
* heli_control_acro.pde - init and run calls for acro flight mode for trad heli
*/
// heli_acro_init - initialise acro controller
bool Copter::heli_acro_init(bool ignore_checks)
{
// if heli is equipped with a flybar, then tell the attitude controller to pass through controls directly to servos
attitude_control.use_flybar_passthrough(motors.has_flybar(), motors.supports_yaw_passthrough());
motors.set_acro_tail(true);
// set stab collective false to use full collective pitch range
input_manager.set_use_stab_col(false);
// always successfully enter acro
return true;
}
// heli_acro_run - runs the acro controller
// should be called at 100hz or more
void Copter::heli_acro_run()
{
float target_roll, target_pitch, target_yaw;
int16_t pilot_throttle_scaled;
// Tradheli should not reset roll, pitch, yaw targets when motors are not runup, because
// we may be in autorotation flight. These should be reset only when transitioning from disarmed
// to armed, because the pilot will have placed the helicopter down on the landing pad. This is so
// that the servos move in a realistic fashion while disarmed for operational checks.
// Also, unlike multicopters we do not set throttle (i.e. collective pitch) to zero so the swash servos move
if(!motors.armed()) {
heli_flags.init_targets_on_arming=true;
attitude_control.set_yaw_target_to_current_heading();
}
if(motors.armed() && heli_flags.init_targets_on_arming) {
attitude_control.set_yaw_target_to_current_heading();
if (motors.rotor_speed_above_critical()) {
heli_flags.init_targets_on_arming=false;
}
}
// send RC inputs direct into motors library for use during manual passthrough for helicopter setup
heli_radio_passthrough();
if (!motors.has_flybar()){
// convert the input to the desired body frame rate
get_pilot_desired_angle_rates(channel_roll->control_in, channel_pitch->control_in, channel_yaw->control_in, target_roll, target_pitch, target_yaw);
if (motors.supports_yaw_passthrough()) {
// if the tail on a flybar heli has an external gyro then
// also use no deadzone for the yaw control and
// pass-through the input direct to output.
target_yaw = channel_yaw->pwm_to_angle_dz(0);
}
// run attitude controller
attitude_control.input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw);
}else{
/*
for fly-bar passthrough use control_in values with no
deadzone. This gives true pass-through.
*/
float roll_in = channel_roll->pwm_to_angle_dz(0);
float pitch_in = channel_pitch->pwm_to_angle_dz(0);
float yaw_in;
if (motors.supports_yaw_passthrough()) {
// if the tail on a flybar heli has an external gyro then
// also use no deadzone for the yaw control and
// pass-through the input direct to output.
yaw_in = channel_yaw->pwm_to_angle_dz(0);
} else {
// if there is no external gyro then run the usual
// ACRO_YAW_P gain on the input control, including
// deadzone
yaw_in = get_pilot_desired_yaw_rate(channel_yaw->control_in);
}
// run attitude controller
attitude_control.passthrough_bf_roll_pitch_rate_yaw(roll_in, pitch_in, yaw_in);
}
// get pilot's desired throttle
pilot_throttle_scaled = input_manager.get_pilot_desired_collective(channel_throttle->control_in);
// output pilot's throttle without angle boost
attitude_control.set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt);
}
#endif //HELI_FRAME

72
ArduSub/heli_control_stabilize.cpp Normal file
View File

@ -0,0 +1,72 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#if FRAME_CONFIG == HELI_FRAME
/*
* heli_control_stabilize.pde - init and run calls for stabilize flight mode for trad heli
*/
// stabilize_init - initialise stabilize controller
bool Copter::heli_stabilize_init(bool ignore_checks)
{
// set target altitude to zero for reporting
// To-Do: make pos controller aware when it's active/inactive so it can always report the altitude error?
pos_control.set_alt_target(0);
// set stab collective true to use stabilize scaled collective pitch range
input_manager.set_use_stab_col(true);
return true;
}
// stabilize_run - runs the main stabilize controller
// should be called at 100hz or more
void Copter::heli_stabilize_run()
{
float target_roll, target_pitch;
float target_yaw_rate;
int16_t pilot_throttle_scaled;
// Tradheli should not reset roll, pitch, yaw targets when motors are not runup, because
// we may be in autorotation flight. These should be reset only when transitioning from disarmed
// to armed, because the pilot will have placed the helicopter down on the landing pad. This is so
// that the servos move in a realistic fashion while disarmed for operational checks.
// Also, unlike multicopters we do not set throttle (i.e. collective pitch) to zero so the swash servos move
if(!motors.armed()) {
heli_flags.init_targets_on_arming=true;
attitude_control.set_yaw_target_to_current_heading();
}
if(motors.armed() && heli_flags.init_targets_on_arming) {
attitude_control.set_yaw_target_to_current_heading();
if (motors.rotor_speed_above_critical()) {
heli_flags.init_targets_on_arming=false;
}
}
// send RC inputs direct into motors library for use during manual passthrough for helicopter setup
heli_radio_passthrough();
// apply SIMPLE mode transform to pilot inputs
update_simple_mode();
// convert pilot input to lean angles
// To-Do: convert get_pilot_desired_lean_angles to return angles as floats
get_pilot_desired_lean_angles(channel_roll->control_in, channel_pitch->control_in, target_roll, target_pitch, aparm.angle_max);
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);
// get pilot's desired throttle
pilot_throttle_scaled = input_manager.get_pilot_desired_collective(channel_throttle->control_in);
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
// output pilot's throttle - note that TradHeli does not used angle-boost
attitude_control.set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt);
}
#endif //HELI_FRAME

32
ArduSub/inertia.cpp Normal file
View File

@ -0,0 +1,32 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
// read_inertia - read inertia in from accelerometers
void Copter::read_inertia()
{
// inertial altitude estimates
inertial_nav.update(G_Dt);
}
// read_inertial_altitude - pull altitude and climb rate from inertial nav library
void Copter::read_inertial_altitude()
{
// exit immediately if we do not have an altitude estimate
if (!inertial_nav.get_filter_status().flags.vert_pos) {
return;
}
// without home return alt above the EKF origin
if (ap.home_state == HOME_UNSET) {
// with inertial nav we can update the altitude and climb rate at 50hz
current_loc.alt = inertial_nav.get_altitude();
} else {
// with inertial nav we can update the altitude and climb rate at 50hz
current_loc.alt = pv_alt_above_home(inertial_nav.get_altitude());
}
// set flags and get velocity
current_loc.flags.relative_alt = true;
climb_rate = inertial_nav.get_velocity_z();
}

156
ArduSub/land_detector.cpp Normal file
View File

@ -0,0 +1,156 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
// counter to verify landings
static uint32_t land_detector_count = 0;
// run land and crash detectors
// called at MAIN_LOOP_RATE
void Copter::update_land_and_crash_detectors()
{
// update 1hz filtered acceleration
Vector3f accel_ef = ahrs.get_accel_ef_blended();
accel_ef.z += GRAVITY_MSS;
land_accel_ef_filter.apply(accel_ef, MAIN_LOOP_SECONDS);
update_land_detector();
#if PARACHUTE == ENABLED
// check parachute
parachute_check();
#endif
crash_check();
}
// update_land_detector - checks if we have landed and updates the ap.land_complete flag
// called at MAIN_LOOP_RATE
void Copter::update_land_detector()
{
// land detector can not use the following sensors because they are unreliable during landing
// barometer altitude : ground effect can cause errors larger than 4m
// EKF vertical velocity or altitude : poor barometer and large acceleration from ground impact
// earth frame angle or angle error : landing on an uneven surface will force the airframe to match the ground angle
// gyro output : on uneven surface the airframe may rock back an forth after landing
// range finder : tend to be problematic at very short distances
// input throttle : in slow land the input throttle may be only slightly less than hover
if (!motors.armed()) {
// if disarmed, always landed.
set_land_complete(true);
} else if (ap.land_complete) {
#if FRAME_CONFIG == HELI_FRAME
// if rotor speed and collective pitch are high then clear landing flag
if (motors.get_throttle() > get_non_takeoff_throttle() && motors.rotor_runup_complete()) {
#else
// if throttle output is high then clear landing flag
if (motors.get_throttle() > get_non_takeoff_throttle()) {
#endif
set_land_complete(false);
}
} else {
#if FRAME_CONFIG == HELI_FRAME
// check that collective pitch is on lower limit (should be constrained by LAND_COL_MIN)
bool motor_at_lower_limit = motors.limit.throttle_lower;
#else
// check that the average throttle output is near minimum (less than 12.5% hover throttle)
bool motor_at_lower_limit = motors.limit.throttle_lower && motors.is_throttle_mix_min();
#endif
// check that the airframe is not accelerating (not falling or breaking after fast forward flight)
bool accel_stationary = (land_accel_ef_filter.get().length() <= LAND_DETECTOR_ACCEL_MAX);
if (motor_at_lower_limit && accel_stationary) {
// landed criteria met - increment the counter and check if we've triggered
if( land_detector_count < ((float)LAND_DETECTOR_TRIGGER_SEC)*MAIN_LOOP_RATE) {
land_detector_count++;
} else {
set_land_complete(true);
}
} else {
// we've sensed movement up or down so reset land_detector
land_detector_count = 0;
}
}
set_land_complete_maybe(ap.land_complete || (land_detector_count >= LAND_DETECTOR_MAYBE_TRIGGER_SEC*MAIN_LOOP_RATE));
}
void Copter::set_land_complete(bool b)
{
// if no change, exit immediately
if( ap.land_complete == b )
return;
land_detector_count = 0;
if(b){
Log_Write_Event(DATA_LAND_COMPLETE);
} else {
Log_Write_Event(DATA_NOT_LANDED);
}
ap.land_complete = b;
}
// set land complete maybe flag
void Copter::set_land_complete_maybe(bool b)
{
// if no change, exit immediately
if (ap.land_complete_maybe == b)
return;
if (b) {
Log_Write_Event(DATA_LAND_COMPLETE_MAYBE);
}
ap.land_complete_maybe = b;
}
// update_throttle_thr_mix - sets motors throttle_low_comp value depending upon vehicle state
// low values favour pilot/autopilot throttle over attitude control, high values favour attitude control over throttle
// has no effect when throttle is above hover throttle
void Copter::update_throttle_thr_mix()
{
#if FRAME_CONFIG != HELI_FRAME
// if disarmed or landed prioritise throttle
if(!motors.armed() || ap.land_complete) {
motors.set_throttle_mix_min();
return;
}
if (mode_has_manual_throttle(control_mode)) {
// manual throttle
if(channel_throttle->control_in <= 0) {
motors.set_throttle_mix_min();
} else {
motors.set_throttle_mix_mid();
}
} else {
// autopilot controlled throttle
// check for aggressive flight requests - requested roll or pitch angle below 15 degrees
const Vector3f angle_target = attitude_control.get_att_target_euler_cd();
bool large_angle_request = (pythagorous2(angle_target.x, angle_target.y) > 1500.0f);
// check for large external disturbance - angle error over 30 degrees
const Vector3f angle_error = attitude_control.get_att_error_rot_vec_cd();
bool large_angle_error = (pythagorous2(angle_error.x, angle_error.y) > 3000.0f);
// check for large acceleration - falling or high turbulence
Vector3f accel_ef = ahrs.get_accel_ef_blended();
accel_ef.z += GRAVITY_MSS;
bool accel_moving = (accel_ef.length() > 3.0f);
// check for requested decent
bool descent_not_demanded = pos_control.get_desired_velocity().z >= 0.0f;
if ( large_angle_request || large_angle_error || accel_moving || descent_not_demanded) {
motors.set_throttle_mix_max();
} else {
motors.set_throttle_mix_min();
}
}
#endif
}

37
ArduSub/landing_gear.cpp Normal file
View File

@ -0,0 +1,37 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
// Run landing gear controller at 10Hz
void Copter::landinggear_update(){
// If landing gear control is active, run update function.
if (check_if_auxsw_mode_used(AUXSW_LANDING_GEAR)){
// last status (deployed or retracted) used to check for changes
static bool last_deploy_status;
// if we are doing an automatic landing procedure, force the landing gear to deploy.
// To-Do: should we pause the auto-land procedure to give time for gear to come down?
if (control_mode == LAND ||
(control_mode==RTL && (rtl_state == RTL_LoiterAtHome || rtl_state == RTL_Land || rtl_state == RTL_FinalDescent)) ||
(control_mode == AUTO && auto_mode == Auto_Land) ||
(control_mode == AUTO && auto_mode == Auto_RTL && (rtl_state == RTL_LoiterAtHome || rtl_state == RTL_Land || rtl_state == RTL_FinalDescent))) {
landinggear.force_deploy(true);
}
landinggear.update();
// send event message to datalog if status has changed
if (landinggear.deployed() != last_deploy_status){
if (landinggear.deployed()) {
Log_Write_Event(DATA_LANDING_GEAR_DEPLOYED);
} else {
Log_Write_Event(DATA_LANDING_GEAR_RETRACTED);
}
}
last_deploy_status = landinggear.deployed();
}
}

12
ArduSub/leds.cpp Normal file
View File

@ -0,0 +1,12 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
// updates the status of notify
// should be called at 50hz
void Copter::update_notify()
{
notify.update();
}

61
ArduSub/make.inc Normal file
View File

@ -0,0 +1,61 @@
LIBRARIES += AP_Common
LIBRARIES += AP_Menu
LIBRARIES += AP_Param
LIBRARIES += StorageManager
LIBRARIES += GCS
LIBRARIES += GCS_MAVLink
LIBRARIES += AP_SerialManager
LIBRARIES += AP_GPS
LIBRARIES += DataFlash
LIBRARIES += AP_ADC
LIBRARIES += AP_Baro
LIBRARIES += AP_Compass
LIBRARIES += AP_Math
LIBRARIES += AP_InertialSensor
LIBRARIES += AP_AccelCal
LIBRARIES += AP_AHRS
LIBRARIES += AP_NavEKF
LIBRARIES += AP_NavEKF2
LIBRARIES += AP_Mission
LIBRARIES += AP_Rally
LIBRARIES += AC_PID
LIBRARIES += AC_PI_2D
LIBRARIES += AC_HELI_PID
LIBRARIES += AC_P
LIBRARIES += AC_AttitudeControl
LIBRARIES += AC_AttitudeControl_Heli
LIBRARIES += AC_PosControl
LIBRARIES += RC_Channel
LIBRARIES += AP_Motors
LIBRARIES += AP_RangeFinder
LIBRARIES += AP_OpticalFlow
LIBRARIES += AP_RSSI
LIBRARIES += Filter
LIBRARIES += AP_Buffer
LIBRARIES += AP_Relay
LIBRARIES += AP_ServoRelayEvents
LIBRARIES += AP_Camera
LIBRARIES += AP_Mount
LIBRARIES += AP_Airspeed
LIBRARIES += AP_Vehicle
LIBRARIES += AP_InertialNav
LIBRARIES += AC_WPNav
LIBRARIES += AC_Circle
LIBRARIES += AP_Declination
LIBRARIES += AC_Fence
LIBRARIES += AP_Scheduler
LIBRARIES += AP_RCMapper
LIBRARIES += AP_Notify
LIBRARIES += AP_BattMonitor
LIBRARIES += AP_BoardConfig
LIBRARIES += AP_Frsky_Telem
LIBRARIES += AC_Sprayer
LIBRARIES += AP_EPM
LIBRARIES += AP_Parachute
LIBRARIES += AP_LandingGear
LIBRARIES += AP_Terrain
LIBRARIES += AP_RPM
LIBRARIES += AC_PrecLand
LIBRARIES += AP_IRLock
LIBRARIES += AC_InputManager
LIBRARIES += AP_ADSB

168
ArduSub/motor_test.cpp Normal file
View File

@ -0,0 +1,168 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
/*
mavlink motor test - implements the MAV_CMD_DO_MOTOR_TEST mavlink command so that the GCS/pilot can test an individual motor or flaps
to ensure proper wiring, rotation.
*/
// motor test definitions
#define MOTOR_TEST_PWM_MIN 800 // min pwm value accepted by the test
#define MOTOR_TEST_PWM_MAX 2200 // max pwm value accepted by the test
#define MOTOR_TEST_TIMEOUT_MS_MAX 30000 // max timeout is 30 seconds
static uint32_t motor_test_start_ms = 0; // system time the motor test began
static uint32_t motor_test_timeout_ms = 0; // test will timeout this many milliseconds after the motor_test_start_ms
static uint8_t motor_test_seq = 0; // motor sequence number of motor being tested
static uint8_t motor_test_throttle_type = 0; // motor throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through)
static uint16_t motor_test_throttle_value = 0; // throttle to be sent to motor, value depends upon it's type
// motor_test_output - checks for timeout and sends updates to motors objects
void Copter::motor_test_output()
{
// exit immediately if the motor test is not running
if (!ap.motor_test) {
return;
}
// check for test timeout
if ((AP_HAL::millis() - motor_test_start_ms) >= motor_test_timeout_ms) {
// stop motor test
motor_test_stop();
} else {
int16_t pwm = 0; // pwm that will be output to the motors
// calculate pwm based on throttle type
switch (motor_test_throttle_type) {
case MOTOR_TEST_THROTTLE_PERCENT:
// sanity check motor_test_throttle value
if (motor_test_throttle_value <= 100) {
pwm = channel_throttle->radio_min + (channel_throttle->radio_max - channel_throttle->radio_min) * (float)motor_test_throttle_value/100.0f;
}
break;
case MOTOR_TEST_THROTTLE_PWM:
pwm = motor_test_throttle_value;
break;
case MOTOR_TEST_THROTTLE_PILOT:
pwm = channel_throttle->radio_in;
break;
default:
motor_test_stop();
return;
break;
}
// sanity check throttle values
if (pwm >= MOTOR_TEST_PWM_MIN && pwm <= MOTOR_TEST_PWM_MAX ) {
// turn on motor to specified pwm vlaue
motors.output_test(motor_test_seq, pwm);
} else {
motor_test_stop();
}
}
}
// mavlink_motor_test_check - perform checks before motor tests can begin
// return true if tests can continue, false if not
bool Copter::mavlink_motor_test_check(mavlink_channel_t chan, bool check_rc)
{
// check rc has been calibrated
pre_arm_rc_checks();
if(check_rc && !ap.pre_arm_rc_check) {
gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL,"Motor Test: RC not calibrated");
return false;
}
// ensure we are landed
if (!ap.land_complete) {
gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL,"Motor Test: vehicle not landed");
return false;
}
// check if safety switch has been pushed
if (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) {
gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL,"Motor Test: Safety switch");
return false;
}
// if we got this far the check was successful and the motor test can continue
return true;
}
// mavlink_motor_test_start - start motor test - spin a single motor at a specified pwm
// returns MAV_RESULT_ACCEPTED on success, MAV_RESULT_FAILED on failure
uint8_t Copter::mavlink_motor_test_start(mavlink_channel_t chan, uint8_t motor_seq, uint8_t throttle_type, uint16_t throttle_value, float timeout_sec)
{
// if test has not started try to start it
if (!ap.motor_test) {
/* perform checks that it is ok to start test
The RC calibrated check can be skipped if direct pwm is
supplied
*/
if (!mavlink_motor_test_check(chan, throttle_type != 1)) {
return MAV_RESULT_FAILED;
} else {
// start test
ap.motor_test = true;
// enable and arm motors
if (!motors.armed()) {
init_rc_out();
enable_motor_output();
motors.armed(true);
}
// disable throttle, battery and gps failsafe
g.failsafe_throttle = FS_THR_DISABLED;
g.failsafe_battery_enabled = FS_BATT_DISABLED;
g.failsafe_gcs = FS_GCS_DISABLED;
// turn on notify leds
AP_Notify::flags.esc_calibration = true;
}
}
// set timeout
motor_test_start_ms = AP_HAL::millis();
motor_test_timeout_ms = MIN(timeout_sec * 1000, MOTOR_TEST_TIMEOUT_MS_MAX);
// store required output
motor_test_seq = motor_seq;
motor_test_throttle_type = throttle_type;
motor_test_throttle_value = throttle_value;
// return success
return MAV_RESULT_ACCEPTED;
}
// motor_test_stop - stops the motor test
void Copter::motor_test_stop()
{
// exit immediately if the test is not running
if (!ap.motor_test) {
return;
}
// flag test is complete
ap.motor_test = false;
// disarm motors
motors.armed(false);
// reset timeout
motor_test_start_ms = 0;
motor_test_timeout_ms = 0;
// re-enable failsafes
g.failsafe_throttle.load();
g.failsafe_battery_enabled.load();
g.failsafe_gcs.load();
// turn off notify leds
AP_Notify::flags.esc_calibration = false;
}

302
ArduSub/motors.cpp Normal file
View File

@ -0,0 +1,302 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#define ARM_DELAY 20 // called at 10hz so 2 seconds
#define DISARM_DELAY 20 // called at 10hz so 2 seconds
#define AUTO_TRIM_DELAY 100 // called at 10hz so 10 seconds
#define LOST_VEHICLE_DELAY 10 // called at 10hz so 1 second
static uint32_t auto_disarm_begin;
// arm_motors_check - checks for pilot input to arm or disarm the copter
// called at 10hz
void Copter::arm_motors_check()
{
static int16_t arming_counter;
// ensure throttle is down
if (channel_throttle->control_in > 0) {
arming_counter = 0;
return;
}
int16_t tmp = channel_yaw->control_in;
// full right
if (tmp > 4000) {
// increase the arming counter to a maximum of 1 beyond the auto trim counter
if( arming_counter <= AUTO_TRIM_DELAY ) {
arming_counter++;
}
// arm the motors and configure for flight
if (arming_counter == ARM_DELAY && !motors.armed()) {
// reset arming counter if arming fail
if (!init_arm_motors(false)) {
arming_counter = 0;
}
}
// arm the motors and configure for flight
if (arming_counter == AUTO_TRIM_DELAY && motors.armed() && control_mode == STABILIZE) {
auto_trim_counter = 250;
// ensure auto-disarm doesn't trigger immediately
auto_disarm_begin = millis();
}
// full left
}else if (tmp < -4000) {
if (!mode_has_manual_throttle(control_mode) && !ap.land_complete) {
arming_counter = 0;
return;
}
// increase the counter to a maximum of 1 beyond the disarm delay
if( arming_counter <= DISARM_DELAY ) {
arming_counter++;
}
// disarm the motors
if (arming_counter == DISARM_DELAY && motors.armed()) {
init_disarm_motors();
}
// Yaw is centered so reset arming counter
}else{
arming_counter = 0;
}
}
// auto_disarm_check - disarms the copter if it has been sitting on the ground in manual mode with throttle low for at least 15 seconds
void Copter::auto_disarm_check()
{
uint32_t tnow_ms = millis();
uint32_t disarm_delay_ms = 1000*constrain_int16(g.disarm_delay, 0, 127);
// exit immediately if we are already disarmed, or if auto
// disarming is disabled
if (!motors.armed() || disarm_delay_ms == 0) {
auto_disarm_begin = tnow_ms;
return;
}
#if FRAME_CONFIG == HELI_FRAME
// if the rotor is still spinning, don't initiate auto disarm
if (motors.rotor_speed_above_critical()) {
auto_disarm_begin = tnow_ms;
return;
}
#endif
// always allow auto disarm if using interlock switch or motors are Emergency Stopped
if ((ap.using_interlock && !motors.get_interlock()) || ap.motor_emergency_stop) {
#if FRAME_CONFIG != HELI_FRAME
// use a shorter delay if using throttle interlock switch or Emergency Stop, because it is less
// obvious the copter is armed as the motors will not be spinning
disarm_delay_ms /= 2;
#endif
} else {
bool sprung_throttle_stick = (g.throttle_behavior & THR_BEHAVE_FEEDBACK_FROM_MID_STICK) != 0;
bool thr_low;
if (mode_has_manual_throttle(control_mode) || !sprung_throttle_stick) {
thr_low = ap.throttle_zero;
} else {
float deadband_top = g.rc_3.get_control_mid() + g.throttle_deadzone;
thr_low = g.rc_3.control_in <= deadband_top;
}
if (!thr_low || !ap.land_complete) {
// reset timer
auto_disarm_begin = tnow_ms;
}
}
// disarm once timer expires
if ((tnow_ms-auto_disarm_begin) >= disarm_delay_ms) {
init_disarm_motors();
auto_disarm_begin = tnow_ms;
}
}
// init_arm_motors - performs arming process including initialisation of barometer and gyros
// returns false if arming failed because of pre-arm checks, arming checks or a gyro calibration failure
bool Copter::init_arm_motors(bool arming_from_gcs)
{
static bool in_arm_motors = false;
// exit immediately if already in this function
if (in_arm_motors) {
return false;
}
in_arm_motors = true;
// run pre-arm-checks and display failures
if (!all_arming_checks_passing(arming_from_gcs)) {
AP_Notify::events.arming_failed = true;
in_arm_motors = false;
return false;
}
// disable cpu failsafe because initialising everything takes a while
failsafe_disable();
// reset battery failsafe
set_failsafe_battery(false);
// notify that arming will occur (we do this early to give plenty of warning)
AP_Notify::flags.armed = true;
// call update_notify a few times to ensure the message gets out
for (uint8_t i=0; i<=10; i++) {
update_notify();
}
#if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_SITL
gcs_send_text(MAV_SEVERITY_INFO, "Arming motors");
#endif
// Remember Orientation
// --------------------
init_simple_bearing();
initial_armed_bearing = ahrs.yaw_sensor;
if (ap.home_state == HOME_UNSET) {
// Reset EKF altitude if home hasn't been set yet (we use EKF altitude as substitute for alt above home)
ahrs.resetHeightDatum();
Log_Write_Event(DATA_EKF_ALT_RESET);
} else if (ap.home_state == HOME_SET_NOT_LOCKED) {
// Reset home position if it has already been set before (but not locked)
set_home_to_current_location();
}
calc_distance_and_bearing();
// enable gps velocity based centrefugal force compensation
ahrs.set_correct_centrifugal(true);
hal.util->set_soft_armed(true);
#if SPRAYER == ENABLED
// turn off sprayer's test if on
sprayer.test_pump(false);
#endif
// short delay to allow reading of rc inputs
delay(30);
// enable output to motors
enable_motor_output();
// finally actually arm the motors
motors.armed(true);
// log arming to dataflash
Log_Write_Event(DATA_ARMED);
// log flight mode in case it was changed while vehicle was disarmed
DataFlash.Log_Write_Mode(control_mode);
// reenable failsafe
failsafe_enable();
// perf monitor ignores delay due to arming
perf_ignore_this_loop();
// flag exiting this function
in_arm_motors = false;
// return success
return true;
}
// init_disarm_motors - disarm motors
void Copter::init_disarm_motors()
{
// return immediately if we are already disarmed
if (!motors.armed()) {
return;
}
#if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_SITL
gcs_send_text(MAV_SEVERITY_INFO, "Disarming motors");
#endif
// save compass offsets learned by the EKF
Vector3f magOffsets;
if (ahrs.use_compass() && ahrs.getMagOffsets(magOffsets)) {
compass.set_and_save_offsets(compass.get_primary(), magOffsets);
}
#if AUTOTUNE_ENABLED == ENABLED
// save auto tuned parameters
autotune_save_tuning_gains();
#endif
// we are not in the air
set_land_complete(true);
set_land_complete_maybe(true);
// log disarm to the dataflash
Log_Write_Event(DATA_DISARMED);
// send disarm command to motors
motors.armed(false);
// reset the mission
mission.reset();
// suspend logging
if (!(g.log_bitmask & MASK_LOG_WHEN_DISARMED)) {
DataFlash.EnableWrites(false);
}
// disable gps velocity based centrefugal force compensation
ahrs.set_correct_centrifugal(false);
hal.util->set_soft_armed(false);
}
// motors_output - send output to motors library which will adjust and send to ESCs and servos
void Copter::motors_output()
{
// check if we are performing the motor test
if (ap.motor_test) {
motor_test_output();
} else {
if (!ap.using_interlock){
// if not using interlock switch, set according to Emergency Stop status
// where Emergency Stop is forced false during arming if Emergency Stop switch
// is not used. Interlock enabled means motors run, so we must
// invert motor_emergency_stop status for motors to run.
motors.set_interlock(!ap.motor_emergency_stop);
}
motors.output();
}
}
// check for pilot stick input to trigger lost vehicle alarm
void Copter::lost_vehicle_check()
{
static uint8_t soundalarm_counter;
// disable if aux switch is setup to vehicle alarm as the two could interfere
if (check_if_auxsw_mode_used(AUXSW_LOST_COPTER_SOUND)) {
return;
}
// ensure throttle is down, motors not armed, pitch and roll rc at max. Note: rc1=roll rc2=pitch
if (ap.throttle_zero && !motors.armed() && (channel_roll->control_in > 4000) && (channel_pitch->control_in > 4000)) {
if (soundalarm_counter >= LOST_VEHICLE_DELAY) {
if (AP_Notify::flags.vehicle_lost == false) {
AP_Notify::flags.vehicle_lost = true;
gcs_send_text(MAV_SEVERITY_NOTICE,"Locate Copter alarm");
}
} else {
soundalarm_counter++;
}
} else {
soundalarm_counter = 0;
if (AP_Notify::flags.vehicle_lost == true) {
AP_Notify::flags.vehicle_lost = false;
}
}
}

85
ArduSub/navigation.cpp Normal file
View File

@ -0,0 +1,85 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
// run_nav_updates - top level call for the autopilot
// ensures calculations such as "distance to waypoint" are calculated before autopilot makes decisions
// To-Do - rename and move this function to make it's purpose more clear
void Copter::run_nav_updates(void)
{
// fetch position from inertial navigation
calc_position();
// calculate distance and bearing for reporting and autopilot decisions
calc_distance_and_bearing();
// run autopilot to make high level decisions about control modes
run_autopilot();
}
// calc_position - get lat and lon positions from inertial nav library
void Copter::calc_position()
{
// pull position from interial nav library
current_loc.lng = inertial_nav.get_longitude();
current_loc.lat = inertial_nav.get_latitude();
}
// calc_distance_and_bearing - calculate distance and bearing to next waypoint and home
void Copter::calc_distance_and_bearing()
{
calc_wp_distance();
calc_wp_bearing();
calc_home_distance_and_bearing();
}
// calc_wp_distance - calculate distance to next waypoint for reporting and autopilot decisions
void Copter::calc_wp_distance()
{
// get target from loiter or wpinav controller
if (control_mode == LOITER || control_mode == CIRCLE) {
wp_distance = wp_nav.get_loiter_distance_to_target();
}else if (control_mode == AUTO || control_mode == RTL || (control_mode == GUIDED && guided_mode == Guided_WP)) {
wp_distance = wp_nav.get_wp_distance_to_destination();
}else{
wp_distance = 0;
}
}
// calc_wp_bearing - calculate bearing to next waypoint for reporting and autopilot decisions
void Copter::calc_wp_bearing()
{
// get target from loiter or wpinav controller
if (control_mode == LOITER || control_mode == CIRCLE) {
wp_bearing = wp_nav.get_loiter_bearing_to_target();
} else if (control_mode == AUTO || control_mode == RTL || (control_mode == GUIDED && guided_mode == Guided_WP)) {
wp_bearing = wp_nav.get_wp_bearing_to_destination();
} else {
wp_bearing = 0;
}
}
// calc_home_distance_and_bearing - calculate distance and bearing to home for reporting and autopilot decisions
void Copter::calc_home_distance_and_bearing()
{
// calculate home distance and bearing
if (position_ok()) {
Vector3f home = pv_location_to_vector(ahrs.get_home());
Vector3f curr = inertial_nav.get_position();
home_distance = pv_get_horizontal_distance_cm(curr, home);
home_bearing = pv_get_bearing_cd(curr,home);
// update super simple bearing (if required) because it relies on home_bearing
update_super_simple_bearing(false);
}
}
// run_autopilot - highest level call to process mission commands
void Copter::run_autopilot()
{
if (control_mode == AUTO) {
// update state of mission
// may call commands_process.pde's start_command and verify_command functions
mission.update();
}
}

79
ArduSub/perf_info.cpp Normal file
View File

@ -0,0 +1,79 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
//
// high level performance monitoring
//
// we measure the main loop time
//
// 400hz loop update rate
#define PERF_INFO_OVERTIME_THRESHOLD_MICROS 3000
static uint16_t perf_info_loop_count;
static uint32_t perf_info_max_time;
static uint32_t perf_info_min_time;
static uint16_t perf_info_long_running;
static bool perf_ignore_loop = false;
// perf_info_reset - reset all records of loop time to zero
void Copter::perf_info_reset()
{
perf_info_loop_count = 0;
perf_info_max_time = 0;
perf_info_min_time = 0;
perf_info_long_running = 0;
}
// perf_ignore_loop - ignore this loop from performance measurements (used to reduce false positive when arming)
void Copter::perf_ignore_this_loop()
{
perf_ignore_loop = true;
}
// perf_info_check_loop_time - check latest loop time vs min, max and overtime threshold
void Copter::perf_info_check_loop_time(uint32_t time_in_micros)
{
perf_info_loop_count++;
// exit if this loop should be ignored
if (perf_ignore_loop) {
perf_ignore_loop = false;
return;
}
if( time_in_micros > perf_info_max_time) {
perf_info_max_time = time_in_micros;
}
if( perf_info_min_time == 0 || time_in_micros < perf_info_min_time) {
perf_info_min_time = time_in_micros;
}
if( time_in_micros > PERF_INFO_OVERTIME_THRESHOLD_MICROS ) {
perf_info_long_running++;
}
}
// perf_info_get_long_running_percentage - get number of long running loops as a percentage of the total number of loops
uint16_t Copter::perf_info_get_num_loops()
{
return perf_info_loop_count;
}
// perf_info_get_max_time - return maximum loop time (in microseconds)
uint32_t Copter::perf_info_get_max_time()
{
return perf_info_max_time;
}
// perf_info_get_max_time - return maximum loop time (in microseconds)
uint32_t Copter::perf_info_get_min_time()
{
return perf_info_min_time;
}
// perf_info_get_num_long_running - get number of long running loops
uint16_t Copter::perf_info_get_num_long_running()
{
return perf_info_long_running;
}

68
ArduSub/position_vector.cpp Normal file
View File

@ -0,0 +1,68 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
// position_vector.pde related utility functions
// position vectors are Vector3f
// .x = latitude from home in cm
// .y = longitude from home in cm
// .z = altitude above home in cm
// pv_location_to_vector - convert lat/lon coordinates to a position vector
Vector3f Copter::pv_location_to_vector(const Location& loc)
{
const struct Location &origin = inertial_nav.get_origin();
float alt_above_origin = pv_alt_above_origin(loc.alt); // convert alt-relative-to-home to alt-relative-to-origin
return Vector3f((loc.lat-origin.lat) * LATLON_TO_CM, (loc.lng-origin.lng) * LATLON_TO_CM * scaleLongDown, alt_above_origin);
}
// pv_location_to_vector_with_default - convert lat/lon coordinates to a position vector,
// defaults to the default_posvec's lat/lon and alt if the provided lat/lon or alt are zero
Vector3f Copter::pv_location_to_vector_with_default(const Location& loc, const Vector3f& default_posvec)
{
Vector3f pos = pv_location_to_vector(loc);
// set target altitude to default if not provided
if (loc.alt == 0) {
pos.z = default_posvec.z;
}
// set target position to default if not provided
if (loc.lat == 0 && loc.lng == 0) {
pos.x = default_posvec.x;
pos.y = default_posvec.y;
}
return pos;
}
// pv_alt_above_origin - convert altitude above home to altitude above EKF origin
float Copter::pv_alt_above_origin(float alt_above_home_cm)
{
const struct Location &origin = inertial_nav.get_origin();
return alt_above_home_cm + (ahrs.get_home().alt - origin.alt);
}
// pv_alt_above_home - convert altitude above EKF origin to altitude above home
float Copter::pv_alt_above_home(float alt_above_origin_cm)
{
const struct Location &origin = inertial_nav.get_origin();
return alt_above_origin_cm + (origin.alt - ahrs.get_home().alt);
}
// pv_get_bearing_cd - return bearing in centi-degrees between two positions
float Copter::pv_get_bearing_cd(const Vector3f &origin, const Vector3f &destination)
{
float bearing = atan2f(destination.y-origin.y, destination.x-origin.x) * DEGX100;
if (bearing < 0) {
bearing += 36000;
}
return bearing;
}
// pv_get_horizontal_distance_cm - return distance between two positions in cm
float Copter::pv_get_horizontal_distance_cm(const Vector3f &origin, const Vector3f &destination)
{
return pythagorous2(destination.x-origin.x,destination.y-origin.y);
}

30
ArduSub/precision_landing.cpp Normal file
View File

@ -0,0 +1,30 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
//
// functions to support precision landing
//
#include "Copter.h"
#if PRECISION_LANDING == ENABLED
void Copter::init_precland()
{
copter.precland.init();
}
void Copter::update_precland()
{
float final_alt = current_loc.alt;
// use range finder altitude if it is valid
if (sonar_enabled && (sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) {
final_alt = sonar_alt;
}
copter.precland.update(final_alt);
// log output
Log_Write_Precland();
}
#endif

190
ArduSub/radio.cpp Normal file
View File

@ -0,0 +1,190 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
// Function that will read the radio data, limit servos and trigger a failsafe
// ----------------------------------------------------------------------------
void Copter::default_dead_zones()
{
channel_roll->set_default_dead_zone(30);
channel_pitch->set_default_dead_zone(30);
#if FRAME_CONFIG == HELI_FRAME
channel_throttle->set_default_dead_zone(10);
channel_yaw->set_default_dead_zone(15);
g.rc_8.set_default_dead_zone(10);
#else
channel_throttle->set_default_dead_zone(30);
channel_yaw->set_default_dead_zone(40);
#endif
g.rc_6.set_default_dead_zone(0);
}
void Copter::init_rc_in()
{
channel_roll = RC_Channel::rc_channel(rcmap.roll()-1);
channel_pitch = RC_Channel::rc_channel(rcmap.pitch()-1);
channel_throttle = RC_Channel::rc_channel(rcmap.throttle()-1);
channel_yaw = RC_Channel::rc_channel(rcmap.yaw()-1);
// set rc channel ranges
channel_roll->set_angle(ROLL_PITCH_INPUT_MAX);
channel_pitch->set_angle(ROLL_PITCH_INPUT_MAX);
channel_yaw->set_angle(4500);
channel_throttle->set_range(g.throttle_min, THR_MAX);
channel_roll->set_type(RC_CHANNEL_TYPE_ANGLE_RAW);
channel_pitch->set_type(RC_CHANNEL_TYPE_ANGLE_RAW);
channel_yaw->set_type(RC_CHANNEL_TYPE_ANGLE_RAW);
//set auxiliary servo ranges
g.rc_5.set_range(0,1000);
g.rc_6.set_range(0,1000);
g.rc_7.set_range(0,1000);
g.rc_8.set_range(0,1000);
// set default dead zones
default_dead_zones();
// initialise throttle_zero flag
ap.throttle_zero = true;
}
// init_rc_out -- initialise motors and check if pilot wants to perform ESC calibration
void Copter::init_rc_out()
{
motors.set_update_rate(g.rc_speed);
motors.set_frame_orientation(g.frame_orientation);
motors.Init(); // motor initialisation
#if FRAME_CONFIG != HELI_FRAME
motors.set_throttle_range(g.throttle_min, channel_throttle->radio_min, channel_throttle->radio_max);
motors.set_hover_throttle(g.throttle_mid);
#endif
for(uint8_t i = 0; i < 5; i++) {
delay(20);
read_radio();
}
// we want the input to be scaled correctly
channel_throttle->set_range_out(0,1000);
// check if we should enter esc calibration mode
esc_calibration_startup_check();
// enable output to motors
pre_arm_rc_checks();
if (ap.pre_arm_rc_check) {
enable_motor_output();
}
// refresh auxiliary channel to function map
RC_Channel_aux::update_aux_servo_function();
// setup correct scaling for ESCs like the UAVCAN PX4ESC which
// take a proportion of speed.
hal.rcout->set_esc_scaling(channel_throttle->radio_min, channel_throttle->radio_max);
}
// enable_motor_output() - enable and output lowest possible value to motors
void Copter::enable_motor_output()
{
// enable motors
motors.enable();
motors.output_min();
}
void Copter::read_radio()
{
static uint32_t last_update_ms = 0;
uint32_t tnow_ms = millis();
if (hal.rcin->new_input()) {
last_update_ms = tnow_ms;
ap.new_radio_frame = true;
RC_Channel::set_pwm_all();
set_throttle_and_failsafe(channel_throttle->radio_in);
set_throttle_zero_flag(channel_throttle->control_in);
// flag we must have an rc receiver attached
if (!failsafe.rc_override_active) {
ap.rc_receiver_present = true;
}
// update output on any aux channels, for manual passthru
RC_Channel_aux::output_ch_all();
}else{
uint32_t elapsed = tnow_ms - last_update_ms;
// turn on throttle failsafe if no update from the RC Radio for 500ms or 2000ms if we are using RC_OVERRIDE
if (((!failsafe.rc_override_active && (elapsed >= FS_RADIO_TIMEOUT_MS)) || (failsafe.rc_override_active && (elapsed >= FS_RADIO_RC_OVERRIDE_TIMEOUT_MS))) &&
(g.failsafe_throttle && (ap.rc_receiver_present||motors.armed()) && !failsafe.radio)) {
Log_Write_Error(ERROR_SUBSYSTEM_RADIO, ERROR_CODE_RADIO_LATE_FRAME);
set_failsafe_radio(true);
}
}
}
#define FS_COUNTER 3 // radio failsafe kicks in after 3 consecutive throttle values below failsafe_throttle_value
void Copter::set_throttle_and_failsafe(uint16_t throttle_pwm)
{
// if failsafe not enabled pass through throttle and exit
if(g.failsafe_throttle == FS_THR_DISABLED) {
channel_throttle->set_pwm(throttle_pwm);
return;
}
//check for low throttle value
if (throttle_pwm < (uint16_t)g.failsafe_throttle_value) {
// if we are already in failsafe or motors not armed pass through throttle and exit
if (failsafe.radio || !(ap.rc_receiver_present || motors.armed())) {
channel_throttle->set_pwm(throttle_pwm);
return;
}
// check for 3 low throttle values
// Note: we do not pass through the low throttle until 3 low throttle values are recieved
failsafe.radio_counter++;
if( failsafe.radio_counter >= FS_COUNTER ) {
failsafe.radio_counter = FS_COUNTER; // check to ensure we don't overflow the counter
set_failsafe_radio(true);
channel_throttle->set_pwm(throttle_pwm); // pass through failsafe throttle
}
}else{
// we have a good throttle so reduce failsafe counter
failsafe.radio_counter--;
if( failsafe.radio_counter <= 0 ) {
failsafe.radio_counter = 0; // check to ensure we don't underflow the counter
// disengage failsafe after three (nearly) consecutive valid throttle values
if (failsafe.radio) {
set_failsafe_radio(false);
}
}
// pass through throttle
channel_throttle->set_pwm(throttle_pwm);
}
}
#define THROTTLE_ZERO_DEBOUNCE_TIME_MS 400
// set_throttle_zero_flag - set throttle_zero flag from debounced throttle control
// throttle_zero is used to determine if the pilot intends to shut down the motors
// Basically, this signals when we are not flying. We are either on the ground
// or the pilot has shut down the copter in the air and it is free-falling
void Copter::set_throttle_zero_flag(int16_t throttle_control)
{
static uint32_t last_nonzero_throttle_ms = 0;
uint32_t tnow_ms = millis();
// if not using throttle interlock and non-zero throttle and not E-stopped,
// or using motor interlock and it's enabled, then motors are running,
// and we are flying. Immediately set as non-zero
if ((!ap.using_interlock && (throttle_control > 0) && !ap.motor_emergency_stop) || (ap.using_interlock && motors.get_interlock())) {
last_nonzero_throttle_ms = tnow_ms;
ap.throttle_zero = false;
} else if (tnow_ms - last_nonzero_throttle_ms > THROTTLE_ZERO_DEBOUNCE_TIME_MS) {
ap.throttle_zero = true;
}
}

4
ArduSub/readme.txt Normal file
View File

@ -0,0 +1,4 @@
ArduCopter README
User Manual: http://copter.ardupilot.com/
Developer Manual: http://dev.ardupilot.com/

203
ArduSub/sensors.cpp Normal file
View File

@ -0,0 +1,203 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
void Copter::init_barometer(bool full_calibration)
{
gcs_send_text(MAV_SEVERITY_INFO, "Calibrating barometer");
if (full_calibration) {
barometer.calibrate();
}else{
barometer.update_calibration();
}
gcs_send_text(MAV_SEVERITY_INFO, "Barometer calibration complete");
}
// return barometric altitude in centimeters
void Copter::read_barometer(void)
{
barometer.update();
if (should_log(MASK_LOG_IMU)) {
Log_Write_Baro();
}
baro_alt = barometer.get_altitude() * 100.0f;
baro_climbrate = barometer.get_climb_rate() * 100.0f;
motors.set_air_density_ratio(barometer.get_air_density_ratio());
}
#if CONFIG_SONAR == ENABLED
void Copter::init_sonar(void)
{
sonar.init();
}
#endif
// return sonar altitude in centimeters
int16_t Copter::read_sonar(void)
{
#if CONFIG_SONAR == ENABLED
sonar.update();
// exit immediately if sonar is disabled
if (sonar.status() != RangeFinder::RangeFinder_Good) {
sonar_alt_health = 0;
return 0;
}
int16_t temp_alt = sonar.distance_cm();
if (temp_alt >= sonar.min_distance_cm() &&
temp_alt <= sonar.max_distance_cm() * SONAR_RELIABLE_DISTANCE_PCT) {
if ( sonar_alt_health < SONAR_ALT_HEALTH_MAX ) {
sonar_alt_health++;
}
}else{
sonar_alt_health = 0;
}
#if SONAR_TILT_CORRECTION == 1
// correct alt for angle of the sonar
float temp = ahrs.cos_pitch() * ahrs.cos_roll();
temp = MAX(temp, 0.707f);
temp_alt = (float)temp_alt * temp;
#endif
return temp_alt;
#else
return 0;
#endif
}
/*
update RPM sensors
*/
void Copter::rpm_update(void)
{
rpm_sensor.update();
if (rpm_sensor.enabled(0) || rpm_sensor.enabled(1)) {
if (should_log(MASK_LOG_RCIN)) {
DataFlash.Log_Write_RPM(rpm_sensor);
}
}
}
// initialise compass
void Copter::init_compass()
{
if (!compass.init() || !compass.read()) {
// make sure we don't pass a broken compass to DCM
cliSerial->println("COMPASS INIT ERROR");
Log_Write_Error(ERROR_SUBSYSTEM_COMPASS,ERROR_CODE_FAILED_TO_INITIALISE);
return;
}
ahrs.set_compass(&compass);
}
// initialise optical flow sensor
void Copter::init_optflow()
{
#if OPTFLOW == ENABLED
// exit immediately if not enabled
if (!optflow.enabled()) {
return;
}
// initialise optical flow sensor
optflow.init();
#endif // OPTFLOW == ENABLED
}
// called at 200hz
#if OPTFLOW == ENABLED
void Copter::update_optical_flow(void)
{
static uint32_t last_of_update = 0;
// exit immediately if not enabled
if (!optflow.enabled()) {
return;
}
// read from sensor
optflow.update();
// write to log and send to EKF if new data has arrived
if (optflow.last_update() != last_of_update) {
last_of_update = optflow.last_update();
uint8_t flowQuality = optflow.quality();
Vector2f flowRate = optflow.flowRate();
Vector2f bodyRate = optflow.bodyRate();
ahrs.writeOptFlowMeas(flowQuality, flowRate, bodyRate, last_of_update);
if (g.log_bitmask & MASK_LOG_OPTFLOW) {
Log_Write_Optflow();
}
}
}
#endif // OPTFLOW == ENABLED
// read_battery - check battery voltage and current and invoke failsafe if necessary
// called at 10hz
void Copter::read_battery(void)
{
battery.read();
// update compass with current value
if (battery.has_current()) {
compass.set_current(battery.current_amps());
}
// update motors with voltage and current
if (battery.get_type() != AP_BattMonitor::BattMonitor_TYPE_NONE) {
motors.set_voltage(battery.voltage());
}
if (battery.has_current()) {
motors.set_current(battery.current_amps());
}
// check for low voltage or current if the low voltage check hasn't already been triggered
// we only check when we're not powered by USB to avoid false alarms during bench tests
if (!ap.usb_connected && !failsafe.battery && battery.exhausted(g.fs_batt_voltage, g.fs_batt_mah)) {
failsafe_battery_event();
}
// log battery info to the dataflash
if (should_log(MASK_LOG_CURRENT)) {
Log_Write_Current();
}
}
// read the receiver RSSI as an 8 bit number for MAVLink
// RC_CHANNELS_SCALED message
void Copter::read_receiver_rssi(void)
{
receiver_rssi = rssi.read_receiver_rssi_uint8();
}
void Copter::compass_cal_update()
{
if (!hal.util->get_soft_armed()) {
compass.compass_cal_update();
}
}
void Copter::accel_cal_update()
{
if (hal.util->get_soft_armed()) {
return;
}
ins.acal_update();
// check if new trim values, and set them
float trim_roll, trim_pitch;
if(ins.get_new_trim(trim_roll, trim_pitch)) {
ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
}
}
#if EPM_ENABLED == ENABLED
// epm update - moves epm pwm output back to neutral after grab or release is completed
void Copter::epm_update()
{
epm.update();
}
#endif

508
ArduSub/setup.cpp Normal file
View File

@ -0,0 +1,508 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#if CLI_ENABLED == ENABLED
#define PWM_CALIB_MIN 1000
#define PWM_CALIB_MAX 2000
#define PWM_HIGHEST_MAX 2200
#define PWM_LOWEST_MAX 1200
#define PWM_HIGHEST_MIN 1800
#define PWM_LOWEST_MIN 800
// Command/function table for the setup menu
static const struct Menu::command setup_menu_commands[] = {
{"reset", MENU_FUNC(setup_factory)},
{"show", MENU_FUNC(setup_show)},
{"set", MENU_FUNC(setup_set)},
{"esc_calib", MENU_FUNC(esc_calib)},
};
// Create the setup menu object.
MENU(setup_menu, "setup", setup_menu_commands);
// Called from the top-level menu to run the setup menu.
int8_t Copter::setup_mode(uint8_t argc, const Menu::arg *argv)
{
// Give the user some guidance
cliSerial->printf("Setup Mode\n\n\n");
// Run the setup menu. When the menu exits, we will return to the main menu.
setup_menu.run();
return 0;
}
// Initialise the EEPROM to 'factory' settings (mostly defined in APM_Config.h or via defaults).
// Called by the setup menu 'factoryreset' command.
int8_t Copter::setup_factory(uint8_t argc, const Menu::arg *argv)
{
int16_t c;
cliSerial->printf("\n'Y' = factory reset, any other key to abort:\n");
do {
c = cliSerial->read();
} while (-1 == c);
if (('y' != c) && ('Y' != c))
return(-1);
AP_Param::erase_all();
cliSerial->printf("\nReboot board");
delay(1000);
for (;; ) {
}
// note, cannot actually return here
return(0);
}
//Set a parameter to a specified value. It will cast the value to the current type of the
//parameter and make sure it fits in case of INT8 and INT16
int8_t Copter::setup_set(uint8_t argc, const Menu::arg *argv)
{
int8_t value_int8;
int16_t value_int16;
AP_Param *param;
enum ap_var_type p_type;
if(argc!=3)
{
cliSerial->printf("Invalid command. Usage: set <name> <value>\n");
return 0;
}
param = AP_Param::find(argv[1].str, &p_type);
if(!param)
{
cliSerial->printf("Param not found: %s\n", argv[1].str);
return 0;
}
switch(p_type)
{
case AP_PARAM_INT8:
value_int8 = (int8_t)(argv[2].i);
if(argv[2].i!=value_int8)
{
cliSerial->printf("Value out of range for type INT8\n");
return 0;
}
((AP_Int8*)param)->set_and_save(value_int8);
break;
case AP_PARAM_INT16:
value_int16 = (int16_t)(argv[2].i);
if(argv[2].i!=value_int16)
{
cliSerial->printf("Value out of range for type INT16\n");
return 0;
}
((AP_Int16*)param)->set_and_save(value_int16);
break;
//int32 and float don't need bounds checking, just use the value provoded by Menu::arg
case AP_PARAM_INT32:
((AP_Int32*)param)->set_and_save(argv[2].i);
break;
case AP_PARAM_FLOAT:
((AP_Float*)param)->set_and_save(argv[2].f);
break;
default:
cliSerial->printf("Cannot set parameter of type %d.\n", p_type);
break;
}
return 0;
}
// Print the current configuration.
// Called by the setup menu 'show' command.
int8_t Copter::setup_show(uint8_t argc, const Menu::arg *argv)
{
AP_Param *param;
ap_var_type type;
//If a parameter name is given as an argument to show, print only that parameter
if(argc>=2)
{
param=AP_Param::find(argv[1].str, &type);
if(!param)
{
cliSerial->printf("Parameter not found: '%s'\n", argv[1].str);
return 0;
}
AP_Param::show(param, argv[1].str, type, cliSerial);
return 0;
}
// clear the area
print_blanks(8);
report_version();
report_radio();
report_frame();
report_batt_monitor();
report_flight_modes();
report_ins();
report_compass();
report_optflow();
AP_Param::show_all(cliSerial);
return(0);
}
int8_t Copter::esc_calib(uint8_t argc,const Menu::arg *argv)
{
char c;
unsigned max_channels = 0;
uint32_t set_mask = 0;
uint16_t pwm_high = PWM_CALIB_MAX;
uint16_t pwm_low = PWM_CALIB_MIN;
if (argc < 2) {
cliSerial->printf("Pls provide Channel Mask\n"
"\tusage: esc_calib 1010 - enables calibration for 2nd and 4th Motor\n");
return(0);
}
set_mask = strtol (argv[1].str, NULL, 2);
if (set_mask == 0)
cliSerial->printf("no channels chosen");
//cliSerial->printf("\n%d\n",set_mask);
set_mask<<=1;
/* wait 50 ms */
hal.scheduler->delay(50);
cliSerial->printf("\nATTENTION, please remove or fix propellers before starting calibration!\n"
"\n"
"Make sure\n"
"\t - that the ESCs are not powered\n"
"\t - that safety is off\n"
"\t - that the controllers are stopped\n"
"\n"
"Do you want to start calibration now: y or n?\n");
/* wait for user input */
while (1) {
c= cliSerial->read();
if (c == 'y' || c == 'Y') {
break;
} else if (c == 0x03 || c == 0x63 || c == 'q') {
cliSerial->printf("ESC calibration exited\n");
return(0);
} else if (c == 'n' || c == 'N') {
cliSerial->printf("ESC calibration aborted\n");
return(0);
}
/* rate limit to ~ 20 Hz */
hal.scheduler->delay(50);
}
/* get number of channels available on the device */
max_channels = AP_MOTORS_MAX_NUM_MOTORS;
/* tell IO/FMU that the system is armed (it will output values if safety is off) */
motors.armed(true);
cliSerial->printf("Outputs armed\n");
/* wait for user confirmation */
cliSerial->printf("\nHigh PWM set: %d\n"
"\n"
"Connect battery now and hit c+ENTER after the ESCs confirm the first calibration step\n"
"\n", pwm_high);
while (1) {
/* set max PWM */
for (unsigned i = 0; i < max_channels; i++) {
if (set_mask & 1<<i) {
motors.output_test(i, pwm_high);
}
}
c = cliSerial->read();
if (c == 'c') {
break;
} else if (c == 0x03 || c == 0x63 || c == 'q') {
cliSerial->printf("ESC calibration exited\n");
return(0);
}
/* rate limit to ~ 20 Hz */
hal.scheduler->delay(50);
}
cliSerial->printf("Low PWM set: %d\n"
"\n"
"Hit c+Enter when finished\n"
"\n", pwm_low);
while (1) {
/* set disarmed PWM */
for (unsigned i = 0; i < max_channels; i++) {
if (set_mask & 1<<i) {
motors.output_test(i, pwm_low);
}
}
c = cliSerial->read();
if (c == 'c') {
break;
} else if (c == 0x03 || c == 0x63 || c == 'q') {
cliSerial->printf("ESC calibration exited\n");
return(0);
}
/* rate limit to ~ 20 Hz */
hal.scheduler->delay(50);
}
/* disarm */
motors.armed(false);
cliSerial->printf("Outputs disarmed\n");
cliSerial->printf("ESC calibration finished\n");
return(0);
}
/***************************************************************************/
// CLI reports
/***************************************************************************/
void Copter::report_batt_monitor()
{
cliSerial->printf("\nBatt Mon:\n");
print_divider();
if (battery.num_instances() == 0) {
print_enabled(false);
} else if (!battery.has_current()) {
cliSerial->printf("volts");
} else {
cliSerial->printf("volts and cur");
}
print_blanks(2);
}
void Copter::report_frame()
{
cliSerial->printf("Frame\n");
print_divider();
#if FRAME_CONFIG == QUAD_FRAME
cliSerial->printf("Quad frame\n");
#elif FRAME_CONFIG == TRI_FRAME
cliSerial->printf("TRI frame\n");
#elif FRAME_CONFIG == HEXA_FRAME
cliSerial->printf("Hexa frame\n");
#elif FRAME_CONFIG == Y6_FRAME
cliSerial->printf("Y6 frame\n");
#elif FRAME_CONFIG == OCTA_FRAME
cliSerial->printf("Octa frame\n");
#elif FRAME_CONFIG == HELI_FRAME
cliSerial->printf("Heli frame\n");
#endif
print_blanks(2);
}
void Copter::report_radio()
{
cliSerial->printf("Radio\n");
print_divider();
// radio
print_radio_values();
print_blanks(2);
}
void Copter::report_ins()
{
cliSerial->printf("INS\n");
print_divider();
print_gyro_offsets();
print_accel_offsets_and_scaling();
print_blanks(2);
}
void Copter::report_flight_modes()
{
cliSerial->printf("Flight modes\n");
print_divider();
for(int16_t i = 0; i < 6; i++ ) {
print_switch(i, flight_modes[i], BIT_IS_SET(g.simple_modes, i));
}
print_blanks(2);
}
void Copter::report_optflow()
{
#if OPTFLOW == ENABLED
cliSerial->printf("OptFlow\n");
print_divider();
print_enabled(optflow.enabled());
print_blanks(2);
#endif // OPTFLOW == ENABLED
}
/***************************************************************************/
// CLI utilities
/***************************************************************************/
void Copter::print_radio_values()
{
cliSerial->printf("CH1: %d | %d\n", (int)channel_roll->radio_min, (int)channel_roll->radio_max);
cliSerial->printf("CH2: %d | %d\n", (int)channel_pitch->radio_min, (int)channel_pitch->radio_max);
cliSerial->printf("CH3: %d | %d\n", (int)channel_throttle->radio_min, (int)channel_throttle->radio_max);
cliSerial->printf("CH4: %d | %d\n", (int)channel_yaw->radio_min, (int)channel_yaw->radio_max);
cliSerial->printf("CH5: %d | %d\n", (int)g.rc_5.radio_min, (int)g.rc_5.radio_max);
cliSerial->printf("CH6: %d | %d\n", (int)g.rc_6.radio_min, (int)g.rc_6.radio_max);
cliSerial->printf("CH7: %d | %d\n", (int)g.rc_7.radio_min, (int)g.rc_7.radio_max);
cliSerial->printf("CH8: %d | %d\n", (int)g.rc_8.radio_min, (int)g.rc_8.radio_max);
}
void Copter::print_switch(uint8_t p, uint8_t m, bool b)
{
cliSerial->printf("Pos %d:\t",p);
print_flight_mode(cliSerial, m);
cliSerial->printf(",\t\tSimple: ");
if(b)
cliSerial->printf("ON\n");
else
cliSerial->printf("OFF\n");
}
void Copter::print_accel_offsets_and_scaling(void)
{
const Vector3f &accel_offsets = ins.get_accel_offsets();
const Vector3f &accel_scale = ins.get_accel_scale();
cliSerial->printf("A_off: %4.2f, %4.2f, %4.2f\nA_scale: %4.2f, %4.2f, %4.2f\n",
(double)accel_offsets.x, // Pitch
(double)accel_offsets.y, // Roll
(double)accel_offsets.z, // YAW
(double)accel_scale.x, // Pitch
(double)accel_scale.y, // Roll
(double)accel_scale.z); // YAW
}
void Copter::print_gyro_offsets(void)
{
const Vector3f &gyro_offsets = ins.get_gyro_offsets();
cliSerial->printf("G_off: %4.2f, %4.2f, %4.2f\n",
(double)gyro_offsets.x,
(double)gyro_offsets.y,
(double)gyro_offsets.z);
}
#endif // CLI_ENABLED
// report_compass - displays compass information. Also called by compassmot.pde
void Copter::report_compass()
{
cliSerial->printf("Compass\n");
print_divider();
print_enabled(g.compass_enabled);
// mag declination
cliSerial->printf("Mag Dec: %4.4f\n",
(double)degrees(compass.get_declination()));
// mag offsets
Vector3f offsets;
for (uint8_t i=0; i<compass.get_count(); i++) {
offsets = compass.get_offsets(i);
// mag offsets
cliSerial->printf("Mag%d off: %4.4f, %4.4f, %4.4f\n",
(int)i,
(double)offsets.x,
(double)offsets.y,
(double)offsets.z);
}
// motor compensation
cliSerial->print("Motor Comp: ");
if( compass.get_motor_compensation_type() == AP_COMPASS_MOT_COMP_DISABLED ) {
cliSerial->print("Off\n");
}else{
if( compass.get_motor_compensation_type() == AP_COMPASS_MOT_COMP_THROTTLE ) {
cliSerial->print("Throttle");
}
if( compass.get_motor_compensation_type() == AP_COMPASS_MOT_COMP_CURRENT ) {
cliSerial->print("Current");
}
Vector3f motor_compensation;
for (uint8_t i=0; i<compass.get_count(); i++) {
motor_compensation = compass.get_motor_compensation(i);
cliSerial->printf("\nComMot%d: %4.2f, %4.2f, %4.2f\n",
(int)i,
(double)motor_compensation.x,
(double)motor_compensation.y,
(double)motor_compensation.z);
}
}
print_blanks(1);
}
void Copter::print_blanks(int16_t num)
{
while(num > 0) {
num--;
cliSerial->println("");
}
}
void Copter::print_divider(void)
{
for (int i = 0; i < 40; i++) {
cliSerial->print("-");
}
cliSerial->println();
}
void Copter::print_enabled(bool b)
{
if(b)
cliSerial->print("en");
else
cliSerial->print("dis");
cliSerial->print("abled\n");
}
void Copter::report_version()
{
cliSerial->printf("FW Ver: %d\n",(int)g.k_format_version);
print_divider();
print_blanks(2);
}

624
ArduSub/switches.cpp Normal file
View File

@ -0,0 +1,624 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#define CONTROL_SWITCH_DEBOUNCE_TIME_MS 200
//Documentation of Aux Switch Flags:
static union {
struct {
uint8_t CH6_flag : 2; // 0, 1 // ch6 aux switch : 0 is low or false, 1 is center or true, 2 is high
uint8_t CH7_flag : 2; // 2, 3 // ch7 aux switch : 0 is low or false, 1 is center or true, 2 is high
uint8_t CH8_flag : 2; // 4, 5 // ch8 aux switch : 0 is low or false, 1 is center or true, 2 is high
uint8_t CH9_flag : 2; // 6, 7 // ch9 aux switch : 0 is low or false, 1 is center or true, 2 is high
uint8_t CH10_flag : 2; // 8, 9 // ch10 aux switch : 0 is low or false, 1 is center or true, 2 is high
uint8_t CH11_flag : 2; // 10,11 // ch11 aux switch : 0 is low or false, 1 is center or true, 2 is high
uint8_t CH12_flag : 2; // 12,13 // ch12 aux switch : 0 is low or false, 1 is center or true, 2 is high
};
uint32_t value;
} aux_con;
void Copter::read_control_switch()
{
uint32_t tnow_ms = millis();
// calculate position of flight mode switch
int8_t switch_position;
if (g.rc_5.radio_in < 1231) switch_position = 0;
else if (g.rc_5.radio_in < 1361) switch_position = 1;
else if (g.rc_5.radio_in < 1491) switch_position = 2;
else if (g.rc_5.radio_in < 1621) switch_position = 3;
else if (g.rc_5.radio_in < 1750) switch_position = 4;
else switch_position = 5;
// store time that switch last moved
if(control_switch_state.last_switch_position != switch_position) {
control_switch_state.last_edge_time_ms = tnow_ms;
}
// debounce switch
bool control_switch_changed = control_switch_state.debounced_switch_position != switch_position;
bool sufficient_time_elapsed = tnow_ms - control_switch_state.last_edge_time_ms > CONTROL_SWITCH_DEBOUNCE_TIME_MS;
bool failsafe_disengaged = !failsafe.radio && failsafe.radio_counter == 0;
if (control_switch_changed && sufficient_time_elapsed && failsafe_disengaged) {
// set flight mode and simple mode setting
if (set_mode(flight_modes[switch_position])) {
// play a tone
if (control_switch_state.debounced_switch_position != -1) {
// alert user to mode change failure (except if autopilot is just starting up)
if (ap.initialised) {
AP_Notify::events.user_mode_change = 1;
}
}
if(!check_if_auxsw_mode_used(AUXSW_SIMPLE_MODE) && !check_if_auxsw_mode_used(AUXSW_SUPERSIMPLE_MODE)) {
// if none of the Aux Switches are set to Simple or Super Simple Mode then
// set Simple Mode using stored parameters from EEPROM
if (BIT_IS_SET(g.super_simple, switch_position)) {
set_simple_mode(2);
}else{
set_simple_mode(BIT_IS_SET(g.simple_modes, switch_position));
}
}
} else if (control_switch_state.last_switch_position != -1) {
// alert user to mode change failure
AP_Notify::events.user_mode_change_failed = 1;
}
// set the debounced switch position
control_switch_state.debounced_switch_position = switch_position;
}
control_switch_state.last_switch_position = switch_position;
}
// check_if_auxsw_mode_used - Check to see if any of the Aux Switches are set to a given mode.
bool Copter::check_if_auxsw_mode_used(uint8_t auxsw_mode_check)
{
bool ret = g.ch7_option == auxsw_mode_check || g.ch8_option == auxsw_mode_check || g.ch9_option == auxsw_mode_check
|| g.ch10_option == auxsw_mode_check || g.ch11_option == auxsw_mode_check || g.ch12_option == auxsw_mode_check;
return ret;
}
// check_duplicate_auxsw - Check to see if any Aux Switch Functions are duplicated
bool Copter::check_duplicate_auxsw(void)
{
bool ret = ((g.ch7_option != AUXSW_DO_NOTHING) && (g.ch7_option == g.ch8_option ||
g.ch7_option == g.ch9_option || g.ch7_option == g.ch10_option ||
g.ch7_option == g.ch11_option || g.ch7_option == g.ch12_option));
ret = ret || ((g.ch8_option != AUXSW_DO_NOTHING) && (g.ch8_option == g.ch9_option ||
g.ch8_option == g.ch10_option || g.ch8_option == g.ch11_option ||
g.ch8_option == g.ch12_option));
ret = ret || ((g.ch9_option != AUXSW_DO_NOTHING) && (g.ch9_option == g.ch10_option ||
g.ch9_option == g.ch11_option || g.ch9_option == g.ch12_option));
ret = ret || ((g.ch10_option != AUXSW_DO_NOTHING) && (g.ch10_option == g.ch11_option ||
g.ch10_option == g.ch12_option));
ret = ret || ((g.ch11_option != AUXSW_DO_NOTHING) && (g.ch11_option == g.ch12_option));
return ret;
}
void Copter::reset_control_switch()
{
control_switch_state.last_switch_position = control_switch_state.debounced_switch_position = -1;
read_control_switch();
}
// read_3pos_switch
uint8_t Copter::read_3pos_switch(int16_t radio_in)
{
if (radio_in < AUX_SWITCH_PWM_TRIGGER_LOW) return AUX_SWITCH_LOW; // switch is in low position
if (radio_in > AUX_SWITCH_PWM_TRIGGER_HIGH) return AUX_SWITCH_HIGH; // switch is in high position
return AUX_SWITCH_MIDDLE; // switch is in middle position
}
// read_aux_switches - checks aux switch positions and invokes configured actions
void Copter::read_aux_switches()
{
uint8_t switch_position;
// exit immediately during radio failsafe
if (failsafe.radio || failsafe.radio_counter != 0) {
return;
}
// check if ch7 switch has changed position
switch_position = read_3pos_switch(g.rc_7.radio_in);
if (aux_con.CH7_flag != switch_position) {
// set the CH7 flag
aux_con.CH7_flag = switch_position;
// invoke the appropriate function
do_aux_switch_function(g.ch7_option, aux_con.CH7_flag);
}
// check if Ch8 switch has changed position
switch_position = read_3pos_switch(g.rc_8.radio_in);
if (aux_con.CH8_flag != switch_position) {
// set the CH8 flag
aux_con.CH8_flag = switch_position;
// invoke the appropriate function
do_aux_switch_function(g.ch8_option, aux_con.CH8_flag);
}
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
// check if Ch9 switch has changed position
switch_position = read_3pos_switch(g.rc_9.radio_in);
if (aux_con.CH9_flag != switch_position) {
// set the CH9 flag
aux_con.CH9_flag = switch_position;
// invoke the appropriate function
do_aux_switch_function(g.ch9_option, aux_con.CH9_flag);
}
#endif
// check if Ch10 switch has changed position
switch_position = read_3pos_switch(g.rc_10.radio_in);
if (aux_con.CH10_flag != switch_position) {
// set the CH10 flag
aux_con.CH10_flag = switch_position;
// invoke the appropriate function
do_aux_switch_function(g.ch10_option, aux_con.CH10_flag);
}
// check if Ch11 switch has changed position
switch_position = read_3pos_switch(g.rc_11.radio_in);
if (aux_con.CH11_flag != switch_position) {
// set the CH11 flag
aux_con.CH11_flag = switch_position;
// invoke the appropriate function
do_aux_switch_function(g.ch11_option, aux_con.CH11_flag);
}
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
// check if Ch12 switch has changed position
switch_position = read_3pos_switch(g.rc_12.radio_in);
if (aux_con.CH12_flag != switch_position) {
// set the CH12 flag
aux_con.CH12_flag = switch_position;
// invoke the appropriate function
do_aux_switch_function(g.ch12_option, aux_con.CH12_flag);
}
#endif
}
// init_aux_switches - invoke configured actions at start-up for aux function where it is safe to do so
void Copter::init_aux_switches()
{
// set the CH7 ~ CH12 flags
aux_con.CH7_flag = read_3pos_switch(g.rc_7.radio_in);
aux_con.CH8_flag = read_3pos_switch(g.rc_8.radio_in);
aux_con.CH10_flag = read_3pos_switch(g.rc_10.radio_in);
aux_con.CH11_flag = read_3pos_switch(g.rc_11.radio_in);
// ch9, ch12 only supported on some boards
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
aux_con.CH9_flag = read_3pos_switch(g.rc_9.radio_in);
aux_con.CH12_flag = read_3pos_switch(g.rc_12.radio_in);
#endif
// initialise functions assigned to switches
init_aux_switch_function(g.ch7_option, aux_con.CH7_flag);
init_aux_switch_function(g.ch8_option, aux_con.CH8_flag);
init_aux_switch_function(g.ch10_option, aux_con.CH10_flag);
init_aux_switch_function(g.ch11_option, aux_con.CH11_flag);
// ch9, ch12 only supported on some boards
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
init_aux_switch_function(g.ch9_option, aux_con.CH9_flag);
init_aux_switch_function(g.ch12_option, aux_con.CH12_flag);
#endif
}
// init_aux_switch_function - initialize aux functions
void Copter::init_aux_switch_function(int8_t ch_option, uint8_t ch_flag)
{
// init channel options
switch(ch_option) {
case AUXSW_SIMPLE_MODE:
case AUXSW_SONAR:
case AUXSW_FENCE:
case AUXSW_RESETTOARMEDYAW:
case AUXSW_SUPERSIMPLE_MODE:
case AUXSW_ACRO_TRAINER:
case AUXSW_EPM:
case AUXSW_SPRAYER:
case AUXSW_PARACHUTE_ENABLE:
case AUXSW_PARACHUTE_3POS: // we trust the vehicle will be disarmed so even if switch is in release position the chute will not release
case AUXSW_RETRACT_MOUNT:
case AUXSW_MISSION_RESET:
case AUXSW_ATTCON_FEEDFWD:
case AUXSW_ATTCON_ACCEL_LIM:
case AUXSW_RELAY:
case AUXSW_LANDING_GEAR:
case AUXSW_MOTOR_ESTOP:
case AUXSW_MOTOR_INTERLOCK:
do_aux_switch_function(ch_option, ch_flag);
break;
}
}
// do_aux_switch_function - implement the function invoked by the ch7 or ch8 switch
void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
{
switch(ch_function) {
case AUXSW_FLIP:
// flip if switch is on, positive throttle and we're actually flying
if(ch_flag == AUX_SWITCH_HIGH) {
set_mode(FLIP);
}
break;
case AUXSW_SIMPLE_MODE:
// low = simple mode off, middle or high position turns simple mode on
set_simple_mode(ch_flag == AUX_SWITCH_HIGH || ch_flag == AUX_SWITCH_MIDDLE);
break;
case AUXSW_SUPERSIMPLE_MODE:
// low = simple mode off, middle = simple mode, high = super simple mode
set_simple_mode(ch_flag);
break;
case AUXSW_RTL:
if (ch_flag == AUX_SWITCH_HIGH) {
// engage RTL (if not possible we remain in current flight mode)
set_mode(RTL);
}else{
// return to flight mode switch's flight mode if we are currently in RTL
if (control_mode == RTL) {
reset_control_switch();
}
}
break;
case AUXSW_SAVE_TRIM:
if ((ch_flag == AUX_SWITCH_HIGH) && (control_mode <= ACRO) && (channel_throttle->control_in == 0)) {
save_trim();
}
break;
case AUXSW_SAVE_WP:
// save waypoint when switch is brought high
if (ch_flag == AUX_SWITCH_HIGH) {
// do not allow saving new waypoints while we're in auto or disarmed
if(control_mode == AUTO || !motors.armed()) {
return;
}
// do not allow saving the first waypoint with zero throttle
if((mission.num_commands() == 0) && (channel_throttle->control_in == 0)){
return;
}
// create new mission command
AP_Mission::Mission_Command cmd = {};
// if the mission is empty save a takeoff command
if(mission.num_commands() == 0) {
// set our location ID to 16, MAV_CMD_NAV_WAYPOINT
cmd.id = MAV_CMD_NAV_TAKEOFF;
cmd.content.location.options = 0;
cmd.p1 = 0;
cmd.content.location.lat = 0;
cmd.content.location.lng = 0;
cmd.content.location.alt = MAX(current_loc.alt,100);
// use the current altitude for the target alt for takeoff.
// only altitude will matter to the AP mission script for takeoff.
if(mission.add_cmd(cmd)) {
// log event
Log_Write_Event(DATA_SAVEWP_ADD_WP);
}
}
// set new waypoint to current location
cmd.content.location = current_loc;
// if throttle is above zero, create waypoint command
if(channel_throttle->control_in > 0) {
cmd.id = MAV_CMD_NAV_WAYPOINT;
}else{
// with zero throttle, create LAND command
cmd.id = MAV_CMD_NAV_LAND;
}
// save command
if(mission.add_cmd(cmd)) {
// log event
Log_Write_Event(DATA_SAVEWP_ADD_WP);
}
}
break;
#if CAMERA == ENABLED
case AUXSW_CAMERA_TRIGGER:
if (ch_flag == AUX_SWITCH_HIGH) {
do_take_picture();
}
break;
#endif
case AUXSW_SONAR:
// enable or disable the sonar
#if CONFIG_SONAR == ENABLED
if (ch_flag == AUX_SWITCH_HIGH) {
sonar_enabled = true;
}else{
sonar_enabled = false;
}
#endif
break;
#if AC_FENCE == ENABLED
case AUXSW_FENCE:
// enable or disable the fence
if (ch_flag == AUX_SWITCH_HIGH) {
fence.enable(true);
Log_Write_Event(DATA_FENCE_ENABLE);
}else{
fence.enable(false);
Log_Write_Event(DATA_FENCE_DISABLE);
}
break;
#endif
// To-Do: add back support for this feature
//case AUXSW_RESETTOARMEDYAW:
// if (ch_flag == AUX_SWITCH_HIGH) {
// set_yaw_mode(YAW_RESETTOARMEDYAW);
// }else{
// set_yaw_mode(YAW_HOLD);
// }
// break;
case AUXSW_ACRO_TRAINER:
switch(ch_flag) {
case AUX_SWITCH_LOW:
g.acro_trainer = ACRO_TRAINER_DISABLED;
Log_Write_Event(DATA_ACRO_TRAINER_DISABLED);
break;
case AUX_SWITCH_MIDDLE:
g.acro_trainer = ACRO_TRAINER_LEVELING;
Log_Write_Event(DATA_ACRO_TRAINER_LEVELING);
break;
case AUX_SWITCH_HIGH:
g.acro_trainer = ACRO_TRAINER_LIMITED;
Log_Write_Event(DATA_ACRO_TRAINER_LIMITED);
break;
}
break;
#if EPM_ENABLED == ENABLED
case AUXSW_EPM:
switch(ch_flag) {
case AUX_SWITCH_LOW:
epm.release();
Log_Write_Event(DATA_EPM_RELEASE);
break;
case AUX_SWITCH_HIGH:
epm.grab();
Log_Write_Event(DATA_EPM_GRAB);
break;
}
break;
#endif
#if SPRAYER == ENABLED
case AUXSW_SPRAYER:
sprayer.enable(ch_flag == AUX_SWITCH_HIGH);
// if we are disarmed the pilot must want to test the pump
sprayer.test_pump((ch_flag == AUX_SWITCH_HIGH) && !motors.armed());
break;
#endif
case AUXSW_AUTO:
if (ch_flag == AUX_SWITCH_HIGH) {
set_mode(AUTO);
}else{
// return to flight mode switch's flight mode if we are currently in AUTO
if (control_mode == AUTO) {
reset_control_switch();
}
}
break;
#if AUTOTUNE_ENABLED == ENABLED
case AUXSW_AUTOTUNE:
// turn on auto tuner
switch(ch_flag) {
case AUX_SWITCH_LOW:
case AUX_SWITCH_MIDDLE:
// restore flight mode based on flight mode switch position
if (control_mode == AUTOTUNE) {
reset_control_switch();
}
break;
case AUX_SWITCH_HIGH:
// start an autotuning session
set_mode(AUTOTUNE);
break;
}
break;
#endif
case AUXSW_LAND:
if (ch_flag == AUX_SWITCH_HIGH) {
set_mode(LAND);
}else{
// return to flight mode switch's flight mode if we are currently in LAND
if (control_mode == LAND) {
reset_control_switch();
}
}
break;
#if PARACHUTE == ENABLED
case AUXSW_PARACHUTE_ENABLE:
// Parachute enable/disable
parachute.enabled(ch_flag == AUX_SWITCH_HIGH);
break;
case AUXSW_PARACHUTE_RELEASE:
if (ch_flag == AUX_SWITCH_HIGH) {
parachute_manual_release();
}
break;
case AUXSW_PARACHUTE_3POS:
// Parachute disable, enable, release with 3 position switch
switch (ch_flag) {
case AUX_SWITCH_LOW:
parachute.enabled(false);
Log_Write_Event(DATA_PARACHUTE_DISABLED);
break;
case AUX_SWITCH_MIDDLE:
parachute.enabled(true);
Log_Write_Event(DATA_PARACHUTE_ENABLED);
break;
case AUX_SWITCH_HIGH:
parachute.enabled(true);
parachute_manual_release();
break;
}
break;
#endif
case AUXSW_MISSION_RESET:
if (ch_flag == AUX_SWITCH_HIGH) {
mission.reset();
}
break;
case AUXSW_ATTCON_FEEDFWD:
// enable or disable feed forward
attitude_control.bf_feedforward(ch_flag == AUX_SWITCH_HIGH);
break;
case AUXSW_ATTCON_ACCEL_LIM:
// enable or disable accel limiting by restoring defaults
attitude_control.accel_limiting(ch_flag == AUX_SWITCH_HIGH);
break;
#if MOUNT == ENABLE
case AUXSW_RETRACT_MOUNT:
switch (ch_flag) {
case AUX_SWITCH_HIGH:
camera_mount.set_mode(MAV_MOUNT_MODE_RETRACT);
break;
case AUX_SWITCH_LOW:
camera_mount.set_mode_to_default();
break;
}
break;
#endif
case AUXSW_RELAY:
ServoRelayEvents.do_set_relay(0, ch_flag == AUX_SWITCH_HIGH);
break;
case AUXSW_LANDING_GEAR:
switch (ch_flag) {
case AUX_SWITCH_LOW:
landinggear.set_cmd_mode(LandingGear_Deploy);
break;
case AUX_SWITCH_MIDDLE:
landinggear.set_cmd_mode(LandingGear_Auto);
break;
case AUX_SWITCH_HIGH:
landinggear.set_cmd_mode(LandingGear_Retract);
break;
}
break;
case AUXSW_LOST_COPTER_SOUND:
switch (ch_flag) {
case AUX_SWITCH_HIGH:
AP_Notify::flags.vehicle_lost = true;
break;
case AUX_SWITCH_LOW:
AP_Notify::flags.vehicle_lost = false;
break;
}
break;
case AUXSW_MOTOR_ESTOP:
// Turn on Emergency Stop logic when channel is high
set_motor_emergency_stop(ch_flag == AUX_SWITCH_HIGH);
break;
case AUXSW_MOTOR_INTERLOCK:
// Turn on when above LOW, because channel will also be used for speed
// control signal in tradheli
motors.set_interlock(ch_flag == AUX_SWITCH_HIGH || ch_flag == AUX_SWITCH_MIDDLE);
// Log new status
if (motors.get_interlock()){
Log_Write_Event(DATA_MOTORS_INTERLOCK_ENABLED);
} else {
Log_Write_Event(DATA_MOTORS_INTERLOCK_DISABLED);
}
break;
case AUXSW_BRAKE:
// brake flight mode
if (ch_flag == AUX_SWITCH_HIGH) {
set_mode(BRAKE);
}else{
// return to flight mode switch's flight mode if we are currently in BRAKE
if (control_mode == BRAKE) {
reset_control_switch();
}
}
break;
}
}
// save_trim - adds roll and pitch trims from the radio to ahrs
void Copter::save_trim()
{
// save roll and pitch trim
float roll_trim = ToRad((float)channel_roll->control_in/100.0f);
float pitch_trim = ToRad((float)channel_pitch->control_in/100.0f);
ahrs.add_trim(roll_trim, pitch_trim);
Log_Write_Event(DATA_SAVE_TRIM);
gcs_send_text(MAV_SEVERITY_INFO, "Trim saved");
}
// auto_trim - slightly adjusts the ahrs.roll_trim and ahrs.pitch_trim towards the current stick positions
// meant to be called continuously while the pilot attempts to keep the copter level
void Copter::auto_trim()
{
if(auto_trim_counter > 0) {
auto_trim_counter--;
// flash the leds
AP_Notify::flags.save_trim = true;
// calculate roll trim adjustment
float roll_trim_adjustment = ToRad((float)channel_roll->control_in / 4000.0f);
// calculate pitch trim adjustment
float pitch_trim_adjustment = ToRad((float)channel_pitch->control_in / 4000.0f);
// add trim to ahrs object
// save to eeprom on last iteration
ahrs.add_trim(roll_trim_adjustment, pitch_trim_adjustment, (auto_trim_counter == 0));
// on last iteration restore leds and accel gains to normal
if(auto_trim_counter == 0) {
AP_Notify::flags.save_trim = false;
}
}
}

452
ArduSub/system.cpp Normal file
View File

@ -0,0 +1,452 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
/*****************************************************************************
* The init_ardupilot function processes everything we need for an in - air restart
* We will determine later if we are actually on the ground and process a
* ground start in that case.
*
*****************************************************************************/
#if CLI_ENABLED == ENABLED
// This is the help function
int8_t Copter::main_menu_help(uint8_t argc, const Menu::arg *argv)
{
cliSerial->printf("Commands:\n"
" logs\n"
" setup\n"
" test\n"
" reboot\n"
"\n");
return(0);
}
// Command/function table for the top-level menu.
const struct Menu::command main_menu_commands[] = {
// command function called
// ======= ===============
{"logs", MENU_FUNC(process_logs)},
{"setup", MENU_FUNC(setup_mode)},
{"test", MENU_FUNC(test_mode)},
{"reboot", MENU_FUNC(reboot_board)},
{"help", MENU_FUNC(main_menu_help)},
};
// Create the top-level menu object.
MENU(main_menu, THISFIRMWARE, main_menu_commands);
int8_t Copter::reboot_board(uint8_t argc, const Menu::arg *argv)
{
hal.scheduler->reboot(false);
return 0;
}
// the user wants the CLI. It never exits
void Copter::run_cli(AP_HAL::UARTDriver *port)
{
cliSerial = port;
Menu::set_port(port);
port->set_blocking_writes(true);
// disable the mavlink delay callback
hal.scheduler->register_delay_callback(NULL, 5);
// disable main_loop failsafe
failsafe_disable();
// cut the engines
if(motors.armed()) {
motors.armed(false);
motors.output();
}
while (1) {
main_menu.run();
}
}
#endif // CLI_ENABLED
static void mavlink_delay_cb_static()
{
copter.mavlink_delay_cb();
}
static void failsafe_check_static()
{
copter.failsafe_check();
}
void Copter::init_ardupilot()
{
if (!hal.gpio->usb_connected()) {
// USB is not connected, this means UART0 may be a Xbee, with
// its darned bricking problem. We can't write to it for at
// least one second after powering up. Simplest solution for
// now is to delay for 1 second. Something more elegant may be
// added later
delay(1000);
}
// initialise serial port
serial_manager.init_console();
cliSerial->printf("\n\nInit " FIRMWARE_STRING
"\n\nFree RAM: %u\n",
(unsigned)hal.util->available_memory());
//
// Report firmware version code expect on console (check of actual EEPROM format version is done in load_parameters function)
//
report_version();
// load parameters from EEPROM
load_parameters();
BoardConfig.init();
// initialise serial port
serial_manager.init();
// init EPM cargo gripper
#if EPM_ENABLED == ENABLED
epm.init();
#endif
// initialise notify system
// disable external leds if epm is enabled because of pin conflict on the APM
notify.init(true);
// initialise battery monitor
battery.init();
// Init RSSI
rssi.init();
barometer.init();
// Register the mavlink service callback. This will run
// anytime there are more than 5ms remaining in a call to
// hal.scheduler->delay.
hal.scheduler->register_delay_callback(mavlink_delay_cb_static, 5);
// we start by assuming USB connected, as we initialed the serial
// port with SERIAL0_BAUD. check_usb_mux() fixes this if need be.
ap.usb_connected = true;
check_usb_mux();
// init the GCS connected to the console
gcs[0].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_Console, 0);
// init telemetry port
gcs[1].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 0);
// setup serial port for telem2
gcs[2].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 1);
// setup serial port for fourth telemetry port (not used by default)
gcs[3].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 2);
#if FRSKY_TELEM_ENABLED == ENABLED
// setup frsky
frsky_telemetry.init(serial_manager);
#endif
// identify ourselves correctly with the ground station
mavlink_system.sysid = g.sysid_this_mav;
#if LOGGING_ENABLED == ENABLED
log_init();
#endif
// update motor interlock state
update_using_interlock();
#if FRAME_CONFIG == HELI_FRAME
// trad heli specific initialisation
heli_init();
#endif
init_rc_in(); // sets up rc channels from radio
init_rc_out(); // sets up motors and output to escs
// initialise which outputs Servo and Relay events can use
ServoRelayEvents.set_channel_mask(~motors.get_motor_mask());
relay.init();
/*
* setup the 'main loop is dead' check. Note that this relies on
* the RC library being initialised.
*/
hal.scheduler->register_timer_failsafe(failsafe_check_static, 1000);
// Do GPS init
gps.init(&DataFlash, serial_manager);
if(g.compass_enabled)
init_compass();
#if OPTFLOW == ENABLED
// make optflow available to AHRS
ahrs.set_optflow(&optflow);
#endif
// initialise attitude and position controllers
attitude_control.set_dt(MAIN_LOOP_SECONDS);
pos_control.set_dt(MAIN_LOOP_SECONDS);
// init the optical flow sensor
init_optflow();
#if MOUNT == ENABLED
// initialise camera mount
camera_mount.init(serial_manager);
#endif
#if PRECISION_LANDING == ENABLED
// initialise precision landing
init_precland();
#endif
#ifdef USERHOOK_INIT
USERHOOK_INIT
#endif
#if CLI_ENABLED == ENABLED
if (g.cli_enabled) {
const char *msg = "\nPress ENTER 3 times to start interactive setup\n";
cliSerial->println(msg);
if (gcs[1].initialised && (gcs[1].get_uart() != NULL)) {
gcs[1].get_uart()->println(msg);
}
if (num_gcs > 2 && gcs[2].initialised && (gcs[2].get_uart() != NULL)) {
gcs[2].get_uart()->println(msg);
}
}
#endif // CLI_ENABLED
#if HIL_MODE != HIL_MODE_DISABLED
while (barometer.get_last_update() == 0) {
// the barometer begins updating when we get the first
// HIL_STATE message
gcs_send_text(MAV_SEVERITY_WARNING, "Waiting for first HIL_STATE message");
delay(1000);
}
// set INS to HIL mode
ins.set_hil_mode();
#endif
// read Baro pressure at ground
//-----------------------------
init_barometer(true);
// initialise sonar
#if CONFIG_SONAR == ENABLED
init_sonar();
#endif
// initialise AP_RPM library
rpm_sensor.init();
// initialise mission library
mission.init();
// initialise the flight mode and aux switch
// ---------------------------
reset_control_switch();
init_aux_switches();
startup_INS_ground();
// set landed flags
set_land_complete(true);
set_land_complete_maybe(true);
// we don't want writes to the serial port to cause us to pause
// mid-flight, so set the serial ports non-blocking once we are
// ready to fly
serial_manager.set_blocking_writes_all(false);
// enable CPU failsafe
failsafe_enable();
ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW));
ins.set_dataflash(&DataFlash);
// init vehicle capabilties
init_capabilities();
cliSerial->print("\nReady to FLY ");
// flag that initialisation has completed
ap.initialised = true;
}
//******************************************************************************
//This function does all the calibrations, etc. that we need during a ground start
//******************************************************************************
void Copter::startup_INS_ground()
{
// initialise ahrs (may push imu calibration into the mpu6000 if using that device).
ahrs.init();
ahrs.set_vehicle_class(AHRS_VEHICLE_COPTER);
// Warm up and calibrate gyro offsets
ins.init(scheduler.get_loop_rate_hz());
// reset ahrs including gyro bias
ahrs.reset();
}
// calibrate gyros - returns true if succesfully calibrated
bool Copter::calibrate_gyros()
{
// gyro offset calibration
copter.ins.init_gyro();
// reset ahrs gyro bias
if (copter.ins.gyro_calibrated_ok_all()) {
copter.ahrs.reset_gyro_drift();
return true;
}
return false;
}
// position_ok - returns true if the horizontal absolute position is ok and home position is set
bool Copter::position_ok()
{
// return false if ekf failsafe has triggered
if (failsafe.ekf) {
return false;
}
// check ekf position estimate
return (ekf_position_ok() || optflow_position_ok());
}
// ekf_position_ok - returns true if the ekf claims it's horizontal absolute position estimate is ok and home position is set
bool Copter::ekf_position_ok()
{
if (!ahrs.have_inertial_nav()) {
// do not allow navigation with dcm position
return false;
}
// with EKF use filter status and ekf check
nav_filter_status filt_status = inertial_nav.get_filter_status();
// if disarmed we accept a predicted horizontal position
if (!motors.armed()) {
return ((filt_status.flags.horiz_pos_abs || filt_status.flags.pred_horiz_pos_abs));
} else {
// once armed we require a good absolute position and EKF must not be in const_pos_mode
return (filt_status.flags.horiz_pos_abs && !filt_status.flags.const_pos_mode);
}
}
// optflow_position_ok - returns true if optical flow based position estimate is ok
bool Copter::optflow_position_ok()
{
#if OPTFLOW != ENABLED
return false;
#else
// return immediately if optflow is not enabled or EKF not used
if (!optflow.enabled() || !ahrs.have_inertial_nav()) {
return false;
}
// get filter status from EKF
nav_filter_status filt_status = inertial_nav.get_filter_status();
// if disarmed we accept a predicted horizontal relative position
if (!motors.armed()) {
return (filt_status.flags.pred_horiz_pos_rel);
} else {
return (filt_status.flags.horiz_pos_rel && !filt_status.flags.const_pos_mode);
}
#endif
}
// update_auto_armed - update status of auto_armed flag
void Copter::update_auto_armed()
{
// disarm checks
if(ap.auto_armed){
// if motors are disarmed, auto_armed should also be false
if(!motors.armed()) {
set_auto_armed(false);
return;
}
// if in stabilize or acro flight mode and throttle is zero, auto-armed should become false
if(mode_has_manual_throttle(control_mode) && ap.throttle_zero && !failsafe.radio) {
set_auto_armed(false);
}
#if FRAME_CONFIG == HELI_FRAME
// if helicopters are on the ground, and the motor is switched off, auto-armed should be false
// so that rotor runup is checked again before attempting to take-off
if(ap.land_complete && !motors.rotor_runup_complete()) {
set_auto_armed(false);
}
#endif // HELI_FRAME
}else{
// arm checks
#if FRAME_CONFIG == HELI_FRAME
// for tradheli if motors are armed and throttle is above zero and the motor is started, auto_armed should be true
if(motors.armed() && !ap.throttle_zero && motors.rotor_runup_complete()) {
set_auto_armed(true);
}
#else
// if motors are armed and throttle is above zero auto_armed should be true
if(motors.armed() && !ap.throttle_zero) {
set_auto_armed(true);
}
#endif // HELI_FRAME
}
}
void Copter::check_usb_mux(void)
{
bool usb_check = hal.gpio->usb_connected();
if (usb_check == ap.usb_connected) {
return;
}
// the user has switched to/from the telemetry port
ap.usb_connected = usb_check;
}
// frsky_telemetry_send - sends telemetry data using frsky telemetry
// should be called at 5Hz by scheduler
#if FRSKY_TELEM_ENABLED == ENABLED
void Copter::frsky_telemetry_send(void)
{
frsky_telemetry.send_frames((uint8_t)control_mode);
}
#endif
/*
should we log a message type now?
*/
bool Copter::should_log(uint32_t mask)
{
#if LOGGING_ENABLED == ENABLED
if (!(mask & g.log_bitmask) || in_mavlink_delay) {
return false;
}
bool ret = motors.armed() || (g.log_bitmask & MASK_LOG_WHEN_DISARMED) != 0;
if (ret && !DataFlash.logging_started() && !in_log_download) {
start_logging();
}
return ret;
#else
return false;
#endif
}

141
ArduSub/takeoff.cpp Normal file
View File

@ -0,0 +1,141 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
// This file contains the high-level takeoff logic for Loiter, PosHold, AltHold, Sport modes.
// The take-off can be initiated from a GCS NAV_TAKEOFF command which includes a takeoff altitude
// A safe takeoff speed is calculated and used to calculate a time_ms
// the pos_control target is then slowly increased until time_ms expires
// return true if this flight mode supports user takeoff
// must_nagivate is true if mode must also control horizontal position
bool Copter::current_mode_has_user_takeoff(bool must_navigate)
{
switch (control_mode) {
case GUIDED:
case LOITER:
case POSHOLD:
return true;
case ALT_HOLD:
case SPORT:
return !must_navigate;
default:
return false;
}
}
// initiate user takeoff - called when MAVLink TAKEOFF command is received
bool Copter::do_user_takeoff(float takeoff_alt_cm, bool must_navigate)
{
if (motors.armed() && ap.land_complete && current_mode_has_user_takeoff(must_navigate) && takeoff_alt_cm > current_loc.alt) {
#if FRAME_CONFIG == HELI_FRAME
// Helicopters should return false if MAVlink takeoff command is received while the rotor is not spinning
if (!motors.rotor_runup_complete()) {
return false;
}
#endif
switch(control_mode) {
case GUIDED:
set_auto_armed(true);
guided_takeoff_start(takeoff_alt_cm);
return true;
case LOITER:
case POSHOLD:
case ALT_HOLD:
case SPORT:
set_auto_armed(true);
takeoff_timer_start(takeoff_alt_cm);
return true;
}
}
return false;
}
// start takeoff to specified altitude above home in centimeters
void Copter::takeoff_timer_start(float alt_cm)
{
// calculate climb rate
float speed = MIN(wp_nav.get_speed_up(), MAX(g.pilot_velocity_z_max*2.0f/3.0f, g.pilot_velocity_z_max-50.0f));
// sanity check speed and target
if (takeoff_state.running || speed <= 0.0f || alt_cm <= 0.0f) {
return;
}
// initialise takeoff state
takeoff_state.running = true;
takeoff_state.max_speed = speed;
takeoff_state.start_ms = millis();
takeoff_state.alt_delta = alt_cm;
}
// stop takeoff
void Copter::takeoff_stop()
{
takeoff_state.running = false;
takeoff_state.start_ms = 0;
}
// returns pilot and takeoff climb rates
// pilot_climb_rate is both an input and an output
// takeoff_climb_rate is only an output
// has side-effect of turning takeoff off when timeout as expired
void Copter::takeoff_get_climb_rates(float& pilot_climb_rate, float& takeoff_climb_rate)
{
// return pilot_climb_rate if take-off inactive
if (!takeoff_state.running) {
takeoff_climb_rate = 0.0f;
return;
}
// acceleration of 50cm/s/s
static const float takeoff_accel = 50.0f;
float takeoff_minspeed = MIN(50.0f,takeoff_state.max_speed);
float time_elapsed = (millis()-takeoff_state.start_ms)*1.0e-3f;
float speed = MIN(time_elapsed*takeoff_accel+takeoff_minspeed, takeoff_state.max_speed);
float time_to_max_speed = (takeoff_state.max_speed-takeoff_minspeed)/takeoff_accel;
float height_gained;
if (time_elapsed <= time_to_max_speed) {
height_gained = 0.5f*takeoff_accel*sq(time_elapsed) + takeoff_minspeed*time_elapsed;
} else {
height_gained = 0.5f*takeoff_accel*sq(time_to_max_speed) + takeoff_minspeed*time_to_max_speed +
(time_elapsed-time_to_max_speed)*takeoff_state.max_speed;
}
// check if the takeoff is over
if (height_gained >= takeoff_state.alt_delta) {
takeoff_stop();
}
// if takeoff climb rate is zero return
if (speed <= 0.0f) {
takeoff_climb_rate = 0.0f;
return;
}
// default take-off climb rate to maximum speed
takeoff_climb_rate = speed;
// if pilot's commands descent
if (pilot_climb_rate < 0.0f) {
// if overall climb rate is still positive, move to take-off climb rate
if (takeoff_climb_rate + pilot_climb_rate > 0.0f) {
takeoff_climb_rate = takeoff_climb_rate + pilot_climb_rate;
pilot_climb_rate = 0;
} else {
// if overall is negative, move to pilot climb rate
pilot_climb_rate = pilot_climb_rate + takeoff_climb_rate;
takeoff_climb_rate = 0.0f;
}
} else { // pilot commands climb
// pilot climb rate is zero until it surpasses the take-off climb rate
if (pilot_climb_rate > takeoff_climb_rate) {
pilot_climb_rate = pilot_climb_rate - takeoff_climb_rate;
} else {
pilot_climb_rate = 0.0f;
}
}
}

276
ArduSub/test.cpp Normal file
View File

@ -0,0 +1,276 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#if CLI_ENABLED == ENABLED
// Creates a constant array of structs representing menu options
// and stores them in Flash memory, not RAM.
// User enters the string in the console to call the functions on the right.
// See class Menu in AP_Coommon for implementation details
static const struct Menu::command test_menu_commands[] = {
#if HIL_MODE == HIL_MODE_DISABLED
{"baro", MENU_FUNC(test_baro)},
#endif
{"compass", MENU_FUNC(test_compass)},
{"ins", MENU_FUNC(test_ins)},
{"optflow", MENU_FUNC(test_optflow)},
{"relay", MENU_FUNC(test_relay)},
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
{"shell", MENU_FUNC(test_shell)},
#endif
#if HIL_MODE == HIL_MODE_DISABLED
{"rangefinder", MENU_FUNC(test_sonar)},
#endif
};
// A Macro to create the Menu
MENU(test_menu, "test", test_menu_commands);
int8_t Copter::test_mode(uint8_t argc, const Menu::arg *argv)
{
test_menu.run();
return 0;
}
#if HIL_MODE == HIL_MODE_DISABLED
int8_t Copter::test_baro(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
init_barometer(true);
while(1) {
delay(100);
read_barometer();
if (!barometer.healthy()) {
cliSerial->println("not healthy");
} else {
cliSerial->printf("Alt: %0.2fm, Raw: %f Temperature: %.1f\n",
(double)(baro_alt / 100.0f),
(double)barometer.get_pressure(),
(double)barometer.get_temperature());
}
if(cliSerial->available() > 0) {
return (0);
}
}
return 0;
}
#endif
int8_t Copter::test_compass(uint8_t argc, const Menu::arg *argv)
{
uint8_t delta_ms_fast_loop;
uint8_t medium_loopCounter = 0;
if (!g.compass_enabled) {
cliSerial->printf("Compass: ");
print_enabled(false);
return (0);
}
if (!compass.init()) {
cliSerial->println("Compass initialisation failed!");
return 0;
}
ahrs.init();
ahrs.set_fly_forward(true);
ahrs.set_compass(&compass);
#if OPTFLOW == ENABLED
ahrs.set_optflow(&optflow);
#endif
report_compass();
// we need the AHRS initialised for this test
ins.init(scheduler.get_loop_rate_hz());
ahrs.reset();
int16_t counter = 0;
float heading = 0;
print_hit_enter();
while(1) {
delay(20);
if (millis() - fast_loopTimer > 19) {
delta_ms_fast_loop = millis() - fast_loopTimer;
G_Dt = (float)delta_ms_fast_loop / 1000.0f; // used by DCM integrator
fast_loopTimer = millis();
// INS
// ---
ahrs.update();
medium_loopCounter++;
if(medium_loopCounter == 5) {
if (compass.read()) {
// Calculate heading
const Matrix3f &m = ahrs.get_rotation_body_to_ned();
heading = compass.calculate_heading(m);
compass.learn_offsets();
}
medium_loopCounter = 0;
}
counter++;
if (counter>20) {
if (compass.healthy()) {
const Vector3f &mag_ofs = compass.get_offsets();
const Vector3f &mag = compass.get_field();
cliSerial->printf("Heading: %d, XYZ: %.0f, %.0f, %.0f,\tXYZoff: %6.2f, %6.2f, %6.2f\n",
(int)(wrap_360_cd(ToDeg(heading) * 100)) /100,
(double)mag.x,
(double)mag.y,
(double)mag.z,
(double)mag_ofs.x,
(double)mag_ofs.y,
(double)mag_ofs.z);
} else {
cliSerial->println("compass not healthy");
}
counter=0;
}
}
if (cliSerial->available() > 0) {
break;
}
}
// save offsets. This allows you to get sane offset values using
// the CLI before you go flying.
cliSerial->println("saving offsets");
compass.save_offsets();
return (0);
}
int8_t Copter::test_ins(uint8_t argc, const Menu::arg *argv)
{
Vector3f gyro, accel;
print_hit_enter();
cliSerial->printf("INS\n");
delay(1000);
ahrs.init();
ins.init(scheduler.get_loop_rate_hz());
cliSerial->printf("...done\n");
delay(50);
while(1) {
ins.update();
gyro = ins.get_gyro();
accel = ins.get_accel();
float test = accel.length() / GRAVITY_MSS;
cliSerial->printf("a %7.4f %7.4f %7.4f g %7.4f %7.4f %7.4f t %7.4f \n",
(double)accel.x, (double)accel.y, (double)accel.z,
(double)gyro.x, (double)gyro.y, (double)gyro.z,
(double)test);
delay(40);
if(cliSerial->available() > 0) {
return (0);
}
}
}
int8_t Copter::test_optflow(uint8_t argc, const Menu::arg *argv)
{
#if OPTFLOW == ENABLED
if(optflow.enabled()) {
cliSerial->printf("dev id: %d\t",(int)optflow.device_id());
print_hit_enter();
while(1) {
delay(200);
optflow.update();
const Vector2f& flowRate = optflow.flowRate();
cliSerial->printf("flowX : %7.4f\t flowY : %7.4f\t flow qual : %d\n",
(double)flowRate.x,
(double)flowRate.y,
(int)optflow.quality());
if(cliSerial->available() > 0) {
return (0);
}
}
} else {
cliSerial->printf("OptFlow: ");
print_enabled(false);
}
return (0);
#else
return (0);
#endif // OPTFLOW == ENABLED
}
int8_t Copter::test_relay(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
while(1) {
cliSerial->printf("Relay on\n");
relay.on(0);
delay(3000);
if(cliSerial->available() > 0) {
return (0);
}
cliSerial->printf("Relay off\n");
relay.off(0);
delay(3000);
if(cliSerial->available() > 0) {
return (0);
}
}
}
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
/*
* run a debug shell
*/
int8_t Copter::test_shell(uint8_t argc, const Menu::arg *argv)
{
hal.util->run_debug_shell(cliSerial);
return 0;
}
#endif
#if HIL_MODE == HIL_MODE_DISABLED
/*
* test the rangefinders
*/
int8_t Copter::test_sonar(uint8_t argc, const Menu::arg *argv)
{
#if CONFIG_SONAR == ENABLED
sonar.init();
cliSerial->printf("RangeFinder: %d devices detected\n", sonar.num_sensors());
print_hit_enter();
while(1) {
delay(100);
sonar.update();
cliSerial->printf("Primary: status %d distance_cm %d \n", (int)sonar.status(), sonar.distance_cm());
cliSerial->printf("All: device_0 type %d status %d distance_cm %d, device_1 type %d status %d distance_cm %d\n",
(int)sonar._type[0], (int)sonar.status(0), sonar.distance_cm(0), (int)sonar._type[1], (int)sonar.status(1), sonar.distance_cm(1));
if(cliSerial->available() > 0) {
return (0);
}
}
#endif
return (0);
}
#endif
void Copter::print_hit_enter()
{
cliSerial->printf("Hit Enter to exit.\n\n");
}
#endif // CLI_ENABLED

212
ArduSub/tuning.cpp Normal file
View File

@ -0,0 +1,212 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
/*
* tuning.pde - function to update various parameters in flight using the ch6 tuning knob
* This should not be confused with the AutoTune feature which can bve found in control_autotune.pde
*/
// tuning - updates parameters based on the ch6 tuning knob's position
// should be called at 3.3hz
void Copter::tuning() {
// exit immediately if not tuning of when radio failsafe is invoked so tuning values are not set to zero
if ((g.radio_tuning <= 0) || failsafe.radio || failsafe.radio_counter != 0) {
return;
}
float tuning_value = (float)g.rc_6.control_in / 1000.0f;
g.rc_6.set_range(g.radio_tuning_low,g.radio_tuning_high);
Log_Write_Parameter_Tuning(g.radio_tuning, tuning_value, g.rc_6.control_in, g.radio_tuning_low, g.radio_tuning_high);
switch(g.radio_tuning) {
// Roll, Pitch tuning
case TUNING_STABILIZE_ROLL_PITCH_KP:
g.p_stabilize_roll.kP(tuning_value);
g.p_stabilize_pitch.kP(tuning_value);
break;
case TUNING_RATE_ROLL_PITCH_KP:
g.pid_rate_roll.kP(tuning_value);
g.pid_rate_pitch.kP(tuning_value);
break;
case TUNING_RATE_ROLL_PITCH_KI:
g.pid_rate_roll.kI(tuning_value);
g.pid_rate_pitch.kI(tuning_value);
break;
case TUNING_RATE_ROLL_PITCH_KD:
g.pid_rate_roll.kD(tuning_value);
g.pid_rate_pitch.kD(tuning_value);
break;
// Yaw tuning
case TUNING_STABILIZE_YAW_KP:
g.p_stabilize_yaw.kP(tuning_value);
break;
case TUNING_YAW_RATE_KP:
g.pid_rate_yaw.kP(tuning_value);
break;
case TUNING_YAW_RATE_KD:
g.pid_rate_yaw.kD(tuning_value);
break;
// Altitude and throttle tuning
case TUNING_ALTITUDE_HOLD_KP:
g.p_alt_hold.kP(tuning_value);
break;
case TUNING_THROTTLE_RATE_KP:
g.p_vel_z.kP(tuning_value);
break;
case TUNING_ACCEL_Z_KP:
g.pid_accel_z.kP(tuning_value);
break;
case TUNING_ACCEL_Z_KI:
g.pid_accel_z.kI(tuning_value);
break;
case TUNING_ACCEL_Z_KD:
g.pid_accel_z.kD(tuning_value);
break;
// Loiter and navigation tuning
case TUNING_LOITER_POSITION_KP:
g.p_pos_xy.kP(tuning_value);
break;
case TUNING_VEL_XY_KP:
g.pi_vel_xy.kP(tuning_value);
break;
case TUNING_VEL_XY_KI:
g.pi_vel_xy.kI(tuning_value);
break;
case TUNING_WP_SPEED:
// set waypoint navigation horizontal speed to 0 ~ 1000 cm/s
wp_nav.set_speed_xy(g.rc_6.control_in);
break;
// Acro roll pitch gain
case TUNING_ACRO_RP_KP:
g.acro_rp_p = tuning_value;
break;
// Acro yaw gain
case TUNING_ACRO_YAW_KP:
g.acro_yaw_p = tuning_value;
break;
#if FRAME_CONFIG == HELI_FRAME
case TUNING_HELI_EXTERNAL_GYRO:
motors.ext_gyro_gain(g.rc_6.control_in);
break;
case TUNING_RATE_PITCH_FF:
g.pid_rate_pitch.ff(tuning_value);
break;
case TUNING_RATE_ROLL_FF:
g.pid_rate_roll.ff(tuning_value);
break;
case TUNING_RATE_YAW_FF:
g.pid_rate_yaw.ff(tuning_value);
break;
#endif
case TUNING_DECLINATION:
// set declination to +-20degrees
compass.set_declination(ToRad((2.0f * g.rc_6.control_in - g.radio_tuning_high)/100.0f), false); // 2nd parameter is false because we do not want to save to eeprom because this would have a performance impact
break;
case TUNING_CIRCLE_RATE:
// set circle rate up to approximately 45 deg/sec in either direction
circle_nav.set_rate((float)g.rc_6.control_in/25.0f-20.0f);
break;
case TUNING_SONAR_GAIN:
// set sonar gain
g.sonar_gain.set(tuning_value);
break;
#if 0
// disabled for now - we need accessor functions
case TUNING_EKF_VERTICAL_POS:
// Tune the EKF that is being used
// EKF's baro vs accel (higher rely on accels more, baro impact is reduced)
if (!ahrs.get_NavEKF2().enabled()) {
ahrs.get_NavEKF()._gpsVertPosNoise = tuning_value;
} else {
ahrs.get_NavEKF2()._gpsVertPosNoise = tuning_value;
}
break;
case TUNING_EKF_HORIZONTAL_POS:
// EKF's gps vs accel (higher rely on accels more, gps impact is reduced)
if (!ahrs.get_NavEKF2().enabled()) {
ahrs.get_NavEKF()._gpsHorizPosNoise = tuning_value;
} else {
ahrs.get_NavEKF2()._gpsHorizPosNoise = tuning_value;
}
break;
case TUNING_EKF_ACCEL_NOISE:
// EKF's accel noise (lower means trust accels more, gps & baro less)
if (!ahrs.get_NavEKF2().enabled()) {
ahrs.get_NavEKF()._accNoise = tuning_value;
} else {
ahrs.get_NavEKF2()._accNoise = tuning_value;
}
break;
#endif
case TUNING_RC_FEEL_RP:
// roll-pitch input smoothing
g.rc_feel_rp = g.rc_6.control_in / 10;
break;
case TUNING_RATE_PITCH_KP:
g.pid_rate_pitch.kP(tuning_value);
break;
case TUNING_RATE_PITCH_KI:
g.pid_rate_pitch.kI(tuning_value);
break;
case TUNING_RATE_PITCH_KD:
g.pid_rate_pitch.kD(tuning_value);
break;
case TUNING_RATE_ROLL_KP:
g.pid_rate_roll.kP(tuning_value);
break;
case TUNING_RATE_ROLL_KI:
g.pid_rate_roll.kI(tuning_value);
break;
case TUNING_RATE_ROLL_KD:
g.pid_rate_roll.kD(tuning_value);
break;
#if FRAME_CONFIG != HELI_FRAME
case TUNING_RATE_MOT_YAW_HEADROOM:
motors.set_yaw_headroom(tuning_value*1000);
break;
#endif
case TUNING_RATE_YAW_FILT:
g.pid_rate_yaw.filt_hz(tuning_value);
break;
}
}

41
ArduSub/wscript Normal file
View File

@ -0,0 +1,41 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
vehicle = bld.path.name
ardupilotwaf.vehicle_stlib(
bld,
name=vehicle + '_libs',
vehicle=vehicle,
libraries=ardupilotwaf.COMMON_VEHICLE_DEPENDENT_LIBRARIES + [
'AP_ADSB',
'AC_AttitudeControl',
'AC_Fence',
'AC_PID',
'AC_PrecLand',
'AC_Sprayer',
'AC_WPNav',
'AP_Camera',
'AP_EPM',
'AP_Frsky_Telem',
'AP_IRLock',
'AP_InertialNav',
'AP_LandingGear',
'AP_Menu',
'AP_Motors',
'AP_Mount',
'AP_Parachute',
'AP_RCMapper',
'AP_RPM',
'AP_RSSI',
'AP_Relay',
'AP_ServoRelayEvents',
],
)
ardupilotwaf.program(
bld,
use=vehicle + '_libs',
)