diff --git a/ArduSub/APM_Config.h b/ArduSub/APM_Config.h new file mode 100644 index 00000000000..4b5dddc7fba --- /dev/null +++ b/ArduSub/APM_Config.h @@ -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 diff --git a/ArduSub/APM_Config_mavlink_hil.h b/ArduSub/APM_Config_mavlink_hil.h new file mode 100644 index 00000000000..094874b1062 --- /dev/null +++ b/ArduSub/APM_Config_mavlink_hil.h @@ -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 + diff --git a/ArduSub/AP_State.cpp b/ArduSub/AP_State.cpp new file mode 100644 index 00000000000..5c98ecdc882 --- /dev/null +++ b/ArduSub/AP_State.cpp @@ -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); + } +} diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp new file mode 100644 index 00000000000..c1d129245ad --- /dev/null +++ b/ArduSub/ArduSub.cpp @@ -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 . + */ +/* + * 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= 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); diff --git a/ArduSub/Attitude.cpp b/ArduSub/Attitude.cpp new file mode 100644 index 00000000000..d79b26e21b3 --- /dev/null +++ b/ArduSub/Attitude.cpp @@ -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; +} diff --git a/ArduSub/Copter.cpp b/ArduSub/Copter.cpp new file mode 100644 index 00000000000..21c413c941b --- /dev/null +++ b/ArduSub/Copter.cpp @@ -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 . + */ +/* + 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(¤t_loc, 0, sizeof(current_loc)); + + // init sensor error logging flags + sensor_health.baro = true; + sensor_health.compass = true; +} + +Copter copter; diff --git a/ArduSub/Copter.h b/ArduSub/Copter.h new file mode 100644 index 00000000000..90ccce86919 --- /dev/null +++ b/ArduSub/Copter.h @@ -0,0 +1,1045 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#ifndef _COPTER_H +#define _COPTER_H + +#define THISFIRMWARE "APM:Copter V3.4-dev" +#define FIRMWARE_VERSION 3,4,0,FIRMWARE_VERSION_TYPE_DEV + +/* + 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 . + */ +/* + This is the main Copter class + */ + +//////////////////////////////////////////////////////////////////////////////// +// Header includes +//////////////////////////////////////////////////////////////////////////////// + +#include +#include +#include + +#include + +// Common dependencies +#include +#include +#include +#include + +// Application dependencies +#include +#include // MAVLink GCS definitions +#include // Serial manager library +#include // ArduPilot GPS library +#include // ArduPilot Mega Flash Memory Library +#include // ArduPilot Mega Analog to Digital Converter Library +#include +#include // ArduPilot Mega Magnetometer Library +#include // ArduPilot Mega Vector/Matrix math Library +#include // interface and maths for accelerometer calibration +#include // ArduPilot Mega Inertial Sensor (accel & gyro) Library +#include +#include +#include +#include // Mission command library +#include // Rally point library +#include // PID library +#include // PID library (2-axis) +#include // Heli specific Rate PID library +#include // P library +#include // Attitude control library +#include // Attitude control library for traditional helicopter +#include // Position control library +#include // RC Channel Library +#include // AP Motors library +#include // Range finder library +#include // Optical Flow library +#include // RSSI Library +#include // Filter library +#include // APM FIFO Buffer +#include // APM relay +#include +#include // Photo or video camera +#include // Camera/Antenna mount +#include // needed for AHRS build +#include // needed for AHRS build +#include // ArduPilot Mega inertial navigation library +#include // ArduCopter waypoint navigation library +#include // circle navigation library +#include // ArduPilot Mega Declination Helper Library +#include // Arducopter Fence library +#include // main loop scheduler +#include // RC input mapping library +#include // Notify library +#include // Battery monitor library +#include // board configuration library +#include +#if SPRAYER == ENABLED +#include // crop sprayer library +#endif +#if EPM_ENABLED == ENABLED +#include // EPM cargo gripper stuff +#endif +#if PARACHUTE == ENABLED +#include // Parachute release library +#endif +#include // Landing Gear library +#include +#include +#include +#if PRECISION_LANDING == ENABLED +#include +#include +#endif +#include // Pilot input handling library +#include // Heli specific pilot input handling library + + +// AP_HAL to Arduino compatibility layer +// Configuration +#include "defines.h" +#include "config.h" +#include "config_channels.h" + +// Local modules +#include "Parameters.h" + +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#include +#endif + +class Copter : public AP_HAL::HAL::Callbacks { +public: + friend class GCS_MAVLINK; + friend class Parameters; + + Copter(void); + + // HAL::Callbacks implementation. + void setup() override; + void loop() override; + +private: + // key aircraft parameters passed to multiple libraries + AP_Vehicle::MultiCopter aparm; + + + // cliSerial isn't strictly necessary - it is an alias for hal.console. It may + // be deprecated in favor of hal.console in later releases. + AP_HAL::BetterStream* cliSerial; + + // Global parameters are all contained within the 'g' class. + Parameters g; + + // main loop scheduler + AP_Scheduler scheduler; + + // AP_Notify instance + AP_Notify notify; + + // used to detect MAVLink acks from GCS to stop compassmot + uint8_t command_ack_counter; + + // has a log download started? + bool in_log_download; + + // primary input control channels + RC_Channel *channel_roll; + RC_Channel *channel_pitch; + RC_Channel *channel_throttle; + RC_Channel *channel_yaw; + + // Dataflash + DataFlash_Class DataFlash{FIRMWARE_STRING}; + + AP_GPS gps; + + // flight modes convenience array + AP_Int8 *flight_modes; + + AP_Baro barometer; + Compass compass; + AP_InertialSensor ins; + +#if CONFIG_SONAR == ENABLED + RangeFinder sonar {serial_manager}; + bool sonar_enabled; // enable user switch for sonar +#endif + + AP_RPM rpm_sensor; + + // Inertial Navigation EKF + NavEKF EKF{&ahrs, barometer, sonar}; + NavEKF2 EKF2{&ahrs, barometer, sonar}; + AP_AHRS_NavEKF ahrs{ins, barometer, gps, sonar, EKF, EKF2, AP_AHRS_NavEKF::FLAG_ALWAYS_USE_EKF}; + +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + SITL::SITL sitl; +#endif + + // Mission library + AP_Mission mission; + + // Optical flow sensor +#if OPTFLOW == ENABLED + OpticalFlow optflow{ahrs}; +#endif + + // gnd speed limit required to observe optical flow sensor limits + float ekfGndSpdLimit; + + // scale factor applied to velocity controller gain to prevent optical flow noise causing excessive angle demand noise + float ekfNavVelGainScaler; + + // system time in milliseconds of last recorded yaw reset from ekf + uint32_t ekfYawReset_ms = 0; + + // GCS selection + AP_SerialManager serial_manager; + static const uint8_t num_gcs = MAVLINK_COMM_NUM_BUFFERS; + + GCS_MAVLINK gcs[MAVLINK_COMM_NUM_BUFFERS]; + + // User variables +#ifdef USERHOOK_VARIABLES +# include USERHOOK_VARIABLES +#endif + + // Documentation of GLobals: + union { + struct { + uint8_t unused1 : 1; // 0 + uint8_t simple_mode : 2; // 1,2 // This is the state of simple mode : 0 = disabled ; 1 = SIMPLE ; 2 = SUPERSIMPLE + uint8_t pre_arm_rc_check : 1; // 3 // true if rc input pre-arm checks have been completed successfully + uint8_t pre_arm_check : 1; // 4 // true if all pre-arm checks (rc, accel calibration, gps lock) have been performed + uint8_t auto_armed : 1; // 5 // stops auto missions from beginning until throttle is raised + uint8_t logging_started : 1; // 6 // true if dataflash logging has started + uint8_t land_complete : 1; // 7 // true if we have detected a landing + uint8_t new_radio_frame : 1; // 8 // Set true if we have new PWM data to act on from the Radio + uint8_t usb_connected : 1; // 9 // true if APM is powered from USB connection + uint8_t rc_receiver_present : 1; // 10 // true if we have an rc receiver present (i.e. if we've ever received an update + uint8_t compass_mot : 1; // 11 // true if we are currently performing compassmot calibration + uint8_t motor_test : 1; // 12 // true if we are currently performing the motors test + uint8_t initialised : 1; // 13 // true once the init_ardupilot function has completed. Extended status to GCS is not sent until this completes + uint8_t land_complete_maybe : 1; // 14 // true if we may have landed (less strict version of land_complete) + uint8_t throttle_zero : 1; // 15 // true if the throttle stick is at zero, debounced, determines if pilot intends shut-down when not using motor interlock + uint8_t system_time_set : 1; // 16 // true if the system time has been set from the GPS + uint8_t gps_base_pos_set : 1; // 17 // true when the gps base position has been set (used for RTK gps only) + enum HomeState home_state : 2; // 18,19 // home status (unset, set, locked) + uint8_t using_interlock : 1; // 20 // aux switch motor interlock function is in use + uint8_t motor_emergency_stop: 1; // 21 // motor estop switch, shuts off motors when enabled + uint8_t land_repo_active : 1; // 22 // true if the pilot is overriding the landing position + }; + uint32_t value; + } ap; + + // This is the state of the flight control system + // There are multiple states defined such as STABILIZE, ACRO, + int8_t control_mode; + + // Structure used to detect changes in the flight mode control switch + struct { + int8_t debounced_switch_position; // currently used switch position + int8_t last_switch_position; // switch position in previous iteration + uint32_t last_edge_time_ms; // system time that switch position was last changed + } control_switch_state; + + struct { + bool running; + float max_speed; + float alt_delta; + uint32_t start_ms; + } takeoff_state; + + RCMapper rcmap; + + // board specific config + AP_BoardConfig BoardConfig; + + // receiver RSSI + uint8_t receiver_rssi; + + // Failsafe + struct { + uint8_t rc_override_active : 1; // 0 // true if rc control are overwritten by ground station + uint8_t radio : 1; // 1 // A status flag for the radio failsafe + uint8_t battery : 1; // 2 // A status flag for the battery failsafe + uint8_t gcs : 1; // 4 // A status flag for the ground station failsafe + uint8_t ekf : 1; // 5 // true if ekf failsafe has occurred + + int8_t radio_counter; // number of iterations with throttle below throttle_fs_value + + uint32_t last_heartbeat_ms; // the time when the last HEARTBEAT message arrived from a GCS - used for triggering gcs failsafe + } failsafe; + + // sensor health for logging + struct { + uint8_t baro : 1; // true if baro is healthy + uint8_t compass : 1; // true if compass is healthy + } sensor_health; + + // Motor Output +#if FRAME_CONFIG == QUAD_FRAME + #define MOTOR_CLASS AP_MotorsQuad +#elif FRAME_CONFIG == TRI_FRAME + #define MOTOR_CLASS AP_MotorsTri +#elif FRAME_CONFIG == HEXA_FRAME + #define MOTOR_CLASS AP_MotorsHexa +#elif FRAME_CONFIG == Y6_FRAME + #define MOTOR_CLASS AP_MotorsY6 +#elif FRAME_CONFIG == OCTA_FRAME + #define MOTOR_CLASS AP_MotorsOcta +#elif FRAME_CONFIG == OCTA_QUAD_FRAME + #define MOTOR_CLASS AP_MotorsOctaQuad +#elif FRAME_CONFIG == HELI_FRAME + #define MOTOR_CLASS AP_MotorsHeli_Single +#elif FRAME_CONFIG == SINGLE_FRAME + #define MOTOR_CLASS AP_MotorsSingle +#elif FRAME_CONFIG == COAX_FRAME + #define MOTOR_CLASS AP_MotorsCoax +#else + #error Unrecognised frame type +#endif + + MOTOR_CLASS motors; + + // GPS variables + // Sometimes we need to remove the scaling for distance calcs + float scaleLongDown; + + // Location & Navigation + int32_t wp_bearing; + // The location of home in relation to the copter in centi-degrees + int32_t home_bearing; + // distance between plane and home in cm + int32_t home_distance; + // distance between plane and next waypoint in cm. + uint32_t wp_distance; + uint8_t land_state; // records state of land (flying to location, descending) + + // Auto + AutoMode auto_mode; // controls which auto controller is run + + // Guided + GuidedMode guided_mode; // controls which controller is run (pos or vel) + + // RTL + RTLState rtl_state; // records state of rtl (initial climb, returning home, etc) + bool rtl_state_complete; // set to true if the current state is completed + float rtl_alt; // altitude the vehicle is returning at + + // Circle + bool circle_pilot_yaw_override; // true if pilot is overriding yaw + + // SIMPLE Mode + // Used to track the orientation of the copter for Simple mode. This value is reset at each arming + // or in SuperSimple mode when the copter leaves a 20m radius from home. + float simple_cos_yaw; + float simple_sin_yaw; + int32_t super_simple_last_bearing; + float super_simple_cos_yaw; + float super_simple_sin_yaw; + + // Stores initial bearing when armed - initial simple bearing is modified in super simple mode so not suitable + int32_t initial_armed_bearing; + + // Throttle variables + float throttle_average; // estimated throttle required to hover + int16_t desired_climb_rate; // pilot desired climb rate - for logging purposes only + + // Loiter control + uint16_t loiter_time_max; // How long we should stay in Loiter Mode for mission scripting (time in seconds) + uint32_t loiter_time; // How long have we been loitering - The start time in millis + + // Flip + Vector3f flip_orig_attitude; // original copter attitude before flip + + // Battery Sensors + AP_BattMonitor battery; + + // FrSky telemetry support +#if FRSKY_TELEM_ENABLED == ENABLED + AP_Frsky_Telem frsky_telemetry; +#endif + + // Altitude + // The cm/s we are moving up or down based on filtered data - Positive = UP + int16_t climb_rate; + // The altitude as reported by Sonar in cm - Values are 20 to 700 generally. + int16_t sonar_alt; + uint8_t sonar_alt_health; // true if we can trust the altitude from the sonar + float target_sonar_alt; // desired altitude in cm above the ground + int32_t baro_alt; // barometer altitude in cm above home + float baro_climbrate; // barometer climbrate in cm/s + LowPassFilterVector3f land_accel_ef_filter; // accelerations for land and crash detector tests + + // 3D Location vectors + // Current location of the copter (altitude is relative to home) + struct Location current_loc; + + // Navigation Yaw control + // auto flight mode's yaw mode + uint8_t auto_yaw_mode; + + // Yaw will point at this location if auto_yaw_mode is set to AUTO_YAW_ROI + Vector3f roi_WP; + + // bearing from current location to the yaw_look_at_WP + float yaw_look_at_WP_bearing; + + // yaw used for YAW_LOOK_AT_HEADING yaw_mode + int32_t yaw_look_at_heading; + + // Deg/s we should turn + int16_t yaw_look_at_heading_slew; + + // heading when in yaw_look_ahead_bearing + float yaw_look_ahead_bearing; + + // Delay Mission Scripting Command + int32_t condition_value; // used in condition commands (eg delay, change alt, etc.) + uint32_t condition_start; + + // IMU variables + // Integration time (in seconds) for the gyros (DCM algorithm) + // Updated with the fast loop + float G_Dt; + + // Inertial Navigation + AP_InertialNav_NavEKF inertial_nav; + + // Attitude, Position and Waypoint navigation objects + // To-Do: move inertial nav up or other navigation variables down here +#if FRAME_CONFIG == HELI_FRAME + AC_AttitudeControl_Heli attitude_control; +#else + AC_AttitudeControl_Multi attitude_control; +#endif + AC_PosControl pos_control; + AC_WPNav wp_nav; + AC_Circle circle_nav; + + // Performance monitoring + int16_t pmTest1; + + // System Timers + // -------------- + // Time in microseconds of main control loop + uint32_t fast_loopTimer; + // Counter of main loop executions. Used for performance monitoring and failsafe processing + uint16_t mainLoop_count; + // Loiter timer - Records how long we have been in loiter + uint32_t rtl_loiter_start_time; + + // Used to exit the roll and pitch auto trim function + uint8_t auto_trim_counter; + + // Reference to the relay object + AP_Relay relay; + + // handle repeated servo and relay events + AP_ServoRelayEvents ServoRelayEvents; + + // Reference to the camera object (it uses the relay object inside it) +#if CAMERA == ENABLED + AP_Camera camera; +#endif + + // Camera/Antenna mount tracking and stabilisation stuff +#if MOUNT == ENABLED + // current_loc uses the baro/gps soloution for altitude rather than gps only. + AP_Mount camera_mount; +#endif + + // AC_Fence library to reduce fly-aways +#if AC_FENCE == ENABLED + AC_Fence fence; +#endif + + // Rally library +#if AC_RALLY == ENABLED + AP_Rally rally; +#endif + + // RSSI + AP_RSSI rssi; + + // Crop Sprayer +#if SPRAYER == ENABLED + AC_Sprayer sprayer; +#endif + + // EPM Cargo Griper +#if EPM_ENABLED == ENABLED + AP_EPM epm; +#endif + + // Parachute release +#if PARACHUTE == ENABLED + AP_Parachute parachute; +#endif + + // Landing Gear Controller + AP_LandingGear landinggear; + + // terrain handling +#if AP_TERRAIN_AVAILABLE && AC_TERRAIN + AP_Terrain terrain; +#endif + + // Precision Landing +#if PRECISION_LANDING == ENABLED + AC_PrecLand precland; +#endif + + // Pilot Input Management Library + // Only used for Helicopter for AC3.3, to be expanded to include Multirotor + // child class for AC3.4 +#if FRAME_CONFIG == HELI_FRAME + AC_InputManager_Heli input_manager; +#endif + + AP_ADSB adsb {ahrs}; + + // use this to prevent recursion during sensor init + bool in_mavlink_delay; + + // true if we are out of time in our event timeslice + bool gcs_out_of_time; + + // Top-level logic + // setup the var_info table + AP_Param param_loader; + +#if FRAME_CONFIG == HELI_FRAME + // Mode filter to reject RC Input glitches. Filter size is 5, and it draws the 4th element, so it can reject 3 low glitches, + // and 1 high glitch. This is because any "off" glitches can be highly problematic for a helicopter running an ESC + // governor. Even a single "off" frame can cause the rotor to slow dramatically and take a long time to restart. + ModeFilterInt16_Size5 rotor_speed_deglitch_filter {4}; + + int16_t rsc_control_deglitched; + + // Tradheli flags + struct { + uint8_t dynamic_flight : 1; // 0 // true if we are moving at a significant speed (used to turn on/off leaky I terms) + uint8_t init_targets_on_arming : 1; // 1 // true if we have been disarmed, and need to reset rate controller targets when we arm + } heli_flags; +#endif + +#if GNDEFFECT_COMPENSATION == ENABLED + // ground effect detector + struct { + bool takeoff_expected; + bool touchdown_expected; + uint32_t takeoff_time_ms; + float takeoff_alt_cm; + } gndeffect_state; +#endif // GNDEFFECT_COMPENSATION == ENABLED + + static const AP_Scheduler::Task scheduler_tasks[]; + static const AP_Param::Info var_info[]; + static const struct LogStructure log_structure[]; + + void compass_accumulate(void); + void compass_cal_update(void); + void barometer_accumulate(void); + void perf_update(void); + void fast_loop(); + void rc_loop(); + void throttle_loop(); + void update_mount(); + void update_batt_compass(void); + void ten_hz_logging_loop(); + void fifty_hz_logging_loop(); + void full_rate_logging_loop(); + void three_hz_loop(); + void one_hz_loop(); + void update_GPS(void); + void init_simple_bearing(); + void update_simple_mode(void); + void update_super_simple_bearing(bool force_update); + void read_AHRS(void); + void update_altitude(); + void set_home_state(enum HomeState new_home_state); + bool home_is_set(); + void set_auto_armed(bool b); + void set_simple_mode(uint8_t b); + void set_failsafe_radio(bool b); + void set_failsafe_battery(bool b); + void set_failsafe_gcs(bool b); + void set_land_complete(bool b); + void set_land_complete_maybe(bool b); + void set_pre_arm_check(bool b); + void set_pre_arm_rc_check(bool b); + void update_using_interlock(); + void set_motor_emergency_stop(bool b); + float get_smoothing_gain(); + void get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &roll_out, float &pitch_out, float angle_max); + float get_pilot_desired_yaw_rate(int16_t stick_angle); + void check_ekf_yaw_reset(); + float get_roi_yaw(); + float get_look_ahead_yaw(); + void update_thr_average(); + void set_throttle_takeoff(); + int16_t get_pilot_desired_throttle(int16_t throttle_control); + float get_pilot_desired_climb_rate(float throttle_control); + float get_non_takeoff_throttle(); + float get_takeoff_trigger_throttle(); + float get_throttle_pre_takeoff(float input_thr); + float get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt); + void set_accel_throttle_I_from_pilot_throttle(int16_t pilot_throttle); + void update_poscon_alt_max(); + void rotate_body_frame_to_NE(float &x, float &y); + void gcs_send_heartbeat(void); + void gcs_send_deferred(void); + void send_heartbeat(mavlink_channel_t chan); + void send_attitude(mavlink_channel_t chan); + void send_limits_status(mavlink_channel_t chan); + void send_extended_status1(mavlink_channel_t chan); + void send_location(mavlink_channel_t chan); + void send_nav_controller_output(mavlink_channel_t chan); + void send_simstate(mavlink_channel_t chan); + void send_hwstatus(mavlink_channel_t chan); + void send_servo_out(mavlink_channel_t chan); + void send_radio_out(mavlink_channel_t chan); + void send_vfr_hud(mavlink_channel_t chan); + void send_current_waypoint(mavlink_channel_t chan); + void send_rangefinder(mavlink_channel_t chan); + void send_rpm(mavlink_channel_t chan); + void rpm_update(); + void send_pid_tuning(mavlink_channel_t chan); + void send_statustext(mavlink_channel_t chan); + bool telemetry_delayed(mavlink_channel_t chan); + void gcs_send_message(enum ap_message id); + void gcs_send_mission_item_reached_message(uint16_t mission_index); + void gcs_data_stream_send(void); + void gcs_check_input(void); + void gcs_send_text(MAV_SEVERITY severity, const char *str); + void do_erase_logs(void); + void 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 Log_Write_AutoTuneDetails(float angle_cd, float rate_cds); + void Log_Write_Current(); + void Log_Write_Optflow(); + void Log_Write_Nav_Tuning(); + void Log_Write_Control_Tuning(); + void Log_Write_Performance(); + void Log_Write_Attitude(); + void Log_Write_Rate(); + void Log_Write_MotBatt(); + void Log_Write_Startup(); + void Log_Write_Event(uint8_t id); + void Log_Write_Data(uint8_t id, int32_t value); + void Log_Write_Data(uint8_t id, uint32_t value); + void Log_Write_Data(uint8_t id, int16_t value); + void Log_Write_Data(uint8_t id, uint16_t value); + void Log_Write_Data(uint8_t id, float value); + void Log_Write_Error(uint8_t sub_system, uint8_t error_code); + void Log_Write_Baro(void); + void Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, int16_t control_in, int16_t tune_low, int16_t tune_high); + void Log_Write_Home_And_Origin(); + void Log_Sensor_Health(); +#if FRAME_CONFIG == HELI_FRAME + void Log_Write_Heli(void); +#endif + void Log_Write_Precland(); + void Log_Write_Vehicle_Startup_Messages(); + void Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page); + void start_logging() ; + void load_parameters(void); + void userhook_init(); + void userhook_FastLoop(); + void userhook_50Hz(); + void userhook_MediumLoop(); + void userhook_SlowLoop(); + void userhook_SuperSlowLoop(); + void update_home_from_EKF(); + void set_home_to_current_location_inflight(); + bool set_home_to_current_location(); + bool set_home_to_current_location_and_lock(); + bool set_home_and_lock(const Location& loc); + bool set_home(const Location& loc); + bool far_from_EKF_origin(const Location& loc); + void set_system_time_from_GPS(); + void exit_mission(); + void do_RTL(void); + bool verify_takeoff(); + bool verify_land(); + bool verify_loiter_unlimited(); + bool verify_loiter_time(); + bool verify_RTL(); + bool verify_wait_delay(); + bool verify_change_alt(); + bool verify_within_distance(); + bool verify_yaw(); + void do_take_picture(); + void log_picture(); + uint8_t mavlink_compassmot(mavlink_channel_t chan); + void delay(uint32_t ms); + bool acro_init(bool ignore_checks); + void acro_run(); + void 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); + bool althold_init(bool ignore_checks); + void althold_run(); + bool auto_init(bool ignore_checks); + void auto_run(); + void auto_takeoff_start(float final_alt_above_home); + void auto_takeoff_run(); + void auto_wp_start(const Vector3f& destination); + void auto_wp_run(); + void auto_spline_run(); + void auto_land_start(); + void auto_land_start(const Vector3f& destination); + void auto_land_run(); + void auto_rtl_start(); + void auto_rtl_run(); + void auto_circle_movetoedge_start(); + void auto_circle_start(); + void auto_circle_run(); + void auto_nav_guided_start(); + void auto_nav_guided_run(); + bool auto_loiter_start(); + void auto_loiter_run(); + uint8_t get_default_auto_yaw_mode(bool rtl); + void set_auto_yaw_mode(uint8_t yaw_mode); + void set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, uint8_t relative_angle); + void set_auto_yaw_roi(const Location &roi_location); + float get_auto_heading(void); + bool autotune_init(bool ignore_checks); + void autotune_stop(); + bool autotune_start(bool ignore_checks); + void autotune_run(); + void autotune_attitude_control(); + void autotune_backup_gains_and_initialise(); + void autotune_load_orig_gains(); + void autotune_load_tuned_gains(); + void autotune_load_intra_test_gains(); + void autotune_load_twitch_gains(); + void autotune_save_tuning_gains(); + void autotune_update_gcs(uint8_t message_id); + bool autotune_roll_enabled(); + bool autotune_pitch_enabled(); + bool autotune_yaw_enabled(); + void autotune_twitching_test(float measurement, float target, float &measurement_min, float &measurement_max); + void autotune_updating_d_up(float &tune_d, float tune_d_min, float tune_d_max, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max); + void autotune_updating_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max); + void autotune_updating_p_down(float &tune_p, float tune_p_min, float tune_p_step_ratio, float target, float measurement_max); + void autotune_updating_p_up(float &tune_p, float tune_p_max, float tune_p_step_ratio, float target, float measurement_max); + void autotune_updating_p_up_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max); + void autotune_twitching_measure_acceleration(float &rate_of_change, float rate_measurement, float &rate_measurement_max); + void adsb_update(void); + void adsb_handle_vehicle_threats(void); + bool brake_init(bool ignore_checks); + void brake_run(); + bool circle_init(bool ignore_checks); + void circle_run(); + bool drift_init(bool ignore_checks); + void drift_run(); + int16_t get_throttle_assist(float velz, int16_t pilot_throttle_scaled); + bool flip_init(bool ignore_checks); + void flip_run(); + bool guided_init(bool ignore_checks); + void guided_takeoff_start(float final_alt_above_home); + void guided_pos_control_start(); + void guided_vel_control_start(); + void guided_posvel_control_start(); + void guided_angle_control_start(); + void guided_set_destination(const Vector3f& destination); + void guided_set_velocity(const Vector3f& velocity); + void guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity); + void guided_set_angle(const Quaternion &q, float climb_rate_cms); + void guided_run(); + void guided_takeoff_run(); + void guided_pos_control_run(); + void guided_vel_control_run(); + void guided_posvel_control_run(); + void guided_angle_control_run(); + void guided_limit_clear(); + void guided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm); + void guided_limit_init_time_and_pos(); + bool guided_limit_check(); + bool land_init(bool ignore_checks); + void land_run(); + void land_gps_run(); + void land_nogps_run(); + float get_land_descent_speed(); + void land_do_not_use_GPS(); + void set_mode_land_with_pause(); + bool landing_with_GPS(); + bool loiter_init(bool ignore_checks); + void loiter_run(); + bool poshold_init(bool ignore_checks); + void poshold_run(); + void poshold_update_pilot_lean_angle(float &lean_angle_filtered, float &lean_angle_raw); + int16_t poshold_mix_controls(float mix_ratio, int16_t first_control, int16_t second_control); + void poshold_update_brake_angle_from_velocity(int16_t &brake_angle, float velocity); + void poshold_update_wind_comp_estimate(); + void poshold_get_wind_comp_lean_angles(int16_t &roll_angle, int16_t &pitch_angle); + void poshold_roll_controller_to_pilot_override(); + void poshold_pitch_controller_to_pilot_override(); + + bool rtl_init(bool ignore_checks); + void rtl_run(); + void rtl_climb_start(); + void rtl_return_start(); + void rtl_climb_return_run(); + void rtl_loiterathome_start(); + void rtl_loiterathome_run(); + void rtl_descent_start(); + void rtl_descent_run(); + void rtl_land_start(); + void rtl_land_run(); + float get_RTL_alt(); + bool sport_init(bool ignore_checks); + void sport_run(); + bool stabilize_init(bool ignore_checks); + void stabilize_run(); + void crash_check(); + void parachute_check(); + void parachute_release(); + void parachute_manual_release(); + void ekf_check(); + bool ekf_over_threshold(); + void failsafe_ekf_event(); + void failsafe_ekf_off_event(void); + void esc_calibration_startup_check(); + void esc_calibration_passthrough(); + void esc_calibration_auto(); + void failsafe_radio_on_event(); + void failsafe_radio_off_event(); + void failsafe_battery_event(void); + void failsafe_gcs_check(); + void failsafe_gcs_off_event(void); + void set_mode_RTL_or_land_with_pause(); + void update_events(); + void failsafe_enable(); + void failsafe_disable(); + void fence_check(); + void fence_send_mavlink_status(mavlink_channel_t chan); + bool set_mode(uint8_t mode); + void update_flight_mode(); + void exit_mode(uint8_t old_control_mode, uint8_t new_control_mode); + bool mode_requires_GPS(uint8_t mode); + bool mode_has_manual_throttle(uint8_t mode); + bool mode_allows_arming(uint8_t mode, bool arming_from_gcs); + void notify_flight_mode(uint8_t mode); + void heli_init(); + void check_dynamic_flight(void); + void update_heli_control_dynamics(void); + void heli_update_landing_swash(); + void heli_update_rotor_speed_targets(); + void heli_radio_passthrough(); + bool heli_acro_init(bool ignore_checks); + void heli_acro_run(); + bool heli_stabilize_init(bool ignore_checks); + void heli_stabilize_run(); + void read_inertia(); + void read_inertial_altitude(); + bool land_complete_maybe(); + void update_land_and_crash_detectors(); + void update_land_detector(); + void update_throttle_thr_mix(); +#if GNDEFFECT_COMPENSATION == ENABLED + void update_ground_effect_detector(void); +#endif // GNDEFFECT_COMPENSATION == ENABLED + void landinggear_update(); + void update_notify(); + void motor_test_output(); + bool mavlink_motor_test_check(mavlink_channel_t chan, bool check_rc); + uint8_t mavlink_motor_test_start(mavlink_channel_t chan, uint8_t motor_seq, uint8_t throttle_type, uint16_t throttle_value, float timeout_sec); + void motor_test_stop(); + void arm_motors_check(); + void auto_disarm_check(); + bool init_arm_motors(bool arming_from_gcs); + void update_arming_checks(void); + bool all_arming_checks_passing(bool arming_from_gcs); + bool pre_arm_checks(bool display_failure); + void pre_arm_rc_checks(); + bool pre_arm_gps_checks(bool display_failure); + bool pre_arm_ekf_attitude_check(); + bool arm_checks(bool display_failure, bool arming_from_gcs); + void init_disarm_motors(); + void motors_output(); + void lost_vehicle_check(); + void run_nav_updates(void); + void calc_position(); + void calc_distance_and_bearing(); + void calc_wp_distance(); + void calc_wp_bearing(); + void calc_home_distance_and_bearing(); + void run_autopilot(); + void perf_info_reset(); + void perf_ignore_this_loop(); + void perf_info_check_loop_time(uint32_t time_in_micros); + uint16_t perf_info_get_num_loops(); + uint32_t perf_info_get_max_time(); + uint32_t perf_info_get_min_time(); + uint16_t perf_info_get_num_long_running(); + Vector3f pv_location_to_vector(const Location& loc); + Vector3f pv_location_to_vector_with_default(const Location& loc, const Vector3f& default_posvec); + float pv_alt_above_origin(float alt_above_home_cm); + float pv_alt_above_home(float alt_above_origin_cm); + float pv_get_bearing_cd(const Vector3f &origin, const Vector3f &destination); + float pv_get_horizontal_distance_cm(const Vector3f &origin, const Vector3f &destination); + void default_dead_zones(); + void init_rc_in(); + void init_rc_out(); + void enable_motor_output(); + void read_radio(); + void set_throttle_and_failsafe(uint16_t throttle_pwm); + void set_throttle_zero_flag(int16_t throttle_control); + void init_barometer(bool full_calibration); + void read_barometer(void); + void init_sonar(void); + int16_t read_sonar(void); + void init_compass(); + void init_optflow(); + void update_optical_flow(void); + void init_precland(); + void update_precland(); + void read_battery(void); + void read_receiver_rssi(void); + void epm_update(); + void report_batt_monitor(); + void report_frame(); + void report_radio(); + void report_ins(); + void report_flight_modes(); + void report_optflow(); + void print_radio_values(); + void print_switch(uint8_t p, uint8_t m, bool b); + void print_accel_offsets_and_scaling(void); + void print_gyro_offsets(void); + void report_compass(); + void print_blanks(int16_t num); + void print_divider(void); + void print_enabled(bool b); + void report_version(); + void read_control_switch(); + bool check_if_auxsw_mode_used(uint8_t auxsw_mode_check); + bool check_duplicate_auxsw(void); + void reset_control_switch(); + uint8_t read_3pos_switch(int16_t radio_in); + void read_aux_switches(); + void init_aux_switches(); + void init_aux_switch_function(int8_t ch_option, uint8_t ch_flag); + void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag); + void save_trim(); + void auto_trim(); + void init_ardupilot(); + void startup_INS_ground(); + bool calibrate_gyros(); + bool position_ok(); + bool ekf_position_ok(); + bool optflow_position_ok(); + void update_auto_armed(); + void check_usb_mux(void); + void frsky_telemetry_send(void); + bool should_log(uint32_t mask); + bool current_mode_has_user_takeoff(bool must_navigate); + bool do_user_takeoff(float takeoff_alt_cm, bool must_navigate); + void takeoff_timer_start(float alt_cm); + void takeoff_stop(); + void takeoff_get_climb_rates(float& pilot_climb_rate, float& takeoff_climb_rate); + void print_hit_enter(); + void tuning(); + void gcs_send_text_fmt(MAV_SEVERITY severity, const char *fmt, ...); + bool start_command(const AP_Mission::Mission_Command& cmd); + bool verify_command(const AP_Mission::Mission_Command& cmd); + bool verify_command_callback(const AP_Mission::Mission_Command& cmd); + + bool do_guided(const AP_Mission::Mission_Command& cmd); + void do_takeoff(const AP_Mission::Mission_Command& cmd); + void do_nav_wp(const AP_Mission::Mission_Command& cmd); + void do_land(const AP_Mission::Mission_Command& cmd); + void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd); + void do_circle(const AP_Mission::Mission_Command& cmd); + void do_loiter_time(const AP_Mission::Mission_Command& cmd); + void do_spline_wp(const AP_Mission::Mission_Command& cmd); +#if NAV_GUIDED == ENABLED + void do_nav_guided_enable(const AP_Mission::Mission_Command& cmd); + void do_guided_limits(const AP_Mission::Mission_Command& cmd); +#endif + void do_wait_delay(const AP_Mission::Mission_Command& cmd); + void do_within_distance(const AP_Mission::Mission_Command& cmd); + void do_change_alt(const AP_Mission::Mission_Command& cmd); + void do_yaw(const AP_Mission::Mission_Command& cmd); + void do_change_speed(const AP_Mission::Mission_Command& cmd); + void do_set_home(const AP_Mission::Mission_Command& cmd); + void do_roi(const AP_Mission::Mission_Command& cmd); + void do_mount_control(const AP_Mission::Mission_Command& cmd); +#if CAMERA == ENABLED + void do_digicam_configure(const AP_Mission::Mission_Command& cmd); + void do_digicam_control(const AP_Mission::Mission_Command& cmd); +#endif +#if PARACHUTE == ENABLED + void do_parachute(const AP_Mission::Mission_Command& cmd); +#endif +#if EPM_ENABLED == ENABLED + void do_gripper(const AP_Mission::Mission_Command& cmd); +#endif + bool verify_nav_wp(const AP_Mission::Mission_Command& cmd); + bool verify_circle(const AP_Mission::Mission_Command& cmd); + bool verify_spline_wp(const AP_Mission::Mission_Command& cmd); +#if NAV_GUIDED == ENABLED + bool verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd); +#endif + void auto_spline_start(const Vector3f& destination, bool stopped_at_start, AC_WPNav::spline_segment_end_type seg_end_type, const Vector3f& next_spline_destination); + + void print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode); + void log_init(void); + void run_cli(AP_HAL::UARTDriver *port); + void init_capabilities(void); + void dataflash_periodic(void); + void accel_cal_update(void); + +public: + void mavlink_delay_cb(); + void failsafe_check(); + int8_t dump_log(uint8_t argc, const Menu::arg *argv); + int8_t erase_logs(uint8_t argc, const Menu::arg *argv); + int8_t select_logs(uint8_t argc, const Menu::arg *argv); + bool print_log_menu(void); + + int8_t process_logs(uint8_t argc, const Menu::arg *argv); + int8_t main_menu_help(uint8_t, const Menu::arg*); + int8_t setup_mode(uint8_t argc, const Menu::arg *argv); + int8_t setup_factory(uint8_t argc, const Menu::arg *argv); + int8_t setup_set(uint8_t argc, const Menu::arg *argv); + int8_t setup_show(uint8_t argc, const Menu::arg *argv); + int8_t esc_calib(uint8_t argc, const Menu::arg *argv); + + int8_t test_mode(uint8_t argc, const Menu::arg *argv); + int8_t test_baro(uint8_t argc, const Menu::arg *argv); + int8_t test_compass(uint8_t argc, const Menu::arg *argv); + int8_t test_ins(uint8_t argc, const Menu::arg *argv); + int8_t test_optflow(uint8_t argc, const Menu::arg *argv); + int8_t test_relay(uint8_t argc, const Menu::arg *argv); + int8_t test_shell(uint8_t argc, const Menu::arg *argv); + int8_t test_sonar(uint8_t argc, const Menu::arg *argv); + + int8_t reboot_board(uint8_t argc, const Menu::arg *argv); +}; + +#define MENU_FUNC(func) FUNCTOR_BIND(&copter, &Copter::func, int8_t, uint8_t, const Menu::arg *) + +extern const AP_HAL::HAL& hal; +extern Copter copter; + +using AP_HAL::millis; +using AP_HAL::micros; + +#endif // _COPTER_H_ diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp new file mode 100644 index 00000000000..0acde9dbed1 --- /dev/null +++ b/ArduSub/GCS_Mavlink.cpp @@ -0,0 +1,2053 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include "Copter.h" + +// default sensors are present and healthy: gyro, accelerometer, barometer, rate_control, attitude_stabilization, yaw_position, altitude control, x/y position control, motor_control +#define MAVLINK_SENSOR_PRESENT_DEFAULT (MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL | MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE | MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL | MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION | MAV_SYS_STATUS_SENSOR_YAW_POSITION | MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL | MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL | MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS | MAV_SYS_STATUS_AHRS) + +void Copter::gcs_send_heartbeat(void) +{ + gcs_send_message(MSG_HEARTBEAT); +} + +void Copter::gcs_send_deferred(void) +{ + gcs_send_message(MSG_RETRY_DEFERRED); +} + +/* + * !!NOTE!! + * + * the use of NOINLINE separate functions for each message type avoids + * a compiler bug in gcc that would cause it to use far more stack + * space than is needed. Without the NOINLINE we use the sum of the + * stack needed for each message type. Please be careful to follow the + * pattern below when adding any new messages + */ + +NOINLINE void Copter::send_heartbeat(mavlink_channel_t chan) +{ + uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; + uint8_t system_status = ap.land_complete ? MAV_STATE_STANDBY : MAV_STATE_ACTIVE; + uint32_t custom_mode = control_mode; + + // set system as critical if any failsafe have triggered + if (failsafe.radio || failsafe.battery || failsafe.gcs || failsafe.ekf) { + system_status = MAV_STATE_CRITICAL; + } + + // work out the base_mode. This value is not very useful + // for APM, but we calculate it as best we can so a generic + // MAVLink enabled ground station can work out something about + // what the MAV is up to. The actual bit values are highly + // ambiguous for most of the APM flight modes. In practice, you + // only get useful information from the custom_mode, which maps to + // the APM flight mode and has a well defined meaning in the + // ArduPlane documentation + base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED; + switch (control_mode) { + case AUTO: + case RTL: + case LOITER: + case GUIDED: + case CIRCLE: + case POSHOLD: + case BRAKE: + base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; + // note that MAV_MODE_FLAG_AUTO_ENABLED does not match what + // APM does in any mode, as that is defined as "system finds its own goal + // positions", which APM does not currently do + break; + } + + // all modes except INITIALISING have some form of manual + // override if stick mixing is enabled + base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; + +#if HIL_MODE != HIL_MODE_DISABLED + base_mode |= MAV_MODE_FLAG_HIL_ENABLED; +#endif + + // we are armed if we are not initialising + if (motors.armed()) { + base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; + } + + // indicate we have set a custom mode + base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; + + mavlink_msg_heartbeat_send( + chan, +#if (FRAME_CONFIG == QUAD_FRAME) + MAV_TYPE_QUADROTOR, +#elif (FRAME_CONFIG == TRI_FRAME) + MAV_TYPE_TRICOPTER, +#elif (FRAME_CONFIG == HEXA_FRAME || FRAME_CONFIG == Y6_FRAME) + MAV_TYPE_HEXAROTOR, +#elif (FRAME_CONFIG == OCTA_FRAME || FRAME_CONFIG == OCTA_QUAD_FRAME) + MAV_TYPE_OCTOROTOR, +#elif (FRAME_CONFIG == HELI_FRAME) + MAV_TYPE_HELICOPTER, +#elif (FRAME_CONFIG == SINGLE_FRAME) //because mavlink did not define a singlecopter, we use a rocket + MAV_TYPE_ROCKET, +#elif (FRAME_CONFIG == COAX_FRAME) //because mavlink did not define a singlecopter, we use a rocket + MAV_TYPE_ROCKET, +#else + #error Unrecognised frame type +#endif + MAV_AUTOPILOT_ARDUPILOTMEGA, + base_mode, + custom_mode, + system_status); +} + +NOINLINE void Copter::send_attitude(mavlink_channel_t chan) +{ + const Vector3f &gyro = ins.get_gyro(); + mavlink_msg_attitude_send( + chan, + millis(), + ahrs.roll, + ahrs.pitch, + ahrs.yaw, + gyro.x, + gyro.y, + gyro.z); +} + +#if AC_FENCE == ENABLED +NOINLINE void Copter::send_limits_status(mavlink_channel_t chan) +{ + fence_send_mavlink_status(chan); +} +#endif + + +NOINLINE void Copter::send_extended_status1(mavlink_channel_t chan) +{ + uint32_t control_sensors_present; + uint32_t control_sensors_enabled; + uint32_t control_sensors_health; + + // default sensors present + control_sensors_present = MAVLINK_SENSOR_PRESENT_DEFAULT; + + // first what sensors/controllers we have + if (g.compass_enabled) { + control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG; // compass present + } + if (gps.status() > AP_GPS::NO_GPS) { + control_sensors_present |= MAV_SYS_STATUS_SENSOR_GPS; + } +#if OPTFLOW == ENABLED + if (optflow.enabled()) { + control_sensors_present |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; + } +#endif + if (ap.rc_receiver_present) { + control_sensors_present |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER; + } + + // all present sensors enabled by default except altitude and position control and motors which we will set individually + control_sensors_enabled = control_sensors_present & (~MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL & + ~MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL & + ~MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS); + + switch (control_mode) { + case ALT_HOLD: + case AUTO: + case GUIDED: + case LOITER: + case RTL: + case CIRCLE: + case LAND: + case OF_LOITER: + case POSHOLD: + case BRAKE: + control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL; + control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; + break; + case SPORT: + control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL; + break; + } + + // set motors outputs as enabled if safety switch is not disarmed (i.e. either NONE or ARMED) + if (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED) { + control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS; + } + + // default to all healthy except baro, compass, gps and receiver which we set individually + control_sensors_health = control_sensors_present & ~(MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE | + MAV_SYS_STATUS_SENSOR_3D_MAG | + MAV_SYS_STATUS_SENSOR_GPS | + MAV_SYS_STATUS_SENSOR_RC_RECEIVER); + if (barometer.all_healthy()) { + control_sensors_health |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; + } + if (g.compass_enabled && compass.healthy() && ahrs.use_compass()) { + control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG; + } + if (gps.status() > AP_GPS::NO_GPS) { + control_sensors_health |= MAV_SYS_STATUS_SENSOR_GPS; + } +#if OPTFLOW == ENABLED + if (optflow.healthy()) { + control_sensors_health |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; + } +#endif + if (ap.rc_receiver_present && !failsafe.radio) { + control_sensors_health |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER; + } + if (!ins.get_gyro_health_all() || !ins.gyro_calibrated_ok_all()) { + control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_3D_GYRO; + } + if (!ins.get_accel_health_all()) { + control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_3D_ACCEL; + } + + if (ahrs.initialised() && !ahrs.healthy()) { + // AHRS subsystem is unhealthy + control_sensors_health &= ~MAV_SYS_STATUS_AHRS; + } + + int16_t battery_current = -1; + int8_t battery_remaining = -1; + + if (battery.has_current() && battery.healthy()) { + battery_remaining = battery.capacity_remaining_pct(); + battery_current = battery.current_amps() * 100; + } + +#if AP_TERRAIN_AVAILABLE && AC_TERRAIN + switch (terrain.status()) { + case AP_Terrain::TerrainStatusDisabled: + break; + case AP_Terrain::TerrainStatusUnhealthy: + // To-Do: restore unhealthy terrain status reporting once terrain is used in copter + //control_sensors_present |= MAV_SYS_STATUS_TERRAIN; + //control_sensors_enabled |= MAV_SYS_STATUS_TERRAIN; + //break; + case AP_Terrain::TerrainStatusOK: + control_sensors_present |= MAV_SYS_STATUS_TERRAIN; + control_sensors_enabled |= MAV_SYS_STATUS_TERRAIN; + control_sensors_health |= MAV_SYS_STATUS_TERRAIN; + break; + } +#endif + +#if CONFIG_SONAR == ENABLED + if (sonar.num_sensors() > 0) { + control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; + control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; + if (sonar.has_data()) { + control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; + } + } +#endif + + if (!ap.initialised || ins.calibrating()) { + // while initialising the gyros and accels are not enabled + control_sensors_enabled &= ~(MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL); + control_sensors_health &= ~(MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL); + } + + mavlink_msg_sys_status_send( + chan, + control_sensors_present, + control_sensors_enabled, + control_sensors_health, + (uint16_t)(scheduler.load_average(MAIN_LOOP_MICROS) * 1000), + battery.voltage() * 1000, // mV + battery_current, // in 10mA units + battery_remaining, // in % + 0, // comm drops %, + 0, // comm drops in pkts, + 0, 0, 0, 0); + +} + +void NOINLINE Copter::send_location(mavlink_channel_t chan) +{ + uint32_t fix_time; + // if we have a GPS fix, take the time as the last fix time. That + // allows us to correctly calculate velocities and extrapolate + // positions. + // If we don't have a GPS fix then we are dead reckoning, and will + // use the current boot time as the fix time. + if (gps.status() >= AP_GPS::GPS_OK_FIX_2D) { + fix_time = gps.last_fix_time_ms(); + } else { + fix_time = millis(); + } + const Vector3f &vel = inertial_nav.get_velocity(); + mavlink_msg_global_position_int_send( + chan, + fix_time, + current_loc.lat, // in 1E7 degrees + current_loc.lng, // in 1E7 degrees + (ahrs.get_home().alt + current_loc.alt) * 10UL, // millimeters above sea level + current_loc.alt * 10, // millimeters above ground + vel.x, // X speed cm/s (+ve North) + vel.y, // Y speed cm/s (+ve East) + vel.z, // Z speed cm/s (+ve up) + ahrs.yaw_sensor); // compass heading in 1/100 degree +} + +void NOINLINE Copter::send_nav_controller_output(mavlink_channel_t chan) +{ + const Vector3f &targets = attitude_control.get_att_target_euler_cd(); + mavlink_msg_nav_controller_output_send( + chan, + targets.x / 1.0e2f, + targets.y / 1.0e2f, + targets.z / 1.0e2f, + wp_bearing / 1.0e2f, + wp_distance / 1.0e2f, + pos_control.get_alt_error() / 1.0e2f, + 0, + 0); +} + +// report simulator state +void NOINLINE Copter::send_simstate(mavlink_channel_t chan) +{ +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + sitl.simstate_send(chan); +#endif +} + +void NOINLINE Copter::send_hwstatus(mavlink_channel_t chan) +{ + mavlink_msg_hwstatus_send( + chan, + hal.analogin->board_voltage()*1000, + hal.i2c->lockup_count()); +} + +void NOINLINE Copter::send_servo_out(mavlink_channel_t chan) +{ +#if HIL_MODE != HIL_MODE_DISABLED + // normalized values scaled to -10000 to 10000 + // This is used for HIL. Do not change without discussing with HIL maintainers + +#if FRAME_CONFIG == HELI_FRAME + mavlink_msg_rc_channels_scaled_send( + chan, + millis(), + 0, // port 0 + g.rc_1.servo_out, + g.rc_2.servo_out, + g.rc_3.radio_out, + g.rc_4.servo_out, + 0, + 0, + 0, + 0, + receiver_rssi); +#else + mavlink_msg_rc_channels_scaled_send( + chan, + millis(), + 0, // port 0 + g.rc_1.servo_out, + g.rc_2.servo_out, + g.rc_3.radio_out, + g.rc_4.servo_out, + 10000 * g.rc_1.norm_output(), + 10000 * g.rc_2.norm_output(), + 10000 * g.rc_3.norm_output(), + 10000 * g.rc_4.norm_output(), + receiver_rssi); +#endif +#endif // HIL_MODE +} + +void NOINLINE Copter::send_radio_out(mavlink_channel_t chan) +{ + mavlink_msg_servo_output_raw_send( + chan, + micros(), + 0, // port + hal.rcout->read(0), + hal.rcout->read(1), + hal.rcout->read(2), + hal.rcout->read(3), + hal.rcout->read(4), + hal.rcout->read(5), + hal.rcout->read(6), + hal.rcout->read(7)); +} + +void NOINLINE Copter::send_vfr_hud(mavlink_channel_t chan) +{ + mavlink_msg_vfr_hud_send( + chan, + gps.ground_speed(), + gps.ground_speed(), + (ahrs.yaw_sensor / 100) % 360, + (int16_t)(motors.get_throttle())/10, + current_loc.alt / 100.0f, + climb_rate / 100.0f); +} + +void NOINLINE Copter::send_current_waypoint(mavlink_channel_t chan) +{ + mavlink_msg_mission_current_send(chan, mission.get_current_nav_index()); +} + +#if CONFIG_SONAR == ENABLED +void NOINLINE Copter::send_rangefinder(mavlink_channel_t chan) +{ + // exit immediately if sonar is disabled + if (!sonar.has_data()) { + return; + } + mavlink_msg_rangefinder_send( + chan, + sonar.distance_cm() * 0.01f, + sonar.voltage_mv() * 0.001f); +} +#endif + +/* + send RPM packet + */ +void NOINLINE Copter::send_rpm(mavlink_channel_t chan) +{ + if (rpm_sensor.enabled(0) || rpm_sensor.enabled(1)) { + mavlink_msg_rpm_send( + chan, + rpm_sensor.get_rpm(0), + rpm_sensor.get_rpm(1)); + } +} + + +/* + send PID tuning message + */ +void Copter::send_pid_tuning(mavlink_channel_t chan) +{ + const Vector3f &gyro = ahrs.get_gyro(); + if (g.gcs_pid_mask & 1) { + const DataFlash_Class::PID_Info &pid_info = g.pid_rate_roll.get_pid_info(); + mavlink_msg_pid_tuning_send(chan, PID_TUNING_ROLL, + pid_info.desired*0.01f, + degrees(gyro.x), + pid_info.FF*0.01f, + pid_info.P*0.01f, + pid_info.I*0.01f, + pid_info.D*0.01f); + if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) { + return; + } + } + if (g.gcs_pid_mask & 2) { + const DataFlash_Class::PID_Info &pid_info = g.pid_rate_pitch.get_pid_info(); + mavlink_msg_pid_tuning_send(chan, PID_TUNING_PITCH, + pid_info.desired*0.01f, + degrees(gyro.y), + pid_info.FF*0.01f, + pid_info.P*0.01f, + pid_info.I*0.01f, + pid_info.D*0.01f); + if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) { + return; + } + } + if (g.gcs_pid_mask & 4) { + const DataFlash_Class::PID_Info &pid_info = g.pid_rate_yaw.get_pid_info(); + mavlink_msg_pid_tuning_send(chan, PID_TUNING_YAW, + pid_info.desired*0.01f, + degrees(gyro.z), + pid_info.FF*0.01f, + pid_info.P*0.01f, + pid_info.I*0.01f, + pid_info.D*0.01f); + if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) { + return; + } + } + if (g.gcs_pid_mask & 8) { + const DataFlash_Class::PID_Info &pid_info = g.pid_accel_z.get_pid_info(); + mavlink_msg_pid_tuning_send(chan, PID_TUNING_ACCZ, + pid_info.desired*0.01f, + -(ahrs.get_accel_ef_blended().z + GRAVITY_MSS), + pid_info.FF*0.01f, + pid_info.P*0.01f, + pid_info.I*0.01f, + pid_info.D*0.01f); + if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) { + return; + } + } +} + + +void NOINLINE Copter::send_statustext(mavlink_channel_t chan) +{ + mavlink_statustext_t *s = &gcs[chan-MAVLINK_COMM_0].pending_status; + mavlink_msg_statustext_send( + chan, + s->severity, + s->text); +} + +// are we still delaying telemetry to try to avoid Xbee bricking? +bool Copter::telemetry_delayed(mavlink_channel_t chan) +{ + uint32_t tnow = millis() >> 10; + if (tnow > (uint32_t)g.telem_delay) { + return false; + } + if (chan == MAVLINK_COMM_0 && hal.gpio->usb_connected()) { + // this is USB telemetry, so won't be an Xbee + return false; + } + // we're either on the 2nd UART, or no USB cable is connected + // we need to delay telemetry by the TELEM_DELAY time + return true; +} + + +// try to send a message, return false if it won't fit in the serial tx buffer +bool GCS_MAVLINK::try_send_message(enum ap_message id) +{ + if (copter.telemetry_delayed(chan)) { + return false; + } + +#if HIL_MODE != HIL_MODE_SENSORS + // if we don't have at least 250 micros remaining before the main loop + // wants to fire then don't send a mavlink message. We want to + // prioritise the main flight control loop over communications + if (copter.scheduler.time_available_usec() < 250 && copter.motors.armed()) { + copter.gcs_out_of_time = true; + return false; + } +#endif + + switch(id) { + case MSG_HEARTBEAT: + CHECK_PAYLOAD_SIZE(HEARTBEAT); + copter.gcs[chan-MAVLINK_COMM_0].last_heartbeat_time = AP_HAL::millis(); + copter.send_heartbeat(chan); + break; + + case MSG_EXTENDED_STATUS1: + // send extended status only once vehicle has been initialised + // to avoid unnecessary errors being reported to user + if (copter.ap.initialised) { + CHECK_PAYLOAD_SIZE(SYS_STATUS); + copter.send_extended_status1(chan); + CHECK_PAYLOAD_SIZE(POWER_STATUS); + copter.gcs[chan-MAVLINK_COMM_0].send_power_status(); + } + break; + + case MSG_EXTENDED_STATUS2: + CHECK_PAYLOAD_SIZE(MEMINFO); + copter.gcs[chan-MAVLINK_COMM_0].send_meminfo(); + break; + + case MSG_ATTITUDE: + CHECK_PAYLOAD_SIZE(ATTITUDE); + copter.send_attitude(chan); + break; + + case MSG_LOCATION: + CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT); + copter.send_location(chan); + break; + + case MSG_LOCAL_POSITION: + CHECK_PAYLOAD_SIZE(LOCAL_POSITION_NED); + send_local_position(copter.ahrs); + break; + + case MSG_NAV_CONTROLLER_OUTPUT: + CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT); + copter.send_nav_controller_output(chan); + break; + + case MSG_GPS_RAW: + return copter.gcs[chan-MAVLINK_COMM_0].send_gps_raw(copter.gps); + + case MSG_SYSTEM_TIME: + CHECK_PAYLOAD_SIZE(SYSTEM_TIME); + copter.gcs[chan-MAVLINK_COMM_0].send_system_time(copter.gps); + break; + + case MSG_SERVO_OUT: + CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED); + copter.send_servo_out(chan); + break; + + case MSG_RADIO_IN: + CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW); + copter.gcs[chan-MAVLINK_COMM_0].send_radio_in(copter.receiver_rssi); + break; + + case MSG_RADIO_OUT: + CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW); + copter.send_radio_out(chan); + break; + + case MSG_VFR_HUD: + CHECK_PAYLOAD_SIZE(VFR_HUD); + copter.send_vfr_hud(chan); + break; + + case MSG_RAW_IMU1: + CHECK_PAYLOAD_SIZE(RAW_IMU); + copter.gcs[chan-MAVLINK_COMM_0].send_raw_imu(copter.ins, copter.compass); + break; + + case MSG_RAW_IMU2: + CHECK_PAYLOAD_SIZE(SCALED_PRESSURE); + copter.gcs[chan-MAVLINK_COMM_0].send_scaled_pressure(copter.barometer); + break; + + case MSG_RAW_IMU3: + CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS); + copter.gcs[chan-MAVLINK_COMM_0].send_sensor_offsets(copter.ins, copter.compass, copter.barometer); + break; + + case MSG_CURRENT_WAYPOINT: + CHECK_PAYLOAD_SIZE(MISSION_CURRENT); + copter.send_current_waypoint(chan); + break; + + case MSG_NEXT_PARAM: + CHECK_PAYLOAD_SIZE(PARAM_VALUE); + copter.gcs[chan-MAVLINK_COMM_0].queued_param_send(); + break; + + case MSG_NEXT_WAYPOINT: + CHECK_PAYLOAD_SIZE(MISSION_REQUEST); + copter.gcs[chan-MAVLINK_COMM_0].queued_waypoint_send(); + break; + + case MSG_RANGEFINDER: +#if CONFIG_SONAR == ENABLED + CHECK_PAYLOAD_SIZE(RANGEFINDER); + copter.send_rangefinder(chan); +#endif + break; + + case MSG_RPM: + CHECK_PAYLOAD_SIZE(RPM); + copter.send_rpm(chan); + break; + + case MSG_TERRAIN: +#if AP_TERRAIN_AVAILABLE && AC_TERRAIN + CHECK_PAYLOAD_SIZE(TERRAIN_REQUEST); + copter.terrain.send_request(chan); +#endif + break; + + case MSG_CAMERA_FEEDBACK: +#if CAMERA == ENABLED + CHECK_PAYLOAD_SIZE(CAMERA_FEEDBACK); + copter.camera.send_feedback(chan, copter.gps, copter.ahrs, copter.current_loc); +#endif + break; + + case MSG_STATUSTEXT: + CHECK_PAYLOAD_SIZE(STATUSTEXT); + copter.send_statustext(chan); + break; + + case MSG_LIMITS_STATUS: +#if AC_FENCE == ENABLED + CHECK_PAYLOAD_SIZE(LIMITS_STATUS); + copter.send_limits_status(chan); +#endif + break; + + case MSG_AHRS: + CHECK_PAYLOAD_SIZE(AHRS); + copter.gcs[chan-MAVLINK_COMM_0].send_ahrs(copter.ahrs); + break; + + case MSG_SIMSTATE: +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + CHECK_PAYLOAD_SIZE(SIMSTATE); + copter.send_simstate(chan); +#endif + CHECK_PAYLOAD_SIZE(AHRS2); + copter.gcs[chan-MAVLINK_COMM_0].send_ahrs2(copter.ahrs); + break; + + case MSG_HWSTATUS: + CHECK_PAYLOAD_SIZE(HWSTATUS); + copter.send_hwstatus(chan); + break; + + case MSG_MOUNT_STATUS: +#if MOUNT == ENABLED + CHECK_PAYLOAD_SIZE(MOUNT_STATUS); + copter.camera_mount.status_msg(chan); +#endif // MOUNT == ENABLED + break; + + case MSG_BATTERY2: + CHECK_PAYLOAD_SIZE(BATTERY2); + copter.gcs[chan-MAVLINK_COMM_0].send_battery2(copter.battery); + break; + + case MSG_OPTICAL_FLOW: +#if OPTFLOW == ENABLED + CHECK_PAYLOAD_SIZE(OPTICAL_FLOW); + copter.gcs[chan-MAVLINK_COMM_0].send_opticalflow(copter.ahrs, copter.optflow); +#endif + break; + + case MSG_GIMBAL_REPORT: +#if MOUNT == ENABLED + CHECK_PAYLOAD_SIZE(GIMBAL_REPORT); + copter.camera_mount.send_gimbal_report(chan); +#endif + break; + + case MSG_EKF_STATUS_REPORT: + CHECK_PAYLOAD_SIZE(EKF_STATUS_REPORT); + copter.ahrs.send_ekf_status_report(chan); + break; + + case MSG_FENCE_STATUS: + case MSG_WIND: + // unused + break; + + case MSG_PID_TUNING: + CHECK_PAYLOAD_SIZE(PID_TUNING); + copter.send_pid_tuning(chan); + break; + + case MSG_VIBRATION: + CHECK_PAYLOAD_SIZE(VIBRATION); + send_vibration(copter.ins); + break; + + case MSG_MISSION_ITEM_REACHED: + CHECK_PAYLOAD_SIZE(MISSION_ITEM_REACHED); + mavlink_msg_mission_item_reached_send(chan, mission_item_reached_index); + break; + + case MSG_RETRY_DEFERRED: + break; // just here to prevent a warning + + case MSG_MAG_CAL_PROGRESS: + copter.compass.send_mag_cal_progress(chan); + break; + + case MSG_MAG_CAL_REPORT: + copter.compass.send_mag_cal_report(chan); + break; + } + + return true; +} + + +const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = { + // @Param: RAW_SENS + // @DisplayName: Raw sensor stream rate + // @Description: Stream rate of RAW_IMU, SCALED_IMU2, SCALED_PRESSURE, and SENSOR_OFFSETS to ground station + // @Units: Hz + // @Range: 0 10 + // @Increment: 1 + // @User: Advanced + AP_GROUPINFO("RAW_SENS", 0, GCS_MAVLINK, streamRates[0], 0), + + // @Param: EXT_STAT + // @DisplayName: Extended status stream rate to ground station + // @Description: Stream rate of SYS_STATUS, MEMINFO, MISSION_CURRENT, GPS_RAW_INT, NAV_CONTROLLER_OUTPUT, and LIMITS_STATUS to ground station + // @Units: Hz + // @Range: 0 10 + // @Increment: 1 + // @User: Advanced + AP_GROUPINFO("EXT_STAT", 1, GCS_MAVLINK, streamRates[1], 0), + + // @Param: RC_CHAN + // @DisplayName: RC Channel stream rate to ground station + // @Description: Stream rate of SERVO_OUTPUT_RAW and RC_CHANNELS_RAW to ground station + // @Units: Hz + // @Range: 0 10 + // @Increment: 1 + // @User: Advanced + AP_GROUPINFO("RC_CHAN", 2, GCS_MAVLINK, streamRates[2], 0), + + // @Param: RAW_CTRL + // @DisplayName: Raw Control stream rate to ground station + // @Description: Stream rate of RC_CHANNELS_SCALED (HIL only) to ground station + // @Units: Hz + // @Range: 0 10 + // @Increment: 1 + // @User: Advanced + AP_GROUPINFO("RAW_CTRL", 3, GCS_MAVLINK, streamRates[3], 0), + + // @Param: POSITION + // @DisplayName: Position stream rate to ground station + // @Description: Stream rate of GLOBAL_POSITION_INT to ground station + // @Units: Hz + // @Range: 0 10 + // @Increment: 1 + // @User: Advanced + AP_GROUPINFO("POSITION", 4, GCS_MAVLINK, streamRates[4], 0), + + // @Param: EXTRA1 + // @DisplayName: Extra data type 1 stream rate to ground station + // @Description: Stream rate of ATTITUDE and SIMSTATE (SITL only) to ground station + // @Units: Hz + // @Range: 0 10 + // @Increment: 1 + // @User: Advanced + AP_GROUPINFO("EXTRA1", 5, GCS_MAVLINK, streamRates[5], 0), + + // @Param: EXTRA2 + // @DisplayName: Extra data type 2 stream rate to ground station + // @Description: Stream rate of VFR_HUD to ground station + // @Units: Hz + // @Range: 0 10 + // @Increment: 1 + // @User: Advanced + AP_GROUPINFO("EXTRA2", 6, GCS_MAVLINK, streamRates[6], 0), + + // @Param: EXTRA3 + // @DisplayName: Extra data type 3 stream rate to ground station + // @Description: Stream rate of AHRS, HWSTATUS, and SYSTEM_TIME to ground station + // @Units: Hz + // @Range: 0 10 + // @Increment: 1 + // @User: Advanced + AP_GROUPINFO("EXTRA3", 7, GCS_MAVLINK, streamRates[7], 0), + + // @Param: PARAMS + // @DisplayName: Parameter stream rate to ground station + // @Description: Stream rate of PARAM_VALUE to ground station + // @Units: Hz + // @Range: 0 10 + // @Increment: 1 + // @User: Advanced + AP_GROUPINFO("PARAMS", 8, GCS_MAVLINK, streamRates[8], 0), + AP_GROUPEND +}; + + +// see if we should send a stream now. Called at 50Hz +bool GCS_MAVLINK::stream_trigger(enum streams stream_num) +{ + if (stream_num >= NUM_STREAMS) { + return false; + } + float rate = (uint8_t)streamRates[stream_num].get(); + + // send at a much lower rate while handling waypoints and + // parameter sends + if ((stream_num != STREAM_PARAMS) && + (waypoint_receiving || _queued_parameter != NULL)) { + rate *= 0.25f; + } + + if (rate <= 0) { + return false; + } + + if (stream_ticks[stream_num] == 0) { + // we're triggering now, setup the next trigger point + if (rate > 50) { + rate = 50; + } + stream_ticks[stream_num] = (50 / rate) - 1 + stream_slowdown; + return true; + } + + // count down at 50Hz + stream_ticks[stream_num]--; + return false; +} + +void +GCS_MAVLINK::data_stream_send(void) +{ + if (waypoint_receiving) { + // don't interfere with mission transfer + return; + } + + if (!copter.in_mavlink_delay && !copter.motors.armed()) { + handle_log_send(copter.DataFlash); + } + + copter.gcs_out_of_time = false; + + if (_queued_parameter != NULL) { + if (streamRates[STREAM_PARAMS].get() <= 0) { + streamRates[STREAM_PARAMS].set(10); + } + if (stream_trigger(STREAM_PARAMS)) { + send_message(MSG_NEXT_PARAM); + } + // don't send anything else at the same time as parameters + return; + } + + if (copter.gcs_out_of_time) return; + + if (copter.in_mavlink_delay) { + // don't send any other stream types while in the delay callback + return; + } + + if (stream_trigger(STREAM_RAW_SENSORS)) { + send_message(MSG_RAW_IMU1); + send_message(MSG_RAW_IMU2); + send_message(MSG_RAW_IMU3); + } + + if (copter.gcs_out_of_time) return; + + if (stream_trigger(STREAM_EXTENDED_STATUS)) { + send_message(MSG_EXTENDED_STATUS1); + send_message(MSG_EXTENDED_STATUS2); + send_message(MSG_CURRENT_WAYPOINT); + send_message(MSG_GPS_RAW); + send_message(MSG_NAV_CONTROLLER_OUTPUT); + send_message(MSG_LIMITS_STATUS); + } + + if (copter.gcs_out_of_time) return; + + if (stream_trigger(STREAM_POSITION)) { + send_message(MSG_LOCATION); + send_message(MSG_LOCAL_POSITION); + } + + if (copter.gcs_out_of_time) return; + + if (stream_trigger(STREAM_RAW_CONTROLLER)) { + send_message(MSG_SERVO_OUT); + } + + if (copter.gcs_out_of_time) return; + + if (stream_trigger(STREAM_RC_CHANNELS)) { + send_message(MSG_RADIO_OUT); + send_message(MSG_RADIO_IN); + } + + if (copter.gcs_out_of_time) return; + + if (stream_trigger(STREAM_EXTRA1)) { + send_message(MSG_ATTITUDE); + send_message(MSG_SIMSTATE); + send_message(MSG_PID_TUNING); + } + + if (copter.gcs_out_of_time) return; + + if (stream_trigger(STREAM_EXTRA2)) { + send_message(MSG_VFR_HUD); + } + + if (copter.gcs_out_of_time) return; + + if (stream_trigger(STREAM_EXTRA3)) { + send_message(MSG_AHRS); + send_message(MSG_HWSTATUS); + send_message(MSG_SYSTEM_TIME); + send_message(MSG_RANGEFINDER); +#if AP_TERRAIN_AVAILABLE && AC_TERRAIN + send_message(MSG_TERRAIN); +#endif + send_message(MSG_BATTERY2); + send_message(MSG_MOUNT_STATUS); + send_message(MSG_OPTICAL_FLOW); + send_message(MSG_GIMBAL_REPORT); + send_message(MSG_MAG_CAL_REPORT); + send_message(MSG_MAG_CAL_PROGRESS); + send_message(MSG_EKF_STATUS_REPORT); + send_message(MSG_VIBRATION); + send_message(MSG_RPM); + } +} + + +void GCS_MAVLINK::handle_guided_request(AP_Mission::Mission_Command &cmd) +{ + copter.do_guided(cmd); +} + +void GCS_MAVLINK::handle_change_alt_request(AP_Mission::Mission_Command &cmd) +{ + // add home alt if needed + if (cmd.content.location.flags.relative_alt) { + cmd.content.location.alt += copter.ahrs.get_home().alt; + } + + // To-Do: update target altitude for loiter or waypoint controller depending upon nav mode +} + +void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) +{ + uint8_t result = MAV_RESULT_FAILED; // assume failure. Each messages id is responsible for return ACK or NAK if required + + switch (msg->msgid) { + + case MAVLINK_MSG_ID_HEARTBEAT: // MAV ID: 0 + { + // We keep track of the last time we received a heartbeat from our GCS for failsafe purposes + if(msg->sysid != copter.g.sysid_my_gcs) break; + copter.failsafe.last_heartbeat_ms = AP_HAL::millis(); + copter.pmTest1++; + break; + } + + case MAVLINK_MSG_ID_SET_MODE: // MAV ID: 11 + { + handle_set_mode(msg, FUNCTOR_BIND(&copter, &Copter::set_mode, bool, uint8_t)); + break; + } + + case MAVLINK_MSG_ID_PARAM_REQUEST_READ: // MAV ID: 20 + { + handle_param_request_read(msg); + break; + } + + case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: // MAV ID: 21 + { + // mark the firmware version in the tlog + send_text(MAV_SEVERITY_INFO, FIRMWARE_STRING); + +#if defined(PX4_GIT_VERSION) && defined(NUTTX_GIT_VERSION) + send_text(MAV_SEVERITY_INFO, "PX4: " PX4_GIT_VERSION " NuttX: " NUTTX_GIT_VERSION); +#endif + send_text(MAV_SEVERITY_INFO, "Frame: " FRAME_CONFIG_STRING); + handle_param_request_list(msg); + break; + } + + case MAVLINK_MSG_ID_PARAM_SET: // 23 + { + handle_param_set(msg, &copter.DataFlash); + break; + } + + case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST: // MAV ID: 38 + { + handle_mission_write_partial_list(copter.mission, msg); + break; + } + + // GCS has sent us a mission item, store to EEPROM + case MAVLINK_MSG_ID_MISSION_ITEM: // MAV ID: 39 + { + if (handle_mission_item(msg, copter.mission)) { + copter.DataFlash.Log_Write_EntireMission(copter.mission); + } + break; + } + + // read an individual command from EEPROM and send it to the GCS + case MAVLINK_MSG_ID_MISSION_REQUEST: // MAV ID: 40 + { + handle_mission_request(copter.mission, msg); + break; + } + + case MAVLINK_MSG_ID_MISSION_SET_CURRENT: // MAV ID: 41 + { + handle_mission_set_current(copter.mission, msg); + break; + } + + // GCS request the full list of commands, we return just the number and leave the GCS to then request each command individually + case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: // MAV ID: 43 + { + handle_mission_request_list(copter.mission, msg); + break; + } + + // GCS provides the full number of commands it wishes to upload + // individual commands will then be sent from the GCS using the MAVLINK_MSG_ID_MISSION_ITEM message + case MAVLINK_MSG_ID_MISSION_COUNT: // MAV ID: 44 + { + handle_mission_count(copter.mission, msg); + break; + } + + case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: // MAV ID: 45 + { + handle_mission_clear_all(copter.mission, msg); + break; + } + + case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: // MAV ID: 66 + { + handle_request_data_stream(msg, false); + break; + } + + case MAVLINK_MSG_ID_GIMBAL_REPORT: + { +#if MOUNT == ENABLED + handle_gimbal_report(copter.camera_mount, msg); +#endif + break; + } + + case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: // MAV ID: 70 + { + // allow override of RC channel values for HIL + // or for complete GCS control of switch position + // and RC PWM values. + if(msg->sysid != copter.g.sysid_my_gcs) break; // Only accept control from our gcs + mavlink_rc_channels_override_t packet; + int16_t v[8]; + mavlink_msg_rc_channels_override_decode(msg, &packet); + + v[0] = packet.chan1_raw; + v[1] = packet.chan2_raw; + v[2] = packet.chan3_raw; + v[3] = packet.chan4_raw; + v[4] = packet.chan5_raw; + v[5] = packet.chan6_raw; + v[6] = packet.chan7_raw; + v[7] = packet.chan8_raw; + + // record that rc are overwritten so we can trigger a failsafe if we lose contact with groundstation + copter.failsafe.rc_override_active = hal.rcin->set_overrides(v, 8); + + // a RC override message is considered to be a 'heartbeat' from the ground station for failsafe purposes + copter.failsafe.last_heartbeat_ms = AP_HAL::millis(); + break; + } + + // Pre-Flight calibration requests + case MAVLINK_MSG_ID_COMMAND_LONG: // MAV ID: 76 + { + // decode packet + mavlink_command_long_t packet; + mavlink_msg_command_long_decode(msg, &packet); + + switch(packet.command) { + + case MAV_CMD_START_RX_PAIR: + // initiate bind procedure + if (!hal.rcin->rc_bind(packet.param1)) { + result = MAV_RESULT_FAILED; + } else { + result = MAV_RESULT_ACCEPTED; + } + break; + + case MAV_CMD_NAV_TAKEOFF: { + // param3 : horizontal navigation by pilot acceptable + // param4 : yaw angle (not supported) + // param5 : latitude (not supported) + // param6 : longitude (not supported) + // param7 : altitude [metres] + + float takeoff_alt = packet.param7 * 100; // Convert m to cm + + if(copter.do_user_takeoff(takeoff_alt, is_zero(packet.param3))) { + result = MAV_RESULT_ACCEPTED; + } else { + result = MAV_RESULT_FAILED; + } + break; + } + + + case MAV_CMD_NAV_LOITER_UNLIM: + if (copter.set_mode(LOITER)) { + result = MAV_RESULT_ACCEPTED; + } + break; + + case MAV_CMD_NAV_RETURN_TO_LAUNCH: + if (copter.set_mode(RTL)) { + result = MAV_RESULT_ACCEPTED; + } + break; + + case MAV_CMD_NAV_LAND: + if (copter.set_mode(LAND)) { + result = MAV_RESULT_ACCEPTED; + } + break; + + case MAV_CMD_CONDITION_YAW: + // param1 : target angle [0-360] + // param2 : speed during change [deg per second] + // param3 : direction (-1:ccw, +1:cw) + // param4 : relative offset (1) or absolute angle (0) + if ((packet.param1 >= 0.0f) && + (packet.param1 <= 360.0f) && + (is_zero(packet.param4) || is_equal(packet.param4,1.0f))) { + copter.set_auto_yaw_look_at_heading(packet.param1, packet.param2, (int8_t)packet.param3, (uint8_t)packet.param4); + result = MAV_RESULT_ACCEPTED; + } else { + result = MAV_RESULT_FAILED; + } + break; + + case MAV_CMD_DO_CHANGE_SPEED: + // param1 : unused + // param2 : new speed in m/s + // param3 : unused + // param4 : unused + if (packet.param2 > 0.0f) { + copter.wp_nav.set_speed_xy(packet.param2 * 100.0f); + result = MAV_RESULT_ACCEPTED; + } else { + result = MAV_RESULT_FAILED; + } + break; + + case MAV_CMD_DO_SET_HOME: + // param1 : use current (1=use current location, 0=use specified location) + // param5 : latitude + // param6 : longitude + // param7 : altitude (absolute) + result = MAV_RESULT_FAILED; // assume failure + if(is_equal(packet.param1,1.0f) || (is_zero(packet.param5) && is_zero(packet.param6) && is_zero(packet.param7))) { + if (copter.set_home_to_current_location_and_lock()) { + result = MAV_RESULT_ACCEPTED; + } + } else { + // sanity check location + if (fabsf(packet.param5) > 90.0f || fabsf(packet.param6) > 180.0f) { + break; + } + Location new_home_loc; + new_home_loc.lat = (int32_t)(packet.param5 * 1.0e7f); + new_home_loc.lng = (int32_t)(packet.param6 * 1.0e7f); + new_home_loc.alt = (int32_t)(packet.param7 * 100.0f); + if (!copter.far_from_EKF_origin(new_home_loc)) { + if (copter.set_home_and_lock(new_home_loc)) { + result = MAV_RESULT_ACCEPTED; + } + } + } + break; + + case MAV_CMD_DO_FLIGHTTERMINATION: + if (packet.param1 > 0.5f) { + copter.init_disarm_motors(); + result = MAV_RESULT_ACCEPTED; + } + break; + + case MAV_CMD_DO_SET_ROI: + // param1 : regional of interest mode (not supported) + // param2 : mission index/ target id (not supported) + // param3 : ROI index (not supported) + // param5 : x / lat + // param6 : y / lon + // param7 : z / alt + // sanity check location + if (fabsf(packet.param5) > 90.0f || fabsf(packet.param6) > 180.0f) { + break; + } + Location roi_loc; + roi_loc.lat = (int32_t)(packet.param5 * 1.0e7f); + roi_loc.lng = (int32_t)(packet.param6 * 1.0e7f); + roi_loc.alt = (int32_t)(packet.param7 * 100.0f); + copter.set_auto_yaw_roi(roi_loc); + result = MAV_RESULT_ACCEPTED; + break; + +#if CAMERA == ENABLED + case MAV_CMD_DO_DIGICAM_CONFIGURE: + copter.camera.configure(packet.param1, + packet.param2, + packet.param3, + packet.param4, + packet.param5, + packet.param6, + packet.param7); + + result = MAV_RESULT_ACCEPTED; + break; + + case MAV_CMD_DO_DIGICAM_CONTROL: + copter.camera.control(packet.param1, + packet.param2, + packet.param3, + packet.param4, + packet.param5, + packet.param6); + + result = MAV_RESULT_ACCEPTED; + break; +#endif // CAMERA == ENABLED + case MAV_CMD_DO_MOUNT_CONTROL: +#if MOUNT == ENABLED + copter.camera_mount.control(packet.param1, packet.param2, packet.param3, (MAV_MOUNT_MODE) packet.param7); +#endif + break; + + case MAV_CMD_MISSION_START: + if (copter.motors.armed() && copter.set_mode(AUTO)) { + copter.set_auto_armed(true); + if (copter.mission.state() != AP_Mission::MISSION_RUNNING) { + copter.mission.start_or_resume(); + } + result = MAV_RESULT_ACCEPTED; + } + break; + + case MAV_CMD_PREFLIGHT_CALIBRATION: + // exit immediately if armed + if (copter.motors.armed()) { + result = MAV_RESULT_FAILED; + break; + } + if (is_equal(packet.param1,1.0f)) { + if (copter.calibrate_gyros()) { + result = MAV_RESULT_ACCEPTED; + } else { + result = MAV_RESULT_FAILED; + } + } else if (is_equal(packet.param3,1.0f)) { + // fast barometer calibration + copter.init_barometer(false); + result = MAV_RESULT_ACCEPTED; + } else if (is_equal(packet.param4,1.0f)) { + result = MAV_RESULT_UNSUPPORTED; + } else if (is_equal(packet.param5,1.0f)) { + // 3d accel calibration + result = MAV_RESULT_ACCEPTED; + if (!copter.calibrate_gyros()) { + result = MAV_RESULT_FAILED; + break; + } + copter.ins.acal_init(); + copter.ins.get_acal()->start(this); + + } else if (is_equal(packet.param5,2.0f)) { + // calibrate gyros + if (!copter.calibrate_gyros()) { + result = MAV_RESULT_FAILED; + break; + } + // accel trim + float trim_roll, trim_pitch; + if(copter.ins.calibrate_trim(trim_roll, trim_pitch)) { + // reset ahrs's trim to suggested values from calibration routine + copter.ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0)); + result = MAV_RESULT_ACCEPTED; + } else { + result = MAV_RESULT_FAILED; + } + } else if (is_equal(packet.param6,1.0f)) { + // compassmot calibration + result = copter.mavlink_compassmot(chan); + } + break; + + case MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: + if (is_equal(packet.param1,2.0f)) { + // save first compass's offsets + copter.compass.set_and_save_offsets(0, packet.param2, packet.param3, packet.param4); + result = MAV_RESULT_ACCEPTED; + } + if (is_equal(packet.param1,5.0f)) { + // save secondary compass's offsets + copter.compass.set_and_save_offsets(1, packet.param2, packet.param3, packet.param4); + result = MAV_RESULT_ACCEPTED; + } + break; + + case MAV_CMD_COMPONENT_ARM_DISARM: + if (is_equal(packet.param1,1.0f)) { + // attempt to arm and return success or failure + if (copter.init_arm_motors(true)) { + result = MAV_RESULT_ACCEPTED; + } + } else if (is_zero(packet.param1) && (copter.ap.land_complete || is_equal(packet.param2,21196.0f))) { + // force disarming by setting param2 = 21196 is deprecated + copter.init_disarm_motors(); + result = MAV_RESULT_ACCEPTED; + } else { + result = MAV_RESULT_UNSUPPORTED; + } + break; + + case MAV_CMD_GET_HOME_POSITION: + if (copter.ap.home_state != HOME_UNSET) { + send_home(copter.ahrs.get_home()); + result = MAV_RESULT_ACCEPTED; + } + break; + + case MAV_CMD_DO_SET_SERVO: + if (copter.ServoRelayEvents.do_set_servo(packet.param1, packet.param2)) { + result = MAV_RESULT_ACCEPTED; + } + break; + + case MAV_CMD_DO_REPEAT_SERVO: + if (copter.ServoRelayEvents.do_repeat_servo(packet.param1, packet.param2, packet.param3, packet.param4*1000)) { + result = MAV_RESULT_ACCEPTED; + } + break; + + case MAV_CMD_DO_SET_RELAY: + if (copter.ServoRelayEvents.do_set_relay(packet.param1, packet.param2)) { + result = MAV_RESULT_ACCEPTED; + } + break; + + case MAV_CMD_DO_REPEAT_RELAY: + if (copter.ServoRelayEvents.do_repeat_relay(packet.param1, packet.param2, packet.param3*1000)) { + result = MAV_RESULT_ACCEPTED; + } + break; + + case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN: + if (is_equal(packet.param1,1.0f) || is_equal(packet.param1,3.0f)) { + AP_Notify::flags.firmware_update = 1; + copter.update_notify(); + hal.scheduler->delay(200); + // when packet.param1 == 3 we reboot to hold in bootloader + hal.scheduler->reboot(is_equal(packet.param1,3.0f)); + result = MAV_RESULT_ACCEPTED; + } + break; + + case MAV_CMD_DO_FENCE_ENABLE: +#if AC_FENCE == ENABLED + result = MAV_RESULT_ACCEPTED; + switch ((uint16_t)packet.param1) { + case 0: + copter.fence.enable(false); + break; + case 1: + copter.fence.enable(true); + break; + default: + result = MAV_RESULT_FAILED; + break; + } +#else + // if fence code is not included return failure + result = MAV_RESULT_FAILED; +#endif + break; + +#if PARACHUTE == ENABLED + case MAV_CMD_DO_PARACHUTE: + // configure or release parachute + result = MAV_RESULT_ACCEPTED; + switch ((uint16_t)packet.param1) { + case PARACHUTE_DISABLE: + copter.parachute.enabled(false); + copter.Log_Write_Event(DATA_PARACHUTE_DISABLED); + break; + case PARACHUTE_ENABLE: + copter.parachute.enabled(true); + copter.Log_Write_Event(DATA_PARACHUTE_ENABLED); + break; + case PARACHUTE_RELEASE: + // treat as a manual release which performs some additional check of altitude + copter.parachute_manual_release(); + break; + default: + result = MAV_RESULT_FAILED; + break; + } + break; +#endif + + case MAV_CMD_DO_MOTOR_TEST: + // param1 : motor sequence number (a number from 1 to max number of motors on the vehicle) + // param2 : throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through. See MOTOR_TEST_THROTTLE_TYPE enum) + // param3 : throttle (range depends upon param2) + // param4 : timeout (in seconds) + result = copter.mavlink_motor_test_start(chan, (uint8_t)packet.param1, (uint8_t)packet.param2, (uint16_t)packet.param3, packet.param4); + break; + +#if EPM_ENABLED == ENABLED + case MAV_CMD_DO_GRIPPER: + // param1 : gripper number (ignored) + // param2 : action (0=release, 1=grab). See GRIPPER_ACTIONS enum. + if(!copter.epm.enabled()) { + result = MAV_RESULT_FAILED; + } else { + result = MAV_RESULT_ACCEPTED; + switch ((uint8_t)packet.param2) { + case GRIPPER_ACTION_RELEASE: + copter.epm.release(); + break; + case GRIPPER_ACTION_GRAB: + copter.epm.grab(); + break; + default: + result = MAV_RESULT_FAILED; + break; + } + } + break; +#endif + + case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: { + if (is_equal(packet.param1,1.0f)) { + copter.gcs[chan-MAVLINK_COMM_0].send_autopilot_version(FIRMWARE_VERSION); + result = MAV_RESULT_ACCEPTED; + } + break; + } + + case MAV_CMD_DO_START_MAG_CAL: + case MAV_CMD_DO_ACCEPT_MAG_CAL: + case MAV_CMD_DO_CANCEL_MAG_CAL: + result = copter.compass.handle_mag_cal_command(packet); + + break; + + case MAV_CMD_DO_SEND_BANNER: { + result = MAV_RESULT_ACCEPTED; + + send_text(MAV_SEVERITY_INFO, FIRMWARE_STRING); + + #if defined(PX4_GIT_VERSION) && defined(NUTTX_GIT_VERSION) + send_text(MAV_SEVERITY_INFO, "PX4: " PX4_GIT_VERSION " NuttX: " NUTTX_GIT_VERSION); + #endif + + send_text(MAV_SEVERITY_INFO, "Frame: " FRAME_CONFIG_STRING); + + // send system ID if we can + char sysid[40]; + if (hal.util->get_system_id(sysid)) { + send_text(MAV_SEVERITY_INFO, sysid); + } + + break; + } + + default: + result = MAV_RESULT_UNSUPPORTED; + break; + } + + // send ACK or NAK + mavlink_msg_command_ack_send_buf(msg, chan, packet.command, result); + + break; + } + + case MAVLINK_MSG_ID_COMMAND_ACK: // MAV ID: 77 + { + copter.command_ack_counter++; + break; + } + + case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET: // MAV ID: 82 + { + // decode packet + mavlink_set_attitude_target_t packet; + mavlink_msg_set_attitude_target_decode(msg, &packet); + + // ensure type_mask specifies to use attitude and thrust + if ((packet.type_mask & ((1<<7)|(1<<6))) != 0) { + break; + } + + // convert thrust to climb rate + packet.thrust = constrain_float(packet.thrust, 0.0f, 1.0f); + float climb_rate_cms = 0.0f; + if (is_equal(packet.thrust, 0.5f)) { + climb_rate_cms = 0.0f; + } else if (packet.thrust > 0.5f) { + // climb at up to WPNAV_SPEED_UP + climb_rate_cms = (packet.thrust - 0.5f) * 2.0f * copter.wp_nav.get_speed_up(); + } else { + // descend at up to WPNAV_SPEED_DN + climb_rate_cms = (0.5f - packet.thrust) * 2.0f * -fabsf(copter.wp_nav.get_speed_down()); + } + copter.guided_set_angle(Quaternion(packet.q[0],packet.q[1],packet.q[2],packet.q[3]), climb_rate_cms); + break; + } + + case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: // MAV ID: 84 + { + // decode packet + mavlink_set_position_target_local_ned_t packet; + mavlink_msg_set_position_target_local_ned_decode(msg, &packet); + + // exit if vehicle is not in Guided mode or Auto-Guided mode + if ((copter.control_mode != GUIDED) && !(copter.control_mode == AUTO && copter.auto_mode == Auto_NavGuided)) { + break; + } + + // check for supported coordinate frames + if (packet.coordinate_frame != MAV_FRAME_LOCAL_NED && + packet.coordinate_frame != MAV_FRAME_LOCAL_OFFSET_NED && + packet.coordinate_frame != MAV_FRAME_BODY_NED && + packet.coordinate_frame != MAV_FRAME_BODY_OFFSET_NED) { + break; + } + + bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE; + bool vel_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE; + bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE; + + /* + * for future use: + * bool force = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_FORCE; + * bool yaw_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE; + * bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE; + */ + + // prepare position + Vector3f pos_vector; + if (!pos_ignore) { + // convert to cm + pos_vector = Vector3f(packet.x * 100.0f, packet.y * 100.0f, -packet.z * 100.0f); + // rotate to body-frame if necessary + if (packet.coordinate_frame == MAV_FRAME_BODY_NED || + packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) { + copter.rotate_body_frame_to_NE(pos_vector.x, pos_vector.y); + } + // add body offset if necessary + if (packet.coordinate_frame == MAV_FRAME_LOCAL_OFFSET_NED || + packet.coordinate_frame == MAV_FRAME_BODY_NED || + packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) { + pos_vector += copter.inertial_nav.get_position(); + } else { + // convert from alt-above-home to alt-above-ekf-origin + pos_vector.z = copter.pv_alt_above_origin(pos_vector.z); + } + } + + // prepare velocity + Vector3f vel_vector; + if (!vel_ignore) { + // convert to cm + vel_vector = Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f); + // rotate to body-frame if necessary + if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) { + copter.rotate_body_frame_to_NE(vel_vector.x, vel_vector.y); + } + } + + // send request + if (!pos_ignore && !vel_ignore && acc_ignore) { + copter.guided_set_destination_posvel(pos_vector, vel_vector); + } else if (pos_ignore && !vel_ignore && acc_ignore) { + copter.guided_set_velocity(vel_vector); + } else if (!pos_ignore && vel_ignore && acc_ignore) { + copter.guided_set_destination(pos_vector); + } else { + result = MAV_RESULT_FAILED; + } + + break; + } + + case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT: // MAV ID: 86 + { + // decode packet + mavlink_set_position_target_global_int_t packet; + mavlink_msg_set_position_target_global_int_decode(msg, &packet); + + // exit if vehicle is not in Guided mode or Auto-Guided mode + if ((copter.control_mode != GUIDED) && !(copter.control_mode == AUTO && copter.auto_mode == Auto_NavGuided)) { + break; + } + + // check for supported coordinate frames + if (packet.coordinate_frame != MAV_FRAME_GLOBAL_INT && + packet.coordinate_frame != MAV_FRAME_GLOBAL_RELATIVE_ALT_INT && + packet.coordinate_frame != MAV_FRAME_GLOBAL_TERRAIN_ALT_INT) { + break; + } + + bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE; + bool vel_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE; + bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE; + + /* + * for future use: + * bool force = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_FORCE; + * bool yaw_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE; + * bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE; + */ + + Vector3f pos_ned; + + if(!pos_ignore) { + Location loc; + loc.lat = packet.lat_int; + loc.lng = packet.lon_int; + loc.alt = packet.alt*100; + switch (packet.coordinate_frame) { + case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT: + loc.flags.relative_alt = true; + loc.flags.terrain_alt = false; + break; + case MAV_FRAME_GLOBAL_TERRAIN_ALT_INT: + loc.flags.relative_alt = true; + loc.flags.terrain_alt = true; + break; + case MAV_FRAME_GLOBAL_INT: + default: + loc.flags.relative_alt = false; + loc.flags.terrain_alt = false; + break; + } + pos_ned = copter.pv_location_to_vector(loc); + } + + if (!pos_ignore && !vel_ignore && acc_ignore) { + copter.guided_set_destination_posvel(pos_ned, Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f)); + } else if (pos_ignore && !vel_ignore && acc_ignore) { + copter.guided_set_velocity(Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f)); + } else if (!pos_ignore && vel_ignore && acc_ignore) { + copter.guided_set_destination(pos_ned); + } else { + result = MAV_RESULT_FAILED; + } + + break; + } + +#if HIL_MODE != HIL_MODE_DISABLED + case MAVLINK_MSG_ID_HIL_STATE: // MAV ID: 90 + { + mavlink_hil_state_t packet; + mavlink_msg_hil_state_decode(msg, &packet); + + // set gps hil sensor + Location loc; + loc.lat = packet.lat; + loc.lng = packet.lon; + loc.alt = packet.alt/10; + Vector3f vel(packet.vx, packet.vy, packet.vz); + vel *= 0.01f; + + gps.setHIL(0, AP_GPS::GPS_OK_FIX_3D, + packet.time_usec/1000, + loc, vel, 10, 0, true); + + // rad/sec + Vector3f gyros; + gyros.x = packet.rollspeed; + gyros.y = packet.pitchspeed; + gyros.z = packet.yawspeed; + + // m/s/s + Vector3f accels; + accels.x = packet.xacc * (GRAVITY_MSS/1000.0f); + accels.y = packet.yacc * (GRAVITY_MSS/1000.0f); + accels.z = packet.zacc * (GRAVITY_MSS/1000.0f); + + ins.set_gyro(0, gyros); + + ins.set_accel(0, accels); + + copter.barometer.setHIL(packet.alt*0.001f); + copter.compass.setHIL(0, packet.roll, packet.pitch, packet.yaw); + copter.compass.setHIL(1, packet.roll, packet.pitch, packet.yaw); + + break; + } +#endif // HIL_MODE != HIL_MODE_DISABLED + + case MAVLINK_MSG_ID_RADIO: + case MAVLINK_MSG_ID_RADIO_STATUS: // MAV ID: 109 + { + handle_radio_status(msg, copter.DataFlash, copter.should_log(MASK_LOG_PM)); + break; + } + + case MAVLINK_MSG_ID_LOG_REQUEST_DATA: + case MAVLINK_MSG_ID_LOG_ERASE: + copter.in_log_download = true; + /* no break */ + case MAVLINK_MSG_ID_LOG_REQUEST_LIST: + if (!copter.in_mavlink_delay && !copter.motors.armed()) { + handle_log_message(msg, copter.DataFlash); + } + break; + case MAVLINK_MSG_ID_LOG_REQUEST_END: + copter.in_log_download = false; + if (!copter.in_mavlink_delay && !copter.motors.armed()) { + handle_log_message(msg, copter.DataFlash); + } + break; + + case MAVLINK_MSG_ID_SERIAL_CONTROL: + handle_serial_control(msg, copter.gps); + break; + + case MAVLINK_MSG_ID_GPS_INJECT_DATA: + handle_gps_inject(msg, copter.gps); + result = MAV_RESULT_ACCEPTED; + break; + +#if PRECISION_LANDING == ENABLED + case MAVLINK_MSG_ID_LANDING_TARGET: + // configure or release parachute + result = MAV_RESULT_ACCEPTED; + copter.precland.handle_msg(msg); +#endif + +#if CAMERA == ENABLED + //deprecated. Use MAV_CMD_DO_DIGICAM_CONFIGURE + case MAVLINK_MSG_ID_DIGICAM_CONFIGURE: // MAV ID: 202 + break; + + //deprecated. Use MAV_CMD_DO_DIGICAM_CONTROL + case MAVLINK_MSG_ID_DIGICAM_CONTROL: + copter.camera.control_msg(msg); + copter.log_picture(); + break; +#endif // CAMERA == ENABLED + +#if MOUNT == ENABLED + //deprecated. Use MAV_CMD_DO_MOUNT_CONFIGURE + case MAVLINK_MSG_ID_MOUNT_CONFIGURE: // MAV ID: 204 + copter.camera_mount.configure_msg(msg); + break; + //deprecated. Use MAV_CMD_DO_MOUNT_CONTROL + case MAVLINK_MSG_ID_MOUNT_CONTROL: + copter.camera_mount.control_msg(msg); + break; +#endif // MOUNT == ENABLED + + case MAVLINK_MSG_ID_TERRAIN_DATA: + case MAVLINK_MSG_ID_TERRAIN_CHECK: +#if AP_TERRAIN_AVAILABLE && AC_TERRAIN + copter.terrain.handle_data(chan, msg); +#endif + break; + +#if AC_RALLY == ENABLED + // receive a rally point from GCS and store in EEPROM + case MAVLINK_MSG_ID_RALLY_POINT: { + mavlink_rally_point_t packet; + mavlink_msg_rally_point_decode(msg, &packet); + + if (packet.idx >= copter.rally.get_rally_total() || + packet.idx >= copter.rally.get_rally_max()) { + send_text(MAV_SEVERITY_NOTICE,"Bad rally point message ID"); + break; + } + + if (packet.count != copter.rally.get_rally_total()) { + send_text(MAV_SEVERITY_NOTICE,"Bad rally point message count"); + break; + } + + RallyLocation rally_point; + rally_point.lat = packet.lat; + rally_point.lng = packet.lng; + rally_point.alt = packet.alt; + rally_point.break_alt = packet.break_alt; + rally_point.land_dir = packet.land_dir; + rally_point.flags = packet.flags; + + if (!copter.rally.set_rally_point_with_index(packet.idx, rally_point)) { + send_text(MAV_SEVERITY_CRITICAL, "Error setting rally point"); + } + + break; + } + + //send a rally point to the GCS + case MAVLINK_MSG_ID_RALLY_FETCH_POINT: { + mavlink_rally_fetch_point_t packet; + mavlink_msg_rally_fetch_point_decode(msg, &packet); + + if (packet.idx > copter.rally.get_rally_total()) { + send_text(MAV_SEVERITY_NOTICE, "Bad rally point index"); + break; + } + + RallyLocation rally_point; + if (!copter.rally.get_rally_point_with_index(packet.idx, rally_point)) { + send_text(MAV_SEVERITY_NOTICE, "Failed to set rally point"); + break; + } + + mavlink_msg_rally_point_send_buf(msg, + chan, msg->sysid, msg->compid, packet.idx, + copter.rally.get_rally_total(), rally_point.lat, rally_point.lng, + rally_point.alt, rally_point.break_alt, rally_point.land_dir, + rally_point.flags); + break; + } +#endif // AC_RALLY == ENABLED + + case MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS: + copter.DataFlash.remote_log_block_status_msg(chan, msg); + break; + + case MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST: + copter.gcs[chan-MAVLINK_COMM_0].send_autopilot_version(FIRMWARE_VERSION); + break; + + case MAVLINK_MSG_ID_LED_CONTROL: + // send message to Notify + AP_Notify::handle_led_control(msg); + break; + + case MAVLINK_MSG_ID_SET_HOME_POSITION: + { + mavlink_set_home_position_t packet; + mavlink_msg_set_home_position_decode(msg, &packet); + if((packet.latitude == 0) && (packet.longitude == 0) && (packet.altitude == 0)) { + copter.set_home_to_current_location_and_lock(); + } else { + // sanity check location + if (labs(packet.latitude) > 90*10e7 || labs(packet.longitude) > 180 * 10e7) { + break; + } + Location new_home_loc; + new_home_loc.lat = packet.latitude; + new_home_loc.lng = packet.longitude; + new_home_loc.alt = packet.altitude * 100; + if (copter.far_from_EKF_origin(new_home_loc)) { + break; + } + copter.set_home_and_lock(new_home_loc); + } + break; + } + + case MAVLINK_MSG_ID_ADSB_VEHICLE: +#if ADSB_ENABLED == ENABLED + copter.adsb.update_vehicle(msg); +#endif + break; + + } // end switch +} // end handle mavlink + + +/* + * a delay() callback that processes MAVLink packets. We set this as the + * callback in long running library initialisation routines to allow + * MAVLink to process packets while waiting for the initialisation to + * complete + */ +void Copter::mavlink_delay_cb() +{ + static uint32_t last_1hz, last_50hz, last_5s; + if (!gcs[0].initialised || in_mavlink_delay) return; + + in_mavlink_delay = true; + + uint32_t tnow = millis(); + if (tnow - last_1hz > 1000) { + last_1hz = tnow; + gcs_send_heartbeat(); + gcs_send_message(MSG_EXTENDED_STATUS1); + } + if (tnow - last_50hz > 20) { + last_50hz = tnow; + gcs_check_input(); + gcs_data_stream_send(); + gcs_send_deferred(); + notify.update(); + } + if (tnow - last_5s > 5000) { + last_5s = tnow; + gcs_send_text(MAV_SEVERITY_INFO, "Initialising APM"); + } + check_usb_mux(); + + in_mavlink_delay = false; +} + +/* + * send a message on both GCS links + */ +void Copter::gcs_send_message(enum ap_message id) +{ + for (uint8_t i=0; ivsnprintf((char *)gcs[0].pending_status.text, + sizeof(gcs[0].pending_status.text), fmt, arg_list); + va_end(arg_list); + gcs[0].send_message(MSG_STATUSTEXT); + for (uint8_t i=1; iprintf("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. + */ + +/* + * ArduCopter parameter definitions + * + */ + +#define GSCALAR(v, name, def) { copter.g.v.vtype, name, Parameters::k_param_ ## v, &copter.g.v, {def_value : def} } +#define ASCALAR(v, name, def) { copter.aparm.v.vtype, name, Parameters::k_param_ ## v, (const void *)&copter.aparm.v, {def_value : def} } +#define GGROUP(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &copter.g.v, {group_info : class::var_info} } +#define GOBJECT(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, (const void *)&copter.v, {group_info : class::var_info} } +#define GOBJECTN(v, pname, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## pname, (const void *)&copter.v, {group_info : class::var_info} } + +const AP_Param::Info Copter::var_info[] = { + // @Param: SYSID_SW_MREV + // @DisplayName: Eeprom format version number + // @Description: This value is incremented when changes are made to the eeprom format + // @User: Advanced + GSCALAR(format_version, "SYSID_SW_MREV", 0), + + // @Param: SYSID_SW_TYPE + // @DisplayName: Software Type + // @Description: This is used by the ground station to recognise the software type (eg ArduPlane vs ArduCopter) + // @Values: 0:ArduPlane,4:AntennaTracker,10:Copter,20:Rover + // @User: Advanced + GSCALAR(software_type, "SYSID_SW_TYPE", Parameters::k_software_type), + + // @Param: SYSID_THISMAV + // @DisplayName: MAVLink system ID of this vehicle + // @Description: Allows setting an individual MAVLink system id for this vehicle to distinguish it from others on the same network + // @Range: 1 255 + // @User: Advanced + GSCALAR(sysid_this_mav, "SYSID_THISMAV", MAV_SYSTEM_ID), + + // @Param: SYSID_MYGCS + // @DisplayName: My ground station number + // @Description: Allows restricting radio overrides to only come from my ground station + // @Values: 255:Mission Planner and DroidPlanner, 252: AP Planner 2 + // @User: Advanced + GSCALAR(sysid_my_gcs, "SYSID_MYGCS", 255), + +#if CLI_ENABLED == ENABLED + // @Param: CLI_ENABLED + // @DisplayName: CLI Enable + // @Description: This enables/disables the checking for three carriage returns on telemetry links on startup to enter the diagnostics command line interface + // @Values: 0:Disabled,1:Enabled + // @User: Advanced + GSCALAR(cli_enabled, "CLI_ENABLED", 0), +#endif + + // @Param: PILOT_THR_FILT + // @DisplayName: Throttle filter cutoff + // @Description: Throttle filter cutoff (Hz) - active whenever altitude control is inactive - 0 to disable + // @User: Advanced + // @Units: Hz + // @Range: 0 10 + // @Increment: .5 + GSCALAR(throttle_filt, "PILOT_THR_FILT", 0), + + // @Param: PILOT_TKOFF_ALT + // @DisplayName: Pilot takeoff altitude + // @Description: Altitude that altitude control modes will climb to when a takeoff is triggered with the throttle stick. + // @User: Standard + // @Units: Centimeters + // @Range: 0.0 1000.0 + // @Increment: 10 + GSCALAR(pilot_takeoff_alt, "PILOT_TKOFF_ALT", PILOT_TKOFF_ALT_DEFAULT), + + // @Param: PILOT_TKOFF_DZ + // @DisplayName: Takeoff trigger deadzone + // @Description: Offset from mid stick at which takeoff is triggered + // @User: Standard + // @Range: 0.0 500.0 + // @Increment: 10 + GSCALAR(takeoff_trigger_dz, "PILOT_TKOFF_DZ", THR_DZ_DEFAULT), + + // @Param: PILOT_THR_BHV + // @DisplayName: Throttle stick behavior + // @Description: Bits for: Feedback starts from mid stick + // @User: Standard + // @Values: 0:None,1:FeedbackFromMid + GSCALAR(throttle_behavior, "PILOT_THR_BHV", 0), + + // @Group: SERIAL + // @Path: ../libraries/AP_SerialManager/AP_SerialManager.cpp + GOBJECT(serial_manager, "SERIAL", AP_SerialManager), + + // @Param: TELEM_DELAY + // @DisplayName: Telemetry startup delay + // @Description: The amount of time (in seconds) to delay radio telemetry to prevent an Xbee bricking on power up + // @User: Advanced + // @Units: seconds + // @Range: 0 10 + // @Increment: 1 + GSCALAR(telem_delay, "TELEM_DELAY", 0), + + // @Param: GCS_PID_MASK + // @DisplayName: GCS PID tuning mask + // @Description: bitmask of PIDs to send MAVLink PID_TUNING messages for + // @User: Advanced + // @Values: 0:None,1:Roll,2:Pitch,4:Yaw + // @Bitmask: 0:Roll,1:Pitch,2:Yaw + GSCALAR(gcs_pid_mask, "GCS_PID_MASK", 0), + + // @Param: RTL_ALT + // @DisplayName: RTL Altitude + // @Description: The minimum relative altitude the model will move to before Returning to Launch. Set to zero to return at current altitude. + // @Units: Centimeters + // @Range: 0 8000 + // @Increment: 1 + // @User: Standard + GSCALAR(rtl_altitude, "RTL_ALT", RTL_ALT), + + // @Param: RTL_SPEED + // @DisplayName: RTL speed + // @Description: Defines the speed in cm/s which the aircraft will attempt to maintain horizontally while flying home. If this is set to zero, WPNAV_SPEED will be used instead. + // @Units: cm/s + // @Range: 0 2000 + // @Increment: 50 + // @User: Standard + GSCALAR(rtl_speed_cms, "RTL_SPEED", 0), + + // @Param: RNGFND_GAIN + // @DisplayName: Rangefinder gain + // @Description: Used to adjust the speed with which the target altitude is changed when objects are sensed below the copter + // @Range: 0.01 2.0 + // @Increment: 0.01 + // @User: Standard + GSCALAR(sonar_gain, "RNGFND_GAIN", SONAR_GAIN_DEFAULT), + + // @Param: FS_BATT_ENABLE + // @DisplayName: Battery Failsafe Enable + // @Description: Controls whether failsafe will be invoked when battery voltage or current runs low + // @Values: 0:Disabled,1:Land,2:RTL + // @User: Standard + GSCALAR(failsafe_battery_enabled, "FS_BATT_ENABLE", FS_BATT_DISABLED), + + // @Param: FS_BATT_VOLTAGE + // @DisplayName: Failsafe battery voltage + // @Description: Battery voltage to trigger failsafe. Set to 0 to disable battery voltage failsafe. If the battery voltage drops below this voltage then the copter will RTL + // @Units: Volts + // @Increment: 0.1 + // @User: Standard + GSCALAR(fs_batt_voltage, "FS_BATT_VOLTAGE", FS_BATT_VOLTAGE_DEFAULT), + + // @Param: FS_BATT_MAH + // @DisplayName: Failsafe battery milliAmpHours + // @Description: Battery capacity remaining to trigger failsafe. Set to 0 to disable battery remaining failsafe. If the battery remaining drops below this level then the copter will RTL + // @Units: mAh + // @Increment: 50 + // @User: Standard + GSCALAR(fs_batt_mah, "FS_BATT_MAH", FS_BATT_MAH_DEFAULT), + + // @Param: FS_GCS_ENABLE + // @DisplayName: Ground Station Failsafe Enable + // @Description: Controls whether failsafe will be invoked (and what action to take) when connection with Ground station is lost for at least 5 seconds. NB. The GCS Failsafe is only active when RC_OVERRIDE is being used to control the vehicle. + // @Values: 0:Disabled,1:Enabled always RTL,2:Enabled Continue with Mission in Auto Mode + // @User: Standard + GSCALAR(failsafe_gcs, "FS_GCS_ENABLE", FS_GCS_ENABLED_ALWAYS_RTL), + + // @Param: GPS_HDOP_GOOD + // @DisplayName: GPS Hdop Good + // @Description: GPS Hdop value at or below this value represent a good position. Used for pre-arm checks + // @Range: 100 900 + // @User: Advanced + GSCALAR(gps_hdop_good, "GPS_HDOP_GOOD", GPS_HDOP_GOOD_DEFAULT), + + // @Param: MAG_ENABLE + // @DisplayName: Compass enable/disable + // @Description: Setting this to Enabled(1) will enable the compass. Setting this to Disabled(0) will disable the compass + // @Values: 0:Disabled,1:Enabled + // @User: Standard + GSCALAR(compass_enabled, "MAG_ENABLE", MAGNETOMETER), + + // @Param: SUPER_SIMPLE + // @DisplayName: Super Simple Mode + // @Description: Bitmask to enable Super Simple mode for some flight modes. Setting this to Disabled(0) will disable Super Simple Mode + // @Values: 0:Disabled,1:Mode1,2:Mode2,3:Mode1+2,4:Mode3,5:Mode1+3,6:Mode2+3,7:Mode1+2+3,8:Mode4,9:Mode1+4,10:Mode2+4,11:Mode1+2+4,12:Mode3+4,13:Mode1+3+4,14:Mode2+3+4,15:Mode1+2+3+4,16:Mode5,17:Mode1+5,18:Mode2+5,19:Mode1+2+5,20:Mode3+5,21:Mode1+3+5,22:Mode2+3+5,23:Mode1+2+3+5,24:Mode4+5,25:Mode1+4+5,26:Mode2+4+5,27:Mode1+2+4+5,28:Mode3+4+5,29:Mode1+3+4+5,30:Mode2+3+4+5,31:Mode1+2+3+4+5,32:Mode6,33:Mode1+6,34:Mode2+6,35:Mode1+2+6,36:Mode3+6,37:Mode1+3+6,38:Mode2+3+6,39:Mode1+2+3+6,40:Mode4+6,41:Mode1+4+6,42:Mode2+4+6,43:Mode1+2+4+6,44:Mode3+4+6,45:Mode1+3+4+6,46:Mode2+3+4+6,47:Mode1+2+3+4+6,48:Mode5+6,49:Mode1+5+6,50:Mode2+5+6,51:Mode1+2+5+6,52:Mode3+5+6,53:Mode1+3+5+6,54:Mode2+3+5+6,55:Mode1+2+3+5+6,56:Mode4+5+6,57:Mode1+4+5+6,58:Mode2+4+5+6,59:Mode1+2+4+5+6,60:Mode3+4+5+6,61:Mode1+3+4+5+6,62:Mode2+3+4+5+6,63:Mode1+2+3+4+5+6 + // @User: Standard + GSCALAR(super_simple, "SUPER_SIMPLE", 0), + + // @Param: RTL_ALT_FINAL + // @DisplayName: RTL Final Altitude + // @Description: This is the altitude the vehicle will move to as the final stage of Returning to Launch or after completing a mission. Set to zero to land. + // @Units: Centimeters + // @Range: -1 1000 + // @Increment: 1 + // @User: Standard + GSCALAR(rtl_alt_final, "RTL_ALT_FINAL", RTL_ALT_FINAL), + + // @Param: RTL_CLIMB_MIN + // @DisplayName: RTL minimum climb + // @Description: The vehicle will climb this many cm during the initial climb portion of the RTL + // @Units: Centimeters + // @Range: 0 3000 + // @Increment: 10 + // @User: Standard + GSCALAR(rtl_climb_min, "RTL_CLIMB_MIN", RTL_CLIMB_MIN_DEFAULT), + + // @Param: WP_YAW_BEHAVIOR + // @DisplayName: Yaw behaviour during missions + // @Description: Determines how the autopilot controls the yaw during missions and RTL + // @Values: 0:Never change yaw, 1:Face next waypoint, 2:Face next waypoint except RTL, 3:Face along GPS course + // @User: Standard + GSCALAR(wp_yaw_behavior, "WP_YAW_BEHAVIOR", WP_YAW_BEHAVIOR_DEFAULT), + + // @Param: RTL_LOIT_TIME + // @DisplayName: RTL loiter time + // @Description: Time (in milliseconds) to loiter above home before beginning final descent + // @Units: ms + // @Range: 0 60000 + // @Increment: 1000 + // @User: Standard + GSCALAR(rtl_loiter_time, "RTL_LOIT_TIME", RTL_LOITER_TIME), + + // @Param: LAND_SPEED + // @DisplayName: Land speed + // @Description: The descent speed for the final stage of landing in cm/s + // @Units: cm/s + // @Range: 30 200 + // @Increment: 10 + // @User: Standard + GSCALAR(land_speed, "LAND_SPEED", LAND_SPEED), + + // @Param: PILOT_VELZ_MAX + // @DisplayName: Pilot maximum vertical speed + // @Description: The maximum vertical velocity the pilot may request in cm/s + // @Units: Centimeters/Second + // @Range: 50 500 + // @Increment: 10 + // @User: Standard + GSCALAR(pilot_velocity_z_max, "PILOT_VELZ_MAX", PILOT_VELZ_MAX), + + // @Param: PILOT_ACCEL_Z + // @DisplayName: Pilot vertical acceleration + // @Description: The vertical acceleration used when pilot is controlling the altitude + // @Units: cm/s/s + // @Range: 50 500 + // @Increment: 10 + // @User: Standard + GSCALAR(pilot_accel_z, "PILOT_ACCEL_Z", PILOT_ACCEL_Z_DEFAULT), + + // @Param: THR_MIN + // @DisplayName: Throttle Minimum + // @Description: The minimum throttle that will be sent to the motors to keep them spinning + // @Units: Percent*10 + // @Range: 0 300 + // @Increment: 1 + // @User: Standard + GSCALAR(throttle_min, "THR_MIN", THR_MIN_DEFAULT), + + // @Param: FS_THR_ENABLE + // @DisplayName: Throttle Failsafe Enable + // @Description: The throttle failsafe allows you to configure a software failsafe activated by a setting on the throttle input channel + // @Values: 0:Disabled,1:Enabled always RTL,2:Enabled Continue with Mission in Auto Mode,3:Enabled always LAND + // @User: Standard + GSCALAR(failsafe_throttle, "FS_THR_ENABLE", FS_THR_DISABLED), + + // @Param: FS_THR_VALUE + // @DisplayName: Throttle Failsafe Value + // @Description: The PWM level on channel 3 below which throttle sailsafe triggers + // @Range: 925 1100 + // @Units: pwm + // @Increment: 1 + // @User: Standard + GSCALAR(failsafe_throttle_value, "FS_THR_VALUE", FS_THR_VALUE_DEFAULT), + + // @Param: THR_MID + // @DisplayName: Throttle Mid Position + // @Description: The throttle output (0 ~ 1000) when throttle stick is in mid position. Used to scale the manual throttle so that the mid throttle stick position is close to the throttle required to hover + // @User: Standard + // @Range: 300 700 + // @Units: Percent*10 + // @Increment: 1 + GSCALAR(throttle_mid, "THR_MID", THR_MID_DEFAULT), + + // @Param: THR_DZ + // @DisplayName: Throttle deadzone + // @Description: The deadzone above and below mid throttle. Used in AltHold, Loiter, PosHold flight modes + // @User: Standard + // @Range: 0 300 + // @Units: pwm + // @Increment: 1 + GSCALAR(throttle_deadzone, "THR_DZ", THR_DZ_DEFAULT), + + // @Param: FLTMODE1 + // @DisplayName: Flight Mode 1 + // @Description: Flight mode when Channel 5 pwm is <= 1230 + // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake + // @User: Standard + GSCALAR(flight_mode1, "FLTMODE1", FLIGHT_MODE_1), + + // @Param: FLTMODE2 + // @DisplayName: Flight Mode 2 + // @Description: Flight mode when Channel 5 pwm is >1230, <= 1360 + // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake + // @User: Standard + GSCALAR(flight_mode2, "FLTMODE2", FLIGHT_MODE_2), + + // @Param: FLTMODE3 + // @DisplayName: Flight Mode 3 + // @Description: Flight mode when Channel 5 pwm is >1360, <= 1490 + // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake + // @User: Standard + GSCALAR(flight_mode3, "FLTMODE3", FLIGHT_MODE_3), + + // @Param: FLTMODE4 + // @DisplayName: Flight Mode 4 + // @Description: Flight mode when Channel 5 pwm is >1490, <= 1620 + // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake + // @User: Standard + GSCALAR(flight_mode4, "FLTMODE4", FLIGHT_MODE_4), + + // @Param: FLTMODE5 + // @DisplayName: Flight Mode 5 + // @Description: Flight mode when Channel 5 pwm is >1620, <= 1749 + // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake + // @User: Standard + GSCALAR(flight_mode5, "FLTMODE5", FLIGHT_MODE_5), + + // @Param: FLTMODE6 + // @DisplayName: Flight Mode 6 + // @Description: Flight mode when Channel 5 pwm is >=1750 + // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake + // @User: Standard + GSCALAR(flight_mode6, "FLTMODE6", FLIGHT_MODE_6), + + // @Param: SIMPLE + // @DisplayName: Simple mode bitmask + // @Description: Bitmask which holds which flight modes use simple heading mode (eg bit 0 = 1 means Flight Mode 0 uses simple mode) + // @User: Advanced + GSCALAR(simple_modes, "SIMPLE", 0), + + // @Param: LOG_BITMASK + // @DisplayName: Log bitmask + // @Description: 4 byte bitmap of log types to enable + // @Values: 830:Default,894:Default+RCIN,958:Default+IMU,1854:Default+Motors,-6146:NearlyAll-AC315,45054:NearlyAll,131070:All+DisarmedLogging,131071:All+FastATT,262142:All+MotBatt,393214:All+FastIMU,397310:All+FastIMU+PID,655358:All+FullIMU,0:Disabled + // @Bitmask: 0:ATTITUDE_FAST,1:ATTITUDE_MED,2:GPS,3:PM,4:CTUN,5:NTUN,6:RCIN,7:IMU,8:CMD,9:CURRENT,10:RCOUT,11:OPTFLOW,12:PID,13:COMPASS,14:INAV,15:CAMERA,16:WHEN_DISARMED,17:MOTBATT,18:IMU_FAST,19:IMU_RAW + // @User: Standard + GSCALAR(log_bitmask, "LOG_BITMASK", DEFAULT_LOG_BITMASK), + + // @Param: ESC_CALIBRATION + // @DisplayName: ESC Calibration + // @Description: Controls whether ArduCopter will enter ESC calibration on the next restart. Do not adjust this parameter manually. + // @User: Advanced + // @Values: 0:Normal Start-up, 1:Start-up in ESC Calibration mode if throttle high, 2:Start-up in ESC Calibration mode regardless of throttle, 9:Disabled + GSCALAR(esc_calibrate, "ESC_CALIBRATION", 0), + + // @Param: TUNE + // @DisplayName: Channel 6 Tuning + // @Description: Controls which parameters (normally PID gains) are being tuned with transmitter's channel 6 knob + // @User: Standard + // @Values: 0:None,1:Stab Roll/Pitch kP,4:Rate Roll/Pitch kP,5:Rate Roll/Pitch kI,21:Rate Roll/Pitch kD,3:Stab Yaw kP,6:Rate Yaw kP,26:Rate Yaw kD,14:Altitude Hold kP,7:Throttle Rate kP,34:Throttle Accel kP,35:Throttle Accel kI,36:Throttle Accel kD,42:Loiter Speed,12:Loiter Pos kP,22:Velocity XY kP,28:Velocity XY kI,10:WP Speed,25:Acro RollPitch kP,40:Acro Yaw kP,13:Heli Ext Gyro,17:OF Loiter kP,18:OF Loiter kI,19:OF Loiter kD,38:Declination,39:Circle Rate,41:RangeFinder Gain,46:Rate Pitch kP,47:Rate Pitch kI,48:Rate Pitch kD,49:Rate Roll kP,50:Rate Roll kI,51:Rate Roll kD,52:Rate Pitch FF,53:Rate Roll FF,54:Rate Yaw FF + GSCALAR(radio_tuning, "TUNE", 0), + + // @Param: TUNE_LOW + // @DisplayName: Tuning minimum + // @Description: The minimum value that will be applied to the parameter currently being tuned with the transmitter's channel 6 knob + // @User: Standard + // @Range: 0 32767 + GSCALAR(radio_tuning_low, "TUNE_LOW", 0), + + // @Param: TUNE_HIGH + // @DisplayName: Tuning maximum + // @Description: The maximum value that will be applied to the parameter currently being tuned with the transmitter's channel 6 knob + // @User: Standard + // @Range: 0 32767 + GSCALAR(radio_tuning_high, "TUNE_HIGH", 1000), + + // @Param: FRAME + // @DisplayName: Frame Orientation (+, X or V) + // @Description: Controls motor mixing for multicopters. Not used for Tri or Traditional Helicopters. + // @Values: 0:Plus, 1:X, 2:V, 3:H, 4:V-Tail, 5:A-Tail, 10:Y6B (New) + // @User: Standard + GSCALAR(frame_orientation, "FRAME", AP_MOTORS_X_FRAME), + + // @Param: CH7_OPT + // @DisplayName: Channel 7 option + // @Description: Select which function if performed when CH7 is above 1800 pwm + // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake + // @User: Standard + GSCALAR(ch7_option, "CH7_OPT", AUXSW_DO_NOTHING), + + // @Param: CH8_OPT + // @DisplayName: Channel 8 option + // @Description: Select which function if performed when CH8 is above 1800 pwm + // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake + // @User: Standard + GSCALAR(ch8_option, "CH8_OPT", AUXSW_DO_NOTHING), + + // @Param: CH9_OPT + // @DisplayName: Channel 9 option + // @Description: Select which function if performed when CH9 is above 1800 pwm + // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake + // @User: Standard + GSCALAR(ch9_option, "CH9_OPT", AUXSW_DO_NOTHING), + + // @Param: CH10_OPT + // @DisplayName: Channel 10 option + // @Description: Select which function if performed when CH10 is above 1800 pwm + // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake + // @User: Standard + GSCALAR(ch10_option, "CH10_OPT", AUXSW_DO_NOTHING), + + // @Param: CH11_OPT + // @DisplayName: Channel 11 option + // @Description: Select which function if performed when CH11 is above 1800 pwm + // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake + // @User: Standard + GSCALAR(ch11_option, "CH11_OPT", AUXSW_DO_NOTHING), + + // @Param: CH12_OPT + // @DisplayName: Channel 12 option + // @Description: Select which function if performed when CH12 is above 1800 pwm + // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake + // @User: Standard + GSCALAR(ch12_option, "CH12_OPT", AUXSW_DO_NOTHING), + + // @Param: ARMING_CHECK + // @DisplayName: Arming check + // @Description: Allows enabling or disabling of pre-arming checks of receiver, accelerometer, barometer, compass and GPS + // @Values: 0:Disabled, 1:Enabled, -3:Skip Baro, -5:Skip Compass, -9:Skip GPS, -17:Skip INS, -33:Skip Params/Sonar, -65:Skip RC, 127:Skip Voltage + // @Bitmask: 0:All,1:Baro,2:Compass,3:GPS,4:INS,5:Parameters+Sonar,6:RC,7:Voltage + // @User: Standard + GSCALAR(arming_check, "ARMING_CHECK", ARMING_CHECK_ALL), + + // @Param: DISARM_DELAY + // @DisplayName: Disarm delay + // @Description: Delay before automatic disarm in seconds. A value of zero disables auto disarm. + // @Units: Seconds + // @Range: 0 127 + // @User: Advanced + GSCALAR(disarm_delay, "DISARM_DELAY", AUTO_DISARMING_DELAY), + + // @Param: ANGLE_MAX + // @DisplayName: Angle Max + // @Description: Maximum lean angle in all flight modes + // @Units: Centi-degrees + // @Range: 1000 8000 + // @User: Advanced + ASCALAR(angle_max, "ANGLE_MAX", DEFAULT_ANGLE_MAX), + + // @Param: RC_FEEL_RP + // @DisplayName: RC Feel Roll/Pitch + // @Description: RC feel for roll/pitch which controls vehicle response to user input with 0 being extremely soft and 100 being crisp + // @Range: 0 100 + // @Increment: 1 + // @User: Standard + // @Values: 0:Very Soft, 25:Soft, 50:Medium, 75:Crisp, 100:Very Crisp + GSCALAR(rc_feel_rp, "RC_FEEL_RP", RC_FEEL_RP_MEDIUM), + +#if POSHOLD_ENABLED == ENABLED + // @Param: PHLD_BRAKE_RATE + // @DisplayName: PosHold braking rate + // @Description: PosHold flight mode's rotation rate during braking in deg/sec + // @Units: deg/sec + // @Range: 4 12 + // @User: Advanced + GSCALAR(poshold_brake_rate, "PHLD_BRAKE_RATE", POSHOLD_BRAKE_RATE_DEFAULT), + + // @Param: PHLD_BRAKE_ANGLE + // @DisplayName: PosHold braking angle max + // @Description: PosHold flight mode's max lean angle during braking in centi-degrees + // @Units: Centi-degrees + // @Range: 2000 4500 + // @User: Advanced + GSCALAR(poshold_brake_angle_max, "PHLD_BRAKE_ANGLE", POSHOLD_BRAKE_ANGLE_DEFAULT), +#endif + + // @Param: LAND_REPOSITION + // @DisplayName: Land repositioning + // @Description: Enables user input during LAND mode, the landing phase of RTL, and auto mode landings. + // @Values: 0:No repositioning, 1:Repositioning + // @User: Advanced + GSCALAR(land_repositioning, "LAND_REPOSITION", LAND_REPOSITION_DEFAULT), + + // @Param: FS_EKF_ACTION + // @DisplayName: EKF Failsafe Action + // @Description: Controls the action that will be taken when an EKF failsafe is invoked + // @Values: 1:Land, 2:AltHold, 3:Land even in Stabilize + // @User: Advanced + GSCALAR(fs_ekf_action, "FS_EKF_ACTION", FS_EKF_ACTION_DEFAULT), + + // @Param: FS_EKF_THRESH + // @DisplayName: EKF failsafe variance threshold + // @Description: Allows setting the maximum acceptable compass and velocity variance + // @Values: 0.6:Strict, 0.8:Default, 1.0:Relaxed + // @User: Advanced + GSCALAR(fs_ekf_thresh, "FS_EKF_THRESH", FS_EKF_THRESHOLD_DEFAULT), + + // @Param: FS_CRASH_CHECK + // @DisplayName: Crash check enable + // @Description: This enables automatic crash checking. When enabled the motors will disarm if a crash is detected. + // @Values: 0:Disabled, 1:Enabled + // @User: Advanced + GSCALAR(fs_crash_check, "FS_CRASH_CHECK", 1), + +#if FRAME_CONFIG == HELI_FRAME + // @Group: HS1_ + // @Path: ../libraries/RC_Channel/RC_Channel.cpp + GGROUP(heli_servo_1, "HS1_", RC_Channel), + // @Group: HS2_ + // @Path: ../libraries/RC_Channel/RC_Channel.cpp + GGROUP(heli_servo_2, "HS2_", RC_Channel), + // @Group: HS3_ + // @Path: ../libraries/RC_Channel/RC_Channel.cpp + GGROUP(heli_servo_3, "HS3_", RC_Channel), + // @Group: HS4_ + // @Path: ../libraries/RC_Channel/RC_Channel.cpp + GGROUP(heli_servo_4, "HS4_", RC_Channel), + // @Group: H_RSC_ + // @Path: ../libraries/RC_Channel/RC_Channel.cpp + GGROUP(heli_servo_rsc, "H_RSC_", RC_Channel), +#endif + + // RC channel + //----------- + // @Group: RC1_ + // @Path: ../libraries/RC_Channel/RC_Channel.cpp + GGROUP(rc_1, "RC1_", RC_Channel), + // @Group: RC2_ + // @Path: ../libraries/RC_Channel/RC_Channel.cpp + GGROUP(rc_2, "RC2_", RC_Channel), + // @Group: RC3_ + // @Path: ../libraries/RC_Channel/RC_Channel.cpp + GGROUP(rc_3, "RC3_", RC_Channel), + // @Group: RC4_ + // @Path: ../libraries/RC_Channel/RC_Channel.cpp + GGROUP(rc_4, "RC4_", RC_Channel), + // @Group: RC5_ + // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp + GGROUP(rc_5, "RC5_", RC_Channel_aux), + // @Group: RC6_ + // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp + GGROUP(rc_6, "RC6_", RC_Channel_aux), + // @Group: RC7_ + // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp + GGROUP(rc_7, "RC7_", RC_Channel_aux), + // @Group: RC8_ + // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp + GGROUP(rc_8, "RC8_", RC_Channel_aux), + +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN + // @Group: RC9_ + // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp + GGROUP(rc_9, "RC9_", RC_Channel_aux), +#endif + + // @Group: RC10_ + // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp + GGROUP(rc_10, "RC10_", RC_Channel_aux), + // @Group: RC11_ + // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp + GGROUP(rc_11, "RC11_", RC_Channel_aux), + +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN + // @Group: RC12_ + // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp + GGROUP(rc_12, "RC12_", RC_Channel_aux), + + // @Group: RC13_ + // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp + GGROUP(rc_13, "RC13_", RC_Channel_aux), + + // @Group: RC14_ + // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp + GGROUP(rc_14, "RC14_", RC_Channel_aux), +#endif + + // @Param: RC_SPEED + // @DisplayName: ESC Update Speed + // @Description: This is the speed in Hertz that your ESCs will receive updates + // @Units: Hz + // @Range: 50 490 + // @Increment: 1 + // @User: Advanced + GSCALAR(rc_speed, "RC_SPEED", RC_FAST_SPEED), + + // @Param: ACRO_RP_P + // @DisplayName: Acro Roll and Pitch P gain + // @Description: Converts pilot roll and pitch into a desired rate of rotation in ACRO and SPORT mode. Higher values mean faster rate of rotation. + // @Range: 1 10 + // @User: Standard + GSCALAR(acro_rp_p, "ACRO_RP_P", ACRO_RP_P), + + // @Param: ACRO_YAW_P + // @DisplayName: Acro Yaw P gain + // @Description: Converts pilot yaw input into a desired rate of rotation in ACRO, Stabilize and SPORT modes. Higher values mean faster rate of rotation. + // @Range: 1 10 + // @User: Standard + GSCALAR(acro_yaw_p, "ACRO_YAW_P", ACRO_YAW_P), + + // @Param: ACRO_BAL_ROLL + // @DisplayName: Acro Balance Roll + // @Description: rate at which roll angle returns to level in acro mode. A higher value causes the vehicle to return to level faster. + // @Range: 0 3 + // @Increment: 0.1 + // @User: Advanced + GSCALAR(acro_balance_roll, "ACRO_BAL_ROLL", ACRO_BALANCE_ROLL), + + // @Param: ACRO_BAL_PITCH + // @DisplayName: Acro Balance Pitch + // @Description: rate at which pitch angle returns to level in acro mode. A higher value causes the vehicle to return to level faster. + // @Range: 0 3 + // @Increment: 0.1 + // @User: Advanced + GSCALAR(acro_balance_pitch, "ACRO_BAL_PITCH", ACRO_BALANCE_PITCH), + + // @Param: ACRO_TRAINER + // @DisplayName: Acro Trainer + // @Description: Type of trainer used in acro mode + // @Values: 0:Disabled,1:Leveling,2:Leveling and Limited + // @User: Advanced + GSCALAR(acro_trainer, "ACRO_TRAINER", ACRO_TRAINER_LIMITED), + + // @Param: ACRO_EXPO + // @DisplayName: Acro Expo + // @Description: Acro roll/pitch Expo to allow faster rotation when stick at edges + // @Values: 0:Disabled,0.1:Very Low,0.2:Low,0.3:Medium,0.4:High,0.5:Very High + // @User: Advanced + GSCALAR(acro_expo, "ACRO_EXPO", ACRO_EXPO_DEFAULT), + + // PID controller + //--------------- + + // @Param: RATE_RLL_P + // @DisplayName: Roll axis rate controller P gain + // @Description: Roll axis rate controller P gain. Converts the difference between desired roll rate and actual roll rate into a motor speed output + // @Range: 0.08 0.30 + // @Increment: 0.005 + // @User: Standard + + // @Param: RATE_RLL_I + // @DisplayName: Roll axis rate controller I gain + // @Description: Roll axis rate controller I gain. Corrects long-term difference in desired roll rate vs actual roll rate + // @Range: 0.01 0.5 + // @Increment: 0.01 + // @User: Standard + + // @Param: RATE_RLL_IMAX + // @DisplayName: Roll axis rate controller I gain maximum + // @Description: Roll axis rate controller I gain maximum. Constrains the maximum motor output that the I gain will output + // @Range: 0 4500 + // @Increment: 10 + // @Units: Percent*10 + // @User: Standard + + // @Param: RATE_RLL_D + // @DisplayName: Roll axis rate controller D gain + // @Description: Roll axis rate controller D gain. Compensates for short-term change in desired roll rate vs actual roll rate + // @Range: 0.001 0.02 + // @Increment: 0.001 + // @User: Standard +#if FRAME_CONFIG == HELI_FRAME + GGROUP(pid_rate_roll, "RATE_RLL_", AC_HELI_PID), +#else + GGROUP(pid_rate_roll, "RATE_RLL_", AC_PID), +#endif + + // @Param: RATE_PIT_P + // @DisplayName: Pitch axis rate controller P gain + // @Description: Pitch axis rate controller P gain. Converts the difference between desired pitch rate and actual pitch rate into a motor speed output + // @Range: 0.08 0.30 + // @Increment: 0.005 + // @User: Standard + + // @Param: RATE_PIT_I + // @DisplayName: Pitch axis rate controller I gain + // @Description: Pitch axis rate controller I gain. Corrects long-term difference in desired pitch rate vs actual pitch rate + // @Range: 0.01 0.5 + // @Increment: 0.01 + // @User: Standard + + // @Param: RATE_PIT_IMAX + // @DisplayName: Pitch axis rate controller I gain maximum + // @Description: Pitch axis rate controller I gain maximum. Constrains the maximum motor output that the I gain will output + // @Range: 0 4500 + // @Increment: 10 + // @Units: Percent*10 + // @User: Standard + + // @Param: RATE_PIT_D + // @DisplayName: Pitch axis rate controller D gain + // @Description: Pitch axis rate controller D gain. Compensates for short-term change in desired pitch rate vs actual pitch rate + // @Range: 0.001 0.02 + // @Increment: 0.001 + // @User: Standard +#if FRAME_CONFIG == HELI_FRAME + GGROUP(pid_rate_pitch, "RATE_PIT_", AC_HELI_PID), +#else + GGROUP(pid_rate_pitch, "RATE_PIT_", AC_PID), +#endif + + // @Param: RATE_YAW_P + // @DisplayName: Yaw axis rate controller P gain + // @Description: Yaw axis rate controller P gain. Converts the difference between desired yaw rate and actual yaw rate into a motor speed output + // @Range: 0.150 0.50 + // @Increment: 0.005 + // @User: Standard + + // @Param: RATE_YAW_I + // @DisplayName: Yaw axis rate controller I gain + // @Description: Yaw axis rate controller I gain. Corrects long-term difference in desired yaw rate vs actual yaw rate + // @Range: 0.010 0.05 + // @Increment: 0.01 + // @User: Standard + + // @Param: RATE_YAW_IMAX + // @DisplayName: Yaw axis rate controller I gain maximum + // @Description: Yaw axis rate controller I gain maximum. Constrains the maximum motor output that the I gain will output + // @Range: 0 4500 + // @Increment: 10 + // @Units: Percent*10 + // @User: Standard + + // @Param: RATE_YAW_D + // @DisplayName: Yaw axis rate controller D gain + // @Description: Yaw axis rate controller D gain. Compensates for short-term change in desired yaw rate vs actual yaw rate + // @Range: 0.000 0.02 + // @Increment: 0.001 + // @User: Standard +#if FRAME_CONFIG == HELI_FRAME + GGROUP(pid_rate_yaw, "RATE_YAW_", AC_HELI_PID), +#else + GGROUP(pid_rate_yaw, "RATE_YAW_", AC_PID), +#endif + + // @Param: VEL_XY_P + // @DisplayName: Velocity (horizontal) P gain + // @Description: Velocity (horizontal) P gain. Converts the difference between desired velocity to a target acceleration + // @Range: 0.1 6.0 + // @Increment: 0.1 + // @User: Advanced + + // @Param: VEL_XY_I + // @DisplayName: Velocity (horizontal) I gain + // @Description: Velocity (horizontal) I gain. Corrects long-term difference in desired velocity to a target acceleration + // @Range: 0.02 1.00 + // @Increment: 0.01 + // @User: Advanced + + // @Param: VEL_XY_IMAX + // @DisplayName: Velocity (horizontal) integrator maximum + // @Description: Velocity (horizontal) integrator maximum. Constrains the target acceleration that the I gain will output + // @Range: 0 4500 + // @Increment: 10 + // @Units: cm/s/s + // @User: Advanced + GGROUP(pi_vel_xy, "VEL_XY_", AC_PI_2D), + + // @Param: VEL_Z_P + // @DisplayName: Velocity (vertical) P gain + // @Description: Velocity (vertical) P gain. Converts the difference between desired vertical speed and actual speed into a desired acceleration that is passed to the throttle acceleration controller + // @Range: 1.000 8.000 + // @User: Standard + GGROUP(p_vel_z, "VEL_Z_", AC_P), + + // @Param: ACCEL_Z_P + // @DisplayName: Throttle acceleration controller P gain + // @Description: Throttle acceleration controller P gain. Converts the difference between desired vertical acceleration and actual acceleration into a motor output + // @Range: 0.500 1.500 + // @User: Standard + + // @Param: ACCEL_Z_I + // @DisplayName: Throttle acceleration controller I gain + // @Description: Throttle acceleration controller I gain. Corrects long-term difference in desired vertical acceleration and actual acceleration + // @Range: 0.000 3.000 + // @User: Standard + + // @Param: ACCEL_Z_IMAX + // @DisplayName: Throttle acceleration controller I gain maximum + // @Description: Throttle acceleration controller I gain maximum. Constrains the maximum pwm that the I term will generate + // @Range: 0 1000 + // @Units: Percent*10 + // @User: Standard + + // @Param: ACCEL_Z_D + // @DisplayName: Throttle acceleration controller D gain + // @Description: Throttle acceleration controller D gain. Compensates for short-term change in desired vertical acceleration vs actual acceleration + // @Range: 0.000 0.400 + // @User: Standard + + // @Param: ACCEL_Z_FILT_HZ + // @DisplayName: Throttle acceleration filter + // @Description: Filter applied to acceleration to reduce noise. Lower values reduce noise but add delay. + // @Range: 1.000 100.000 + // @Units: Hz + // @User: Standard + GGROUP(pid_accel_z, "ACCEL_Z_", AC_PID), + + // P controllers + //-------------- + // @Param: STB_RLL_P + // @DisplayName: Roll axis stabilize controller P gain + // @Description: Roll axis stabilize (i.e. angle) controller P gain. Converts the error between the desired roll angle and actual angle to a desired roll rate + // @Range: 3.000 12.000 + // @User: Standard + GGROUP(p_stabilize_roll, "STB_RLL_", AC_P), + + // @Param: STB_PIT_P + // @DisplayName: Pitch axis stabilize controller P gain + // @Description: Pitch axis stabilize (i.e. angle) controller P gain. Converts the error between the desired pitch angle and actual angle to a desired pitch rate + // @Range: 3.000 12.000 + // @User: Standard + GGROUP(p_stabilize_pitch, "STB_PIT_", AC_P), + + // @Param: STB_YAW_P + // @DisplayName: Yaw axis stabilize controller P gain + // @Description: Yaw axis stabilize (i.e. angle) controller P gain. Converts the error between the desired yaw angle and actual angle to a desired yaw rate + // @Range: 3.000 6.000 + // @User: Standard + GGROUP(p_stabilize_yaw, "STB_YAW_", AC_P), + + // @Param: POS_Z_P + // @DisplayName: Position (vertical) controller P gain + // @Description: Position (vertical) controller P gain. Converts the difference between the desired altitude and actual altitude into a climb or descent rate which is passed to the throttle rate controller + // @Range: 1.000 3.000 + // @User: Standard + GGROUP(p_alt_hold, "POS_Z_", AC_P), + + // @Param: POS_XY_P + // @DisplayName: Position (horizonal) controller P gain + // @Description: Loiter position controller P gain. Converts the distance (in the latitude direction) to the target location into a desired speed which is then passed to the loiter latitude rate controller + // @Range: 0.500 2.000 + // @User: Standard + GGROUP(p_pos_xy, "POS_XY_", AC_P), + +#if PRECISION_LANDING == ENABLED + // @Param: PRECLNDVEL_P + // @DisplayName: Precision landing velocity controller P gain + // @Description: Precision landing velocity controller P gain + // @Range: 0.100 5.000 + // @User: Advanced + + // @Param: PRECLNDVEL_I + // @DisplayName: Precision landing velocity controller I gain + // @Description: Precision landing velocity controller I gain + // @Range: 0.100 5.000 + // @User: Advanced + + // @Param: PRECLNDVEL_IMAX + // @DisplayName: Precision landing velocity controller I gain maximum + // @Description: Precision landing velocity controller I gain maximum + // @Range: 0 1000 + // @Units: cm/s + // @User: Standard + GGROUP(pi_precland, "PLAND_", AC_PI_2D), +#endif + + // variables not in the g class which contain EEPROM saved variables + +#if CAMERA == ENABLED + // @Group: CAM_ + // @Path: ../libraries/AP_Camera/AP_Camera.cpp + GOBJECT(camera, "CAM_", AP_Camera), +#endif + + // @Group: RELAY_ + // @Path: ../libraries/AP_Relay/AP_Relay.cpp + GOBJECT(relay, "RELAY_", AP_Relay), + +#if EPM_ENABLED == ENABLED + // @Group: EPM_ + // @Path: ../libraries/AP_EPM/AP_EPM.cpp + GOBJECT(epm, "EPM_", AP_EPM), +#endif + +#if PARACHUTE == ENABLED + // @Group: CHUTE_ + // @Path: ../libraries/AP_Parachute/AP_Parachute.cpp + GOBJECT(parachute, "CHUTE_", AP_Parachute), +#endif + + // @Group: LGR_ + // @Path: ../libraries/AP_LandingGear/AP_LandingGear.cpp + GOBJECT(landinggear, "LGR_", AP_LandingGear), + +#if FRAME_CONFIG == HELI_FRAME + // @Group: IM_ + // @Path: ../libraries/AC_InputManager/AC_InputManager_Heli.cpp + GOBJECT(input_manager, "IM_", AC_InputManager_Heli), +#endif + + // @Group: COMPASS_ + // @Path: ../libraries/AP_Compass/Compass.cpp + GOBJECT(compass, "COMPASS_", Compass), + + // @Group: INS_ + // @Path: ../libraries/AP_InertialSensor/AP_InertialSensor.cpp + GOBJECT(ins, "INS_", AP_InertialSensor), + + // @Group: WPNAV_ + // @Path: ../libraries/AC_WPNav/AC_WPNav.cpp + GOBJECT(wp_nav, "WPNAV_", AC_WPNav), + + // @Group: CIRCLE_ + // @Path: ../libraries/AC_WPNav/AC_Circle.cpp + GOBJECT(circle_nav, "CIRCLE_", AC_Circle), + +#if FRAME_CONFIG == HELI_FRAME + GOBJECT(attitude_control, "ATC_", AC_AttitudeControl_Heli), +#else + // @Group: ATC_ + // @Path: ../libraries/AC_AttitudeControl/AC_AttitudeControl.cpp + GOBJECT(attitude_control, "ATC_", AC_AttitudeControl), +#endif + + // @Group: POSCON_ + // @Path: ../libraries/AC_AttitudeControl/AC_PosControl.cpp + GOBJECT(pos_control, "PSC", AC_PosControl), + + // @Group: SR0_ + // @Path: GCS_Mavlink.cpp + GOBJECTN(gcs[0], gcs0, "SR0_", GCS_MAVLINK), + + // @Group: SR1_ + // @Path: GCS_Mavlink.cpp + GOBJECTN(gcs[1], gcs1, "SR1_", GCS_MAVLINK), + + // @Group: SR2_ + // @Path: GCS_Mavlink.cpp + GOBJECTN(gcs[2], gcs2, "SR2_", GCS_MAVLINK), + + // @Group: SR3_ + // @Path: GCS_Mavlink.cpp + GOBJECTN(gcs[3], gcs3, "SR3_", GCS_MAVLINK), + + // @Group: AHRS_ + // @Path: ../libraries/AP_AHRS/AP_AHRS.cpp + GOBJECT(ahrs, "AHRS_", AP_AHRS), + +#if MOUNT == ENABLED + // @Group: MNT + // @Path: ../libraries/AP_Mount/AP_Mount.cpp + GOBJECT(camera_mount, "MNT", AP_Mount), +#endif + + // @Group: LOG + // @Path: ../libraries/DataFlash/DataFlash.cpp + GOBJECT(DataFlash, "LOG", DataFlash_Class), + + // @Group: BATT + // @Path: ../libraries/AP_BattMonitor/AP_BattMonitor.cpp + GOBJECT(battery, "BATT", AP_BattMonitor), + + // @Group: BRD_ + // @Path: ../libraries/AP_BoardConfig/AP_BoardConfig.cpp + GOBJECT(BoardConfig, "BRD_", AP_BoardConfig), + +#if SPRAYER == ENABLED + // @Group: SPRAY_ + // @Path: ../libraries/AC_Sprayer/AC_Sprayer.cpp + GOBJECT(sprayer, "SPRAY_", AC_Sprayer), +#endif + +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + GOBJECT(sitl, "SIM_", SITL::SITL), +#endif + + // @Group: GND_ + // @Path: ../libraries/AP_Baro/AP_Baro.cpp + GOBJECT(barometer, "GND_", AP_Baro), + + // GPS driver + // @Group: GPS_ + // @Path: ../libraries/AP_GPS/AP_GPS.cpp + GOBJECT(gps, "GPS_", AP_GPS), + + // @Group: SCHED_ + // @Path: ../libraries/AP_Scheduler/AP_Scheduler.cpp + GOBJECT(scheduler, "SCHED_", AP_Scheduler), + +#if AC_FENCE == ENABLED + // @Group: FENCE_ + // @Path: ../libraries/AC_Fence/AC_Fence.cpp + GOBJECT(fence, "FENCE_", AC_Fence), +#endif + +#if AC_RALLY == ENABLED + // @Group: RALLY_ + // @Path: ../libraries/AP_Rally/AP_Rally.cpp + GOBJECT(rally, "RALLY_", AP_Rally), +#endif + +#if FRAME_CONFIG == HELI_FRAME + // @Group: H_ + // @Path: ../libraries/AP_Motors/AP_MotorsHeli_Single.cpp + GOBJECT(motors, "H_", AP_MotorsHeli_Single), + +#elif FRAME_CONFIG == SINGLE_FRAME + // @Group: SS1_ + // @Path: ../libraries/RC_Channel/RC_Channel.cpp + GGROUP(single_servo_1, "SS1_", RC_Channel), + // @Group: SS2_ + // @Path: ../libraries/RC_Channel/RC_Channel.cpp + GGROUP(single_servo_2, "SS2_", RC_Channel), + // @Group: SS3_ + // @Path: ../libraries/RC_Channel/RC_Channel.cpp + GGROUP(single_servo_3, "SS3_", RC_Channel), + // @Group: SS4_ + // @Path: ../libraries/RC_Channel/RC_Channel.cpp + GGROUP(single_servo_4, "SS4_", RC_Channel), + // @Group: MOT_ + // @Path: ../libraries/AP_Motors/AP_MotorsSingle.cpp + GOBJECT(motors, "MOT_", AP_MotorsSingle), + +#elif FRAME_CONFIG == COAX_FRAME + // @Group: SS1_ + // @Path: ../libraries/RC_Channel/RC_Channel.cpp + GGROUP(single_servo_1, "SS1_", RC_Channel), + // @Group: SS2_ + // @Path: ../libraries/RC_Channel/RC_Channel.cpp + GGROUP(single_servo_2, "SS2_", RC_Channel), + // @Group: MOT_ + // @Path: ../libraries/AP_Motors/AP_MotorsCoax.cpp + GOBJECT(motors, "MOT_", AP_MotorsCoax), + +#elif FRAME_CONFIG == TRI_FRAME + // @Group: MOT_ + // @Path: ../libraries/AP_Motors/AP_MotorsTri.cpp + GOBJECT(motors, "MOT_", AP_MotorsTri), + +#else + // @Group: MOT_ + // @Path: ../libraries/AP_Motors/AP_MotorsMulticopter.cpp + GOBJECT(motors, "MOT_", AP_MotorsMulticopter), +#endif + + // @Group: RCMAP_ + // @Path: ../libraries/AP_RCMapper/AP_RCMapper.cpp + GOBJECT(rcmap, "RCMAP_", RCMapper), + + // @Group: EKF_ + // @Path: ../libraries/AP_NavEKF/AP_NavEKF.cpp + GOBJECTN(EKF, NavEKF, "EKF_", NavEKF), + + // @Group: EK2_ + // @Path: ../libraries/AP_NavEKF2/AP_NavEKF2.cpp + GOBJECTN(EKF2, NavEKF2, "EK2_", NavEKF2), + + // @Group: MIS_ + // @Path: ../libraries/AP_Mission/AP_Mission.cpp + GOBJECT(mission, "MIS_", AP_Mission), + + // @Group: RSSI_ + // @Path: ../libraries/AP_RSSI/AP_RSSI.cpp + GOBJECT(rssi, "RSSI_", AP_RSSI), + +#if CONFIG_SONAR == ENABLED + // @Group: RNGFND + // @Path: ../libraries/AP_RangeFinder/RangeFinder.cpp + GOBJECT(sonar, "RNGFND", RangeFinder), +#endif + +#if AP_TERRAIN_AVAILABLE && AC_TERRAIN + // @Group: TERRAIN_ + // @Path: ../libraries/AP_Terrain/AP_Terrain.cpp + GOBJECT(terrain, "TERRAIN_", AP_Terrain), +#endif + +#if OPTFLOW == ENABLED + // @Group: FLOW + // @Path: ../libraries/AP_OpticalFlow/OpticalFlow.cpp + GOBJECT(optflow, "FLOW", OpticalFlow), +#endif + +#if PRECISION_LANDING == ENABLED + // @Group: PRECLAND_ + // @Path: ../libraries/AC_PrecLand/AC_PrecLand.cpp + GOBJECT(precland, "PLAND_", AC_PrecLand), +#endif + + // @Group: RPM + // @Path: ../libraries/AP_RPM/AP_RPM.cpp + GOBJECT(rpm_sensor, "RPM", AP_RPM), + + // @Group: ADSB_ + // @Path: ../libraries/AP_ADSB/AP_ADSB.cpp + GOBJECT(adsb, "ADSB_", AP_ADSB), + + // @Param: AUTOTUNE_AXES + // @DisplayName: Autotune axis bitmask + // @Description: 1-byte bitmap of axes to autotune + // @Values: 7:All,1:Roll Only,2:Pitch Only,4:Yaw Only,3:Roll and Pitch,5:Roll and Yaw,6:Pitch and Yaw + // @Bitmask: 0:Roll,1:Pitch,2:Yaw + // @User: Standard + GSCALAR(autotune_axis_bitmask, "AUTOTUNE_AXES", 7), // AUTOTUNE_AXIS_BITMASK_DEFAULT + + // @Param: AUTOTUNE_AGGR + // @DisplayName: Autotune aggressiveness + // @Description: Autotune aggressiveness. Defines the bounce back used to detect size of the D term. + // @Range: 0.05 0.10 + // @User: Standard + GSCALAR(autotune_aggressiveness, "AUTOTUNE_AGGR", 0.1f), + + // @Param: AUTOTUNE_MIN_D + // @DisplayName: AutoTune minimum D + // @Description: Defines the minimum D gain + // @Range: 0.001 0.006 + // @User: Standard + GSCALAR(autotune_min_d, "AUTOTUNE_MIN_D", 0.001f), + + AP_VAREND +}; + +/* + This is a conversion table from old parameter values to new + parameter names. The startup code looks for saved values of the old + parameters and will copy them across to the new parameters if the + new parameter does not yet have a saved value. It then saves the new + value. + + Note that this works even if the old parameter has been removed. It + relies on the old k_param index not being removed + + The second column below is the index in the var_info[] table for the + old object. This should be zero for top level parameters. + */ +const AP_Param::ConversionInfo conversion_table[] = { + { Parameters::k_param_battery_monitoring, 0, AP_PARAM_INT8, "BATT_MONITOR" }, + { Parameters::k_param_battery_volt_pin, 0, AP_PARAM_INT8, "BATT_VOLT_PIN" }, + { Parameters::k_param_battery_curr_pin, 0, AP_PARAM_INT8, "BATT_CURR_PIN" }, + { Parameters::k_param_volt_div_ratio, 0, AP_PARAM_FLOAT, "BATT_VOLT_MULT" }, + { Parameters::k_param_curr_amp_per_volt, 0, AP_PARAM_FLOAT, "BATT_AMP_PERVOLT" }, + { Parameters::k_param_pack_capacity, 0, AP_PARAM_INT32, "BATT_CAPACITY" }, + { Parameters::k_param_log_bitmask_old, 0, AP_PARAM_INT16, "LOG_BITMASK" }, + { Parameters::k_param_serial0_baud, 0, AP_PARAM_INT16, "SERIAL0_BAUD" }, + { Parameters::k_param_serial1_baud, 0, AP_PARAM_INT16, "SERIAL1_BAUD" }, + { Parameters::k_param_serial2_baud, 0, AP_PARAM_INT16, "SERIAL2_BAUD" }, +}; + +void Copter::load_parameters(void) +{ + if (!AP_Param::check_var_info()) { + cliSerial->printf("Bad var table\n"); + AP_HAL::panic("Bad var table"); + } + + // disable centrifugal force correction, it will be enabled as part of the arming process + ahrs.set_correct_centrifugal(false); + hal.util->set_soft_armed(false); + + if (!g.format_version.load() || + g.format_version != Parameters::k_format_version) { + + // erase all parameters + cliSerial->printf("Firmware change: erasing EEPROM...\n"); + AP_Param::erase_all(); + + // save the current format version + g.format_version.set_and_save(Parameters::k_format_version); + cliSerial->println("done."); + } else { + uint32_t before = micros(); + // Load all auto-loaded EEPROM variables + AP_Param::load_all(); + AP_Param::convert_old_parameters(&conversion_table[0], ARRAY_SIZE(conversion_table)); + cliSerial->printf("load_all took %uus\n", (unsigned)(micros() - before)); + } +} diff --git a/ArduSub/Parameters.h b/ArduSub/Parameters.h new file mode 100644 index 00000000000..d2aea10550b --- /dev/null +++ b/ArduSub/Parameters.h @@ -0,0 +1,605 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#ifndef PARAMETERS_H +#define PARAMETERS_H + +#include + +// 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 + diff --git a/ArduSub/Parameters.pde b/ArduSub/Parameters.pde new file mode 100644 index 00000000000..10d157b86ce --- /dev/null +++ b/ArduSub/Parameters.pde @@ -0,0 +1,3 @@ +/* + blank file. This is needed to help with upgrades of old versions if MissionPlanner + */ diff --git a/ArduSub/ReleaseNotes.txt b/ArduSub/ReleaseNotes.txt new file mode 100644 index 00000000000..a6af61b1051 --- /dev/null +++ b/ArduSub/ReleaseNotes.txt @@ -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) +------------------------------------------------------------------ diff --git a/ArduSub/UserCode.cpp b/ArduSub/UserCode.cpp new file mode 100644 index 00000000000..e7d2ef9d847 --- /dev/null +++ b/ArduSub/UserCode.cpp @@ -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 diff --git a/ArduSub/UserVariables.h b/ArduSub/UserVariables.h new file mode 100644 index 00000000000..6f596218ed7 --- /dev/null +++ b/ArduSub/UserVariables.h @@ -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 + + diff --git a/ArduSub/adsb.cpp b/ArduSub/adsb.cpp new file mode 100644 index 00000000000..9253e049f1a --- /dev/null +++ b/ArduSub/adsb.cpp @@ -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 . + */ + +#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 diff --git a/ArduSub/arming_checks.cpp b/ArduSub/arming_checks.cpp new file mode 100644 index 00000000000..f1462b98c05 --- /dev/null +++ b/ArduSub/arming_checks.cpp @@ -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= 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 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; +} diff --git a/ArduSub/baro_ground_effect.cpp b/ArduSub/baro_ground_effect.cpp new file mode 100644 index 00000000000..548c349e5f6 --- /dev/null +++ b/ArduSub/baro_ground_effect.cpp @@ -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 diff --git a/ArduSub/capabilities.cpp b/ArduSub/capabilities.cpp new file mode 100644 index 00000000000..b4e4f62ed9f --- /dev/null +++ b/ArduSub/capabilities.cpp @@ -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); +} diff --git a/ArduSub/commands.cpp b/ArduSub/commands.cpp new file mode 100644 index 00000000000..f51bb70b4b4 --- /dev/null +++ b/ArduSub/commands.cpp @@ -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); + } +} diff --git a/ArduSub/commands_logic.cpp b/ArduSub/commands_logic.cpp new file mode 100644 index 00000000000..e156c623d0b --- /dev/null +++ b/ArduSub/commands_logic.cpp @@ -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 +} diff --git a/ArduSub/compassmot.cpp b/ArduSub/compassmot.cpp new file mode 100644 index 00000000000..c204b68e8bf --- /dev/null +++ b/ArduSub/compassmot.cpp @@ -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; icontrol_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; idelay(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= 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 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; idelay(ms); +} diff --git a/ArduSub/config.h b/ArduSub/config.h new file mode 100644 index 00000000000..9d76f8c36d1 --- /dev/null +++ b/ArduSub/config.h @@ -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__ diff --git a/ArduSub/config_channels.h b/ArduSub/config_channels.h new file mode 100644 index 00000000000..35536d693cf --- /dev/null +++ b/ArduSub/config_channels.h @@ -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__ diff --git a/ArduSub/control_acro.cpp b/ArduSub/control_acro.cpp new file mode 100644 index 00000000000..4f9cf85ae8b --- /dev/null +++ b/ArduSub/control_acro.cpp @@ -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; +} diff --git a/ArduSub/control_althold.cpp b/ArduSub/control_althold.cpp new file mode 100644 index 00000000000..ec652e214fe --- /dev/null +++ b/ArduSub/control_althold.cpp @@ -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; + } +} diff --git a/ArduSub/control_auto.cpp b/ArduSub/control_auto.cpp new file mode 100644 index 00000000000..9c7fb6dab9c --- /dev/null +++ b/ArduSub/control_auto.cpp @@ -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; + } +} + diff --git a/ArduSub/control_autotune.cpp b/ArduSub/control_autotune.cpp new file mode 100644 index 00000000000..40a2e8a3805 --- /dev/null +++ b/ArduSub/control_autotune.cpp @@ -0,0 +1,1311 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include "Copter.h" + +#if AUTOTUNE_ENABLED == ENABLED + +/* + * control_autotune.pde - init and run calls for autotune flight mode + * + * Instructions: + * 1) Set up one flight mode switch position to be AltHold. + * 2) Set the Ch7 Opt or Ch8 Opt to AutoTune to allow you to turn the auto tuning on/off with the ch7 or ch8 switch. + * 3) Ensure the ch7 or ch8 switch is in the LOW position. + * 4) Wait for a calm day and go to a large open area. + * 5) Take off and put the vehicle into AltHold mode at a comfortable altitude. + * 6) Set the ch7/ch8 switch to the HIGH position to engage auto tuning: + * a) You will see it twitch about 20 degrees left and right for a few minutes, then it will repeat forward and back. + * b) Use the roll and pitch stick at any time to reposition the copter if it drifts away (it will use the original PID gains during repositioning and between tests). + * When you release the sticks it will continue auto tuning where it left off. + * c) Move the ch7/ch8 switch into the LOW position at any time to abandon the autotuning and return to the origin PIDs. + * d) Make sure that you do not have any trim set on your transmitter or the autotune may not get the signal that the sticks are centered. + * 7) When the tune completes the vehicle will change back to the original PID gains. + * 8) Put the ch7/ch8 switch into the LOW position then back to the HIGH position to test the tuned PID gains. + * 9) Put the ch7/ch8 switch into the LOW position to fly using the original PID gains. + * 10) If you are happy with the autotuned PID gains, leave the ch7/ch8 switch in the HIGH position, land and disarm to save the PIDs permanently. + * If you DO NOT like the new PIDS, switch ch7/ch8 LOW to return to the original PIDs. The gains will not be saved when you disarm + * + * What it's doing during each "twitch": + * a) invokes 90 deg/sec rate request + * b) records maximum "forward" roll rate and bounce back rate + * c) when copter reaches 20 degrees or 1 second has passed, it commands level + * d) tries to keep max rotation rate between 80% ~ 100% of requested rate (90deg/sec) by adjusting rate P + * e) increases rate D until the bounce back becomes greater than 10% of requested rate (90deg/sec) + * f) decreases rate D until the bounce back becomes less than 10% of requested rate (90deg/sec) + * g) increases rate P until the max rotate rate becomes greater than the request rate (90deg/sec) + * h) invokes a 20deg angle request on roll or pitch + * i) increases stab P until the maximum angle becomes greater than 110% of the requested angle (20deg) + * j) decreases stab P by 25% + * + * Notes: AUTOTUNE should not be set-up as a flight mode, it should be invoked only from the ch7/ch8 switch. + * + */ + +#define AUTOTUNE_AXIS_BITMASK_ROLL 1 +#define AUTOTUNE_AXIS_BITMASK_PITCH 2 +#define AUTOTUNE_AXIS_BITMASK_YAW 4 + +#define AUTOTUNE_PILOT_OVERRIDE_TIMEOUT_MS 500 // restart tuning if pilot has left sticks in middle for 2 seconds +#define AUTOTUNE_TESTING_STEP_TIMEOUT_MS 750 // timeout for tuning mode's testing step +#define AUTOTUNE_LEVEL_ANGLE_CD 300 // angle which qualifies as level +#define AUTOTUNE_LEVEL_RATE_RP_CD 1000 // rate which qualifies as level for roll and pitch +#define AUTOTUNE_LEVEL_RATE_Y_CD 750 // rate which qualifies as level for yaw +#define AUTOTUNE_REQUIRED_LEVEL_TIME_MS 500 // time we require the copter to be level +#define AUTOTUNE_RD_STEP 0.05f // minimum increment when increasing/decreasing Rate D term +#define AUTOTUNE_RP_STEP 0.05f // minimum increment when increasing/decreasing Rate P term +#define AUTOTUNE_SP_STEP 0.05f // minimum increment when increasing/decreasing Stab P term +#define AUTOTUNE_PI_RATIO_FOR_TESTING 0.1f // I is set 10x smaller than P during testing +#define AUTOTUNE_PI_RATIO_FINAL 1.0f // I is set 1x P after testing +#define AUTOTUNE_YAW_PI_RATIO_FINAL 0.1f // I is set 1x P after testing +#define AUTOTUNE_RD_MAX 0.050f // maximum Rate D value +#define AUTOTUNE_RLPF_MIN 1.0f // minimum Rate Yaw filter value +#define AUTOTUNE_RLPF_MAX 5.0f // maximum Rate Yaw filter value +#define AUTOTUNE_RP_MIN 0.01f // minimum Rate P value +#define AUTOTUNE_RP_MAX 2.0f // maximum Rate P value +#define AUTOTUNE_SP_MAX 20.0f // maximum Stab P value +#define AUTOTUNE_SP_MIN 0.5f // maximum Stab P value +#define AUTOTUNE_RP_ACCEL_MIN 36000.0f // Minimum acceleration for Roll and Pitch +#define AUTOTUNE_Y_ACCEL_MIN 9000.0f // Minimum acceleration for Yaw +#define AUTOTUNE_Y_FILT_FREQ 10.0f // Autotune filter frequency when testing Yaw +#define AUTOTUNE_SUCCESS_COUNT 4 // The number of successful iterations we need to freeze at current gains +#define AUTOTUNE_D_UP_DOWN_MARGIN 0.2f // The margin below the target that we tune D in +#define AUTOTUNE_RD_BACKOFF 1.0f // Rate D gains are reduced to 50% of their maximum value discovered during tuning +#define AUTOTUNE_RP_BACKOFF 1.0f // Rate P gains are reduced to 97.5% of their maximum value discovered during tuning +#define AUTOTUNE_SP_BACKOFF 0.9f // Stab P gains are reduced to 90% of their maximum value discovered during tuning +#define AUTOTUNE_ACCEL_RP_BACKOFF 1.0f // back off from maximum acceleration +#define AUTOTUNE_ACCEL_Y_BACKOFF 1.0f // back off from maximum acceleration + +// roll and pitch axes +#define AUTOTUNE_TARGET_ANGLE_RLLPIT_CD 2000 // target angle during TESTING_RATE step that will cause us to move to next step +#define AUTOTUNE_TARGET_RATE_RLLPIT_CDS 9000 // target roll/pitch rate during AUTOTUNE_STEP_TWITCHING step +#define AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_CD 1000 // minimum target angle during TESTING_RATE step that will cause us to move to next step +#define AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS 4500 // target roll/pitch rate during AUTOTUNE_STEP_TWITCHING step + +// yaw axis +#define AUTOTUNE_TARGET_ANGLE_YAW_CD 3000 // target angle during TESTING_RATE step that will cause us to move to next step +#define AUTOTUNE_TARGET_RATE_YAW_CDS 9000 // target yaw rate during AUTOTUNE_STEP_TWITCHING step +#define AUTOTUNE_TARGET_MIN_ANGLE_YAW_CD 500 // minimum target angle during TESTING_RATE step that will cause us to move to next step +#define AUTOTUNE_TARGET_MIN_RATE_YAW_CDS 1500 // minimum target yaw rate during AUTOTUNE_STEP_TWITCHING step + +// Auto Tune message ids for ground station +#define AUTOTUNE_MESSAGE_STARTED 0 +#define AUTOTUNE_MESSAGE_STOPPED 1 +#define AUTOTUNE_MESSAGE_SUCCESS 2 +#define AUTOTUNE_MESSAGE_FAILED 3 +#define AUTOTUNE_MESSAGE_SAVED_GAINS 4 + +// autotune modes (high level states) +enum AutoTuneTuneMode { + AUTOTUNE_MODE_UNINITIALISED = 0, // autotune has never been run + AUTOTUNE_MODE_TUNING = 1, // autotune is testing gains + AUTOTUNE_MODE_SUCCESS = 2, // tuning has completed, user is flight testing the new gains + AUTOTUNE_MODE_FAILED = 3, // tuning has failed, user is flying on original gains +}; + +// steps performed while in the tuning mode +enum AutoTuneStepType { + AUTOTUNE_STEP_WAITING_FOR_LEVEL = 0, // autotune is waiting for vehicle to return to level before beginning the next twitch + AUTOTUNE_STEP_TWITCHING = 1, // autotune has begun a twitch and is watching the resulting vehicle movement + AUTOTUNE_STEP_UPDATE_GAINS = 2 // autotune has completed a twitch and is updating the gains based on the results +}; + +// things that can be tuned +enum AutoTuneAxisType { + AUTOTUNE_AXIS_ROLL = 0, // roll axis is being tuned (either angle or rate) + AUTOTUNE_AXIS_PITCH = 1, // pitch axis is being tuned (either angle or rate) + AUTOTUNE_AXIS_YAW = 2, // pitch axis is being tuned (either angle or rate) +}; + +// mini steps performed while in Tuning mode, Testing step +enum AutoTuneTuneType { + AUTOTUNE_TYPE_RD_UP = 0, // rate D is being tuned up + AUTOTUNE_TYPE_RD_DOWN = 1, // rate D is being tuned down + AUTOTUNE_TYPE_RP_UP = 2, // rate P is being tuned up + AUTOTUNE_TYPE_SP_DOWN = 3, // angle P is being tuned up + AUTOTUNE_TYPE_SP_UP = 4 // angle P is being tuned up +}; + +// autotune_state_struct - hold state flags +struct autotune_state_struct { + AutoTuneTuneMode mode : 2; // see AutoTuneTuneMode for what modes are allowed + uint8_t pilot_override : 1; // 1 = pilot is overriding controls so we suspend tuning temporarily + AutoTuneAxisType axis : 2; // see AutoTuneAxisType for which things can be tuned + uint8_t positive_direction : 1; // 0 = tuning in negative direction (i.e. left for roll), 1 = positive direction (i.e. right for roll) + AutoTuneStepType step : 2; // see AutoTuneStepType for what steps are performed + AutoTuneTuneType tune_type : 3; // see AutoTuneTuneType + uint8_t ignore_next : 1; // 1 = ignore the next test +} autotune_state; + +// variables +static uint32_t autotune_override_time; // the last time the pilot overrode the controls +static float autotune_test_min; // the minimum angular rate achieved during TESTING_RATE step +static float autotune_test_max; // the maximum angular rate achieved during TESTING_RATE step +static uint32_t autotune_step_start_time; // start time of current tuning step (used for timeout checks) +static uint32_t autotune_step_stop_time; // start time of current tuning step (used for timeout checks) +static int8_t autotune_counter; // counter for tuning gains +static float autotune_target_rate, autotune_start_rate; // target and start rate +static float autotune_target_angle, autotune_start_angle; // target and start angles +static float autotune_desired_yaw; // yaw heading during tune +static float rate_max, autotune_test_accel_max; // maximum acceleration variables + +LowPassFilterFloat rotation_rate_filt; // filtered rotation rate in radians/second + +// backup of currently being tuned parameter values +static float orig_roll_rp = 0, orig_roll_ri, orig_roll_rd, orig_roll_sp, orig_roll_accel; +static float orig_pitch_rp = 0, orig_pitch_ri, orig_pitch_rd, orig_pitch_sp, orig_pitch_accel; +static float orig_yaw_rp = 0, orig_yaw_ri, orig_yaw_rd, orig_yaw_rLPF, orig_yaw_sp, orig_yaw_accel; +static bool orig_bf_feedforward; + +// currently being tuned parameter values +static float tune_roll_rp, tune_roll_rd, tune_roll_sp, tune_roll_accel; +static float tune_pitch_rp, tune_pitch_rd, tune_pitch_sp, tune_pitch_accel; +static float tune_yaw_rp, tune_yaw_rLPF, tune_yaw_sp, tune_yaw_accel; + +// autotune_init - should be called when autotune mode is selected +bool Copter::autotune_init(bool ignore_checks) +{ + bool success = true; + + switch (autotune_state.mode) { + case AUTOTUNE_MODE_FAILED: + // autotune has been run but failed so reset state to uninitialized + autotune_state.mode = AUTOTUNE_MODE_UNINITIALISED; + // no break to allow fall through to restart the tuning + + case AUTOTUNE_MODE_UNINITIALISED: + // autotune has never been run + success = autotune_start(false); + if (success) { + // so store current gains as original gains + autotune_backup_gains_and_initialise(); + // advance mode to tuning + autotune_state.mode = AUTOTUNE_MODE_TUNING; + // send message to ground station that we've started tuning + autotune_update_gcs(AUTOTUNE_MESSAGE_STARTED); + } + break; + + case AUTOTUNE_MODE_TUNING: + // we are restarting tuning after the user must have switched ch7/ch8 off so we restart tuning where we left off + success = autotune_start(false); + if (success) { + // reset gains to tuning-start gains (i.e. low I term) + autotune_load_intra_test_gains(); + // write dataflash log even and send message to ground station + Log_Write_Event(DATA_AUTOTUNE_RESTART); + autotune_update_gcs(AUTOTUNE_MESSAGE_STARTED); + } + break; + + case AUTOTUNE_MODE_SUCCESS: + // we have completed a tune and the pilot wishes to test the new gains in the current flight mode + // so simply apply tuning gains (i.e. do not change flight mode) + autotune_load_tuned_gains(); + Log_Write_Event(DATA_AUTOTUNE_PILOT_TESTING); + break; + } + + return success; +} + +// autotune_stop - should be called when the ch7/ch8 switch is switched OFF +void Copter::autotune_stop() +{ + // set gains to their original values + autotune_load_orig_gains(); + + // re-enable angle-to-rate request limits + attitude_control.limit_angle_to_rate_request(true); + + // log off event and send message to ground station + autotune_update_gcs(AUTOTUNE_MESSAGE_STOPPED); + Log_Write_Event(DATA_AUTOTUNE_OFF); + + // Note: we leave the autotune_state.mode as it was so that we know how the autotune ended + // we expect the caller will change the flight mode back to the flight mode indicated by the flight mode switch +} + +// autotune_start - Initialize autotune flight mode +bool Copter::autotune_start(bool ignore_checks) +{ + // only allow flip from Stabilize or AltHold flight modes + if (control_mode != STABILIZE && control_mode != ALT_HOLD) { + return false; + } + + // ensure throttle is above zero + if (ap.throttle_zero) { + return false; + } + + // ensure we are flying + if (!motors.armed() || !ap.auto_armed || ap.land_complete) { + return false; + } + + // 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()); + + return true; +} + +// autotune_run - runs the autotune flight mode +// should be called at 100hz or more +void Copter::autotune_run() +{ + float target_roll, target_pitch; + float target_yaw_rate; + int16_t target_climb_rate; + + // 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 + // this should not actually be possible because of the autotune_init() checks + if (!ap.auto_armed || !motors.get_interlock()) { + 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 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); + + // get pilot desired climb rate + target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->control_in); + + // check for pilot requested take-off - this should not actually be possible because of autotune_init() checks + 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(); + } + + // 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{ + // check if pilot is overriding the controls + if (!is_zero(target_roll) || !is_zero(target_pitch) || !is_zero(target_yaw_rate) || !is_zero(target_climb_rate)) { + if (!autotune_state.pilot_override) { + autotune_state.pilot_override = true; + // set gains to their original values + autotune_load_orig_gains(); + attitude_control.limit_angle_to_rate_request(true); + } + // reset pilot override time + autotune_override_time = millis(); + }else if (autotune_state.pilot_override) { + // check if we should resume tuning after pilot's override + if (millis() - autotune_override_time > AUTOTUNE_PILOT_OVERRIDE_TIMEOUT_MS) { + autotune_state.pilot_override = false; // turn off pilot override + // set gains to their intra-test values (which are very close to the original gains) + // autotune_load_intra_test_gains(); //I think we should be keeping the originals here to let the I term settle quickly + autotune_state.step = AUTOTUNE_STEP_WAITING_FOR_LEVEL; // set tuning step back from beginning + autotune_desired_yaw = ahrs.yaw_sensor; + } + } + + // if pilot override call attitude controller + if (autotune_state.pilot_override || autotune_state.mode != AUTOTUNE_MODE_TUNING) { + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); + }else{ + // somehow get attitude requests from autotuning + autotune_attitude_control(); + } + + // call position controller + pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); + pos_control.update_z_controller(); + } +} + +// autotune_attitude_controller - sets attitude control targets during tuning +void Copter::autotune_attitude_control() +{ + float rotation_rate = 0.0f; // rotation rate in radians/second + float lean_angle = 0.0f; + const float direction_sign = autotune_state.positive_direction ? 1.0f : -1.0f; + + // check tuning step + switch (autotune_state.step) { + + case AUTOTUNE_STEP_WAITING_FOR_LEVEL: + // Note: we should be using intra-test gains (which are very close to the original gains but have lower I) + // re-enable rate limits + attitude_control.limit_angle_to_rate_request(true); + + // hold level attitude + attitude_control.input_euler_angle_roll_pitch_yaw( 0.0f, 0.0f, autotune_desired_yaw, true); + + // hold the copter level for 0.5 seconds before we begin a twitch + // reset counter if we are no longer level + if ((labs(ahrs.roll_sensor) > AUTOTUNE_LEVEL_ANGLE_CD) || + (labs(ahrs.pitch_sensor) > AUTOTUNE_LEVEL_ANGLE_CD) || + (labs(wrap_180_cd(ahrs.yaw_sensor-(int32_t)autotune_desired_yaw)) > AUTOTUNE_LEVEL_ANGLE_CD) || + ((ToDeg(ahrs.get_gyro().x) * 100.0f) > AUTOTUNE_LEVEL_RATE_RP_CD) || + ((ToDeg(ahrs.get_gyro().y) * 100.0f) > AUTOTUNE_LEVEL_RATE_RP_CD) || + ((ToDeg(ahrs.get_gyro().z) * 100.0f) > AUTOTUNE_LEVEL_RATE_Y_CD) ) { + autotune_step_start_time = millis(); + } + + // if we have been level for a sufficient amount of time (0.5 seconds) move onto tuning step + if (millis() - autotune_step_start_time >= AUTOTUNE_REQUIRED_LEVEL_TIME_MS) { + // initiate variables for next step + autotune_state.step = AUTOTUNE_STEP_TWITCHING; + autotune_step_start_time = millis(); + autotune_step_stop_time = autotune_step_start_time + AUTOTUNE_TESTING_STEP_TIMEOUT_MS; + autotune_test_max = 0.0f; + autotune_test_min = 0.0f; + rotation_rate_filt.reset(0.0f); + rate_max = 0.0f; + // set gains to their to-be-tested values + autotune_load_twitch_gains(); + } + + switch (autotune_state.axis) { + case AUTOTUNE_AXIS_ROLL: + autotune_target_rate = constrain_float(attitude_control.max_rate_step_bf_roll(), AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, AUTOTUNE_TARGET_RATE_RLLPIT_CDS); + autotune_target_angle = constrain_float(attitude_control.max_angle_step_bf_roll(), AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_CD, AUTOTUNE_TARGET_ANGLE_RLLPIT_CD); + autotune_start_rate = ToDeg(ahrs.get_gyro().x) * 100.0f; + autotune_start_angle = ahrs.roll_sensor; + rotation_rate_filt.set_cutoff_frequency(g.pid_rate_roll.filt_hz()*2.0f); + if ((autotune_state.tune_type == AUTOTUNE_TYPE_SP_DOWN) || (autotune_state.tune_type == AUTOTUNE_TYPE_SP_UP)) { + rotation_rate_filt.reset(autotune_start_rate); + } else { + rotation_rate_filt.reset(0); + } + break; + case AUTOTUNE_AXIS_PITCH: + autotune_target_rate = constrain_float(attitude_control.max_rate_step_bf_pitch(), AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, AUTOTUNE_TARGET_RATE_RLLPIT_CDS); + autotune_target_angle = constrain_float(attitude_control.max_angle_step_bf_pitch(), AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_CD, AUTOTUNE_TARGET_ANGLE_RLLPIT_CD); + autotune_start_rate = ToDeg(ahrs.get_gyro().y) * 100.0f; + autotune_start_angle = ahrs.pitch_sensor; + rotation_rate_filt.set_cutoff_frequency(g.pid_rate_pitch.filt_hz()*2.0f); + if ((autotune_state.tune_type == AUTOTUNE_TYPE_SP_DOWN) || (autotune_state.tune_type == AUTOTUNE_TYPE_SP_UP)) { + rotation_rate_filt.reset(autotune_start_rate); + } else { + rotation_rate_filt.reset(0); + } + break; + case AUTOTUNE_AXIS_YAW: + autotune_target_rate = constrain_float(attitude_control.max_rate_step_bf_yaw()*0.75f, AUTOTUNE_TARGET_MIN_RATE_YAW_CDS, AUTOTUNE_TARGET_RATE_YAW_CDS); + autotune_target_angle = constrain_float(attitude_control.max_angle_step_bf_yaw()*0.75f, AUTOTUNE_TARGET_MIN_ANGLE_YAW_CD, AUTOTUNE_TARGET_ANGLE_YAW_CD); + autotune_start_rate = ToDeg(ahrs.get_gyro().z) * 100.0f; + autotune_start_angle = ahrs.yaw_sensor; + rotation_rate_filt.set_cutoff_frequency(AUTOTUNE_Y_FILT_FREQ); + if ((autotune_state.tune_type == AUTOTUNE_TYPE_SP_DOWN) || (autotune_state.tune_type == AUTOTUNE_TYPE_SP_UP)) { + rotation_rate_filt.reset(autotune_start_rate); + } else { + rotation_rate_filt.reset(0); + } + break; + } + break; + + case AUTOTUNE_STEP_TWITCHING: + // Run the twitching step + // Note: we should be using intra-test gains (which are very close to the original gains but have lower I) + + // disable rate limits + attitude_control.limit_angle_to_rate_request(false); + + if ((autotune_state.tune_type == AUTOTUNE_TYPE_SP_DOWN) || (autotune_state.tune_type == AUTOTUNE_TYPE_SP_UP)) { + // Testing increasing stabilize P gain so will set lean angle target + switch (autotune_state.axis) { + case AUTOTUNE_AXIS_ROLL: + // request roll to 20deg + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw( direction_sign * autotune_target_angle + autotune_start_angle, 0.0f, 0.0f); + break; + case AUTOTUNE_AXIS_PITCH: + // request pitch to 20deg + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw( 0.0f, direction_sign * autotune_target_angle + autotune_start_angle, 0.0f); + break; + case AUTOTUNE_AXIS_YAW: + // request pitch to 20deg + attitude_control.input_euler_angle_roll_pitch_yaw( 0.0f, 0.0f, wrap_180_cd_float(direction_sign * autotune_target_angle + autotune_start_angle), false); + break; + } + } else { + // Testing rate P and D gains so will set body-frame rate targets. + // Rate controller will use existing body-frame rates and convert to motor outputs + // for all axes except the one we override here. + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw( 0.0f, 0.0f, 0.0f); + switch (autotune_state.axis) { + case AUTOTUNE_AXIS_ROLL: + // override body-frame roll rate + attitude_control.rate_bf_roll_target(direction_sign * autotune_target_rate + autotune_start_rate); + break; + case AUTOTUNE_AXIS_PITCH: + // override body-frame pitch rate + attitude_control.rate_bf_pitch_target(direction_sign * autotune_target_rate + autotune_start_rate); + break; + case AUTOTUNE_AXIS_YAW: + // override body-frame yaw rate + attitude_control.rate_bf_yaw_target(direction_sign * autotune_target_rate + autotune_start_rate); + break; + } + } + + // capture this iterations rotation rate and lean angle + // Add filter to measurements + switch (autotune_state.axis) { + case AUTOTUNE_AXIS_ROLL: + if ((autotune_state.tune_type == AUTOTUNE_TYPE_SP_DOWN) || (autotune_state.tune_type == AUTOTUNE_TYPE_SP_UP)) { + rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().x) * 100.0f), MAIN_LOOP_SECONDS); + } else { + rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().x) * 100.0f - autotune_start_rate), MAIN_LOOP_SECONDS); + } + lean_angle = direction_sign * (ahrs.roll_sensor - (int32_t)autotune_start_angle); + break; + case AUTOTUNE_AXIS_PITCH: + if ((autotune_state.tune_type == AUTOTUNE_TYPE_SP_DOWN) || (autotune_state.tune_type == AUTOTUNE_TYPE_SP_UP)) { + rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().y) * 100.0f), MAIN_LOOP_SECONDS); + } else { + rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().y) * 100.0f - autotune_start_rate), MAIN_LOOP_SECONDS); + } + lean_angle = direction_sign * (ahrs.pitch_sensor - (int32_t)autotune_start_angle); + break; + case AUTOTUNE_AXIS_YAW: + if ((autotune_state.tune_type == AUTOTUNE_TYPE_SP_DOWN) || (autotune_state.tune_type == AUTOTUNE_TYPE_SP_UP)) { + rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().z) * 100.0f), MAIN_LOOP_SECONDS); + } else { + rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().z) * 100.0f - autotune_start_rate), MAIN_LOOP_SECONDS); + } + lean_angle = direction_sign * wrap_180_cd(ahrs.yaw_sensor-(int32_t)autotune_start_angle); + break; + } + + switch (autotune_state.tune_type) { + case AUTOTUNE_TYPE_RD_UP: + case AUTOTUNE_TYPE_RD_DOWN: + autotune_twitching_test(rotation_rate, autotune_target_rate, autotune_test_min, autotune_test_max); + autotune_twitching_measure_acceleration(autotune_test_accel_max, rotation_rate, rate_max); + if (lean_angle >= autotune_target_angle) { + autotune_state.step = AUTOTUNE_STEP_UPDATE_GAINS; + } + break; + case AUTOTUNE_TYPE_RP_UP: + autotune_twitching_test(rotation_rate, autotune_target_rate*(1+0.5f*g.autotune_aggressiveness), autotune_test_min, autotune_test_max); + autotune_twitching_measure_acceleration(autotune_test_accel_max, rotation_rate, rate_max); + if (lean_angle >= autotune_target_angle) { + autotune_state.step = AUTOTUNE_STEP_UPDATE_GAINS; + } + break; + case AUTOTUNE_TYPE_SP_DOWN: + case AUTOTUNE_TYPE_SP_UP: + autotune_twitching_test(lean_angle, autotune_target_angle*(1+0.5f*g.autotune_aggressiveness), autotune_test_min, autotune_test_max); + autotune_twitching_measure_acceleration(autotune_test_accel_max, rotation_rate - direction_sign * autotune_start_rate, rate_max); + break; + } + + // log this iterations lean angle and rotation rate + Log_Write_AutoTuneDetails(lean_angle, rotation_rate); + Log_Write_Rate(); + break; + + case AUTOTUNE_STEP_UPDATE_GAINS: + + // re-enable rate limits + attitude_control.limit_angle_to_rate_request(true); + + // log the latest gains + if ((autotune_state.tune_type == AUTOTUNE_TYPE_SP_DOWN) || (autotune_state.tune_type == AUTOTUNE_TYPE_SP_UP)) { + switch (autotune_state.axis) { + case AUTOTUNE_AXIS_ROLL: + Log_Write_AutoTune(autotune_state.axis, autotune_state.tune_type, autotune_target_angle, autotune_test_min, autotune_test_max, tune_roll_rp, tune_roll_rd, tune_roll_sp, autotune_test_accel_max); + break; + case AUTOTUNE_AXIS_PITCH: + Log_Write_AutoTune(autotune_state.axis, autotune_state.tune_type, autotune_target_angle, autotune_test_min, autotune_test_max, tune_pitch_rp, tune_pitch_rd, tune_pitch_sp, autotune_test_accel_max); + break; + case AUTOTUNE_AXIS_YAW: + Log_Write_AutoTune(autotune_state.axis, autotune_state.tune_type, autotune_target_angle, autotune_test_min, autotune_test_max, tune_yaw_rp, tune_yaw_rLPF, tune_yaw_sp, autotune_test_accel_max); + break; + } + } else { + switch (autotune_state.axis) { + case AUTOTUNE_AXIS_ROLL: + Log_Write_AutoTune(autotune_state.axis, autotune_state.tune_type, autotune_target_rate, autotune_test_min, autotune_test_max, tune_roll_rp, tune_roll_rd, tune_roll_sp, autotune_test_accel_max); + break; + case AUTOTUNE_AXIS_PITCH: + Log_Write_AutoTune(autotune_state.axis, autotune_state.tune_type, autotune_target_rate, autotune_test_min, autotune_test_max, tune_pitch_rp, tune_pitch_rd, tune_pitch_sp, autotune_test_accel_max); + break; + case AUTOTUNE_AXIS_YAW: + Log_Write_AutoTune(autotune_state.axis, autotune_state.tune_type, autotune_target_rate, autotune_test_min, autotune_test_max, tune_yaw_rp, tune_yaw_rLPF, tune_yaw_sp, autotune_test_accel_max); + break; + } + } + + // Check results after mini-step to increase rate D gain + switch (autotune_state.tune_type) { + case AUTOTUNE_TYPE_RD_UP: + switch (autotune_state.axis) { + case AUTOTUNE_AXIS_ROLL: + autotune_updating_d_up(tune_roll_rd, g.autotune_min_d, AUTOTUNE_RD_MAX, AUTOTUNE_RD_STEP, tune_roll_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, autotune_target_rate, autotune_test_min, autotune_test_max); + break; + case AUTOTUNE_AXIS_PITCH: + autotune_updating_d_up(tune_pitch_rd, g.autotune_min_d, AUTOTUNE_RD_MAX, AUTOTUNE_RD_STEP, tune_pitch_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, autotune_target_rate, autotune_test_min, autotune_test_max); + break; + case AUTOTUNE_AXIS_YAW: + autotune_updating_d_up(tune_yaw_rLPF, AUTOTUNE_RLPF_MIN, AUTOTUNE_RLPF_MAX, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, autotune_target_rate, autotune_test_min, autotune_test_max); + break; + } + break; + // Check results after mini-step to decrease rate D gain + case AUTOTUNE_TYPE_RD_DOWN: + switch (autotune_state.axis) { + case AUTOTUNE_AXIS_ROLL: + autotune_updating_d_down(tune_roll_rd, g.autotune_min_d, AUTOTUNE_RD_STEP, tune_roll_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, autotune_target_rate, autotune_test_min, autotune_test_max); + break; + case AUTOTUNE_AXIS_PITCH: + autotune_updating_d_down(tune_pitch_rd, g.autotune_min_d, AUTOTUNE_RD_STEP, tune_pitch_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, autotune_target_rate, autotune_test_min, autotune_test_max); + break; + case AUTOTUNE_AXIS_YAW: + autotune_updating_d_down(tune_yaw_rLPF, AUTOTUNE_RLPF_MIN, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, autotune_target_rate, autotune_test_min, autotune_test_max); + break; + } + break; + // Check results after mini-step to increase rate P gain + case AUTOTUNE_TYPE_RP_UP: + switch (autotune_state.axis) { + case AUTOTUNE_AXIS_ROLL: + autotune_updating_p_up_d_down(tune_roll_rd, g.autotune_min_d, AUTOTUNE_RD_STEP, tune_roll_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, autotune_target_rate, autotune_test_min, autotune_test_max); + break; + case AUTOTUNE_AXIS_PITCH: + autotune_updating_p_up_d_down(tune_pitch_rd, g.autotune_min_d, AUTOTUNE_RD_STEP, tune_pitch_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, autotune_target_rate, autotune_test_min, autotune_test_max); + break; + case AUTOTUNE_AXIS_YAW: + autotune_updating_p_up_d_down(tune_yaw_rLPF, AUTOTUNE_RLPF_MIN, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, autotune_target_rate, autotune_test_min, autotune_test_max); + break; + } + break; + // Check results after mini-step to increase stabilize P gain + case AUTOTUNE_TYPE_SP_DOWN: + switch (autotune_state.axis) { + case AUTOTUNE_AXIS_ROLL: + autotune_updating_p_down(tune_roll_sp, AUTOTUNE_SP_MIN, AUTOTUNE_SP_STEP, autotune_target_angle, autotune_test_max); + break; + case AUTOTUNE_AXIS_PITCH: + autotune_updating_p_down(tune_pitch_sp, AUTOTUNE_SP_MIN, AUTOTUNE_SP_STEP, autotune_target_angle, autotune_test_max); + break; + case AUTOTUNE_AXIS_YAW: + autotune_updating_p_down(tune_yaw_sp, AUTOTUNE_SP_MIN, AUTOTUNE_SP_STEP, autotune_target_angle, autotune_test_max); + break; + } + break; + // Check results after mini-step to increase stabilize P gain + case AUTOTUNE_TYPE_SP_UP: + switch (autotune_state.axis) { + case AUTOTUNE_AXIS_ROLL: + autotune_updating_p_up(tune_roll_sp, AUTOTUNE_SP_MAX, AUTOTUNE_SP_STEP, autotune_target_angle, autotune_test_max); + break; + case AUTOTUNE_AXIS_PITCH: + autotune_updating_p_up(tune_pitch_sp, AUTOTUNE_SP_MAX, AUTOTUNE_SP_STEP, autotune_target_angle, autotune_test_max); + break; + case AUTOTUNE_AXIS_YAW: + autotune_updating_p_up(tune_yaw_sp, AUTOTUNE_SP_MAX, AUTOTUNE_SP_STEP, autotune_target_angle, autotune_test_max); + break; + } + break; + } + + // we've complete this step, finalize pids and move to next step + if (autotune_counter >= AUTOTUNE_SUCCESS_COUNT) { + + // reset counter + autotune_counter = 0; + + // move to the next tuning type + switch (autotune_state.tune_type) { + case AUTOTUNE_TYPE_RD_UP: + autotune_state.tune_type = AutoTuneTuneType(autotune_state.tune_type + 1); + break; + case AUTOTUNE_TYPE_RD_DOWN: + autotune_state.tune_type = AutoTuneTuneType(autotune_state.tune_type + 1); + switch (autotune_state.axis) { + case AUTOTUNE_AXIS_ROLL: + tune_roll_rd = MAX(g.autotune_min_d, tune_roll_rd * AUTOTUNE_RD_BACKOFF); + tune_roll_rp = MAX(AUTOTUNE_RP_MIN, tune_roll_rp * AUTOTUNE_RD_BACKOFF); + break; + case AUTOTUNE_AXIS_PITCH: + tune_pitch_rd = MAX(g.autotune_min_d, tune_pitch_rd * AUTOTUNE_RD_BACKOFF); + tune_pitch_rp = MAX(AUTOTUNE_RP_MIN, tune_pitch_rp * AUTOTUNE_RD_BACKOFF); + break; + case AUTOTUNE_AXIS_YAW: + tune_yaw_rLPF = MAX(AUTOTUNE_RLPF_MIN, tune_yaw_rLPF * AUTOTUNE_RD_BACKOFF); + tune_yaw_rp = MAX(AUTOTUNE_RP_MIN, tune_yaw_rp * AUTOTUNE_RD_BACKOFF); + break; + } + break; + case AUTOTUNE_TYPE_RP_UP: + autotune_state.tune_type = AutoTuneTuneType(autotune_state.tune_type + 1); + switch (autotune_state.axis) { + case AUTOTUNE_AXIS_ROLL: + tune_roll_rp = MAX(AUTOTUNE_RP_MIN, tune_roll_rp * AUTOTUNE_RP_BACKOFF); + break; + case AUTOTUNE_AXIS_PITCH: + tune_pitch_rp = MAX(AUTOTUNE_RP_MIN, tune_pitch_rp * AUTOTUNE_RP_BACKOFF); + break; + case AUTOTUNE_AXIS_YAW: + tune_yaw_rp = MAX(AUTOTUNE_RP_MIN, tune_yaw_rp * AUTOTUNE_RP_BACKOFF); + break; + } + break; + case AUTOTUNE_TYPE_SP_DOWN: + autotune_state.tune_type = AutoTuneTuneType(autotune_state.tune_type + 1); + break; + case AUTOTUNE_TYPE_SP_UP: + // we've reached the end of a D-up-down PI-up-down tune type cycle + autotune_state.tune_type = AUTOTUNE_TYPE_RD_UP; + + // advance to the next axis + bool autotune_complete = false; + switch (autotune_state.axis) { + case AUTOTUNE_AXIS_ROLL: + tune_roll_sp = MAX(AUTOTUNE_SP_MIN, tune_roll_sp * AUTOTUNE_SP_BACKOFF); + tune_roll_accel = MAX(AUTOTUNE_RP_ACCEL_MIN, autotune_test_accel_max * AUTOTUNE_ACCEL_RP_BACKOFF); + if (autotune_pitch_enabled()) { + autotune_state.axis = AUTOTUNE_AXIS_PITCH; + } else if (autotune_yaw_enabled()) { + autotune_state.axis = AUTOTUNE_AXIS_YAW; + } else { + autotune_complete = true; + } + break; + case AUTOTUNE_AXIS_PITCH: + tune_pitch_sp = MAX(AUTOTUNE_SP_MIN, tune_pitch_sp * AUTOTUNE_SP_BACKOFF); + tune_pitch_accel = MAX(AUTOTUNE_RP_ACCEL_MIN, autotune_test_accel_max * AUTOTUNE_ACCEL_RP_BACKOFF); + if (autotune_yaw_enabled()) { + autotune_state.axis = AUTOTUNE_AXIS_YAW; + } else { + autotune_complete = true; + } + break; + case AUTOTUNE_AXIS_YAW: + tune_yaw_sp = MAX(AUTOTUNE_SP_MIN, tune_yaw_sp * AUTOTUNE_SP_BACKOFF); + tune_yaw_accel = MAX(AUTOTUNE_Y_ACCEL_MIN, autotune_test_accel_max * AUTOTUNE_ACCEL_Y_BACKOFF); + autotune_complete = true; + break; + } + + // if we've just completed all axes we have successfully completed the autotune + // change to TESTING mode to allow user to fly with new gains + if (autotune_complete) { + autotune_state.mode = AUTOTUNE_MODE_SUCCESS; + autotune_update_gcs(AUTOTUNE_MESSAGE_SUCCESS); + Log_Write_Event(DATA_AUTOTUNE_SUCCESS); + AP_Notify::events.autotune_complete = 1; + } else { + AP_Notify::events.autotune_next_axis = 1; + } + break; + } + } + + // reverse direction + autotune_state.positive_direction = !autotune_state.positive_direction; + + if (autotune_state.axis == AUTOTUNE_AXIS_YAW) { + attitude_control.input_euler_angle_roll_pitch_yaw( 0.0f, 0.0f, ahrs.yaw_sensor, false); + } + + // set gains to their intra-test values (which are very close to the original gains) + autotune_load_intra_test_gains(); + + // reset testing step + autotune_state.step = AUTOTUNE_STEP_WAITING_FOR_LEVEL; + autotune_step_start_time = millis(); + break; + } +} + +// autotune_backup_gains_and_initialise - store current gains as originals +// called before tuning starts to backup original gains +void Copter::autotune_backup_gains_and_initialise() +{ + // initialise state because this is our first time + if (autotune_roll_enabled()) { + autotune_state.axis = AUTOTUNE_AXIS_ROLL; + } else if (autotune_pitch_enabled()) { + autotune_state.axis = AUTOTUNE_AXIS_PITCH; + } else if (autotune_yaw_enabled()) { + autotune_state.axis = AUTOTUNE_AXIS_YAW; + } + autotune_state.positive_direction = false; + autotune_state.step = AUTOTUNE_STEP_WAITING_FOR_LEVEL; + autotune_step_start_time = millis(); + autotune_state.tune_type = AUTOTUNE_TYPE_RD_UP; + + autotune_desired_yaw = ahrs.yaw_sensor; + + g.autotune_aggressiveness = constrain_float(g.autotune_aggressiveness, 0.05f, 0.2f); + + orig_bf_feedforward = attitude_control.get_bf_feedforward(); + + // backup original pids and initialise tuned pid values + orig_roll_rp = g.pid_rate_roll.kP(); + orig_roll_ri = g.pid_rate_roll.kI(); + orig_roll_rd = g.pid_rate_roll.kD(); + orig_roll_sp = g.p_stabilize_roll.kP(); + orig_roll_accel = attitude_control.get_accel_roll_max(); + tune_roll_rp = g.pid_rate_roll.kP(); + tune_roll_rd = g.pid_rate_roll.kD(); + tune_roll_sp = g.p_stabilize_roll.kP(); + tune_roll_accel = attitude_control.get_accel_roll_max(); + + orig_pitch_rp = g.pid_rate_pitch.kP(); + orig_pitch_ri = g.pid_rate_pitch.kI(); + orig_pitch_rd = g.pid_rate_pitch.kD(); + orig_pitch_sp = g.p_stabilize_pitch.kP(); + orig_pitch_accel = attitude_control.get_accel_pitch_max(); + tune_pitch_rp = g.pid_rate_pitch.kP(); + tune_pitch_rd = g.pid_rate_pitch.kD(); + tune_pitch_sp = g.p_stabilize_pitch.kP(); + tune_pitch_accel = attitude_control.get_accel_pitch_max(); + + orig_yaw_rp = g.pid_rate_yaw.kP(); + orig_yaw_ri = g.pid_rate_yaw.kI(); + orig_yaw_rd = g.pid_rate_yaw.kD(); + orig_yaw_rLPF = g.pid_rate_yaw.filt_hz(); + orig_yaw_accel = attitude_control.get_accel_yaw_max(); + orig_yaw_sp = g.p_stabilize_yaw.kP(); + tune_yaw_rp = g.pid_rate_yaw.kP(); + tune_yaw_rLPF = g.pid_rate_yaw.filt_hz(); + tune_yaw_sp = g.p_stabilize_yaw.kP(); + tune_yaw_accel = attitude_control.get_accel_yaw_max(); + + Log_Write_Event(DATA_AUTOTUNE_INITIALISED); +} + +// autotune_load_orig_gains - set gains to their original values +// called by autotune_stop and autotune_failed functions +void Copter::autotune_load_orig_gains() +{ + attitude_control.bf_feedforward(orig_bf_feedforward); + if (autotune_roll_enabled()) { + if (!is_zero(orig_roll_rp)) { + g.pid_rate_roll.kP(orig_roll_rp); + g.pid_rate_roll.kI(orig_roll_ri); + g.pid_rate_roll.kD(orig_roll_rd); + g.p_stabilize_roll.kP(orig_roll_sp); + attitude_control.set_accel_roll_max(orig_roll_accel); + } + } + if (autotune_pitch_enabled()) { + if (!is_zero(orig_pitch_rp)) { + g.pid_rate_pitch.kP(orig_pitch_rp); + g.pid_rate_pitch.kI(orig_pitch_ri); + g.pid_rate_pitch.kD(orig_pitch_rd); + g.p_stabilize_pitch.kP(orig_pitch_sp); + attitude_control.set_accel_pitch_max(orig_pitch_accel); + } + } + if (autotune_yaw_enabled()) { + if (!is_zero(orig_yaw_rp)) { + g.pid_rate_yaw.kP(orig_yaw_rp); + g.pid_rate_yaw.kI(orig_yaw_ri); + g.pid_rate_yaw.kD(orig_yaw_rd); + g.pid_rate_yaw.filt_hz(orig_yaw_rLPF); + g.p_stabilize_yaw.kP(orig_yaw_sp); + attitude_control.set_accel_yaw_max(orig_yaw_accel); + } + } +} + +// autotune_load_tuned_gains - load tuned gains +void Copter::autotune_load_tuned_gains() +{ + if (!attitude_control.get_bf_feedforward()) { + attitude_control.bf_feedforward(true); + attitude_control.set_accel_roll_max(0.0f); + attitude_control.set_accel_pitch_max(0.0f); + } + if (autotune_roll_enabled()) { + if (!is_zero(tune_roll_rp)) { + g.pid_rate_roll.kP(tune_roll_rp); + g.pid_rate_roll.kI(tune_roll_rp*AUTOTUNE_PI_RATIO_FINAL); + g.pid_rate_roll.kD(tune_roll_rd); + g.p_stabilize_roll.kP(tune_roll_sp); + attitude_control.set_accel_roll_max(tune_roll_accel); + } + } + if (autotune_pitch_enabled()) { + if (!is_zero(tune_pitch_rp)) { + g.pid_rate_pitch.kP(tune_pitch_rp); + g.pid_rate_pitch.kI(tune_pitch_rp*AUTOTUNE_PI_RATIO_FINAL); + g.pid_rate_pitch.kD(tune_pitch_rd); + g.p_stabilize_pitch.kP(tune_pitch_sp); + attitude_control.set_accel_pitch_max(tune_pitch_accel); + } + } + if (autotune_yaw_enabled()) { + if (!is_zero(tune_yaw_rp)) { + g.pid_rate_yaw.kP(tune_yaw_rp); + g.pid_rate_yaw.kI(tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL); + g.pid_rate_yaw.kD(0.0f); + g.pid_rate_yaw.filt_hz(tune_yaw_rLPF); + g.p_stabilize_yaw.kP(tune_yaw_sp); + attitude_control.set_accel_yaw_max(tune_yaw_accel); + } + } +} + +// autotune_load_intra_test_gains - gains used between tests +// called during testing mode's update-gains step to set gains ahead of return-to-level step +void Copter::autotune_load_intra_test_gains() +{ + // we are restarting tuning so reset gains to tuning-start gains (i.e. low I term) + // sanity check the gains + attitude_control.bf_feedforward(true); + if (autotune_roll_enabled()) { + g.pid_rate_roll.kP(orig_roll_rp); + g.pid_rate_roll.kI(orig_roll_rp*AUTOTUNE_PI_RATIO_FOR_TESTING); + g.pid_rate_roll.kD(orig_roll_rd); + g.p_stabilize_roll.kP(orig_roll_sp); + } + if (autotune_pitch_enabled()) { + g.pid_rate_pitch.kP(orig_pitch_rp); + g.pid_rate_pitch.kI(orig_pitch_rp*AUTOTUNE_PI_RATIO_FOR_TESTING); + g.pid_rate_pitch.kD(orig_pitch_rd); + g.p_stabilize_pitch.kP(orig_pitch_sp); + } + if (autotune_yaw_enabled()) { + g.pid_rate_yaw.kP(orig_yaw_rp); + g.pid_rate_yaw.kI(orig_yaw_rp*AUTOTUNE_PI_RATIO_FOR_TESTING); + g.pid_rate_yaw.kD(orig_yaw_rd); + g.pid_rate_yaw.filt_hz(orig_yaw_rLPF); + g.p_stabilize_yaw.kP(orig_yaw_sp); + } +} + +// autotune_load_twitch_gains - load the to-be-tested gains for a single axis +// called by autotune_attitude_control() just before it beings testing a gain (i.e. just before it twitches) +void Copter::autotune_load_twitch_gains() +{ + switch (autotune_state.axis) { + case AUTOTUNE_AXIS_ROLL: + g.pid_rate_roll.kP(tune_roll_rp); + g.pid_rate_roll.kI(tune_roll_rp*0.01f); + g.pid_rate_roll.kD(tune_roll_rd); + g.p_stabilize_roll.kP(tune_roll_sp); + break; + case AUTOTUNE_AXIS_PITCH: + g.pid_rate_pitch.kP(tune_pitch_rp); + g.pid_rate_pitch.kI(tune_pitch_rp*0.01f); + g.pid_rate_pitch.kD(tune_pitch_rd); + g.p_stabilize_pitch.kP(tune_pitch_sp); + break; + case AUTOTUNE_AXIS_YAW: + g.pid_rate_yaw.kP(tune_yaw_rp); + g.pid_rate_yaw.kI(tune_yaw_rp*0.01f); + g.pid_rate_yaw.kD(0.0f); + g.pid_rate_yaw.filt_hz(tune_yaw_rLPF); + g.p_stabilize_yaw.kP(tune_yaw_sp); + break; + } +} + +// autotune_save_tuning_gains - save the final tuned gains for each axis +// save discovered gains to eeprom if autotuner is enabled (i.e. switch is in the high position) +void Copter::autotune_save_tuning_gains() +{ + // if we successfully completed tuning + if (autotune_state.mode == AUTOTUNE_MODE_SUCCESS) { + + if (!attitude_control.get_bf_feedforward()) { + attitude_control.bf_feedforward_save(true); + attitude_control.save_accel_roll_max(0.0f); + attitude_control.save_accel_pitch_max(0.0f); + } + + // sanity check the rate P values + if (autotune_roll_enabled() && !is_zero(tune_roll_rp)) { + // rate roll gains + g.pid_rate_roll.kP(tune_roll_rp); + g.pid_rate_roll.kI(tune_roll_rp*AUTOTUNE_PI_RATIO_FINAL); + g.pid_rate_roll.kD(tune_roll_rd); + g.pid_rate_roll.save_gains(); + + // stabilize roll + g.p_stabilize_roll.kP(tune_roll_sp); + g.p_stabilize_roll.save_gains(); + + // acceleration roll + attitude_control.save_accel_roll_max(tune_roll_accel); + + // resave pids to originals in case the autotune is run again + orig_roll_rp = g.pid_rate_roll.kP(); + orig_roll_ri = g.pid_rate_roll.kI(); + orig_roll_rd = g.pid_rate_roll.kD(); + orig_roll_sp = g.p_stabilize_roll.kP(); + } + + if (autotune_pitch_enabled() && !is_zero(tune_pitch_rp)) { + // rate pitch gains + g.pid_rate_pitch.kP(tune_pitch_rp); + g.pid_rate_pitch.kI(tune_pitch_rp*AUTOTUNE_PI_RATIO_FINAL); + g.pid_rate_pitch.kD(tune_pitch_rd); + g.pid_rate_pitch.save_gains(); + + // stabilize pitch + g.p_stabilize_pitch.kP(tune_pitch_sp); + g.p_stabilize_pitch.save_gains(); + + // acceleration pitch + attitude_control.save_accel_pitch_max(tune_pitch_accel); + + // resave pids to originals in case the autotune is run again + orig_pitch_rp = g.pid_rate_pitch.kP(); + orig_pitch_ri = g.pid_rate_pitch.kI(); + orig_pitch_rd = g.pid_rate_pitch.kD(); + orig_pitch_sp = g.p_stabilize_pitch.kP(); + } + + if (autotune_yaw_enabled() && !is_zero(tune_yaw_rp)) { + // rate yaw gains + g.pid_rate_yaw.kP(tune_yaw_rp); + g.pid_rate_yaw.kI(tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL); + g.pid_rate_yaw.kD(0.0f); + g.pid_rate_yaw.filt_hz(tune_yaw_rLPF); + g.pid_rate_yaw.save_gains(); + + // stabilize yaw + g.p_stabilize_yaw.kP(tune_yaw_sp); + g.p_stabilize_yaw.save_gains(); + + // acceleration yaw + attitude_control.save_accel_yaw_max(tune_yaw_accel); + + // resave pids to originals in case the autotune is run again + orig_yaw_rp = g.pid_rate_yaw.kP(); + orig_yaw_ri = g.pid_rate_yaw.kI(); + orig_yaw_rd = g.pid_rate_yaw.kD(); + orig_yaw_rLPF = g.pid_rate_yaw.filt_hz(); + orig_yaw_sp = g.p_stabilize_yaw.kP(); + } + // update GCS and log save gains event + autotune_update_gcs(AUTOTUNE_MESSAGE_SAVED_GAINS); + Log_Write_Event(DATA_AUTOTUNE_SAVEDGAINS); + // reset Autotune so that gains are not saved again and autotune can be run again. + autotune_state.mode = AUTOTUNE_MODE_UNINITIALISED; + } +} + +// autotune_update_gcs - send message to ground station +void Copter::autotune_update_gcs(uint8_t message_id) +{ + switch (message_id) { + case AUTOTUNE_MESSAGE_STARTED: + gcs_send_text(MAV_SEVERITY_INFO,"AutoTune: Started"); + break; + case AUTOTUNE_MESSAGE_STOPPED: + gcs_send_text(MAV_SEVERITY_INFO,"AutoTune: Stopped"); + break; + case AUTOTUNE_MESSAGE_SUCCESS: + gcs_send_text(MAV_SEVERITY_INFO,"AutoTune: Success"); + break; + case AUTOTUNE_MESSAGE_FAILED: + gcs_send_text(MAV_SEVERITY_NOTICE,"AutoTune: Failed"); + break; + case AUTOTUNE_MESSAGE_SAVED_GAINS: + gcs_send_text(MAV_SEVERITY_INFO,"AutoTune: Saved gains"); + break; + } +} + +// axis helper functions +inline bool Copter::autotune_roll_enabled() { + return g.autotune_axis_bitmask & AUTOTUNE_AXIS_BITMASK_ROLL; +} + +inline bool Copter::autotune_pitch_enabled() { + return g.autotune_axis_bitmask & AUTOTUNE_AXIS_BITMASK_PITCH; +} + +inline bool Copter::autotune_yaw_enabled() { + return g.autotune_axis_bitmask & AUTOTUNE_AXIS_BITMASK_YAW; +} + +// autotune_twitching_test - twitching tests +// update min and max and test for end conditions +void Copter::autotune_twitching_test(float measurement, float target, float &measurement_min, float &measurement_max) +{ + // capture maximum measurement + if (measurement > measurement_max) { + // the measurement is continuing to increase without stopping + measurement_max = measurement; + measurement_min = measurement; + } + + // capture minimum measurement after the measurement has peaked (aka "bounce back") + if ((measurement < measurement_min) && (measurement_max > target * 0.5f)) { + // the measurement is bouncing back + measurement_min = measurement; + } + + // calculate early stopping time based on the time it takes to get to 90% + if (measurement_max < target * 0.75f) { + // the measurement not reached the 90% threshold yet + autotune_step_stop_time = autotune_step_start_time + (millis() - autotune_step_start_time) * 3.0f; + autotune_step_stop_time = MIN(autotune_step_stop_time, autotune_step_start_time + AUTOTUNE_TESTING_STEP_TIMEOUT_MS); + } + + if (measurement_max > target) { + // the measurement has passed the target + autotune_state.step = AUTOTUNE_STEP_UPDATE_GAINS; + } + + if (measurement_max-measurement_min > measurement_max*g.autotune_aggressiveness) { + // the measurement has passed 50% of the target and bounce back is larger than the threshold + autotune_state.step = AUTOTUNE_STEP_UPDATE_GAINS; + } + + if (millis() >= autotune_step_stop_time) { + // we have passed the maximum stop time + autotune_state.step = AUTOTUNE_STEP_UPDATE_GAINS; + } +} + +// autotune_updating_d_up - increase D and adjust P to optimize the D term for a little bounce back +// optimize D term while keeping the maximum just below the target by adjusting P +void Copter::autotune_updating_d_up(float &tune_d, float tune_d_min, float tune_d_max, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max) +{ + if (measurement_max > target) { + // if maximum measurement was higher than target + // reduce P gain (which should reduce maximum) + tune_p -= tune_p*tune_p_step_ratio; + if (tune_p < tune_p_min) { + // P gain is at minimum so start reducing D + tune_p = tune_p_min; + tune_d -= tune_d*tune_d_step_ratio; + if (tune_d <= tune_d_min) { + // We have reached minimum D gain so stop tuning + tune_d = tune_d_min; + autotune_counter = AUTOTUNE_SUCCESS_COUNT; + Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT); + } + } + }else if ((measurement_max < target*(1.0f-AUTOTUNE_D_UP_DOWN_MARGIN)) && (tune_p <= tune_p_max)) { + // we have not achieved a high enough maximum to get a good measurement of bounce back. + // increase P gain (which should increase maximum) + tune_p += tune_p*tune_p_step_ratio; + if (tune_p >= tune_p_max) { + tune_p = tune_p_max; + Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT); + } + }else{ + // we have a good measurement of bounce back + if (measurement_max-measurement_min > measurement_max*g.autotune_aggressiveness) { + // ignore the next result unless it is the same as this one + autotune_state.ignore_next = 1; + // bounce back is bigger than our threshold so increment the success counter + autotune_counter++; + }else{ + if (autotune_state.ignore_next == 0){ + // bounce back is smaller than our threshold so decrement the success counter + if (autotune_counter > 0 ) { + autotune_counter--; + } + // increase D gain (which should increase bounce back) + tune_d += tune_d*tune_d_step_ratio*2.0f; + // stop tuning if we hit maximum D + if (tune_d >= tune_d_max) { + tune_d = tune_d_max; + autotune_counter = AUTOTUNE_SUCCESS_COUNT; + Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT); + } + } else { + autotune_state.ignore_next = 0; + } + } + } +} + +// autotune_updating_d_down - decrease D and adjust P to optimize the D term for no bounce back +// optimize D term while keeping the maximum just below the target by adjusting P +void Copter::autotune_updating_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max) +{ + if (measurement_max > target) { + // if maximum measurement was higher than target + // reduce P gain (which should reduce maximum) + tune_p -= tune_p*tune_p_step_ratio; + if (tune_p < tune_p_min) { + // P gain is at minimum so start reducing D gain + tune_p = tune_p_min; + tune_d -= tune_d*tune_d_step_ratio; + if (tune_d <= tune_d_min) { + // We have reached minimum D so stop tuning + tune_d = tune_d_min; + autotune_counter = AUTOTUNE_SUCCESS_COUNT; + Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT); + } + } + }else if ((measurement_max < target*(1.0f-AUTOTUNE_D_UP_DOWN_MARGIN)) && (tune_p <= tune_p_max)) { + // we have not achieved a high enough maximum to get a good measurement of bounce back. + // increase P gain (which should increase maximum) + tune_p += tune_p*tune_p_step_ratio; + if (tune_p >= tune_p_max) { + tune_p = tune_p_max; + Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT); + } + }else{ + // we have a good measurement of bounce back + if (measurement_max-measurement_min < measurement_max*g.autotune_aggressiveness) { + if (autotune_state.ignore_next == 0){ + // bounce back is less than our threshold so increment the success counter + autotune_counter++; + } else { + autotune_state.ignore_next = 0; + } + }else{ + // ignore the next result unless it is the same as this one + autotune_state.ignore_next = 1; + // bounce back is larger than our threshold so decrement the success counter + if (autotune_counter > 0 ) { + autotune_counter--; + } + // decrease D gain (which should decrease bounce back) + tune_d -= tune_d*tune_d_step_ratio; + // stop tuning if we hit minimum D + if (tune_d <= tune_d_min) { + tune_d = tune_d_min; + autotune_counter = AUTOTUNE_SUCCESS_COUNT; + Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT); + } + } + } +} + +// autotune_updating_p_down - decrease P until we don't reach the target before time out +// P is decreased to ensure we are not overshooting the target +void Copter::autotune_updating_p_down(float &tune_p, float tune_p_min, float tune_p_step_ratio, float target, float measurement_max) +{ + if (measurement_max < target*(1+0.5f*g.autotune_aggressiveness)) { + if (autotune_state.ignore_next == 0){ + // if maximum measurement was lower than target so increment the success counter + autotune_counter++; + } else { + autotune_state.ignore_next = 0; + } + }else{ + // ignore the next result unless it is the same as this one + autotune_state.ignore_next = 1; + // if maximum measurement was higher than target so decrement the success counter + if (autotune_counter > 0 ) { + autotune_counter--; + } + // decrease P gain (which should decrease the maximum) + tune_p -= tune_p*tune_p_step_ratio; + // stop tuning if we hit maximum P + if (tune_p <= tune_p_min) { + tune_p = tune_p_min; + autotune_counter = AUTOTUNE_SUCCESS_COUNT; + Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT); + } + } +} + +// autotune_updating_p_up - increase P to ensure the target is reached +// P is increased until we achieve our target within a reasonable time +void Copter::autotune_updating_p_up(float &tune_p, float tune_p_max, float tune_p_step_ratio, float target, float measurement_max) +{ + if (measurement_max > target*(1+0.5f*g.autotune_aggressiveness)) { + // ignore the next result unless it is the same as this one + autotune_state.ignore_next = 1; + // if maximum measurement was greater than target so increment the success counter + autotune_counter++; + }else{ + if (autotune_state.ignore_next == 0){ + // if maximum measurement was lower than target so decrement the success counter + if (autotune_counter > 0 ) { + autotune_counter--; + } + // increase P gain (which should increase the maximum) + tune_p += tune_p*tune_p_step_ratio; + // stop tuning if we hit maximum P + if (tune_p >= tune_p_max) { + tune_p = tune_p_max; + autotune_counter = AUTOTUNE_SUCCESS_COUNT; + Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT); + } + } else { + autotune_state.ignore_next = 0; + } + } +} + +// autotune_updating_p_up - increase P to ensure the target is reached while checking bounce back isn't increasing +// P is increased until we achieve our target within a reasonable time while reducing D if bounce back increases above the threshold +void Copter::autotune_updating_p_up_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max) +{ + if (measurement_max > target*(1+0.5f*g.autotune_aggressiveness)) { + // ignore the next result unless it is the same as this one + autotune_state.ignore_next = 1; + // if maximum measurement was greater than target so increment the success counter + autotune_counter++; + }else if ((measurement_max < target) && (measurement_max-measurement_min > measurement_max*g.autotune_aggressiveness) && (tune_d > tune_d_min)) { + // if bounce back was larger than the threshold so decrement the success counter + if (autotune_counter > 0 ) { + autotune_counter--; + } + // decrease D gain (which should decrease bounce back) + tune_d -= tune_d*tune_d_step_ratio; + // stop tuning if we hit minimum D + if (tune_d <= tune_d_min) { + tune_d = tune_d_min; + Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT); + } + // decrease P gain to match D gain reduction + tune_p -= tune_p*tune_p_step_ratio; + // stop tuning if we hit minimum P + if (tune_p <= tune_p_min) { + tune_p = tune_p_min; + Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT); + } + // cancel change in direction + autotune_state.positive_direction = !autotune_state.positive_direction; + }else{ + if (autotune_state.ignore_next == 0){ + // if maximum measurement was lower than target so decrement the success counter + if (autotune_counter > 0 ) { + autotune_counter--; + } + // increase P gain (which should increase the maximum) + tune_p += tune_p*tune_p_step_ratio; + // stop tuning if we hit maximum P + if (tune_p >= tune_p_max) { + tune_p = tune_p_max; + autotune_counter = AUTOTUNE_SUCCESS_COUNT; + Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT); + } + } else { + autotune_state.ignore_next = 0; + } + } +} + +// autotune_twitching_measure_acceleration - measure rate of change of measurement +void Copter::autotune_twitching_measure_acceleration(float &rate_of_change, float rate_measurement, float &rate_measurement_max) +{ + if (rate_measurement_max < rate_measurement) { + rate_measurement_max = rate_measurement; + rate_of_change = (1000.0f*rate_measurement_max)/(millis() - autotune_step_start_time); + } +} + +#endif // AUTOTUNE_ENABLED == ENABLED diff --git a/ArduSub/control_brake.cpp b/ArduSub/control_brake.cpp new file mode 100644 index 00000000000..a233dbb1c5c --- /dev/null +++ b/ArduSub/control_brake.cpp @@ -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(); +} diff --git a/ArduSub/control_circle.cpp b/ArduSub/control_circle.cpp new file mode 100644 index 00000000000..e9d8c3954c1 --- /dev/null +++ b/ArduSub/control_circle.cpp @@ -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(); +} diff --git a/ArduSub/control_drift.cpp b/ArduSub/control_drift.cpp new file mode 100644 index 00000000000..d3c21280eac --- /dev/null +++ b/ArduSub/control_drift.cpp @@ -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; +} diff --git a/ArduSub/control_flip.cpp b/ArduSub/control_flip.cpp new file mode 100644 index 00000000000..c64fded8cc7 --- /dev/null +++ b/ArduSub/control_flip.cpp @@ -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); + } +} diff --git a/ArduSub/control_guided.cpp b/ArduSub/control_guided.cpp new file mode 100644 index 00000000000..4ba7e36bb59 --- /dev/null +++ b/ArduSub/control_guided.cpp @@ -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; +} diff --git a/ArduSub/control_land.cpp b/ArduSub/control_land.cpp new file mode 100644 index 00000000000..9ae78b4c795 --- /dev/null +++ b/ArduSub/control_land.cpp @@ -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); +} diff --git a/ArduSub/control_loiter.cpp b/ArduSub/control_loiter.cpp new file mode 100644 index 00000000000..736ac60cbd1 --- /dev/null +++ b/ArduSub/control_loiter.cpp @@ -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; + } +} diff --git a/ArduSub/control_poshold.cpp b/ArduSub/control_poshold.cpp new file mode 100644 index 00000000000..73c621542e0 --- /dev/null +++ b/ArduSub/control_poshold.cpp @@ -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 diff --git a/ArduSub/control_rtl.cpp b/ArduSub/control_rtl.cpp new file mode 100644 index 00000000000..cf491f3e9de --- /dev/null +++ b/ArduSub/control_rtl.cpp @@ -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; +} + diff --git a/ArduSub/control_sport.cpp b/ArduSub/control_sport.cpp new file mode 100644 index 00000000000..95173406cc0 --- /dev/null +++ b/ArduSub/control_sport.cpp @@ -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(); + } +} diff --git a/ArduSub/control_stabilize.cpp b/ArduSub/control_stabilize.cpp new file mode 100644 index 00000000000..9ad9fce66f0 --- /dev/null +++ b/ArduSub/control_stabilize.cpp @@ -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); +} diff --git a/ArduSub/crash_check.cpp b/ArduSub/crash_check.cpp new file mode 100644 index 00000000000..be8c5c3a4db --- /dev/null +++ b/ArduSub/crash_check.cpp @@ -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 diff --git a/ArduSub/defines.h b/ArduSub/defines.h new file mode 100644 index 00000000000..2f6deb03f81 --- /dev/null +++ b/ArduSub/defines.h @@ -0,0 +1,424 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#ifndef _DEFINES_H +#define _DEFINES_H + +#include + +// 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 diff --git a/ArduSub/ekf_check.cpp b/ArduSub/ekf_check.cpp new file mode 100644 index 00000000000..e3ae039b5f7 --- /dev/null +++ b/ArduSub/ekf_check.cpp @@ -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); +} diff --git a/ArduSub/esc_calibration.cpp b/ArduSub/esc_calibration.cpp new file mode 100644 index 00000000000..327ae5ebb96 --- /dev/null +++ b/ArduSub/esc_calibration.cpp @@ -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 +} diff --git a/ArduSub/events.cpp b/ArduSub/events.cpp new file mode 100644 index 00000000000..95cb367cadd --- /dev/null +++ b/ArduSub/events.cpp @@ -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(); +} + diff --git a/ArduSub/failsafe.cpp b/ArduSub/failsafe.cpp new file mode 100644 index 00000000000..a63aad1be06 --- /dev/null +++ b/ArduSub/failsafe.cpp @@ -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(); + } + } +} diff --git a/ArduSub/fence.cpp b/ArduSub/fence.cpp new file mode 100644 index 00000000000..3baa7443a10 --- /dev/null +++ b/ArduSub/fence.cpp @@ -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 diff --git a/ArduSub/flight_mode.cpp b/ArduSub/flight_mode.cpp new file mode 100644 index 00000000000..6d4390ceb10 --- /dev/null +++ b/ArduSub/flight_mode.cpp @@ -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; + } +} + diff --git a/ArduSub/heli.cpp b/ArduSub/heli.cpp new file mode 100644 index 00000000000..e0eaac1a529 --- /dev/null +++ b/ArduSub/heli.cpp @@ -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 diff --git a/ArduSub/heli_control_acro.cpp b/ArduSub/heli_control_acro.cpp new file mode 100644 index 00000000000..4200a181ce8 --- /dev/null +++ b/ArduSub/heli_control_acro.cpp @@ -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 diff --git a/ArduSub/heli_control_stabilize.cpp b/ArduSub/heli_control_stabilize.cpp new file mode 100644 index 00000000000..3f3c0ed166a --- /dev/null +++ b/ArduSub/heli_control_stabilize.cpp @@ -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 diff --git a/ArduSub/inertia.cpp b/ArduSub/inertia.cpp new file mode 100644 index 00000000000..ebcc5b80cf2 --- /dev/null +++ b/ArduSub/inertia.cpp @@ -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(); +} diff --git a/ArduSub/land_detector.cpp b/ArduSub/land_detector.cpp new file mode 100644 index 00000000000..ecf40622c2c --- /dev/null +++ b/ArduSub/land_detector.cpp @@ -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 +} diff --git a/ArduSub/landing_gear.cpp b/ArduSub/landing_gear.cpp new file mode 100644 index 00000000000..c31ac83ae4c --- /dev/null +++ b/ArduSub/landing_gear.cpp @@ -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(); + } +} diff --git a/ArduSub/leds.cpp b/ArduSub/leds.cpp new file mode 100644 index 00000000000..f36b0bbebd3 --- /dev/null +++ b/ArduSub/leds.cpp @@ -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(); +} + diff --git a/ArduSub/make.inc b/ArduSub/make.inc new file mode 100644 index 00000000000..4d0cb8eafdd --- /dev/null +++ b/ArduSub/make.inc @@ -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 diff --git a/ArduSub/motor_test.cpp b/ArduSub/motor_test.cpp new file mode 100644 index 00000000000..1fc2b94e1f9 --- /dev/null +++ b/ArduSub/motor_test.cpp @@ -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; +} diff --git a/ArduSub/motors.cpp b/ArduSub/motors.cpp new file mode 100644 index 00000000000..4f1cfb3a114 --- /dev/null +++ b/ArduSub/motors.cpp @@ -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; + } + } +} diff --git a/ArduSub/navigation.cpp b/ArduSub/navigation.cpp new file mode 100644 index 00000000000..a671ad3745a --- /dev/null +++ b/ArduSub/navigation.cpp @@ -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(); + } +} diff --git a/ArduSub/perf_info.cpp b/ArduSub/perf_info.cpp new file mode 100644 index 00000000000..7da62c69f34 --- /dev/null +++ b/ArduSub/perf_info.cpp @@ -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; +} diff --git a/ArduSub/position_vector.cpp b/ArduSub/position_vector.cpp new file mode 100644 index 00000000000..4d81b8ea20c --- /dev/null +++ b/ArduSub/position_vector.cpp @@ -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); +} diff --git a/ArduSub/precision_landing.cpp b/ArduSub/precision_landing.cpp new file mode 100644 index 00000000000..eb938145df2 --- /dev/null +++ b/ArduSub/precision_landing.cpp @@ -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 diff --git a/ArduSub/radio.cpp b/ArduSub/radio.cpp new file mode 100644 index 00000000000..faa517765c1 --- /dev/null +++ b/ArduSub/radio.cpp @@ -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; + } +} diff --git a/ArduSub/readme.txt b/ArduSub/readme.txt new file mode 100644 index 00000000000..3285f2f96b8 --- /dev/null +++ b/ArduSub/readme.txt @@ -0,0 +1,4 @@ +ArduCopter README + +User Manual: http://copter.ardupilot.com/ +Developer Manual: http://dev.ardupilot.com/ \ No newline at end of file diff --git a/ArduSub/sensors.cpp b/ArduSub/sensors.cpp new file mode 100644 index 00000000000..afcbdccc015 --- /dev/null +++ b/ArduSub/sensors.cpp @@ -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 diff --git a/ArduSub/setup.cpp b/ArduSub/setup.cpp new file mode 100644 index 00000000000..26f1d8ec09e --- /dev/null +++ b/ArduSub/setup.cpp @@ -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 \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<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<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; iprintf("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; iprintf("\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); +} diff --git a/ArduSub/switches.cpp b/ArduSub/switches.cpp new file mode 100644 index 00000000000..e8297ddc722 --- /dev/null +++ b/ArduSub/switches.cpp @@ -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; + } + } +} + diff --git a/ArduSub/system.cpp b/ArduSub/system.cpp new file mode 100644 index 00000000000..1f73d546f7d --- /dev/null +++ b/ArduSub/system.cpp @@ -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 +} diff --git a/ArduSub/takeoff.cpp b/ArduSub/takeoff.cpp new file mode 100644 index 00000000000..716685a460d --- /dev/null +++ b/ArduSub/takeoff.cpp @@ -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; + } + } +} diff --git a/ArduSub/test.cpp b/ArduSub/test.cpp new file mode 100644 index 00000000000..3bc6e499deb --- /dev/null +++ b/ArduSub/test.cpp @@ -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 diff --git a/ArduSub/tuning.cpp b/ArduSub/tuning.cpp new file mode 100644 index 00000000000..86fac7a6690 --- /dev/null +++ b/ArduSub/tuning.cpp @@ -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; + } +} diff --git a/ArduSub/wscript b/ArduSub/wscript new file mode 100644 index 00000000000..037716ab1e6 --- /dev/null +++ b/ArduSub/wscript @@ -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', + )