From db3666c28497413237e73099db239c328c446b1e Mon Sep 17 00:00:00 2001 From: Rustom Jehangir Date: Fri, 8 Jan 2016 21:57:01 -0800 Subject: [PATCH] Rebased. --- ArduSub/motors.cpp | 13 +- ArduSubsea/APM_Config.h | 10 - ArduSubsea/APM_Config.h.reference | 949 ------------- ArduSubsea/ArduPlane.cpp | 907 ------------- ArduSubsea/Attitude.cpp | 1136 ---------------- ArduSubsea/GCS_Mavlink.cpp | 2061 ----------------------------- ArduSubsea/Log.cpp | 595 --------- ArduSubsea/Makefile | 2 - ArduSubsea/Makefile.waf | 2 - ArduSubsea/Parameters.cpp | 1282 ------------------ ArduSubsea/Parameters.h | 541 -------- ArduSubsea/Parameters.pde | 3 - ArduSubsea/Plane.cpp | 33 - ArduSubsea/Plane.h | 1033 --------------- ArduSubsea/adsb.cpp | 101 -- ArduSubsea/altitude.cpp | 621 --------- ArduSubsea/arming_checks.cpp | 91 -- ArduSubsea/capabilities.cpp | 9 - ArduSubsea/commands.cpp | 133 -- ArduSubsea/commands_logic.cpp | 996 -------------- ArduSubsea/commands_process.cpp | 20 - ArduSubsea/config.h | 432 ------ ArduSubsea/control_modes.cpp | 164 --- ArduSubsea/createTags | 67 - ArduSubsea/defines.h | 193 --- ArduSubsea/events.cpp | 130 -- ArduSubsea/failsafe.cpp | 111 -- ArduSubsea/geofence.cpp | 487 ------- ArduSubsea/is_flying.cpp | 245 ---- ArduSubsea/landing.cpp | 335 ----- ArduSubsea/make.inc | 52 - ArduSubsea/navigation.cpp | 238 ---- ArduSubsea/parachute.cpp | 58 - ArduSubsea/px4_mixer.cpp | 391 ------ ArduSubsea/radio.cpp | 347 ----- ArduSubsea/release-notes.txt | 1294 ------------------ ArduSubsea/sensors.cpp | 150 --- ArduSubsea/setup.cpp | 81 -- ArduSubsea/system.cpp | 779 ----------- ArduSubsea/takeoff.cpp | 187 --- ArduSubsea/test.cpp | 531 -------- ArduSubsea/wscript | 37 - modules/gbenchmark | 2 +- 43 files changed, 5 insertions(+), 16844 deletions(-) delete mode 100644 ArduSubsea/APM_Config.h delete mode 100644 ArduSubsea/APM_Config.h.reference delete mode 100644 ArduSubsea/ArduPlane.cpp delete mode 100644 ArduSubsea/Attitude.cpp delete mode 100644 ArduSubsea/GCS_Mavlink.cpp delete mode 100644 ArduSubsea/Log.cpp delete mode 100644 ArduSubsea/Makefile delete mode 100644 ArduSubsea/Makefile.waf delete mode 100644 ArduSubsea/Parameters.cpp delete mode 100644 ArduSubsea/Parameters.h delete mode 100644 ArduSubsea/Parameters.pde delete mode 100644 ArduSubsea/Plane.cpp delete mode 100644 ArduSubsea/Plane.h delete mode 100644 ArduSubsea/adsb.cpp delete mode 100644 ArduSubsea/altitude.cpp delete mode 100644 ArduSubsea/arming_checks.cpp delete mode 100644 ArduSubsea/capabilities.cpp delete mode 100644 ArduSubsea/commands.cpp delete mode 100644 ArduSubsea/commands_logic.cpp delete mode 100644 ArduSubsea/commands_process.cpp delete mode 100644 ArduSubsea/config.h delete mode 100644 ArduSubsea/control_modes.cpp delete mode 100755 ArduSubsea/createTags delete mode 100644 ArduSubsea/defines.h delete mode 100644 ArduSubsea/events.cpp delete mode 100644 ArduSubsea/failsafe.cpp delete mode 100644 ArduSubsea/geofence.cpp delete mode 100644 ArduSubsea/is_flying.cpp delete mode 100644 ArduSubsea/landing.cpp delete mode 100644 ArduSubsea/make.inc delete mode 100644 ArduSubsea/navigation.cpp delete mode 100644 ArduSubsea/parachute.cpp delete mode 100644 ArduSubsea/px4_mixer.cpp delete mode 100644 ArduSubsea/radio.cpp delete mode 100644 ArduSubsea/release-notes.txt delete mode 100644 ArduSubsea/sensors.cpp delete mode 100644 ArduSubsea/setup.cpp delete mode 100644 ArduSubsea/system.cpp delete mode 100644 ArduSubsea/takeoff.cpp delete mode 100644 ArduSubsea/test.cpp delete mode 100644 ArduSubsea/wscript diff --git a/ArduSub/motors.cpp b/ArduSub/motors.cpp index 0288c951dd..4f1cfb3a11 100644 --- a/ArduSub/motors.cpp +++ b/ArduSub/motors.cpp @@ -13,12 +13,7 @@ static uint32_t auto_disarm_begin; // called at 10hz void Copter::arm_motors_check() { - // Arm motors automatically - if ( !motors.armed() ) { - init_arm_motors(false); - } - - /*static int16_t arming_counter; + static int16_t arming_counter; // ensure throttle is down if (channel_throttle->control_in > 0) { @@ -71,13 +66,13 @@ void Copter::arm_motors_check() // 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 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 @@ -122,7 +117,7 @@ void Copter::auto_disarm_check() 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 diff --git a/ArduSubsea/APM_Config.h b/ArduSubsea/APM_Config.h deleted file mode 100644 index f2ef2609a8..0000000000 --- a/ArduSubsea/APM_Config.h +++ /dev/null @@ -1,10 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -// This file is just a placeholder for your configuration file. If -// you wish to change any of the setup parameters from their default -// values, place the appropriate #define statements here. - -// If you used to define your CONFIG_APM_HARDWARE setting here, it is no -// longer valid! You should switch to using CONFIG_HAL_BOARD via the HAL_BOARD -// flag in your local config.mk instead. - diff --git a/ArduSubsea/APM_Config.h.reference b/ArduSubsea/APM_Config.h.reference deleted file mode 100644 index 8661f304a0..0000000000 --- a/ArduSubsea/APM_Config.h.reference +++ /dev/null @@ -1,949 +0,0 @@ -// -// Example and reference ArduPilot Mega configuration file -// ======================================================= -// -// This file contains documentation and examples for configuration options -// supported by the ArduPilot Mega software. -// -// Most of these options are just that - optional. You should create -// the APM_Config.h file and use this file as a reference for options -// that you want to change. Don't copy this file directly; the options -// described here and their default values may change over time. -// -// Each item is marked with a keyword describing when you should set it: -// -// REQUIRED -// You must configure this in your APM_Config.h file. The -// software will not compile if the option is not set. -// -// OPTIONAL -// The option has a sensible default (which will be described -// here), but you may wish to override it. -// -// EXPERIMENTAL -// You should configure this option unless you are prepared -// to deal with potential problems. It may relate to a feature -// still in development, or which is not yet adequately tested. -// -// DEBUG -// The option should only be set if you are debugging the -// software, or if you are gathering information for a bug -// report. -// -// NOTE: -// Many of these settings are considered 'factory defaults', and the -// live value is stored and managed in the ArduPilot Mega EEPROM. -// Use the setup 'factoryreset' command after changing options in -// your APM_Config.h file. -// -// Units -// ----- -// -// Unless indicated otherwise, numeric quantities use the following units: -// -// Measurement | Unit -// ------------+------------------------------------- -// angle | degrees -// distance | metres -// speed | metres per second -// servo angle | microseconds -// voltage | volts -// times | seconds -// throttle | percent -// - -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// -// HARDWARE CONFIGURATION AND CONNECTIONS -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////////////////////////// -// GPS_PROTOCOL REQUIRED -// -// GPS configuration, must be one of: -// -// GPS_PROTOCOL_AUTO Auto detect GPS type (must be a supported GPS) -// GPS_PROTOCOL_NONE No GPS attached -// GPS_PROTOCOL_IMU X-Plane interface or ArduPilot IMU. -// GPS_PROTOCOL_MTK MediaTek-based GPS running the DIYDrones firmware 1.4 -// GPS_PROTOCOL_MTK19 MediaTek-based GPS running the DIYDrones firmware 1.6, 1.7, 1.8, 1.9 -// GPS_PROTOCOL_UBLOX UBLOX GPS -// GPS_PROTOCOL_SIRF SiRF-based GPS in Binary mode. NOT TESTED -// GPS_PROTOCOL_NMEA Standard NMEA GPS. NOT SUPPORTED (yet?) -// -//#define GPS_PROTOCOL GPS_PROTOCOL_AUTO -// - -////////////////////////////////////////////////////////////////////////////// -// AIRSPEED_SENSOR OPTIONAL -// AIRSPEED_RATIO OPTIONAL -// -// Set AIRSPEED_SENSOR to ENABLED if you have an airspeed sensor attached. -// Adjust AIRSPEED_RATIO in small increments to calibrate the airspeed -// sensor relative to your GPS. The calculation and default value are optimized for speeds around 12 m/s -// -// The default assumes that an airspeed sensor is connected. -// -//#define AIRSPEED_SENSOR ENABLED -//#define AIRSPEED_RATIO 1.9936 -// - -////////////////////////////////////////////////////////////////////////////// -// MAGNETOMETER OPTIONAL -// -// Set MAGNETOMETER to ENABLED if you have a magnetometer attached. -// -// The default assumes that a magnetometer is not connected. -// -//#define MAGNETOMETER DISABLED -// - -////////////////////////////////////////////////////////////////////////////// -// HIL_PROTOCOL OPTIONAL -// HIL_MODE OPTIONAL -// HIL_PORT OPTIONAL -// -// Configuration for Hardware-in-the-loop simulation. In these modes, -// APM is connected via one or more interfaces to simulation software -// running on another system. -// -// HIL_PROTOCOL_XPLANE Configure for the X-plane HIL interface. -// HIL_PROTOCOL_MAVLINK Configure for HIL over MAVLink. -// -// HIL_MODE_DISABLED Configure for standard flight. -// HIL_MODE_SENSORS Simulator provides raw sensor values. -// -// Note that currently HIL_PROTOCOL_MAVLINK requires HIL_MODE_SENSORS. -// -//#define HIL_MODE HIL_MODE_DISABLED -//#define HIL_PORT 0 -//#define HIL_PROTOCOL HIL_PROTOCOL_MAVLINK -// - -////////////////////////////////////////////////////////////////////////////// -// GCS_PROTOCOL OPTIONAL -// GCS_PORT OPTIONAL -// MAV_SYSTEM_ID OPTIONAL -// -// The GCS_PROTOCOL option determines which (if any) ground control station -// protocol will be used. Must be one of: -// -// GCS_PROTOCOL_NONE No GCS output -// GCS_PROTOCOL_MAVLINK QGroundControl protocol -// -// The GCS_PORT option determines which serial port will be used by the -// GCS protocol. The usual values are 0 for the console/USB port, -// or 3 for the telemetry port on the oilpan. Note that some protocols -// will ignore this value and always use the console port. -// -// The MAV_SYSTEM_ID is a unique identifier for this UAV. The default value is 1. -// If you will be flying multiple UAV's each should be assigned a different ID so -// that ground stations can tell them apart. -// -//#define GCS_PROTOCOL GCS_PROTOCOL_MAVLINK -//#define GCS_PORT 3 -//#define MAV_SYSTEM_ID 1 -// - -////////////////////////////////////////////////////////////////////////////// -// Serial port speeds. -// -// SERIAL0_BAUD OPTIONAL -// -// Baudrate for the console port. Default is 115200bps. -// -// SERIAL3_BAUD OPTIONAL -// -// Baudrate for the telemetry port. Default is 57600bps. -// -//#define SERIAL0_BAUD 115200 -//#define SERIAL3_BAUD 57600 -// - -////////////////////////////////////////////////////////////////////////////// -// Battery monitoring OPTIONAL -// -// See the manual for details on selecting divider resistors for battery -// monitoring via the oilpan. -// -// BATTERY_EVENT OPTIONAL -// -// Set BATTERY_EVENT to ENABLED to enable low voltage or high discharge warnings. -// The default is DISABLED. -// -// LOW_VOLTAGE OPTIONAL if BATTERY_EVENT is set. -// -// Value in volts at which ArduPilot Mega should consider the -// battery to be "low". -// -// VOLT_DIV_RATIO OPTIONAL -// -// See the manual for details. The default value corresponds to the resistors -// recommended by the manual. -// -// CURR_AMPS_PER_VOLT OPTIONAL -// CURR_AMPS_OFFSET OPTIONAL -// -// The sensitivity of the current sensor. This must be scaled if a resistor is installed on APM -// for a voltage divider on input 2 (not recommended). The offset is used for current sensors with an offset -// -// -// HIGH_DISCHARGE OPTIONAL if BATTERY_EVENT is set. -// -// Value in milliamp-hours at which a warning should be triggered. Recommended value = 80% of -// battery capacity. -// -//#define BATTERY_EVENT DISABLED -//#define LOW_VOLTAGE 9.6 -//#define VOLT_DIV_RATIO 3.56 -//#define CURR_AMPS_PER_VOLT 27.32 -//#define CURR_AMPS_OFFSET 0.0 -//#define HIGH_DISCHARGE 1760 - -////////////////////////////////////////////////////////////////////////////// -// INPUT_VOLTAGE OPTIONAL -// -// In order to have accurate pressure and battery voltage readings, this -// value should be set to the voltage measured at the processor. -// -// See the manual for more details. The default value should be close if you are applying 5 volts to the servo rail. -// -//#define INPUT_VOLTAGE 4.68 // 4.68 is the average value for a sample set. This is the value at the processor with 5.02 applied at the servo rail -// - - -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// -// RADIO CONFIGURATION -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////////////////////////// -// FLIGHT_MODE OPTIONAL -// FLIGHT_MODE_CHANNEL OPTIONAL -// -// Flight modes assigned to the control channel, and the input channel that -// is read for the control mode. -// -// Use a servo tester, or the ArduPilotMega_demo test program to check your -// switch settings. -// -// ATTENTION: Some ArduPilot Mega boards have radio channels marked 0-7, and -// others have them marked the standard 1-8. The FLIGHT_MODE_CHANNEL option -// uses channel numbers 1-8 (and defaults to 8). -// -// If you only have a three-position switch or just want three modes, set your -// switch to produce 1165, 1425, and 1815 microseconds and configure -// FLIGHT_MODE 1 & 2, 3 & 4 and 5 & 6 to be the same. This is the default. -// -// If you have FLIGHT_MODE_CHANNEL set to 8 (the default) and your control -// channel connected to input channel 8, the hardware failsafe mode will -// activate for any control input over 1750ms. -// -// For more modes (up to six), set your switch(es) to produce any of 1165, -// 1295, 1425, 1555, 1685, and 1815 microseconds. -// -// Flight mode | Switch Setting (ms) -// ------------+--------------------- -// 1 | 1165 -// 2 | 1295 -// 3 | 1425 -// 4 | 1555 -// 5 | 1685 -// 6 | 1815 (FAILSAFE if using channel 8) -// -// The following standard flight modes are available: -// -// Name | Description -// -----------------+-------------------------------------------- -// | -// MANUAL | Full manual control via the hardware multiplexer. -// | -// STABILIZE | Tries to maintain level flight, but can be overridden with radio control inputs. -// | -// FLY_BY_WIRE_A | Autopilot style control via user input, with manual throttle. -// | -// FLY_BY_WIRE_B | Autopilot style control via user input, aispeed controlled with throttle. -// | -// RTL | Returns to the Home location and then LOITERs at a safe altitude. -// | -// AUTO | Autonomous flight based on programmed waypoints. Use the WaypointWriter -// | application or your Ground Control System to edit and upload -// | waypoints and other commands. -// | -//air -// -// The following non-standard modes are EXPERIMENTAL: -// -// Name | Description -// -----------------+-------------------------------------------- -// | -// LOITER | Flies in a circle around the current location. -// | -// CIRCLE | Flies in a stabilized 'dumb' circle. -// | -// -// -// If you are using channel 8 for mode switching then FLIGHT_MODE_5 and -// FLIGHT_MODE_6 should be MANUAL. -// -// -//#define FLIGHT_MODE_CHANNEL 8 -// -//#define FLIGHT_MODE_1 RTL -//#define FLIGHT_MODE_2 RTL -//#define FLIGHT_MODE_3 STABILIZE -//#define FLIGHT_MODE_4 STABILIZE -//#define FLIGHT_MODE_5 MANUAL -//#define FLIGHT_MODE_6 MANUAL -// - -////////////////////////////////////////////////////////////////////////////// -// For automatic flap operation based on speed setpoint. If the speed setpoint is above flap_1_speed -// then the flap position shall be 0%. If the speed setpoint is between flap_1_speed and flap_2_speed -// then the flap position shall be flap_1_percent. If the speed setpoint is below flap_2_speed -// then the flap position shall be flap_2_percent. Speed setpoint is the current value of -// airspeed_cruise (if airspeed sensor enabled) or throttle_cruise (if no airspeed sensor) - -// FLAP_1_PERCENT OPTIONAL -// FLAP_1_SPEED OPTIONAL -// FLAP_2_PERCENT OPTIONAL -// FLAP_2_SPEED OPTIONAL - - -////////////////////////////////////////////////////////////////////////////// -// THROTTLE_FAILSAFE OPTIONAL -// THROTTLE_FS_VALUE OPTIONAL -// -// The throttle failsafe allows you to configure a software failsafe activated -// by a setting on the throttle input channel (channel 3). Enabling this failsafe -// also enables "short failsafe" conditions (see below) based on loss of -// rc override control from the GCS -// -// This can be used to achieve a failsafe override on loss of radio control -// without having to sacrifice one of your FLIGHT_MODE settings, as the -// throttle failsafe overrides the switch-selected mode. -// -// Throttle failsafe is enabled by setting THROTTLE_FAILSAFE to 1. The default -// is for it to be enabled. -// -// If the throttle failsafe is enabled, THROTTLE_FS_VALUE sets the channel value -// below which the failsafe engages. The default is 975ms, which is a very low -// throttle setting. Most transmitters will let you trim the manual throttle -// position up so that you cannot engage the failsafe with a regular stick movement. -// -// Configure your receiver's failsafe setting for the throttle channel to the -// absolute minimum, and use the ArduPilotMega_demo program to check that -// you cannot reach that value with the throttle control. Leave a margin of -// at least 50 microseconds between the lowest throttle setting and -// THROTTLE_FS_VALUE. -// -//#define THROTTLE_FAILSAFE ENABLED -//#define THROTTLE_FS_VALUE 950 -// - -////////////////////////////////////////////////////////////////////////////// - -// GCS_HEARTBEAT_FAILSAFE OPTIONAL -// SHORT_FAILSAFE_ACTION OPTIONAL -// LONG_FAILSAFE_ACTION OPTIONAL - -// There are two basic conditions which can trigger a failsafe. One is a loss of control signal. -// Normally this means a loss of the radio control RC signal. However if rc override from the -// GCS is in use then this can mean a loss of communication with the GCS. Such a failsafe will be -// classified as either short (greater than 1.5 seconds but less than 20) or long (greater than 20). -// Also, if GCS_HEARTBEAT_FAILSAFE is enabled and a heartbeat signal from the GCS has not been received -// in the preceeding 20 seconds then this will also trigger a "long" failsafe. -// -// The SHORT_FAILSAFE_ACTION and LONG_FAILSAFE_ACTION settings determines what APM will do when -// a failsafe mode is entered while flying in AUTO or LOITER mode. This is important in order to avoid -// accidental failsafe behaviour when flying waypoints that take the aircraft -// out of radio range. -// -// If SHORT_FAILSAFE_ACTION is 1, when failsafe is entered in AUTO or LOITER modes, -// the aircraft will head for home in RTL mode. If the failsafe condition is -// resolved, it will return to AUTO or LOITER mode. - -// If LONG_FAILSAFE_ACTION is 1, when failsafe is entered in AUTO or LOITER modes, -// the aircraft will head for home in RTL mode. If the failsafe condition is -// resolved the aircraft will not be returned to AUTO or LOITER mode, but will continue home - -// If XX_FAILSAFE_ACTION is 0 and the applicable failsafe occurs while in AUTO or LOITER -// mode the aircraft will continue in that mode ignoring the failsafe condition. - -// Note that for Manual, Stabilize, and Fly-By-Wire (A and B) modes the aircraft will always -// enter a circling mode for short failsafe conditions and will be switched to RTL for long -// failsafe conditions. RTL mode is unaffected by failsafe conditions. -// -// The default is to have GCS Heartbeat failsafes DISABLED -// The default behaviour is to ignore failsafes in AUTO and LOITER modes. -// -//#define GCS_HEARTBEAT_FAILSAFE DISABLED -//#define SHORT_FAILSAFE_ACTION 0 -//#define LONG_FAILSAFE_ACTION 0 - - -////////////////////////////////////////////////////////////////////////////// -// AUTO_TRIM OPTIONAL -// -// ArduPilot Mega can update its trim settings by looking at the -// radio inputs when switching out of MANUAL mode. This allows you to -// manually trim your aircraft before switching to an assisted mode, but it -// also means that you should avoid switching out of MANUAL while you have -// any control stick deflection. -// -// The default is to disable AUTO_TRIM. -// -//#define AUTO_TRIM DISABLED -// - -////////////////////////////////////////////////////////////////////////////// -// THROTTLE_REVERSE OPTIONAL -// -// A few speed controls require the throttle servo signal be reversed. Setting -// this to ENABLED will reverse the throttle output signal. Ensure that your -// throttle needs to be reversed by using the hardware failsafe and the -// ArduPilotMega_demo program before setting this option. -// -// The default is to not reverse the signal. -// -//#define THROTTLE_REVERSE DISABLED -// - -////////////////////////////////////////////////////////////////////////////// -// ENABLE_STICK_MIXING OPTIONAL -// -// If this option is set to ENABLED, manual control inputs are are respected -// while in the autopilot modes (AUTO, RTL, LOITER, CIRCLE etc.) -// -// The default is to enable stick mixing, allowing the pilot to take -// emergency action without switching modes. -// -//#define ENABLE_STICK_MIXING ENABLED -// - -////////////////////////////////////////////////////////////////////////////// -// THROTTLE_OUT DEBUG -// -// When debugging, it can be useful to disable the throttle output. Set -// this option to DISABLED to disable throttle output signals. -// -// The default is to not disable throttle output. -// -//#define THROTTLE_OUT ENABLED -// - - -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// -// STARTUP BEHAVIOUR -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////////////////////////// -// GROUND_START_DELAY OPTIONAL -// -// If configured, inserts a delay between power-up and the beginning of INS -// calibration during a ground start. -// -// Use this setting to give you time to position the aircraft horizontally -// for the INS calibration. -// -// The default is to begin INS calibration immediately at startup. -// -//#define GROUND_START_DELAY 0 -// - -////////////////////////////////////////////////////////////////////////////// -// ENABLE_AIR_START OPTIONAL -// -// If air start is disabled then you will get a ground start (including INS -// calibration) every time the AP is powered up. This means that if you get -// a power glitch or reboot for some reason in the air, you will probably -// crash, but it prevents a lot of problems on the ground like unintentional -// motor start-ups, etc. -// -// If air start is enabled then you will get an air start at power up and a -// ground start will be performed if the speed is near zero when we get gps -// lock. -// -// The default is to disable air start. -// -//#define ENABLE_AIR_START 0 -// - - -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// -// FLIGHT AND NAVIGATION CONTROL -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////////////////////////// -// Altitude measurement and control. -// -// ALT_EST_GAIN OPTIONAL -// -// The gain of the altitude estimation function; a lower number results -// in slower error correction and smoother output. The default is a -// reasonable starting point. -// -//#define ALT_EST_GAIN 0.01 -// -// ALTITUDE_MIX OPTIONAL -// -// Configures the blend between GPS and pressure altitude. -// 0 = GPS altitude, 1 = Press alt, 0.5 = half and half, etc. -// -// The default is to use only pressure altitude. -// -//#define ALTITUDE_MIX 1 -// - -////////////////////////////////////////////////////////////////////////////// -// AIRSPEED_CRUISE OPTIONAL -// -// The speed in metres per second to maintain during cruise. The default -// is 10m/s, which is a conservative value suitable for relatively small, -// light aircraft. -// -//#define AIRSPEED_CRUISE 12 -// - -////////////////////////////////////////////////////////////////////////////// -// MIN_GNDSPEED OPTIONAL -// -// The minimum ground speed in metres per second to maintain during -// cruise. A value of 0 will disable any attempt to maintain a minumum -// speed over ground. -// -#define MIN_GNDSPEED 0 -// - -////////////////////////////////////////////////////////////////////////////// -// FLY_BY_WIRE_B airspeed control (also used for throttle "nudging" in AUTO) -// -// AIRSPEED_FBW_MIN OPTIONAL -// AIRSPEED_FBW_MAX OPTIONAL -// -// Airspeed corresponding to minimum and maximum throttle in Fly By Wire B mode. -// The defaults are 6 and 30 metres per second. -// -// AIRSPEED_FBW_MAX also sets the maximum airspeed that the cruise airspeed can be "nudged" to in AUTO mode when ENABLE_STICK_MIXING is set. -// In AUTO the cruise airspeed can be increased between AIRSPEED_CRUISE and AIRSPEED_FBW_MAX by positioning the throttle -// stick in the top 1/2 of its range. Throttle stick in the bottom 1/2 provide regular AUTO control. -// -//#define AIRSPEED_FBW_MIN 6 -//#define AIRSPEED_FBW_MAX 22 -// - -////////////////////////////////////////////////////////////////////////////// -// Servo mapping -// -// THROTTLE_MIN OPTIONAL -// -// The minimum throttle setting to which the autopilot will reduce the -// throttle while descending. The default is zero, which is -// suitable for aircraft with a steady power-off glide. Increase this -// value if your aircraft needs throttle to maintain a stable descent in -// level flight. -// -// THROTTLE_CRUISE OPTIONAL -// -// The approximate throttle setting to achieve AIRSPEED_CRUISE in level flight. -// The default is 45%, which is reasonable for a modestly powered aircraft. -// -// THROTTLE_MAX OPTIONAL -// -// The maximum throttle setting the autopilot will apply. The default is 75%. -// Reduce this value if your aicraft is overpowered, or has complex flight -// characteristics at high throttle settings. -// -//#define THROTTLE_MIN 0 // percent -//#define THROTTLE_CRUISE 45 -//#define THROTTLE_MAX 75 - -////////////////////////////////////////////////////////////////////////////// -// Autopilot control limits -// -// HEAD_MAX OPTIONAL -// -// The maximum commanded bank angle in either direction. -// The default is 45 degrees. Decrease this value if your aircraft is not -// stable or has difficulty maintaining altitude in a steep bank. -// -// PITCH_MAX OPTIONAL -// -// The maximum commanded pitch up angle. -// The default is 15 degrees. Care should be taken not to set this value too -// large, as the aircraft may stall. -// -// PITCH_MIN -// -// The maximum commanded pitch down angle. Note that this value must be -// negative. The default is -25 degrees. Care should be taken not to set -// this value too large as it may result in overspeeding the aircraft. -// -// PITCH_TARGET -// -// The target pitch for cruise flight. When the APM measures this pitch -// value, the pitch error will be calculated to be 0 for the pitch PID -// control loop. -// -//#define HEAD_MAX 45 -//#define PITCH_MAX 15 -//#define PITCH_MIN -25 -//#define PITCH_TARGET 0 - -////////////////////////////////////////////////////////////////////////////// -// Attitude control gains -// -// Tuning values for the attitude control PID loops. -// -// The P term is the primary tuning value. This determines how the control -// deflection varies in proportion to the required correction. -// -// The I term is used to help control surfaces settle. This value should -// normally be kept low. -// -// The D term is used to control overshoot. Avoid using or adjusting this -// term if you are not familiar with tuning PID loops. It should normally -// be zero for most aircraft. -// -// Note: When tuning these values, start with changes of no more than 25% at -// a time. -// -// SERVO_ROLL_P OPTIONAL -// SERVO_ROLL_I OPTIONAL -// SERVO_ROLL_D OPTIONAL -// -// P, I and D terms for roll control. Defaults are 0.4, 0, 0. -// -// SERVO_ROLL_INT_MAX OPTIONAL -// -// Maximum control offset due to the integral. This prevents the control -// output from being overdriven due to a persistent offset (e.g. crosstracking). -// Default is 5 degrees. -// -// ROLL_SLEW_LIMIT EXPERIMENTAL -// -// Limits the slew rate of the roll control in degrees per second. If zero, -// slew rate is not limited. Default is to not limit the roll control slew rate. -// (This feature is currently not implemented.) -// -// SERVO_PITCH_P OPTIONAL -// SERVO_PITCH_I OPTIONAL -// SERVO_PITCH_D OPTIONAL -// -// P, I and D terms for the pitch control. Defaults are 0.6, 0, 0. -// -// SERVO_PITCH_INT_MAX OPTIONAL -// -// Maximum control offset due to the integral. This prevents the control -// output from being overdriven due to a persistent offset (e.g. native flight -// AoA). -// Default is 5 degrees. -// -// PITCH_COMP OPTIONAL -// -// Adds pitch input to compensate for the loss of lift due to roll control. -// Default is 0.20 (20% of roll control also applied to pitch control). -// -// SERVO_YAW_P OPTIONAL -// SERVO_YAW_I OPTIONAL -// SERVO_YAW_D OPTIONAL -// -// P, I and D terms for the YAW control. Defaults are 0., 0., 0. -// Note units of this control loop are unusual. PID input is in m/s**2. -// -// SERVO_YAW_INT_MAX OPTIONAL -// -// Maximum control offset due to the integral. This prevents the control -// output from being overdriven due to a persistent offset (e.g. crosstracking). -// Default is 0. -// -// RUDDER_MIX OPTIONAL -// -// Roll to yaw mixing. This allows for co-ordinated turns. -// Default is 0.50 (50% of roll control also applied to yaw control.) -// -//#define SERVO_ROLL_P 0.4 -//#define SERVO_ROLL_I 0.0 -//#define SERVO_ROLL_D 0.0 -//#define SERVO_ROLL_INT_MAX 5 -//#define ROLL_SLEW_LIMIT 0 -//#define SERVO_PITCH_P 0.6 -//#define SERVO_PITCH_I 0.0 -//#define SERVO_PITCH_D 0.0 -//#define SERVO_PITCH_INT_MAX 5 -//#define PITCH_COMP 0.2 -//#define SERVO_YAW_P 0.0 // Default is zero. A suggested value if you want to use this parameter is 0.5 -//#define SERVO_YAW_I 0.0 -//#define SERVO_YAW_D 0.0 -//#define SERVO_YAW_INT_MAX 5 -//#define RUDDER_MIX 0.5 -// - -////////////////////////////////////////////////////////////////////////////// -// Navigation control gains -// -// Tuning values for the navigation control PID loops. -// -// The P term is the primary tuning value. This determines how the control -// deflection varies in proportion to the required correction. -// -// The I term is used to control drift. -// -// The D term is used to control overshoot. Avoid adjusting this term if -// you are not familiar with tuning PID loops. -// -// Note: When tuning these values, start with changes of no more than 25% at -// a time. -// -// NAV_ROLL_P OPTIONAL -// NAV_ROLL_I OPTIONAL -// NAV_ROLL_D OPTIONAL -// -// P, I and D terms for navigation control over roll, normally used for -// controlling the aircraft's course. The P term controls how aggressively -// the aircraft will bank to change or hold course. -// Defaults are 0.7, 0.0, 0.02. -// -// NAV_ROLL_INT_MAX OPTIONAL -// -// Maximum control offset due to the integral. This prevents the control -// output from being overdriven due to a persistent offset (e.g. crosstracking). -// Default is 5 degrees. -// -// NAV_PITCH_ASP_P OPTIONAL -// NAV_PITCH_ASP_I OPTIONAL -// NAV_PITCH_ASP_D OPTIONAL -// -// P, I and D terms for pitch adjustments made to maintain airspeed. -// Defaults are 0.65, 0, 0. -// -// NAV_PITCH_ASP_INT_MAX OPTIONAL -// -// Maximum pitch offset due to the integral. This limits the control -// output from being overdriven due to a persistent offset (eg. inability -// to maintain the programmed airspeed). -// Default is 5 degrees. -// -// NAV_PITCH_ALT_P OPTIONAL -// NAV_PITCH_ALT_I OPTIONAL -// NAV_PITCH_ALT_D OPTIONAL -// -// P, I and D terms for pitch adjustments made to maintain altitude. -// Defaults are 0.65, 0, 0. -// -// NAV_PITCH_ALT_INT_MAX OPTIONAL -// -// Maximum pitch offset due to the integral. This limits the control -// output from being overdriven due to a persistent offset (eg. inability -// to maintain the programmed altitude). -// Default is 5 meters. -// -//#define NAV_ROLL_P 0.7 -//#define NAV_ROLL_I 0.01 -//#define NAV_ROLL_D 0.02 -//#define NAV_ROLL_INT_MAX 5 -//#define NAV_PITCH_ASP_P 0.65 -//#define NAV_PITCH_ASP_I 0.0 -//#define NAV_PITCH_ASP_D 0.0 -//#define NAV_PITCH_ASP_INT_MAX 5 -//#define NAV_PITCH_ALT_P 0.65 -//#define NAV_PITCH_ALT_I 0.0 -//#define NAV_PITCH_ALT_D 0.0 -//#define NAV_PITCH_ALT_INT_MAX 5 -// - -////////////////////////////////////////////////////////////////////////////// -// Energy/Altitude control gains -// -// The Energy/altitude control system uses throttle input to control aircraft -// altitude. -// -// The P term is the primary tuning value. This determines how the throttle -// setting varies in proportion to the required correction. -// -// The I term is used to compensate for small offsets. -// -// The D term is used to control overshoot. Avoid adjusting this term if -// you are not familiar with tuning PID loops. -// -// Note units of this control loop are unusual. PID input is in m**2/s**2. -// -// THROTTLE_TE_P OPTIONAL -// THROTTLE_TE_I OPTIONAL -// THROTTLE_TE_D OPTIONAL -// -// P, I and D terms for throttle adjustments made to control altitude. -// Defaults are 0.5, 0, 0. -// -// THROTTLE_TE_INT_MAX OPTIONAL -// -// Maximum throttle input due to the integral term. This limits the -// throttle from being overdriven due to a persistent offset (e.g. -// inability to maintain the programmed altitude). -// Default is 20%. -// -// THROTTLE_SLEW_LIMIT OPTIONAL -// -// Limits the slew rate of the throttle, in percent per second. Helps -// avoid sudden throttle changes, which can destabilise the aircraft. -// A setting of zero disables the feature. Range 1 to 100. -// Default is zero (disabled). -// -// P_TO_T OPTIONAL -// -// Pitch to throttle feed-forward gain. Default is 0. -// -// T_TO_P OPTIONAL -// -// Throttle to pitch feed-forward gain. Default is 0. -// -//#define THROTTLE_TE_P 0.50 -//#define THROTTLE_TE_I 0.0 -//#define THROTTLE_TE_D 0.0 -//#define THROTTLE_TE_INT_MAX 20 -//#define THROTTLE_SLEW_LIMIT 0 -//#define P_TO_T 0 -//#define T_TO_P 0 -// - -////////////////////////////////////////////////////////////////////////////// -// Crosstrack compensation -// -// XTRACK_GAIN OPTIONAL -// -// Crosstrack compensation in degrees per metre off track. -// Default value is 1.0 degrees per metre. Values lower than 0.001 will -// disable crosstrack compensation. -// -// XTRACK_ENTRY_ANGLE OPTIONAL -// -// Maximum angle used to correct for track following. -// Default value is 30 degrees. -// -//#define XTRACK_GAIN 1 // deg/m -//#define XTRACK_ENTRY_ANGLE 30 // deg -// - -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// -// DEBUGGING -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////////////////////////// -// Dataflash logging control -// -// Each of these logging options may be set to ENABLED to enable, or DISABLED -// to disable the logging of the respective data. -// -// LOG_ATTITUDE_FAST DEBUG -// -// Logs basic attitude info to the dataflash at 50Hz (uses more space). -// Defaults to DISABLED. -// -// LOG_ATTITUDE_MED OPTIONAL -// -// Logs basic attitude info to the dataflash at 10Hz (uses less space than -// LOG_ATTITUDE_FAST). Defaults to ENABLED. -// -// LOG_GPS OPTIONAL -// -// Logs GPS info to the dataflash at 10Hz. Defaults to ENABLED. -// -// LOG_PM OPTIONAL -// -// Logs INS performance monitoring info every 20 seconds. -// Defaults to DISABLED. -// -// LOG_CTUN OPTIONAL -// -// Logs control loop tuning info at 10 Hz. This information is useful for tuning -// servo control loop gain values. Defaults to DISABLED. -// -// LOG_NTUN OPTIONAL -// -// Logs navigation tuning info at 10 Hz. This information is useful for tuning -// navigation control loop gain values. Defaults to DISABLED. -// -// LOG_ MODE OPTIONAL -// -// Logs changes to the flight mode upon occurrence. Defaults to ENABLED. -// -// LOG_RAW DEBUG -// -// Logs raw accelerometer and gyro data at 50 Hz (uses more space). -// Defaults to DISABLED. -// -// LOG_CMD OPTIONAL -// -// Logs new commands when they process. -// Defaults to ENABLED. -// -//#define LOG_ATTITUDE_FAST DISABLED -//#define LOG_ATTITUDE_MED ENABLED -//#define LOG_GPS ENABLED -//#define LOG_PM ENABLED -//#define LOG_CTUN DISABLED -//#define LOG_NTUN DISABLED -//#define LOG_MODE ENABLED -//#define LOG_RAW DISABLED -//#define LOG_CMD ENABLED -//#define LOG_CUR DISABLED -// - -////////////////////////////////////////////////////////////////////////////// -// Navigation defaults -// -// WP_RADIUS_DEFAULT OPTIONAL -// -// When the user performs a factory reset on the APM, set the waypoint radius -// (the radius from a target waypoint within which the APM will consider -// itself to have arrived at the waypoint) to this value in meters. This is -// mainly intended to allow users to start using the APM without running the -// WaypointWriter first. -// -// LOITER_RADIUS_DEFAULT OPTIONAL -// -// When the user performs a factory reset on the APM, set the loiter radius -// (the distance the APM will attempt to maintain from a waypoint while -// loitering) to this value in meters. This is mainly intended to allow -// users to start using the APM without running the WaypointWriter first. -// -// USE_CURRENT_ALT OPTIONAL -// ALT_HOLD_HOME OPTIONAL -// -// When the user performs a factory reset on the APM, set the flag for weather -// the current altitude or ALT_HOLD_HOME altitude should be used for Return To Launch. -// Also, set the value of USE_CURRENT_ALT in meters. This is mainly intended to allow -// users to start using the APM without running the WaypointWriter first. -// -//#define WP_RADIUS_DEFAULT 30 -//#define LOITER_RADIUS_DEFAULT 60 -//#define USE_CURRENT_ALT FALSE -//#define ALT_HOLD_HOME 100 -// - -////////////////////////////////////////////////////////////////////////////// -// Debugging interface -// -// DEBUG_PORT OPTIONAL -// -// The APM will periodically send messages reporting what it is doing; this -// variable determines to which serial port they will be sent. Port 0 is the -// USB serial port on the shield, port 3 is the telemetry port. -// -//#define DEBUG_PORT 0 -// - - -// -// Do not remove - this is to discourage users from copying this file -// and using it as-is rather than editing their own. -// -#error You should not copy APM_Config.h.reference - make your own APM_Config.h file with just the options you need. diff --git a/ArduSubsea/ArduPlane.cpp b/ArduSubsea/ArduPlane.cpp deleted file mode 100644 index 8cceb0e52a..0000000000 --- a/ArduSubsea/ArduPlane.cpp +++ /dev/null @@ -1,907 +0,0 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -/* - Lead developer: Andrew Tridgell - - Authors: Doug Weibel, Jose Julio, Jordi Munoz, Jason Short, Randy Mackay, Pat Hickey, John Arne Birkeland, Olivier Adler, Amilcar Lucas, Gregory Fletcher, Paul Riseborough, Brandon Jones, Jon Challinger - Thanks to: Chris Anderson, Michael Oborne, Paul Mather, Bill Premerlani, James Cohen, JB from rotorFX, Automatik, Fefenin, Peter Meister, Remzibi, Yury Smirnov, Sandro Benigno, Max Levine, Roberto Navoni, Lorenz Meier, Yury MonZon - - Please contribute your ideas! See http://dev.ardupilot.com for details - - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . - */ - -#include "Plane.h" - -#define SCHED_TASK(func, rate_hz, max_time_micros) SCHED_TASK_CLASS(Plane, &plane, func, rate_hz, max_time_micros) - - -/* - scheduler table - all regular tasks are listed here, along with how - often they should be called (in 20ms units) and the maximum time - they are expected to take (in microseconds) - */ -const AP_Scheduler::Task Plane::scheduler_tasks[] = { - SCHED_TASK(read_radio, 50, 700), - SCHED_TASK(check_short_failsafe, 50, 1000), - SCHED_TASK(ahrs_update, 50, 6400), - SCHED_TASK(update_speed_height, 50, 1600), - SCHED_TASK(update_flight_mode, 50, 1400), - SCHED_TASK(stabilize, 50, 3500), - SCHED_TASK(set_servos, 50, 1600), - SCHED_TASK(read_control_switch, 7, 1000), - SCHED_TASK(gcs_retry_deferred, 50, 1000), - SCHED_TASK(update_GPS_50Hz, 50, 2500), - SCHED_TASK(update_GPS_10Hz, 10, 2500), - SCHED_TASK(navigate, 10, 3000), - SCHED_TASK(update_compass, 10, 1200), - SCHED_TASK(read_airspeed, 10, 1200), - SCHED_TASK(update_alt, 10, 3400), - SCHED_TASK(adjust_altitude_target, 10, 1000), - SCHED_TASK(obc_fs_check, 10, 1000), - SCHED_TASK(gcs_update, 50, 1700), - SCHED_TASK(gcs_data_stream_send, 50, 3000), - SCHED_TASK(update_events, 50, 1500), - SCHED_TASK(check_usb_mux, 10, 300), - SCHED_TASK(read_battery, 10, 1000), - SCHED_TASK(compass_accumulate, 50, 1500), - SCHED_TASK(barometer_accumulate, 50, 900), - SCHED_TASK(update_notify, 50, 300), - SCHED_TASK(read_rangefinder, 50, 500), - SCHED_TASK(compass_cal_update, 50, 100), - SCHED_TASK(accel_cal_update, 10, 100), -#if OPTFLOW == ENABLED - SCHED_TASK(update_optical_flow, 50, 500), -#endif - SCHED_TASK(one_second_loop, 1, 1000), - SCHED_TASK(check_long_failsafe, 3, 1000), - SCHED_TASK(read_receiver_rssi, 10, 1000), - SCHED_TASK(rpm_update, 10, 200), - SCHED_TASK(airspeed_ratio_update, 1, 1000), - SCHED_TASK(update_mount, 50, 1500), - SCHED_TASK(log_perf_info, 0.1, 1000), - SCHED_TASK(compass_save, 0.016, 2500), - SCHED_TASK(update_logging1, 10, 1700), - SCHED_TASK(update_logging2, 10, 1700), - SCHED_TASK(parachute_check, 10, 500), -#if FRSKY_TELEM_ENABLED == ENABLED - SCHED_TASK(frsky_telemetry_send, 5, 100), -#endif - SCHED_TASK(terrain_update, 10, 500), - SCHED_TASK(update_is_flying_5Hz, 5, 100), - SCHED_TASK(dataflash_periodic, 50, 300), - SCHED_TASK(adsb_update, 1, 500), -}; - -void Plane::setup() -{ - cliSerial = hal.console; - - // load the default values of variables listed in var_info[] - AP_Param::setup_sketch_defaults(); - - AP_Notify::flags.failsafe_battery = false; - - notify.init(false); - - rssi.init(); - - init_ardupilot(); - - // initialise the main loop scheduler - scheduler.init(&scheduler_tasks[0], ARRAY_SIZE(scheduler_tasks)); -} - -void Plane::loop() -{ - // wait for an INS sample - ins.wait_for_sample(); - - uint32_t timer = micros(); - - delta_us_fast_loop = timer - fast_loopTimer_us; - G_Dt = delta_us_fast_loop * 1.0e-6f; - - if (delta_us_fast_loop > G_Dt_max && fast_loopTimer_us != 0) { - G_Dt_max = delta_us_fast_loop; - } - - if (delta_us_fast_loop < G_Dt_min || G_Dt_min == 0) { - G_Dt_min = delta_us_fast_loop; - } - fast_loopTimer_us = timer; - - mainLoop_count++; - - // tell the scheduler one tick has passed - scheduler.tick(); - - // run all the tasks that are due to run. Note that we only - // have to call this once per loop, as the tasks are scheduled - // in multiples of the main loop tick. So if they don't run on - // the first call to the scheduler they won't run on a later - // call until scheduler.tick() is called again - uint32_t remaining = (timer + 20000) - micros(); - if (remaining > 19500) { - remaining = 19500; - } - scheduler.run(remaining); -} - -// update AHRS system -void Plane::ahrs_update() -{ - hal.util->set_soft_armed(arming.is_armed() && - hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED); - -#if HIL_SUPPORT - if (g.hil_mode == 1) { - // update hil before AHRS update - gcs_update(); - } -#endif - - ahrs.update(); - - if (should_log(MASK_LOG_ATTITUDE_FAST)) { - Log_Write_Attitude(); - } - - if (should_log(MASK_LOG_IMU)) { - Log_Write_IMU(); - DataFlash.Log_Write_IMUDT(ins); - } - - // calculate a scaled roll limit based on current pitch - roll_limit_cd = g.roll_limit_cd * cosf(ahrs.pitch); - pitch_limit_min_cd = aparm.pitch_limit_min_cd * fabsf(cosf(ahrs.roll)); - - // updated the summed gyro used for ground steering and - // auto-takeoff. Dot product of DCM.c with gyro vector gives earth - // frame yaw rate - steer_state.locked_course_err += ahrs.get_yaw_rate_earth() * G_Dt; - steer_state.locked_course_err = wrap_PI(steer_state.locked_course_err); -} - -/* - update 50Hz speed/height controller - */ -void Plane::update_speed_height(void) -{ - if (auto_throttle_mode) { - // Call TECS 50Hz update. Note that we call this regardless of - // throttle suppressed, as this needs to be running for - // takeoff detection - SpdHgt_Controller->update_50hz(tecs_hgt_afe()); - } -} - - -/* - update camera mount - */ -void Plane::update_mount(void) -{ -#if MOUNT == ENABLED - camera_mount.update(); -#endif - -#if CAMERA == ENABLED - camera.trigger_pic_cleanup(); -#endif -} - -/* - read and update compass - */ -void Plane::update_compass(void) -{ - if (g.compass_enabled && compass.read()) { - ahrs.set_compass(&compass); - compass.learn_offsets(); - if (should_log(MASK_LOG_COMPASS)) { - DataFlash.Log_Write_Compass(compass); - } - } else { - ahrs.set_compass(NULL); - } -} - -/* - if the compass is enabled then try to accumulate a reading - */ -void Plane::compass_accumulate(void) -{ - if (g.compass_enabled) { - compass.accumulate(); - } -} - -/* - try to accumulate a baro reading - */ -void Plane::barometer_accumulate(void) -{ - barometer.accumulate(); -} - -/* - do 10Hz logging - */ -void Plane::update_logging1(void) -{ - if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST)) { - Log_Write_Attitude(); - } - - if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_IMU)) - Log_Write_IMU(); -} - -/* - do 10Hz logging - part2 - */ -void Plane::update_logging2(void) -{ - if (should_log(MASK_LOG_CTUN)) - Log_Write_Control_Tuning(); - - if (should_log(MASK_LOG_NTUN)) - Log_Write_Nav_Tuning(); - - if (should_log(MASK_LOG_RC)) - Log_Write_RC(); - - if (should_log(MASK_LOG_IMU)) - DataFlash.Log_Write_Vibration(ins); -} - - -/* - check for OBC failsafe check - */ -void Plane::obc_fs_check(void) -{ -#if OBC_FAILSAFE == ENABLED - // perform OBC failsafe checks - obc.check(OBC_MODE(control_mode), failsafe.last_heartbeat_ms, geofence_breached(), failsafe.last_valid_rc_ms); -#endif -} - - -/* - update aux servo mappings - */ -void Plane::update_aux(void) -{ - RC_Channel_aux::enable_aux_servos(); -} - -void Plane::one_second_loop() -{ - if (should_log(MASK_LOG_CURRENT)) - Log_Write_Current(); - - // send a heartbeat - gcs_send_message(MSG_HEARTBEAT); - - // make it possible to change control channel ordering at runtime - set_control_channels(); - - // make it possible to change orientation at runtime - ahrs.set_orientation(); - - // sync MAVLink system ID - mavlink_system.sysid = g.sysid_this_mav; - - update_aux(); - - // update notify flags - AP_Notify::flags.pre_arm_check = arming.pre_arm_checks(false); - AP_Notify::flags.pre_arm_gps_check = true; - AP_Notify::flags.armed = arming.is_armed() || arming.arming_required() == AP_Arming::NO; - - crash_detection_update(); - -#if AP_TERRAIN_AVAILABLE - if (should_log(MASK_LOG_GPS)) { - terrain.log_terrain_data(DataFlash); - } -#endif - // piggyback the status log entry on the MODE log entry flag - if (should_log(MASK_LOG_MODE)) { - Log_Write_Status(); - } - - ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW)); -} - -void Plane::log_perf_info() -{ - if (scheduler.debug() != 0) { - gcs_send_text_fmt(MAV_SEVERITY_INFO, "G_Dt_max=%lu G_Dt_min=%lu\n", - (unsigned long)G_Dt_max, - (unsigned long)G_Dt_min); - } - - if (should_log(MASK_LOG_PM)) { - Log_Write_Performance(); - } - - G_Dt_max = 0; - G_Dt_min = 0; - resetPerfData(); -} - -void Plane::compass_save() -{ - if (g.compass_enabled) { - compass.save_offsets(); - } -} - -void Plane::terrain_update(void) -{ -#if AP_TERRAIN_AVAILABLE - terrain.update(); -#endif -} - - -void Plane::dataflash_periodic(void) -{ - DataFlash.periodic_tasks(); -} - -/* - once a second update the airspeed calibration ratio - */ -void Plane::airspeed_ratio_update(void) -{ - if (!airspeed.enabled() || - gps.status() < AP_GPS::GPS_OK_FIX_3D || - gps.ground_speed() < 4) { - // don't calibrate when not moving - return; - } - if (airspeed.get_airspeed() < aparm.airspeed_min && - gps.ground_speed() < (uint32_t)aparm.airspeed_min) { - // don't calibrate when flying below the minimum airspeed. We - // check both airspeed and ground speed to catch cases where - // the airspeed ratio is way too low, which could lead to it - // never coming up again - return; - } - if (labs(ahrs.roll_sensor) > roll_limit_cd || - ahrs.pitch_sensor > aparm.pitch_limit_max_cd || - ahrs.pitch_sensor < pitch_limit_min_cd) { - // don't calibrate when going beyond normal flight envelope - return; - } - const Vector3f &vg = gps.velocity(); - airspeed.update_calibration(vg); - gcs_send_airspeed_calibration(vg); -} - - -/* - read the GPS and update position - */ -void Plane::update_GPS_50Hz(void) -{ - static uint32_t last_gps_reading[GPS_MAX_INSTANCES]; - gps.update(); - - for (uint8_t i=0; i= AP_GPS::GPS_OK_FIX_3D) { - last_gps_msg_ms = gps.last_message_time_ms(); - - if (ground_start_count > 1) { - ground_start_count--; - } else if (ground_start_count == 1) { - // We countdown N number of good GPS fixes - // so that the altitude is more accurate - // ------------------------------------- - if (current_loc.lat == 0) { - ground_start_count = 5; - - } else { - init_home(); - - // set system clock for log timestamps - hal.util->set_system_clock(gps.time_epoch_usec()); - - if (g.compass_enabled) { - // Set compass declination automatically - const Location &loc = gps.location(); - compass.set_initial_location(loc.lat, loc.lng); - } - ground_start_count = 0; - } - } - - // see if we've breached the geo-fence - geofence_check(false); - -#if CAMERA == ENABLED - if (camera.update_location(current_loc, plane.ahrs ) == true) { - do_take_picture(); - } -#endif - - if (!hal.util->get_soft_armed()) { - update_home(); - } - - // update wind estimate - ahrs.estimate_wind(); - } - - calc_gndspeed_undershoot(); -} - -/* - main handling for AUTO mode - */ -void Plane::handle_auto_mode(void) -{ - uint8_t nav_cmd_id; - - // we should be either running a mission or RTLing home - if (mission.state() == AP_Mission::MISSION_RUNNING) { - nav_cmd_id = mission.get_current_nav_cmd().id; - }else{ - nav_cmd_id = auto_rtl_command.id; - } - - if (nav_cmd_id == MAV_CMD_NAV_TAKEOFF || - (nav_cmd_id == MAV_CMD_NAV_LAND && flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT)) { - takeoff_calc_roll(); - takeoff_calc_pitch(); - calc_throttle(); - } else if (nav_cmd_id == MAV_CMD_NAV_LAND) { - calc_nav_roll(); - calc_nav_pitch(); - - if (auto_state.land_complete) { - // during final approach constrain roll to the range - // allowed for level flight - nav_roll_cd = constrain_int32(nav_roll_cd, -g.level_roll_limit*100UL, g.level_roll_limit*100UL); - } - calc_throttle(); - - if (auto_state.land_complete) { - // we are in the final stage of a landing - force - // zero throttle - channel_throttle->servo_out = 0; - } - } else { - // we are doing normal AUTO flight, the special cases - // are for takeoff and landing - if (nav_cmd_id != MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT) { - steer_state.hold_course_cd = -1; - } - auto_state.land_complete = false; - calc_nav_roll(); - calc_nav_pitch(); - calc_throttle(); - } -} - -/* - main flight mode dependent update code - */ -void Plane::update_flight_mode(void) -{ - enum FlightMode effective_mode = control_mode; - if (control_mode == AUTO && g.auto_fbw_steer) { - effective_mode = FLY_BY_WIRE_A; - } - - if (effective_mode != AUTO) { - // hold_course is only used in takeoff and landing - steer_state.hold_course_cd = -1; - } - - switch (effective_mode) - { - case AUTO: - handle_auto_mode(); - break; - - case RTL: - case LOITER: - case GUIDED: - calc_nav_roll(); - calc_nav_pitch(); - calc_throttle(); - break; - - case TRAINING: { - training_manual_roll = false; - training_manual_pitch = false; - update_load_factor(); - - // if the roll is past the set roll limit, then - // we set target roll to the limit - if (ahrs.roll_sensor >= roll_limit_cd) { - nav_roll_cd = roll_limit_cd; - } else if (ahrs.roll_sensor <= -roll_limit_cd) { - nav_roll_cd = -roll_limit_cd; - } else { - training_manual_roll = true; - nav_roll_cd = 0; - } - - // if the pitch is past the set pitch limits, then - // we set target pitch to the limit - if (ahrs.pitch_sensor >= aparm.pitch_limit_max_cd) { - nav_pitch_cd = aparm.pitch_limit_max_cd; - } else if (ahrs.pitch_sensor <= pitch_limit_min_cd) { - nav_pitch_cd = pitch_limit_min_cd; - } else { - training_manual_pitch = true; - nav_pitch_cd = 0; - } - if (fly_inverted()) { - nav_pitch_cd = -nav_pitch_cd; - } - break; - } - - case ACRO: { - // handle locked/unlocked control - if (acro_state.locked_roll) { - nav_roll_cd = acro_state.locked_roll_err; - } else { - nav_roll_cd = ahrs.roll_sensor; - } - if (acro_state.locked_pitch) { - nav_pitch_cd = acro_state.locked_pitch_cd; - } else { - nav_pitch_cd = ahrs.pitch_sensor; - } - break; - } - - case AUTOTUNE: - case FLY_BY_WIRE_A: { - // set nav_roll and nav_pitch using sticks - nav_roll_cd = channel_roll->norm_input() * roll_limit_cd; - nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd); - update_load_factor(); - float pitch_input = channel_pitch->norm_input(); - if (pitch_input > 0) { - nav_pitch_cd = pitch_input * aparm.pitch_limit_max_cd; - } else { - nav_pitch_cd = -(pitch_input * pitch_limit_min_cd); - } - adjust_nav_pitch_throttle(); - nav_pitch_cd = constrain_int32(nav_pitch_cd, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get()); - if (fly_inverted()) { - nav_pitch_cd = -nav_pitch_cd; - } - if (failsafe.ch3_failsafe && g.short_fs_action == 2) { - // FBWA failsafe glide - nav_roll_cd = 0; - nav_pitch_cd = 0; - channel_throttle->servo_out = 0; - } - if (g.fbwa_tdrag_chan > 0) { - // check for the user enabling FBWA taildrag takeoff mode - bool tdrag_mode = (hal.rcin->read(g.fbwa_tdrag_chan-1) > 1700); - if (tdrag_mode && !auto_state.fbwa_tdrag_takeoff_mode) { - if (auto_state.highest_airspeed < g.takeoff_tdrag_speed1) { - auto_state.fbwa_tdrag_takeoff_mode = true; - gcs_send_text(MAV_SEVERITY_WARNING, "FBWA tdrag mode"); - } - } - } - break; - } - - case FLY_BY_WIRE_B: - // Thanks to Yury MonZon for the altitude limit code! - nav_roll_cd = channel_roll->norm_input() * roll_limit_cd; - nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd); - update_load_factor(); - update_fbwb_speed_height(); - break; - - case CRUISE: - /* - in CRUISE mode we use the navigation code to control - roll when heading is locked. Heading becomes unlocked on - any aileron or rudder input - */ - if ((channel_roll->control_in != 0 || - rudder_input != 0)) { - cruise_state.locked_heading = false; - cruise_state.lock_timer_ms = 0; - } - - if (!cruise_state.locked_heading) { - nav_roll_cd = channel_roll->norm_input() * roll_limit_cd; - nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd); - update_load_factor(); - } else { - calc_nav_roll(); - } - update_fbwb_speed_height(); - break; - - case STABILIZE: - nav_roll_cd = 0; - nav_pitch_cd = 0; - // throttle is passthrough - break; - - case CIRCLE: - // we have no GPS installed and have lost radio contact - // or we just want to fly around in a gentle circle w/o GPS, - // holding altitude at the altitude we set when we - // switched into the mode - nav_roll_cd = roll_limit_cd / 3; - update_load_factor(); - calc_nav_pitch(); - calc_throttle(); - break; - - case MANUAL: - // servo_out is for Sim control only - // --------------------------------- - channel_roll->servo_out = channel_roll->pwm_to_angle(); - channel_pitch->servo_out = channel_pitch->pwm_to_angle(); - steering_control.steering = steering_control.rudder = channel_rudder->pwm_to_angle(); - break; - //roll: -13788.000, pitch: -13698.000, thr: 0.000, rud: -13742.000 - - case INITIALISING: - // handled elsewhere - break; - } -} - -void Plane::update_navigation() -{ - // wp_distance is in ACTUAL meters, not the *100 meters we get from the GPS - // ------------------------------------------------------------------------ - - // distance and bearing calcs only - switch(control_mode) { - case AUTO: - update_commands(); - break; - - case RTL: - if (g.rtl_autoland == 1 && - !auto_state.checked_for_autoland && - nav_controller->reached_loiter_target() && - labs(altitude_error_cm) < 1000) { - // we've reached the RTL point, see if we have a landing sequence - jump_to_landing_sequence(); - - // prevent running the expensive jump_to_landing_sequence - // on every loop - auto_state.checked_for_autoland = true; - } - else if (g.rtl_autoland == 2 && - !auto_state.checked_for_autoland) { - // Go directly to the landing sequence - jump_to_landing_sequence(); - - // prevent running the expensive jump_to_landing_sequence - // on every loop - auto_state.checked_for_autoland = true; - } - // fall through to LOITER - - case LOITER: - case GUIDED: - // allow loiter direction to be changed in flight - if (g.loiter_radius < 0) { - loiter.direction = -1; - } else { - loiter.direction = 1; - } - update_loiter(); - break; - - case CRUISE: - update_cruise(); - break; - - case MANUAL: - case STABILIZE: - case TRAINING: - case INITIALISING: - case ACRO: - case FLY_BY_WIRE_A: - case AUTOTUNE: - case FLY_BY_WIRE_B: - case CIRCLE: - // nothing to do - break; - } -} - -/* - set the flight stage - */ -void Plane::set_flight_stage(AP_SpdHgtControl::FlightStage fs) -{ - if (fs == flight_stage) { - return; - } - - switch (fs) { - case AP_SpdHgtControl::FLIGHT_LAND_APPROACH: - gcs_send_text_fmt(MAV_SEVERITY_INFO, "Landing approach start at %.1fm", (double)relative_altitude()); -#if GEOFENCE_ENABLED == ENABLED - if (g.fence_autoenable == 1) { - if (! geofence_set_enabled(false, AUTO_TOGGLED)) { - gcs_send_text(MAV_SEVERITY_NOTICE, "Disable fence failed (autodisable)"); - } else { - gcs_send_text(MAV_SEVERITY_NOTICE, "Fence disabled (autodisable)"); - } - } else if (g.fence_autoenable == 2) { - if (! geofence_set_floor_enabled(false)) { - gcs_send_text(MAV_SEVERITY_NOTICE, "Disable fence floor failed (autodisable)"); - } else { - gcs_send_text(MAV_SEVERITY_NOTICE, "Fence floor disabled (auto disable)"); - } - } -#endif - break; - - case AP_SpdHgtControl::FLIGHT_LAND_ABORT: - gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "Landing aborted via throttle. Climbing to %dm", auto_state.takeoff_altitude_rel_cm/100); - break; - - case AP_SpdHgtControl::FLIGHT_LAND_FINAL: - case AP_SpdHgtControl::FLIGHT_NORMAL: - case AP_SpdHgtControl::FLIGHT_TAKEOFF: - break; - } - - - flight_stage = fs; -} - -void Plane::update_alt() -{ - barometer.update(); - if (should_log(MASK_LOG_IMU)) { - Log_Write_Baro(); - } - - // calculate the sink rate. - float sink_rate; - Vector3f vel; - if (ahrs.get_velocity_NED(vel)) { - sink_rate = vel.z; - } else if (gps.status() >= AP_GPS::GPS_OK_FIX_3D && gps.have_vertical_velocity()) { - sink_rate = gps.velocity().z; - } else { - sink_rate = -barometer.get_climb_rate(); - } - - // low pass the sink rate to take some of the noise out - auto_state.sink_rate = 0.8f * auto_state.sink_rate + 0.2f*sink_rate; - - geofence_check(true); - - update_flight_stage(); -} - -/* - recalculate the flight_stage - */ -void Plane::update_flight_stage(void) -{ - // Update the speed & height controller states - if (auto_throttle_mode && !throttle_suppressed) { - if (control_mode==AUTO) { - if (auto_state.takeoff_complete == false) { - set_flight_stage(AP_SpdHgtControl::FLIGHT_TAKEOFF); - } else if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND) { - - if ((g.land_abort_throttle_enable && channel_throttle->control_in > 95) || - flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT){ - // abort mode is sticky, it must complete while executing NAV_LAND - set_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_ABORT); - } else if (auto_state.land_complete == true) { - set_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_FINAL); - } else if (flight_stage != AP_SpdHgtControl::FLIGHT_LAND_APPROACH) { - float path_progress = location_path_proportion(current_loc, prev_WP_loc, next_WP_loc); - bool lined_up = abs(nav_controller->bearing_error_cd()) < 1000; - bool below_prev_WP = current_loc.alt < prev_WP_loc.alt; - if ((path_progress > 0.15f && lined_up && below_prev_WP) || path_progress > 0.5f) { - set_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_APPROACH); - } else { - set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL); - } - } - - } else { - set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL); - } - } else { - set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL); - } - - SpdHgt_Controller->update_pitch_throttle(relative_target_altitude_cm(), - target_airspeed_cm, - flight_stage, - auto_state.takeoff_pitch_cd, - throttle_nudge, - tecs_hgt_afe(), - aerodynamic_load_factor); - if (should_log(MASK_LOG_TECS)) { - Log_Write_TECS_Tuning(); - } - } - - // tell AHRS the airspeed to true airspeed ratio - airspeed.set_EAS2TAS(barometer.get_EAS2TAS()); -} - - - - -#if OPTFLOW == ENABLED -// called at 50hz -void Plane::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); - Log_Write_Optflow(); - } -} -#endif - -AP_HAL_MAIN_CALLBACKS(&plane); diff --git a/ArduSubsea/Attitude.cpp b/ArduSubsea/Attitude.cpp deleted file mode 100644 index 1098ea6b08..0000000000 --- a/ArduSubsea/Attitude.cpp +++ /dev/null @@ -1,1136 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#include "Plane.h" - -/* - get a speed scaling number for control surfaces. This is applied to - PIDs to change the scaling of the PID with speed. At high speed we - move the surfaces less, and at low speeds we move them more. - */ -float Plane::get_speed_scaler(void) -{ - float aspeed, speed_scaler; - if (ahrs.airspeed_estimate(&aspeed)) { - if (aspeed > auto_state.highest_airspeed) { - auto_state.highest_airspeed = aspeed; - } - if (aspeed > 0) { - speed_scaler = g.scaling_speed / aspeed; - } else { - speed_scaler = 2.0; - } - speed_scaler = constrain_float(speed_scaler, 0.5f, 2.0f); - } else { - if (channel_throttle->servo_out > 0) { - speed_scaler = 0.5f + ((float)THROTTLE_CRUISE / channel_throttle->servo_out / 2.0f); // First order taylor expansion of square root - // Should maybe be to the 2/7 power, but we aren't goint to implement that... - }else{ - speed_scaler = 1.67f; - } - // This case is constrained tighter as we don't have real speed info - speed_scaler = constrain_float(speed_scaler, 0.6f, 1.67f); - } - return speed_scaler; -} - -/* - return true if the current settings and mode should allow for stick mixing - */ -bool Plane::stick_mixing_enabled(void) -{ - if (auto_throttle_mode) { - // we're in an auto mode. Check the stick mixing flag - if (g.stick_mixing != STICK_MIXING_DISABLED && - geofence_stickmixing() && - failsafe.state == FAILSAFE_NONE && - !rc_failsafe_active()) { - // we're in an auto mode, and haven't triggered failsafe - return true; - } else { - return false; - } - } - - if (failsafe.ch3_failsafe && g.short_fs_action == 2) { - // don't do stick mixing in FBWA glide mode - return false; - } - - // non-auto mode. Always do stick mixing - return true; -} - - -/* - this is the main roll stabilization function. It takes the - previously set nav_roll calculates roll servo_out to try to - stabilize the plane at the given roll - */ -void Plane::stabilize_roll(float speed_scaler) -{ - if (fly_inverted()) { - // we want to fly upside down. We need to cope with wrap of - // the roll_sensor interfering with wrap of nav_roll, which - // would really confuse the PID code. The easiest way to - // handle this is to ensure both go in the same direction from - // zero - nav_roll_cd += 18000; - if (ahrs.roll_sensor < 0) nav_roll_cd -= 36000; - } - - bool disable_integrator = false; - if (control_mode == STABILIZE && channel_roll->control_in != 0) { - disable_integrator = true; - } - channel_roll->servo_out = rollController.get_servo_out(nav_roll_cd - ahrs.roll_sensor, - speed_scaler, - disable_integrator); -} - -/* - this is the main pitch stabilization function. It takes the - previously set nav_pitch and calculates servo_out values to try to - stabilize the plane at the given attitude. - */ -void Plane::stabilize_pitch(float speed_scaler) -{ - int8_t force_elevator = takeoff_tail_hold(); - if (force_elevator != 0) { - // we are holding the tail down during takeoff. Just covert - // from a percentage to a -4500..4500 centidegree angle - channel_pitch->servo_out = 45*force_elevator; - return; - } - int32_t demanded_pitch = nav_pitch_cd + g.pitch_trim_cd + channel_throttle->servo_out * g.kff_throttle_to_pitch; - bool disable_integrator = false; - if (control_mode == STABILIZE && channel_pitch->control_in != 0) { - disable_integrator = true; - } - channel_pitch->servo_out = pitchController.get_servo_out(demanded_pitch - ahrs.pitch_sensor, - speed_scaler, - disable_integrator); -} - -/* - perform stick mixing on one channel - This type of stick mixing reduces the influence of the auto - controller as it increases the influence of the users stick input, - allowing the user full deflection if needed - */ -void Plane::stick_mix_channel(RC_Channel *channel, int16_t &servo_out) -{ - float ch_inf; - - ch_inf = (float)channel->radio_in - (float)channel->radio_trim; - ch_inf = fabsf(ch_inf); - ch_inf = MIN(ch_inf, 400.0f); - ch_inf = ((400.0f - ch_inf) / 400.0f); - servo_out *= ch_inf; - servo_out += channel->pwm_to_angle(); -} - -/* - this gives the user control of the aircraft in stabilization modes - */ -void Plane::stabilize_stick_mixing_direct() -{ - if (!stick_mixing_enabled() || - control_mode == ACRO || - control_mode == FLY_BY_WIRE_A || - control_mode == AUTOTUNE || - control_mode == FLY_BY_WIRE_B || - control_mode == CRUISE || - control_mode == TRAINING) { - return; - } - stick_mix_channel(channel_roll, channel_roll->servo_out); - stick_mix_channel(channel_pitch, channel_pitch->servo_out); -} - -/* - this gives the user control of the aircraft in stabilization modes - using FBW style controls - */ -void Plane::stabilize_stick_mixing_fbw() -{ - if (!stick_mixing_enabled() || - control_mode == ACRO || - control_mode == FLY_BY_WIRE_A || - control_mode == AUTOTUNE || - control_mode == FLY_BY_WIRE_B || - control_mode == CRUISE || - control_mode == TRAINING || - (control_mode == AUTO && g.auto_fbw_steer)) { - return; - } - // do FBW style stick mixing. We don't treat it linearly - // however. For inputs up to half the maximum, we use linear - // addition to the nav_roll and nav_pitch. Above that it goes - // non-linear and ends up as 2x the maximum, to ensure that - // the user can direct the plane in any direction with stick - // mixing. - float roll_input = channel_roll->norm_input(); - if (roll_input > 0.5f) { - roll_input = (3*roll_input - 1); - } else if (roll_input < -0.5f) { - roll_input = (3*roll_input + 1); - } - nav_roll_cd += roll_input * roll_limit_cd; - nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd); - - float pitch_input = channel_pitch->norm_input(); - if (fabsf(pitch_input) > 0.5f) { - pitch_input = (3*pitch_input - 1); - } - if (fly_inverted()) { - pitch_input = -pitch_input; - } - if (pitch_input > 0) { - nav_pitch_cd += pitch_input * aparm.pitch_limit_max_cd; - } else { - nav_pitch_cd += -(pitch_input * pitch_limit_min_cd); - } - nav_pitch_cd = constrain_int32(nav_pitch_cd, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get()); -} - - -/* - stabilize the yaw axis. There are 3 modes of operation: - - - hold a specific heading with ground steering - - rate controlled with ground steering - - yaw control for coordinated flight - */ -void Plane::stabilize_yaw(float speed_scaler) -{ - if (control_mode == AUTO && flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL) { - // in land final setup for ground steering - steering_control.ground_steering = true; - } else { - // otherwise use ground steering when no input control and we - // are below the GROUND_STEER_ALT - steering_control.ground_steering = (channel_roll->control_in == 0 && - fabsf(relative_altitude()) < g.ground_steer_alt); - if (control_mode == AUTO && flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH) { - // don't use ground steering on landing approach - steering_control.ground_steering = false; - } - } - - - /* - first calculate steering_control.steering for a nose or tail - wheel. - We use "course hold" mode for the rudder when either in the - final stage of landing (when the wings are help level) or when - in course hold in FBWA mode (when we are below GROUND_STEER_ALT) - */ - if ((control_mode == AUTO && flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL) || - (steer_state.hold_course_cd != -1 && steering_control.ground_steering)) { - calc_nav_yaw_course(); - } else if (steering_control.ground_steering) { - calc_nav_yaw_ground(); - } - - /* - now calculate steering_control.rudder for the rudder - */ - calc_nav_yaw_coordinated(speed_scaler); -} - - -/* - a special stabilization function for training mode - */ -void Plane::stabilize_training(float speed_scaler) -{ - if (training_manual_roll) { - channel_roll->servo_out = channel_roll->control_in; - } else { - // calculate what is needed to hold - stabilize_roll(speed_scaler); - if ((nav_roll_cd > 0 && channel_roll->control_in < channel_roll->servo_out) || - (nav_roll_cd < 0 && channel_roll->control_in > channel_roll->servo_out)) { - // allow user to get out of the roll - channel_roll->servo_out = channel_roll->control_in; - } - } - - if (training_manual_pitch) { - channel_pitch->servo_out = channel_pitch->control_in; - } else { - stabilize_pitch(speed_scaler); - if ((nav_pitch_cd > 0 && channel_pitch->control_in < channel_pitch->servo_out) || - (nav_pitch_cd < 0 && channel_pitch->control_in > channel_pitch->servo_out)) { - // allow user to get back to level - channel_pitch->servo_out = channel_pitch->control_in; - } - } - - stabilize_yaw(speed_scaler); -} - - -/* - this is the ACRO mode stabilization function. It does rate - stabilization on roll and pitch axes - */ -void Plane::stabilize_acro(float speed_scaler) -{ - float roll_rate = (channel_roll->control_in/4500.0f) * g.acro_roll_rate; - float pitch_rate = (channel_pitch->control_in/4500.0f) * g.acro_pitch_rate; - - /* - check for special roll handling near the pitch poles - */ - if (g.acro_locking && is_zero(roll_rate)) { - /* - we have no roll stick input, so we will enter "roll locked" - mode, and hold the roll we had when the stick was released - */ - if (!acro_state.locked_roll) { - acro_state.locked_roll = true; - acro_state.locked_roll_err = 0; - } else { - acro_state.locked_roll_err += ahrs.get_gyro().x * G_Dt; - } - int32_t roll_error_cd = -ToDeg(acro_state.locked_roll_err)*100; - nav_roll_cd = ahrs.roll_sensor + roll_error_cd; - // try to reduce the integrated angular error to zero. We set - // 'stabilze' to true, which disables the roll integrator - channel_roll->servo_out = rollController.get_servo_out(roll_error_cd, - speed_scaler, - true); - } else { - /* - aileron stick is non-zero, use pure rate control until the - user releases the stick - */ - acro_state.locked_roll = false; - channel_roll->servo_out = rollController.get_rate_out(roll_rate, speed_scaler); - } - - if (g.acro_locking && is_zero(pitch_rate)) { - /* - user has zero pitch stick input, so we lock pitch at the - point they release the stick - */ - if (!acro_state.locked_pitch) { - acro_state.locked_pitch = true; - acro_state.locked_pitch_cd = ahrs.pitch_sensor; - } - // try to hold the locked pitch. Note that we have the pitch - // integrator enabled, which helps with inverted flight - nav_pitch_cd = acro_state.locked_pitch_cd; - channel_pitch->servo_out = pitchController.get_servo_out(nav_pitch_cd - ahrs.pitch_sensor, - speed_scaler, - false); - } else { - /* - user has non-zero pitch input, use a pure rate controller - */ - acro_state.locked_pitch = false; - channel_pitch->servo_out = pitchController.get_rate_out(pitch_rate, speed_scaler); - } - - /* - manual rudder for now - */ - steering_control.steering = steering_control.rudder = rudder_input; -} - -/* - main stabilization function for all 3 axes - */ -void Plane::stabilize() -{ - if (control_mode == MANUAL) { - // nothing to do - return; - } - float speed_scaler = get_speed_scaler(); - - if (control_mode == TRAINING) { - stabilize_training(speed_scaler); - } else if (control_mode == ACRO) { - stabilize_acro(speed_scaler); - } else { - if (g.stick_mixing == STICK_MIXING_FBW && control_mode != STABILIZE) { - stabilize_stick_mixing_fbw(); - } - stabilize_roll(speed_scaler); - stabilize_pitch(speed_scaler); - if (g.stick_mixing == STICK_MIXING_DIRECT || control_mode == STABILIZE) { - stabilize_stick_mixing_direct(); - } - stabilize_yaw(speed_scaler); - } - - /* - see if we should zero the attitude controller integrators. - */ - if (channel_throttle->control_in == 0 && - relative_altitude_abs_cm() < 500 && - fabsf(barometer.get_climb_rate()) < 0.5f && - gps.ground_speed() < 3) { - // we are low, with no climb rate, and zero throttle, and very - // low ground speed. Zero the attitude controller - // integrators. This prevents integrator buildup pre-takeoff. - rollController.reset_I(); - pitchController.reset_I(); - yawController.reset_I(); - - // if moving very slowly also zero the steering integrator - if (gps.ground_speed() < 1) { - steerController.reset_I(); - } - } -} - - -void Plane::calc_throttle() -{ - if (aparm.throttle_cruise <= 1) { - // user has asked for zero throttle - this may be done by a - // mission which wants to turn off the engine for a parachute - // landing - channel_throttle->servo_out = 0; - return; - } - - channel_throttle->servo_out = SpdHgt_Controller->get_throttle_demand(); -} - -/***************************************** -* Calculate desired roll/pitch/yaw angles (in medium freq loop) -*****************************************/ - -/* - calculate yaw control for coordinated flight - */ -void Plane::calc_nav_yaw_coordinated(float speed_scaler) -{ - bool disable_integrator = false; - if (control_mode == STABILIZE && rudder_input != 0) { - disable_integrator = true; - } - steering_control.rudder = yawController.get_servo_out(speed_scaler, disable_integrator); - - // add in rudder mixing from roll - steering_control.rudder += channel_roll->servo_out * g.kff_rudder_mix; - steering_control.rudder += rudder_input; - steering_control.rudder = constrain_int16(steering_control.rudder, -4500, 4500); -} - -/* - calculate yaw control for ground steering with specific course - */ -void Plane::calc_nav_yaw_course(void) -{ - // holding a specific navigation course on the ground. Used in - // auto-takeoff and landing - int32_t bearing_error_cd = nav_controller->bearing_error_cd(); - steering_control.steering = steerController.get_steering_out_angle_error(bearing_error_cd); - if (stick_mixing_enabled()) { - stick_mix_channel(channel_rudder, steering_control.steering); - } - steering_control.steering = constrain_int16(steering_control.steering, -4500, 4500); -} - -/* - calculate yaw control for ground steering - */ -void Plane::calc_nav_yaw_ground(void) -{ - if (gps.ground_speed() < 1 && - channel_throttle->control_in == 0 && - flight_stage != AP_SpdHgtControl::FLIGHT_TAKEOFF && - flight_stage != AP_SpdHgtControl::FLIGHT_LAND_ABORT) { - // manual rudder control while still - steer_state.locked_course = false; - steer_state.locked_course_err = 0; - steering_control.steering = rudder_input; - return; - } - - float steer_rate = (rudder_input/4500.0f) * g.ground_steer_dps; - if (flight_stage == AP_SpdHgtControl::FLIGHT_TAKEOFF || - flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT) { - steer_rate = 0; - } - if (!is_zero(steer_rate)) { - // pilot is giving rudder input - steer_state.locked_course = false; - } else if (!steer_state.locked_course) { - // pilot has released the rudder stick or we are still - lock the course - steer_state.locked_course = true; - if (flight_stage != AP_SpdHgtControl::FLIGHT_TAKEOFF && - flight_stage != AP_SpdHgtControl::FLIGHT_LAND_ABORT) { - steer_state.locked_course_err = 0; - } - } - if (!steer_state.locked_course) { - // use a rate controller at the pilot specified rate - steering_control.steering = steerController.get_steering_out_rate(steer_rate); - } else { - // use a error controller on the summed error - int32_t yaw_error_cd = -ToDeg(steer_state.locked_course_err)*100; - steering_control.steering = steerController.get_steering_out_angle_error(yaw_error_cd); - } - steering_control.steering = constrain_int16(steering_control.steering, -4500, 4500); -} - - -/* - calculate a new nav_pitch_cd from the speed height controller - */ -void Plane::calc_nav_pitch() -{ - // Calculate the Pitch of the plane - // -------------------------------- - nav_pitch_cd = SpdHgt_Controller->get_pitch_demand(); - nav_pitch_cd = constrain_int32(nav_pitch_cd, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get()); -} - - -/* - calculate a new nav_roll_cd from the navigation controller - */ -void Plane::calc_nav_roll() -{ - nav_roll_cd = nav_controller->nav_roll_cd(); - update_load_factor(); - nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd); -} - - -/***************************************** -* Throttle slew limit -*****************************************/ -void Plane::throttle_slew_limit(int16_t last_throttle) -{ - uint8_t slewrate = aparm.throttle_slewrate; - if (control_mode==AUTO && auto_state.takeoff_complete == false && g.takeoff_throttle_slewrate != 0) { - slewrate = g.takeoff_throttle_slewrate; - } - // if slew limit rate is set to zero then do not slew limit - if (slewrate) { - // limit throttle change by the given percentage per second - float temp = slewrate * G_Dt * 0.01f * fabsf(channel_throttle->radio_max - channel_throttle->radio_min); - // allow a minimum change of 1 PWM per cycle - if (temp < 1) { - temp = 1; - } - channel_throttle->radio_out = constrain_int16(channel_throttle->radio_out, last_throttle - temp, last_throttle + temp); - } -} - -/***************************************** -Flap slew limit -*****************************************/ -void Plane::flap_slew_limit(int8_t &last_value, int8_t &new_value) -{ - uint8_t slewrate = g.flap_slewrate; - // if slew limit rate is set to zero then do not slew limit - if (slewrate) { - // limit flap change by the given percentage per second - float temp = slewrate * G_Dt; - // allow a minimum change of 1% per cycle. This means the - // slowest flaps we can do is full change over 2 seconds - if (temp < 1) { - temp = 1; - } - new_value = constrain_int16(new_value, last_value - temp, last_value + temp); - } - last_value = new_value; -} - -/* We want to suppress the throttle if we think we are on the ground and in an autopilot controlled throttle mode. - - Disable throttle if following conditions are met: - * 1 - We are in Circle mode (which we use for short term failsafe), or in FBW-B or higher - * AND - * 2 - Our reported altitude is within 10 meters of the home altitude. - * 3 - Our reported speed is under 5 meters per second. - * 4 - We are not performing a takeoff in Auto mode or takeoff speed/accel not yet reached - * OR - * 5 - Home location is not set -*/ -bool Plane::suppress_throttle(void) -{ - if (auto_throttle_mode && parachute.released()) { - // throttle always suppressed in auto-throttle modes after parachute release - throttle_suppressed = true; - return true; - } - - if (!throttle_suppressed) { - // we've previously met a condition for unsupressing the throttle - return false; - } - if (!auto_throttle_mode) { - // the user controls the throttle - throttle_suppressed = false; - return false; - } - - if (control_mode==AUTO && g.auto_fbw_steer) { - // user has throttle control - return false; - } - - bool gps_movement = (gps.status() >= AP_GPS::GPS_OK_FIX_2D && gps.ground_speed() >= 5); - - if (control_mode==AUTO && - auto_state.takeoff_complete == false) { - - uint32_t launch_duration_ms = ((int32_t)g.takeoff_throttle_delay)*100 + 2000; - if (is_flying() && - millis() - started_flying_ms > MAX(launch_duration_ms, 5000U) && // been flying >5s in any mode - adjusted_relative_altitude_cm() > 500 && // are >5m above AGL/home - labs(ahrs.pitch_sensor) < 3000 && // not high pitch, which happens when held before launch - gps_movement) { // definate gps movement - // we're already flying, do not suppress the throttle. We can get - // stuck in this condition if we reset a mission and cmd 1 is takeoff - // but we're currently flying around below the takeoff altitude - throttle_suppressed = false; - return false; - } - if (auto_takeoff_check()) { - // we're in auto takeoff - throttle_suppressed = false; - auto_state.baro_takeoff_alt = barometer.get_altitude(); - return false; - } - // keep throttle suppressed - return true; - } - - if (relative_altitude_abs_cm() >= 1000) { - // we're more than 10m from the home altitude - throttle_suppressed = false; - gcs_send_text_fmt(MAV_SEVERITY_INFO, "Throttle enabled. Altitude %.2f", - (double)(relative_altitude_abs_cm()*0.01f)); - return false; - } - - if (gps_movement) { - // if we have an airspeed sensor, then check it too, and - // require 5m/s. This prevents throttle up due to spiky GPS - // groundspeed with bad GPS reception - if ((!ahrs.airspeed_sensor_enabled()) || airspeed.get_airspeed() >= 5) { - // we're moving at more than 5 m/s - gcs_send_text_fmt(MAV_SEVERITY_INFO, "Throttle enabled. Speed %.2f airspeed %.2f", - (double)gps.ground_speed(), - (double)airspeed.get_airspeed()); - throttle_suppressed = false; - return false; - } - } - - // throttle remains suppressed - return true; -} - -/* - implement a software VTail or elevon mixer. There are 4 different mixing modes - */ -void Plane::channel_output_mixer(uint8_t mixing_type, int16_t &chan1_out, int16_t &chan2_out) -{ - int16_t c1, c2; - int16_t v1, v2; - - // first get desired elevator and rudder as -500..500 values - c1 = chan1_out - 1500; - c2 = chan2_out - 1500; - - v1 = (c1 - c2) * g.mixing_gain; - v2 = (c1 + c2) * g.mixing_gain; - - // now map to mixed output - switch (mixing_type) { - case MIXING_DISABLED: - return; - - case MIXING_UPUP: - break; - - case MIXING_UPDN: - v2 = -v2; - break; - - case MIXING_DNUP: - v1 = -v1; - break; - - case MIXING_DNDN: - v1 = -v1; - v2 = -v2; - break; - } - - // scale for a 1500 center and 900..2100 range, symmetric - v1 = constrain_int16(v1, -600, 600); - v2 = constrain_int16(v2, -600, 600); - - chan1_out = 1500 + v1; - chan2_out = 1500 + v2; -} - -/* - setup flaperon output channels - */ -void Plane::flaperon_update(int8_t flap_percent) -{ - if (!RC_Channel_aux::function_assigned(RC_Channel_aux::k_flaperon1) || - !RC_Channel_aux::function_assigned(RC_Channel_aux::k_flaperon2)) { - return; - } - int16_t ch1, ch2; - /* - flaperons are implemented as a mixer between aileron and a - percentage of flaps. Flap input can come from a manual channel - or from auto flaps. - - Use k_flaperon1 and k_flaperon2 channel trims to center servos. - Then adjust aileron trim for level flight (note that aileron trim is affected - by mixing gain). flapin_channel's trim is not used. - */ - - ch1 = channel_roll->radio_out; - // The *5 is to take a percentage to a value from -500 to 500 for the mixer - ch2 = 1500 - flap_percent * 5; - channel_output_mixer(g.flaperon_output, ch1, ch2); - RC_Channel_aux::set_radio_trimmed(RC_Channel_aux::k_flaperon1, ch1); - RC_Channel_aux::set_radio_trimmed(RC_Channel_aux::k_flaperon2, ch2); -} - -/* - setup servos for idle mode - Idle mode is used during balloon launch to keep servos still, apart - from occasional wiggle to prevent freezing up - */ -void Plane::set_servos_idle(void) -{ - RC_Channel_aux::output_ch_all(); - if (auto_state.idle_wiggle_stage == 0) { - RC_Channel::output_trim_all(); - return; - } - int16_t servo_value = 0; - // move over full range for 2 seconds - auto_state.idle_wiggle_stage += 2; - if (auto_state.idle_wiggle_stage < 50) { - servo_value = auto_state.idle_wiggle_stage * (4500 / 50); - } else if (auto_state.idle_wiggle_stage < 100) { - servo_value = (100 - auto_state.idle_wiggle_stage) * (4500 / 50); - } else if (auto_state.idle_wiggle_stage < 150) { - servo_value = (100 - auto_state.idle_wiggle_stage) * (4500 / 50); - } else if (auto_state.idle_wiggle_stage < 200) { - servo_value = (auto_state.idle_wiggle_stage-200) * (4500 / 50); - } else { - auto_state.idle_wiggle_stage = 0; - } - channel_roll->servo_out = servo_value; - channel_pitch->servo_out = servo_value; - channel_rudder->servo_out = servo_value; - channel_roll->calc_pwm(); - channel_pitch->calc_pwm(); - channel_rudder->calc_pwm(); - channel_roll->output(); - channel_pitch->output(); - channel_throttle->output(); - channel_rudder->output(); - channel_throttle->output_trim(); -} - -/* - return minimum throttle, taking account of throttle reversal - */ -uint16_t Plane::throttle_min(void) const -{ - return channel_throttle->get_reverse() ? channel_throttle->radio_max : channel_throttle->radio_min; -}; - - -/***************************************** -* Set the flight control servos based on the current calculated values -*****************************************/ -void Plane::set_servos(void) -{ - int16_t last_throttle = channel_throttle->radio_out; - - if (control_mode == AUTO && auto_state.idle_mode) { - // special handling for balloon launch - set_servos_idle(); - return; - } - - /* - see if we are doing ground steering. - */ - if (!steering_control.ground_steering) { - // we are not at an altitude for ground steering. Set the nose - // wheel to the rudder just in case the barometer has drifted - // a lot - steering_control.steering = steering_control.rudder; - } else if (!RC_Channel_aux::function_assigned(RC_Channel_aux::k_steering)) { - // we are within the ground steering altitude but don't have a - // dedicated steering channel. Set the rudder to the ground - // steering output - steering_control.rudder = steering_control.steering; - } - channel_rudder->servo_out = steering_control.rudder; - - // clear ground_steering to ensure manual control if the yaw stabilizer doesn't run - steering_control.ground_steering = false; - - RC_Channel_aux::set_servo_out(RC_Channel_aux::k_rudder, steering_control.rudder); - RC_Channel_aux::set_servo_out(RC_Channel_aux::k_steering, steering_control.steering); - - if (control_mode == MANUAL) { - // do a direct pass through of radio values - if (g.mix_mode == 0 || g.elevon_output != MIXING_DISABLED) { - channel_roll->radio_out = channel_roll->radio_in; - channel_pitch->radio_out = channel_pitch->radio_in; - } else { - channel_roll->radio_out = channel_roll->read(); - channel_pitch->radio_out = channel_pitch->read(); - } - channel_throttle->radio_out = channel_throttle->radio_in; - channel_rudder->radio_out = channel_rudder->radio_in; - - // setup extra channels. We want this to come from the - // main input channel, but using the 2nd channels dead - // zone, reverse and min/max settings. We need to use - // pwm_to_angle_dz() to ensure we don't trim the value for the - // deadzone of the main aileron channel, otherwise the 2nd - // aileron won't quite follow the first one - RC_Channel_aux::set_servo_out(RC_Channel_aux::k_aileron, channel_roll->pwm_to_angle_dz(0)); - RC_Channel_aux::set_servo_out(RC_Channel_aux::k_elevator, channel_pitch->pwm_to_angle_dz(0)); - - // this variant assumes you have the corresponding - // input channel setup in your transmitter for manual control - // of the 2nd aileron - RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_aileron_with_input); - RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_elevator_with_input); - - if (g.mix_mode == 0 && g.elevon_output == MIXING_DISABLED) { - // set any differential spoilers to follow the elevons in - // manual mode. - RC_Channel_aux::set_radio(RC_Channel_aux::k_dspoiler1, channel_roll->radio_out); - RC_Channel_aux::set_radio(RC_Channel_aux::k_dspoiler2, channel_pitch->radio_out); - } - } else { - if (g.mix_mode == 0) { - // both types of secondary aileron are slaved to the roll servo out - RC_Channel_aux::set_servo_out(RC_Channel_aux::k_aileron, channel_roll->servo_out); - RC_Channel_aux::set_servo_out(RC_Channel_aux::k_aileron_with_input, channel_roll->servo_out); - - // both types of secondary elevator are slaved to the pitch servo out - RC_Channel_aux::set_servo_out(RC_Channel_aux::k_elevator, channel_pitch->servo_out); - RC_Channel_aux::set_servo_out(RC_Channel_aux::k_elevator_with_input, channel_pitch->servo_out); - }else{ - /*Elevon mode*/ - float ch1; - float ch2; - ch1 = channel_pitch->servo_out - (BOOL_TO_SIGN(g.reverse_elevons) * channel_roll->servo_out); - ch2 = channel_pitch->servo_out + (BOOL_TO_SIGN(g.reverse_elevons) * channel_roll->servo_out); - - /* Differential Spoilers - If differential spoilers are setup, then we translate - rudder control into splitting of the two ailerons on - the side of the aircraft where we want to induce - additional drag. - */ - if (RC_Channel_aux::function_assigned(RC_Channel_aux::k_dspoiler1) && RC_Channel_aux::function_assigned(RC_Channel_aux::k_dspoiler2)) { - float ch3 = ch1; - float ch4 = ch2; - if ( BOOL_TO_SIGN(g.reverse_elevons) * channel_rudder->servo_out < 0) { - ch1 += abs(channel_rudder->servo_out); - ch3 -= abs(channel_rudder->servo_out); - } else { - ch2 += abs(channel_rudder->servo_out); - ch4 -= abs(channel_rudder->servo_out); - } - RC_Channel_aux::set_servo_out(RC_Channel_aux::k_dspoiler1, ch3); - RC_Channel_aux::set_servo_out(RC_Channel_aux::k_dspoiler2, ch4); - } - - // directly set the radio_out values for elevon mode - channel_roll->radio_out = elevon.trim1 + (BOOL_TO_SIGN(g.reverse_ch1_elevon) * (ch1 * 500.0f/ SERVO_MAX)); - channel_pitch->radio_out = elevon.trim2 + (BOOL_TO_SIGN(g.reverse_ch2_elevon) * (ch2 * 500.0f/ SERVO_MAX)); - } - - // push out the PWM values - if (g.mix_mode == 0) { - channel_roll->calc_pwm(); - channel_pitch->calc_pwm(); - } - channel_rudder->calc_pwm(); - -#if THROTTLE_OUT == 0 - channel_throttle->servo_out = 0; -#else - // convert 0 to 100% into PWM - uint8_t min_throttle = aparm.throttle_min.get(); - uint8_t max_throttle = aparm.throttle_max.get(); - if (control_mode == AUTO && flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL) { - min_throttle = 0; - } - if (control_mode == AUTO && - (flight_stage == AP_SpdHgtControl::FLIGHT_TAKEOFF || flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT)) { - if(aparm.takeoff_throttle_max != 0) { - max_throttle = aparm.takeoff_throttle_max; - } else { - max_throttle = aparm.throttle_max; - } - } - channel_throttle->servo_out = constrain_int16(channel_throttle->servo_out, - min_throttle, - max_throttle); - - if (!hal.util->get_soft_armed()) { - channel_throttle->servo_out = 0; - channel_throttle->calc_pwm(); - } else if (suppress_throttle()) { - // throttle is suppressed in auto mode - channel_throttle->servo_out = 0; - if (g.throttle_suppress_manual) { - // manual pass through of throttle while throttle is suppressed - channel_throttle->radio_out = channel_throttle->radio_in; - } else { - channel_throttle->calc_pwm(); - } - } else if (g.throttle_passthru_stabilize && - (control_mode == STABILIZE || - control_mode == TRAINING || - control_mode == ACRO || - control_mode == FLY_BY_WIRE_A || - control_mode == AUTOTUNE)) { - // manual pass through of throttle while in FBWA or - // STABILIZE mode with THR_PASS_STAB set - channel_throttle->radio_out = channel_throttle->radio_in; - } else if (control_mode == GUIDED && - guided_throttle_passthru) { - // manual pass through of throttle while in GUIDED - channel_throttle->radio_out = channel_throttle->radio_in; - } else { - // normal throttle calculation based on servo_out - channel_throttle->calc_pwm(); - } -#endif - } - - // Auto flap deployment - int8_t auto_flap_percent = 0; - int8_t manual_flap_percent = 0; - static int8_t last_auto_flap; - static int8_t last_manual_flap; - - // work out any manual flap input - RC_Channel *flapin = RC_Channel::rc_channel(g.flapin_channel-1); - if (flapin != NULL && !failsafe.ch3_failsafe && failsafe.ch3_counter == 0) { - flapin->input(); - manual_flap_percent = flapin->percent_input(); - } - - if (auto_throttle_mode) { - int16_t flapSpeedSource = 0; - if (ahrs.airspeed_sensor_enabled()) { - flapSpeedSource = target_airspeed_cm * 0.01f; - } else { - flapSpeedSource = aparm.throttle_cruise; - } - if (g.flap_2_speed != 0 && flapSpeedSource <= g.flap_2_speed) { - auto_flap_percent = g.flap_2_percent; - } else if ( g.flap_1_speed != 0 && flapSpeedSource <= g.flap_1_speed) { - auto_flap_percent = g.flap_1_percent; - } //else flaps stay at default zero deflection - - /* - special flap levels for takeoff and landing. This works - better than speed based flaps as it leads to less - possibility of oscillation - */ - if (control_mode == AUTO) { - switch (flight_stage) { - case AP_SpdHgtControl::FLIGHT_TAKEOFF: - case AP_SpdHgtControl::FLIGHT_LAND_ABORT: - if (g.takeoff_flap_percent != 0) { - auto_flap_percent = g.takeoff_flap_percent; - } - break; - case AP_SpdHgtControl::FLIGHT_LAND_APPROACH: - case AP_SpdHgtControl::FLIGHT_LAND_FINAL: - if (g.land_flap_percent != 0) { - auto_flap_percent = g.land_flap_percent; - } - break; - default: - break; - } - } - } - - // manual flap input overrides auto flap input - if (abs(manual_flap_percent) > auto_flap_percent) { - auto_flap_percent = manual_flap_percent; - } - - flap_slew_limit(last_auto_flap, auto_flap_percent); - flap_slew_limit(last_manual_flap, manual_flap_percent); - - RC_Channel_aux::set_servo_out(RC_Channel_aux::k_flap_auto, auto_flap_percent); - RC_Channel_aux::set_servo_out(RC_Channel_aux::k_flap, manual_flap_percent); - - if (control_mode >= FLY_BY_WIRE_B) { - /* only do throttle slew limiting in modes where throttle - * control is automatic */ - throttle_slew_limit(last_throttle); - } - - if (control_mode == TRAINING) { - // copy rudder in training mode - channel_rudder->radio_out = channel_rudder->radio_in; - } - - if (g.flaperon_output != MIXING_DISABLED && g.elevon_output == MIXING_DISABLED && g.mix_mode == 0) { - flaperon_update(auto_flap_percent); - } - if (g.vtail_output != MIXING_DISABLED) { - channel_output_mixer(g.vtail_output, channel_pitch->radio_out, channel_rudder->radio_out); - } else if (g.elevon_output != MIXING_DISABLED) { - channel_output_mixer(g.elevon_output, channel_pitch->radio_out, channel_roll->radio_out); - } - - if (!arming.is_armed()) { - //Some ESCs get noisy (beep error msgs) if PWM == 0. - //This little segment aims to avoid this. - switch (arming.arming_required()) { - case AP_Arming::NO: - //keep existing behavior: do nothing to radio_out - //(don't disarm throttle channel even if AP_Arming class is) - break; - - case AP_Arming::YES_ZERO_PWM: - channel_throttle->radio_out = 0; - break; - - case AP_Arming::YES_MIN_PWM: - default: - channel_throttle->radio_out = throttle_min(); - break; - } - } - -#if OBC_FAILSAFE == ENABLED - // this is to allow the failsafe module to deliberately crash - // the plane. Only used in extreme circumstances to meet the - // OBC rules - obc.check_crash_plane(); -#endif - -#if HIL_SUPPORT - if (g.hil_mode == 1) { - // get the servos to the GCS immediately for HIL - if (comm_get_txspace(MAVLINK_COMM_0) >= - MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) { - send_servo_out(MAVLINK_COMM_0); - } - if (!g.hil_servos) { - return; - } - } -#endif - - // send values to the PWM timers for output - // ---------------------------------------- - if (g.rudder_only == 0) { - // when we RUDDER_ONLY mode we don't send the channel_roll - // output and instead rely on KFF_RDDRMIX. That allows the yaw - // damper to operate. - channel_roll->output(); - } - channel_pitch->output(); - channel_throttle->output(); - channel_rudder->output(); - RC_Channel_aux::output_ch_all(); -} - -void Plane::demo_servos(uint8_t i) -{ - while(i > 0) { - gcs_send_text(MAV_SEVERITY_INFO,"Demo servos"); - demoing_servos = true; - servo_write(1, 1400); - hal.scheduler->delay(400); - servo_write(1, 1600); - hal.scheduler->delay(200); - servo_write(1, 1500); - demoing_servos = false; - hal.scheduler->delay(400); - i--; - } -} - -/* - adjust nav_pitch_cd for STAB_PITCH_DOWN_CD. This is used to make - keeping up good airspeed in FBWA mode easier, as the plane will - automatically pitch down a little when at low throttle. It makes - FBWA landings without stalling much easier. - */ -void Plane::adjust_nav_pitch_throttle(void) -{ - uint8_t throttle = throttle_percentage(); - if (throttle < aparm.throttle_cruise) { - float p = (aparm.throttle_cruise - throttle) / (float)aparm.throttle_cruise; - nav_pitch_cd -= g.stab_pitch_down * 100.0f * p; - } -} - - -/* - calculate a new aerodynamic_load_factor and limit nav_roll_cd to - ensure that the load factor does not take us below the sustainable - airspeed - */ -void Plane::update_load_factor(void) -{ - float demanded_roll = fabsf(nav_roll_cd*0.01f); - if (demanded_roll > 85) { - // limit to 85 degrees to prevent numerical errors - demanded_roll = 85; - } - aerodynamic_load_factor = 1.0f / safe_sqrt(cosf(radians(demanded_roll))); - - if (!aparm.stall_prevention) { - // stall prevention is disabled - return; - } - if (fly_inverted()) { - // no roll limits when inverted - return; - } - - float max_load_factor = smoothed_airspeed / aparm.airspeed_min; - if (max_load_factor <= 1) { - // our airspeed is below the minimum airspeed. Limit roll to - // 25 degrees - nav_roll_cd = constrain_int32(nav_roll_cd, -2500, 2500); - roll_limit_cd = constrain_int32(roll_limit_cd, -2500, 2500); - } else if (max_load_factor < aerodynamic_load_factor) { - // the demanded nav_roll would take us past the aerodymamic - // load limit. Limit our roll to a bank angle that will keep - // the load within what the airframe can handle. We always - // allow at least 25 degrees of roll however, to ensure the - // aircraft can be maneuvered with a bad airspeed estimate. At - // 25 degrees the load factor is 1.1 (10%) - int32_t roll_limit = degrees(acosf(sq(1.0f / max_load_factor)))*100; - if (roll_limit < 2500) { - roll_limit = 2500; - } - nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit, roll_limit); - roll_limit_cd = constrain_int32(roll_limit_cd, -roll_limit, roll_limit); - } -} diff --git a/ArduSubsea/GCS_Mavlink.cpp b/ArduSubsea/GCS_Mavlink.cpp deleted file mode 100644 index f3124a1318..0000000000 --- a/ArduSubsea/GCS_Mavlink.cpp +++ /dev/null @@ -1,2061 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#include "Plane.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 | MAV_SYS_STATUS_SENSOR_RC_RECEIVER) - -void Plane::send_heartbeat(mavlink_channel_t chan) -{ - uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; - uint8_t system_status; - uint32_t custom_mode = control_mode; - - if (failsafe.state != FAILSAFE_NONE) { - system_status = MAV_STATE_CRITICAL; - } else if (plane.crash_state.is_crashed) { - system_status = MAV_STATE_EMERGENCY; - } else if (is_flying()) { - system_status = MAV_STATE_ACTIVE; - } else { - system_status = MAV_STATE_STANDBY; - } - - // 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 - switch (control_mode) { - case MANUAL: - case TRAINING: - case ACRO: - base_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; - break; - case STABILIZE: - case FLY_BY_WIRE_A: - case AUTOTUNE: - case FLY_BY_WIRE_B: - case CRUISE: - base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED; - break; - case AUTO: - case RTL: - case LOITER: - case GUIDED: - case CIRCLE: - base_mode = MAV_MODE_FLAG_GUIDED_ENABLED | - MAV_MODE_FLAG_STABILIZE_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; - case INITIALISING: - system_status = MAV_STATE_CALIBRATING; - break; - } - - if (!training_manual_pitch || !training_manual_roll) { - base_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED; - } - - if (control_mode != MANUAL && control_mode != INITIALISING) { - // stabiliser of some form is enabled - base_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED; - } - - if (g.stick_mixing != STICK_MIXING_DISABLED && control_mode != INITIALISING) { - // 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_SUPPORT - if (g.hil_mode == 1) { - base_mode |= MAV_MODE_FLAG_HIL_ENABLED; - } -#endif - - // we are armed if we are not initialising - if (control_mode != INITIALISING && hal.util->get_soft_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, - MAV_TYPE_FIXED_WING, - MAV_AUTOPILOT_ARDUPILOTMEGA, - base_mode, - custom_mode, - system_status); -} - -void Plane::send_attitude(mavlink_channel_t chan) -{ - const Vector3f &omega = ahrs.get_gyro(); - mavlink_msg_attitude_send( - chan, - millis(), - ahrs.roll, - ahrs.pitch - radians(g.pitch_trim_cd*0.01f), - ahrs.yaw, - omega.x, - omega.y, - omega.z); -} - -#if GEOFENCE_ENABLED == ENABLED -void Plane::send_fence_status(mavlink_channel_t chan) -{ - geofence_send_status(chan); -} -#endif - - -void Plane::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 (airspeed.enabled()) { - control_sensors_present |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE; - } - 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 (geofence_present()) { - control_sensors_present |= MAV_SYS_STATUS_GEOFENCE; - } - - // all present sensors enabled by default except rate control, attitude stabilization, yaw, altitude, position control, geofence and motor output which we will set individually - control_sensors_enabled = control_sensors_present & (~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_GEOFENCE); - - if (airspeed.enabled() && airspeed.use()) { - control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE; - } - - if (geofence_enabled()) { - control_sensors_enabled |= MAV_SYS_STATUS_GEOFENCE; - } - - switch (control_mode) { - case MANUAL: - break; - - case ACRO: - control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control - break; - - case STABILIZE: - case FLY_BY_WIRE_A: - case AUTOTUNE: - control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control - control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION; // attitude stabilisation - break; - - case FLY_BY_WIRE_B: - case CRUISE: - control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control - control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION; // attitude stabilisation - break; - - case TRAINING: - if (!training_manual_roll || !training_manual_pitch) { - control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control - control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION; // attitude stabilisation - } - break; - - case AUTO: - case RTL: - case LOITER: - case GUIDED: - case CIRCLE: - control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control - control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION; // attitude stabilisation - control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_YAW_POSITION; // yaw position - control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL; // altitude control - control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; // X/Y position control - break; - - case INITIALISING: - 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: all present sensors healthy except baro, 3D_MAG, GPS, DIFFERNTIAL_PRESSURE. GEOFENCE always defaults to healthy. - 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_DIFFERENTIAL_PRESSURE); - control_sensors_health |= MAV_SYS_STATUS_GEOFENCE; - - if (ahrs.initialised() && !ahrs.healthy()) { - // AHRS subsystem is unhealthy - control_sensors_health &= ~MAV_SYS_STATUS_AHRS; - } - if (ahrs.have_inertial_nav() && !ins.accel_calibrated_ok_all()) { - // trying to use EKF without properly calibrated accelerometers - control_sensors_health &= ~MAV_SYS_STATUS_AHRS; - } - - if (barometer.all_healthy()) { - control_sensors_health |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; - } - if (g.compass_enabled && compass.healthy(0) && ahrs.use_compass()) { - control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG; - } - if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) { - 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 (!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 (airspeed.healthy()) { - control_sensors_health |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE; - } -#if GEOFENCE_ENABLED - if (geofence_breached()) { - control_sensors_health &= ~MAV_SYS_STATUS_GEOFENCE; - } -#endif - - if (millis() - failsafe.last_valid_rc_ms < 200) { - control_sensors_health |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER; - } else { - control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_RC_RECEIVER; - } - - 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 - switch (terrain.status()) { - case AP_Terrain::TerrainStatusDisabled: - break; - case AP_Terrain::TerrainStatusUnhealthy: - 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 RANGEFINDER_ENABLED == ENABLED - if (rangefinder.num_sensors() > 0) { - control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; - if (g.rangefinder_landing) { - control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; - } - if (rangefinder.has_data()) { - control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; - } - } -#endif - - if (AP_Notify::flags.initialising) { - // 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(20000) * 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 Plane::send_location(mavlink_channel_t chan) -{ - uint32_t fix_time_ms; - // 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_ms = gps.last_fix_time_ms(); - } else { - fix_time_ms = millis(); - } - const Vector3f &vel = gps.velocity(); - mavlink_msg_global_position_int_send( - chan, - fix_time_ms, - current_loc.lat, // in 1E7 degrees - current_loc.lng, // in 1E7 degrees - current_loc.alt * 10UL, // millimeters above sea level - relative_altitude() * 1.0e3f, // millimeters above ground - vel.x * 100, // X speed cm/s (+ve North) - vel.y * 100, // Y speed cm/s (+ve East) - vel.z * -100, // Z speed cm/s (+ve up) - ahrs.yaw_sensor); -} - -void Plane::send_nav_controller_output(mavlink_channel_t chan) -{ - mavlink_msg_nav_controller_output_send( - chan, - nav_roll_cd * 0.01f, - nav_pitch_cd * 0.01f, - nav_controller->nav_bearing_cd() * 0.01f, - nav_controller->target_bearing_cd() * 0.01f, - auto_state.wp_distance, - altitude_error_cm * 0.01f, - airspeed_error_cm, - nav_controller->crosstrack_error()); -} - - -void Plane::send_servo_out(mavlink_channel_t chan) -{ - // normalized values scaled to -10000 to 10000 - // This is used for HIL. Do not change without discussing with - // HIL maintainers - mavlink_msg_rc_channels_scaled_send( - chan, - millis(), - 0, // port 0 - 10000 * channel_roll->norm_output() * (channel_roll->get_reverse()?-1:1), - 10000 * channel_pitch->norm_output() * (channel_pitch->get_reverse()?-1:1), - 10000 * channel_throttle->norm_output() * (channel_throttle->get_reverse()?-1:1), - 10000 * channel_rudder->norm_output() * (channel_rudder->get_reverse()?-1:1), - 0, - 0, - 0, - 0, - receiver_rssi); -} - -void Plane::send_radio_out(mavlink_channel_t chan) -{ -#if HIL_SUPPORT - if (g.hil_mode==1 && !g.hil_servos) { - mavlink_msg_servo_output_raw_send( - chan, - micros(), - 0, // port - RC_Channel::rc_channel(0)->radio_out, - RC_Channel::rc_channel(1)->radio_out, - RC_Channel::rc_channel(2)->radio_out, - RC_Channel::rc_channel(3)->radio_out, - RC_Channel::rc_channel(4)->radio_out, - RC_Channel::rc_channel(5)->radio_out, - RC_Channel::rc_channel(6)->radio_out, - RC_Channel::rc_channel(7)->radio_out); - return; - } -#endif - 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 Plane::send_vfr_hud(mavlink_channel_t chan) -{ - float aspeed; - if (airspeed.enabled()) { - aspeed = airspeed.get_airspeed(); - } else if (!ahrs.airspeed_estimate(&aspeed)) { - aspeed = 0; - } - mavlink_msg_vfr_hud_send( - chan, - aspeed, - gps.ground_speed(), - (ahrs.yaw_sensor / 100) % 360, - throttle_percentage(), - current_loc.alt / 100.0f, - barometer.get_climb_rate()); -} - -/* - keep last HIL_STATE message to allow sending SIM_STATE - */ -#if HIL_SUPPORT -static mavlink_hil_state_t last_hil_state; -#endif - -// report simulator state -void Plane::send_simstate(mavlink_channel_t chan) -{ -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL - sitl.simstate_send(chan); -#elif HIL_SUPPORT - if (g.hil_mode == 1) { - mavlink_msg_simstate_send(chan, - last_hil_state.roll, - last_hil_state.pitch, - last_hil_state.yaw, - last_hil_state.xacc*0.001f*GRAVITY_MSS, - last_hil_state.yacc*0.001f*GRAVITY_MSS, - last_hil_state.zacc*0.001f*GRAVITY_MSS, - last_hil_state.rollspeed, - last_hil_state.pitchspeed, - last_hil_state.yawspeed, - last_hil_state.lat, - last_hil_state.lon); - } -#endif -} - -void Plane::send_hwstatus(mavlink_channel_t chan) -{ - mavlink_msg_hwstatus_send( - chan, - hal.analogin->board_voltage()*1000, - hal.i2c->lockup_count()); -} - -void Plane::send_wind(mavlink_channel_t chan) -{ - Vector3f wind = ahrs.wind_estimate(); - mavlink_msg_wind_send( - chan, - degrees(atan2f(-wind.y, -wind.x)), // use negative, to give - // direction wind is coming from - wind.length(), - wind.z); -} - -/* - send RPM packet - */ -void NOINLINE Plane::send_rpm(mavlink_channel_t chan) -{ - if (rpm_sensor.healthy(0) || rpm_sensor.healthy(1)) { - mavlink_msg_rpm_send( - chan, - rpm_sensor.get_rpm(0), - rpm_sensor.get_rpm(1)); - } -} - -/* - send PID tuning message - */ -void Plane::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 = rollController.get_pid_info(); - mavlink_msg_pid_tuning_send(chan, PID_TUNING_ROLL, - pid_info.desired, - degrees(gyro.x), - pid_info.FF, - pid_info.P, - pid_info.I, - pid_info.D); - if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) { - return; - } - } - if (g.gcs_pid_mask & 2) { - const DataFlash_Class::PID_Info &pid_info = pitchController.get_pid_info(); - mavlink_msg_pid_tuning_send(chan, PID_TUNING_PITCH, - pid_info.desired, - degrees(gyro.y), - pid_info.FF, - pid_info.P, - pid_info.I, - pid_info.D); - if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) { - return; - } - } - if (g.gcs_pid_mask & 4) { - const DataFlash_Class::PID_Info &pid_info = yawController.get_pid_info(); - mavlink_msg_pid_tuning_send(chan, PID_TUNING_YAW, - pid_info.desired, - degrees(gyro.z), - pid_info.FF, - pid_info.P, - pid_info.I, - pid_info.D); - if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) { - return; - } - } - if (g.gcs_pid_mask & 8) { - const DataFlash_Class::PID_Info &pid_info = steerController.get_pid_info(); - mavlink_msg_pid_tuning_send(chan, PID_TUNING_STEER, - pid_info.desired, - degrees(gyro.z), - pid_info.FF, - pid_info.P, - pid_info.I, - pid_info.D); - if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) { - return; - } - } -} - -void Plane::send_rangefinder(mavlink_channel_t chan) -{ -#if RANGEFINDER_ENABLED == ENABLED - if (!rangefinder.has_data()) { - // no sonar to report - return; - } - mavlink_msg_rangefinder_send( - chan, - rangefinder.distance_cm() * 0.01f, - rangefinder.voltage_mv()*0.001f); -#endif -} - -void Plane::send_current_waypoint(mavlink_channel_t chan) -{ - mavlink_msg_mission_current_send(chan, mission.get_current_nav_index()); -} - -void Plane::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 Plane::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 (plane.telemetry_delayed(chan)) { - return false; - } - - // if we don't have at least 1ms 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 (!plane.in_mavlink_delay && plane.scheduler.time_available_usec() < 1200) { - plane.gcs_out_of_time = true; - return false; - } - - switch (id) { - case MSG_HEARTBEAT: - CHECK_PAYLOAD_SIZE(HEARTBEAT); - plane.gcs[chan-MAVLINK_COMM_0].last_heartbeat_time = AP_HAL::millis(); - plane.send_heartbeat(chan); - return true; - - case MSG_EXTENDED_STATUS1: - CHECK_PAYLOAD_SIZE(SYS_STATUS); - plane.send_extended_status1(chan); - CHECK_PAYLOAD_SIZE2(POWER_STATUS); - plane.gcs[chan-MAVLINK_COMM_0].send_power_status(); - break; - - case MSG_EXTENDED_STATUS2: - CHECK_PAYLOAD_SIZE(MEMINFO); - plane.gcs[chan-MAVLINK_COMM_0].send_meminfo(); - break; - - case MSG_ATTITUDE: - CHECK_PAYLOAD_SIZE(ATTITUDE); - plane.send_attitude(chan); - break; - - case MSG_LOCATION: - CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT); - plane.send_location(chan); - break; - - case MSG_LOCAL_POSITION: - CHECK_PAYLOAD_SIZE(LOCAL_POSITION_NED); - send_local_position(plane.ahrs); - break; - - case MSG_NAV_CONTROLLER_OUTPUT: - if (plane.control_mode != MANUAL) { - CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT); - plane.send_nav_controller_output(chan); - } - break; - - case MSG_GPS_RAW: - CHECK_PAYLOAD_SIZE(GPS_RAW_INT); - plane.gcs[chan-MAVLINK_COMM_0].send_gps_raw(plane.gps); - break; - - case MSG_SYSTEM_TIME: - CHECK_PAYLOAD_SIZE(SYSTEM_TIME); - plane.gcs[chan-MAVLINK_COMM_0].send_system_time(plane.gps); - break; - - case MSG_SERVO_OUT: -#if HIL_SUPPORT - if (plane.g.hil_mode == 1) { - CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED); - plane.send_servo_out(chan); - } -#endif - break; - - case MSG_RADIO_IN: - CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW); - plane.gcs[chan-MAVLINK_COMM_0].send_radio_in(plane.receiver_rssi); - break; - - case MSG_RADIO_OUT: - CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW); - plane.send_radio_out(chan); - break; - - case MSG_VFR_HUD: - CHECK_PAYLOAD_SIZE(VFR_HUD); - plane.send_vfr_hud(chan); - break; - - case MSG_RAW_IMU1: - CHECK_PAYLOAD_SIZE(RAW_IMU); - plane.gcs[chan-MAVLINK_COMM_0].send_raw_imu(plane.ins, plane.compass); - break; - - case MSG_RAW_IMU2: - CHECK_PAYLOAD_SIZE(SCALED_PRESSURE); - plane.gcs[chan-MAVLINK_COMM_0].send_scaled_pressure(plane.barometer); - break; - - case MSG_RAW_IMU3: - CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS); - plane.gcs[chan-MAVLINK_COMM_0].send_sensor_offsets(plane.ins, plane.compass, plane.barometer); - break; - - case MSG_CURRENT_WAYPOINT: - CHECK_PAYLOAD_SIZE(MISSION_CURRENT); - plane.send_current_waypoint(chan); - break; - - case MSG_NEXT_PARAM: - CHECK_PAYLOAD_SIZE(PARAM_VALUE); - plane.gcs[chan-MAVLINK_COMM_0].queued_param_send(); - break; - - case MSG_NEXT_WAYPOINT: - CHECK_PAYLOAD_SIZE(MISSION_REQUEST); - plane.gcs[chan-MAVLINK_COMM_0].queued_waypoint_send(); - break; - - case MSG_STATUSTEXT: - CHECK_PAYLOAD_SIZE(STATUSTEXT); - plane.send_statustext(chan); - break; - -#if GEOFENCE_ENABLED == ENABLED - case MSG_FENCE_STATUS: - CHECK_PAYLOAD_SIZE(FENCE_STATUS); - plane.send_fence_status(chan); - break; -#endif - - case MSG_AHRS: - CHECK_PAYLOAD_SIZE(AHRS); - plane.gcs[chan-MAVLINK_COMM_0].send_ahrs(plane.ahrs); - break; - - case MSG_SIMSTATE: - CHECK_PAYLOAD_SIZE(SIMSTATE); - plane.send_simstate(chan); - CHECK_PAYLOAD_SIZE2(AHRS2); - plane.gcs[chan-MAVLINK_COMM_0].send_ahrs2(plane.ahrs); - break; - - case MSG_HWSTATUS: - CHECK_PAYLOAD_SIZE(HWSTATUS); - plane.send_hwstatus(chan); - break; - - case MSG_RANGEFINDER: - CHECK_PAYLOAD_SIZE(RANGEFINDER); - plane.send_rangefinder(chan); - break; - - case MSG_TERRAIN: -#if AP_TERRAIN_AVAILABLE - CHECK_PAYLOAD_SIZE(TERRAIN_REQUEST); - plane.terrain.send_request(chan); -#endif - break; - - case MSG_CAMERA_FEEDBACK: -#if CAMERA == ENABLED - CHECK_PAYLOAD_SIZE(CAMERA_FEEDBACK); - plane.camera.send_feedback(chan, plane.gps, plane.ahrs, plane.current_loc); -#endif - break; - - case MSG_BATTERY2: - CHECK_PAYLOAD_SIZE(BATTERY2); - plane.gcs[chan-MAVLINK_COMM_0].send_battery2(plane.battery); - break; - - case MSG_WIND: - CHECK_PAYLOAD_SIZE(WIND); - plane.send_wind(chan); - break; - - case MSG_MOUNT_STATUS: -#if MOUNT == ENABLED - CHECK_PAYLOAD_SIZE(MOUNT_STATUS); - plane.camera_mount.status_msg(chan); -#endif // MOUNT == ENABLED - break; - - case MSG_OPTICAL_FLOW: -#if OPTFLOW == ENABLED - CHECK_PAYLOAD_SIZE(OPTICAL_FLOW); - plane.gcs[chan-MAVLINK_COMM_0].send_opticalflow(plane.ahrs, plane.optflow); -#endif - break; - - case MSG_EKF_STATUS_REPORT: -#if AP_AHRS_NAVEKF_AVAILABLE - CHECK_PAYLOAD_SIZE(EKF_STATUS_REPORT); - plane.ahrs.send_ekf_status_report(chan); -#endif - break; - - case MSG_GIMBAL_REPORT: -#if MOUNT == ENABLED - CHECK_PAYLOAD_SIZE(GIMBAL_REPORT); - plane.camera_mount.send_gimbal_report(chan); -#endif - break; - - case MSG_RETRY_DEFERRED: - break; // just here to prevent a warning - - case MSG_LIMITS_STATUS: - // unused - break; - - case MSG_PID_TUNING: - CHECK_PAYLOAD_SIZE(PID_TUNING); - plane.send_pid_tuning(chan); - break; - - case MSG_VIBRATION: - CHECK_PAYLOAD_SIZE(VIBRATION); - send_vibration(plane.ins); - break; - - case MSG_RPM: - CHECK_PAYLOAD_SIZE(RPM); - plane.send_rpm(chan); - 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_MAG_CAL_PROGRESS: - CHECK_PAYLOAD_SIZE(MAG_CAL_PROGRESS); - plane.compass.send_mag_cal_progress(chan); - break; - - case MSG_MAG_CAL_REPORT: - CHECK_PAYLOAD_SIZE(MAG_CAL_REPORT); - plane.compass.send_mag_cal_report(chan); - break; - } - return true; -} - - -/* - default stream rates to 1Hz - */ -const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = { - // @Param: RAW_SENS - // @DisplayName: Raw sensor stream rate - // @Description: Raw sensor stream rate to ground station - // @Units: Hz - // @Range: 0 10 - // @Increment: 1 - // @User: Advanced - AP_GROUPINFO("RAW_SENS", 0, GCS_MAVLINK, streamRates[0], 1), - - // @Param: EXT_STAT - // @DisplayName: Extended status stream rate to ground station - // @Description: Extended status stream rate to ground station - // @Units: Hz - // @Range: 0 10 - // @Increment: 1 - // @User: Advanced - AP_GROUPINFO("EXT_STAT", 1, GCS_MAVLINK, streamRates[1], 1), - - // @Param: RC_CHAN - // @DisplayName: RC Channel stream rate to ground station - // @Description: RC Channel stream rate to ground station - // @Units: Hz - // @Range: 0 10 - // @Increment: 1 - // @User: Advanced - AP_GROUPINFO("RC_CHAN", 2, GCS_MAVLINK, streamRates[2], 1), - - // @Param: RAW_CTRL - // @DisplayName: Raw Control stream rate to ground station - // @Description: Raw Control stream rate to ground station - // @Units: Hz - // @Range: 0 10 - // @Increment: 1 - // @User: Advanced - AP_GROUPINFO("RAW_CTRL", 3, GCS_MAVLINK, streamRates[3], 1), - - // @Param: POSITION - // @DisplayName: Position stream rate to ground station - // @Description: Position stream rate to ground station - // @Units: Hz - // @Range: 0 10 - // @Increment: 1 - // @User: Advanced - AP_GROUPINFO("POSITION", 4, GCS_MAVLINK, streamRates[4], 1), - - // @Param: EXTRA1 - // @DisplayName: Extra data type 1 stream rate to ground station - // @Description: Extra data type 1 stream rate to ground station - // @Units: Hz - // @Range: 0 10 - // @Increment: 1 - // @User: Advanced - AP_GROUPINFO("EXTRA1", 5, GCS_MAVLINK, streamRates[5], 1), - - // @Param: EXTRA2 - // @DisplayName: Extra data type 2 stream rate to ground station - // @Description: Extra data type 2 stream rate to ground station - // @Units: Hz - // @Range: 0 10 - // @Increment: 1 - // @User: Advanced - AP_GROUPINFO("EXTRA2", 6, GCS_MAVLINK, streamRates[6], 1), - - // @Param: EXTRA3 - // @DisplayName: Extra data type 3 stream rate to ground station - // @Description: Extra data type 3 stream rate to ground station - // @Units: Hz - // @Range: 0 10 - // @Increment: 1 - // @User: Advanced - AP_GROUPINFO("EXTRA3", 7, GCS_MAVLINK, streamRates[7], 1), - - // @Param: PARAMS - // @DisplayName: Parameter stream rate to ground station - // @Description: Parameter stream rate to ground station - // @Units: Hz - // @Range: 0 10 - // @Increment: 1 - // @User: Advanced - AP_GROUPINFO("PARAMS", 8, GCS_MAVLINK, streamRates[8], 10), - 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) -{ - plane.gcs_out_of_time = false; - - if (!plane.in_mavlink_delay) { - handle_log_send(plane.DataFlash); - } - - 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); - } - } - - if (plane.gcs_out_of_time) return; - - if (plane.in_mavlink_delay) { -#if HIL_SUPPORT - if (plane.g.hil_mode == 1) { - // in HIL we need to keep sending servo values to ensure - // the simulator doesn't pause, otherwise our sensor - // calibration could stall - if (stream_trigger(STREAM_RAW_CONTROLLER)) { - send_message(MSG_SERVO_OUT); - } - if (stream_trigger(STREAM_RC_CHANNELS)) { - send_message(MSG_RADIO_OUT); - } - } -#endif - // don't send any other stream types while in the delay callback - return; - } - - if (plane.gcs_out_of_time) return; - - if (stream_trigger(STREAM_RAW_SENSORS)) { - send_message(MSG_RAW_IMU1); - send_message(MSG_RAW_IMU2); - send_message(MSG_RAW_IMU3); - } - - if (plane.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_FENCE_STATUS); - } - - if (plane.gcs_out_of_time) return; - - if (stream_trigger(STREAM_POSITION)) { - // sent with GPS read - send_message(MSG_LOCATION); - send_message(MSG_LOCAL_POSITION); - } - - if (plane.gcs_out_of_time) return; - - if (stream_trigger(STREAM_RAW_CONTROLLER)) { - send_message(MSG_SERVO_OUT); - } - - if (plane.gcs_out_of_time) return; - - if (stream_trigger(STREAM_RC_CHANNELS)) { - send_message(MSG_RADIO_OUT); - send_message(MSG_RADIO_IN); - } - - if (plane.gcs_out_of_time) return; - - if (stream_trigger(STREAM_EXTRA1)) { - send_message(MSG_ATTITUDE); - send_message(MSG_SIMSTATE); - send_message(MSG_RPM); - if (plane.control_mode != MANUAL) { - send_message(MSG_PID_TUNING); - } - } - - if (plane.gcs_out_of_time) return; - - if (stream_trigger(STREAM_EXTRA2)) { - send_message(MSG_VFR_HUD); - } - - if (plane.gcs_out_of_time) return; - - if (stream_trigger(STREAM_EXTRA3)) { - send_message(MSG_AHRS); - send_message(MSG_HWSTATUS); - send_message(MSG_WIND); - send_message(MSG_RANGEFINDER); - send_message(MSG_SYSTEM_TIME); -#if AP_TERRAIN_AVAILABLE - send_message(MSG_TERRAIN); -#endif - send_message(MSG_MAG_CAL_REPORT); - send_message(MSG_MAG_CAL_PROGRESS); - send_message(MSG_BATTERY2); - send_message(MSG_MOUNT_STATUS); - send_message(MSG_OPTICAL_FLOW); - send_message(MSG_EKF_STATUS_REPORT); - send_message(MSG_GIMBAL_REPORT); - send_message(MSG_VIBRATION); - } -} - - -/* - handle a request to switch to guided mode. This happens via a - callback from handle_mission_item() - */ -void GCS_MAVLINK::handle_guided_request(AP_Mission::Mission_Command &cmd) -{ - if (plane.control_mode != GUIDED) { - // only accept position updates when in GUIDED mode - return; - } - plane.guided_WP_loc = cmd.content.location; - - // add home alt if needed - if (plane.guided_WP_loc.flags.relative_alt) { - plane.guided_WP_loc.alt += plane.home.alt; - plane.guided_WP_loc.flags.relative_alt = 0; - } - - plane.set_guided_WP(); -} - -/* - handle a request to change current WP altitude. This happens via a - callback from handle_mission_item() - */ -void GCS_MAVLINK::handle_change_alt_request(AP_Mission::Mission_Command &cmd) -{ - plane.next_WP_loc.alt = cmd.content.location.alt; - if (cmd.content.location.flags.relative_alt) { - plane.next_WP_loc.alt += plane.home.alt; - } - plane.next_WP_loc.flags.relative_alt = false; - plane.next_WP_loc.flags.terrain_alt = cmd.content.location.flags.terrain_alt; - plane.reset_offset_altitude(); -} - -void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) -{ - switch (msg->msgid) { - - case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: - { - handle_request_data_stream(msg, true); - break; - } - - case MAVLINK_MSG_ID_COMMAND_LONG: - { - // decode - mavlink_command_long_t packet; - mavlink_msg_command_long_decode(msg, &packet); - - uint8_t result = MAV_RESULT_UNSUPPORTED; - - // do command - send_text(MAV_SEVERITY_INFO,"Command received: "); - - 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_LOITER_UNLIM: - plane.set_mode(LOITER); - result = MAV_RESULT_ACCEPTED; - break; - - case MAV_CMD_NAV_RETURN_TO_LAUNCH: - plane.set_mode(RTL); - result = MAV_RESULT_ACCEPTED; - break; - -#if MOUNT == ENABLED - // Sets the region of interest (ROI) for the camera - case MAV_CMD_DO_SET_ROI: - // 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); - if (roi_loc.lat == 0 && roi_loc.lng == 0 && roi_loc.alt == 0) { - // switch off the camera tracking if enabled - if (plane.camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) { - plane.camera_mount.set_mode_to_default(); - } - } else { - // send the command to the camera mount - plane.camera_mount.set_roi_target(roi_loc); - } - result = MAV_RESULT_ACCEPTED; - break; -#endif - -#if CAMERA == ENABLED - case MAV_CMD_DO_DIGICAM_CONFIGURE: - plane.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: - plane.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 - plane.camera_mount.control(packet.param1, packet.param2, packet.param3, (MAV_MOUNT_MODE) packet.param7); -#endif - break; - - case MAV_CMD_MISSION_START: - plane.set_mode(AUTO); - result = MAV_RESULT_ACCEPTED; - break; - - case MAV_CMD_PREFLIGHT_CALIBRATION: - if(hal.util->get_soft_armed()) { - result = MAV_RESULT_FAILED; - break; - } - plane.in_calibration = true; - if (is_equal(packet.param1,1.0f)) { - plane.ins.init_gyro(); - if (plane.ins.gyro_calibrated_ok_all()) { - plane.ahrs.reset_gyro_drift(); - result = MAV_RESULT_ACCEPTED; - } else { - result = MAV_RESULT_FAILED; - } - } else if (is_equal(packet.param3,1.0f)) { - plane.init_barometer(); - if (plane.airspeed.enabled()) { - plane.zero_airspeed(false); - } - result = MAV_RESULT_ACCEPTED; - } else if (is_equal(packet.param4,1.0f)) { - plane.trim_radio(); - result = MAV_RESULT_ACCEPTED; - } else if (is_equal(packet.param5,1.0f)) { - result = MAV_RESULT_ACCEPTED; - // start with gyro calibration - plane.ins.init_gyro(); - // reset ahrs gyro bias - if (plane.ins.gyro_calibrated_ok_all()) { - plane.ahrs.reset_gyro_drift(); - } else { - result = MAV_RESULT_FAILED; - } - plane.ins.acal_init(); - plane.ins.get_acal()->start(this); - - } else if (is_equal(packet.param5,2.0f)) { - // start with gyro calibration - plane.ins.init_gyro(); - // accel trim - float trim_roll, trim_pitch; - if(plane.ins.calibrate_trim(trim_roll, trim_pitch)) { - // reset ahrs's trim to suggested values from calibration routine - plane.ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0)); - result = MAV_RESULT_ACCEPTED; - } else { - result = MAV_RESULT_FAILED; - } - } - else { - send_text(MAV_SEVERITY_WARNING, "Unsupported preflight calibration"); - } - plane.in_calibration = false; - break; - - case MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: - if (is_equal(packet.param1,2.0f)) { - // save first compass's offsets - plane.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 - plane.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)) { - // run pre_arm_checks and arm_checks and display failures - if (plane.arm_motors(AP_Arming::MAVLINK)) { - result = MAV_RESULT_ACCEPTED; - } else { - result = MAV_RESULT_FAILED; - } - } else if (is_zero(packet.param1)) { - if (plane.disarm_motors()) { - result = MAV_RESULT_ACCEPTED; - } else { - result = MAV_RESULT_FAILED; - } - } else { - result = MAV_RESULT_UNSUPPORTED; - } - break; - - case MAV_CMD_GET_HOME_POSITION: - if (plane.home_is_set != HOME_UNSET) { - send_home(plane.ahrs.get_home()); - result = MAV_RESULT_ACCEPTED; - } - break; - - case MAV_CMD_DO_SET_MODE: - switch ((uint16_t)packet.param1) { - case MAV_MODE_MANUAL_ARMED: - case MAV_MODE_MANUAL_DISARMED: - plane.set_mode(MANUAL); - result = MAV_RESULT_ACCEPTED; - break; - - case MAV_MODE_AUTO_ARMED: - case MAV_MODE_AUTO_DISARMED: - plane.set_mode(AUTO); - result = MAV_RESULT_ACCEPTED; - break; - - case MAV_MODE_STABILIZE_DISARMED: - case MAV_MODE_STABILIZE_ARMED: - plane.set_mode(FLY_BY_WIRE_A); - result = MAV_RESULT_ACCEPTED; - break; - - default: - result = MAV_RESULT_UNSUPPORTED; - } - break; - - case MAV_CMD_DO_SET_SERVO: - if (plane.ServoRelayEvents.do_set_servo(packet.param1, packet.param2)) { - result = MAV_RESULT_ACCEPTED; - } - break; - - case MAV_CMD_DO_REPEAT_SERVO: - if (plane.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 (plane.ServoRelayEvents.do_set_relay(packet.param1, packet.param2)) { - result = MAV_RESULT_ACCEPTED; - } - break; - - case MAV_CMD_DO_REPEAT_RELAY: - if (plane.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)) { - // 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_LAND_START: - result = MAV_RESULT_FAILED; - - // attempt to switch to next DO_LAND_START command in the mission - if (plane.jump_to_landing_sequence()) { - result = MAV_RESULT_ACCEPTED; - } - break; - - case MAV_CMD_DO_GO_AROUND: - result = MAV_RESULT_FAILED; - - //Not allowing go around at FLIGHT_LAND_FINAL stage on purpose -- - //if plane is close to the ground a go around coudld be dangerous. - if (plane.flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH) { - //Just tell the autopilot we're done landing so it will - //proceed to the next mission item. If there is no next mission - //item the plane will head to home point and loiter. - plane.auto_state.commanded_go_around = true; - - result = MAV_RESULT_ACCEPTED; - plane.gcs_send_text(MAV_SEVERITY_INFO,"Go around command accepted"); - } else { - plane.gcs_send_text(MAV_SEVERITY_NOTICE,"Rejected go around command"); - } - break; - - case MAV_CMD_DO_FENCE_ENABLE: - result = MAV_RESULT_ACCEPTED; - - if (!plane.geofence_present()) { - result = MAV_RESULT_FAILED; - } switch((uint16_t)packet.param1) { - case 0: - if (! plane.geofence_set_enabled(false, GCS_TOGGLED)) { - result = MAV_RESULT_FAILED; - } - break; - case 1: - if (! plane.geofence_set_enabled(true, GCS_TOGGLED)) { - result = MAV_RESULT_FAILED; - } - break; - case 2: //disable fence floor only - if (! plane.geofence_set_floor_enabled(false)) { - result = MAV_RESULT_FAILED; - } else { - plane.gcs_send_text(MAV_SEVERITY_NOTICE,"Fence floor disabled"); - } - break; - default: - result = MAV_RESULT_FAILED; - break; - } - break; - - case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: { - if (is_equal(packet.param1,1.0f)) { - plane.gcs[chan-MAVLINK_COMM_0].send_autopilot_version(FIRMWARE_VERSION); - result = MAV_RESULT_ACCEPTED; - } - 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)) { - plane.init_home(); - } else { - if (is_zero(packet.param5) && is_zero(packet.param6) && is_zero(packet.param7)) { - // don't allow the 0,0 position - break; - } - // 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); - plane.ahrs.set_home(new_home_loc); - plane.home_is_set = HOME_SET_NOT_LOCKED; - plane.Log_Write_Home_And_Origin(); - GCS_MAVLINK::send_home_all(new_home_loc); - result = MAV_RESULT_ACCEPTED; - plane.gcs_send_text_fmt(MAV_SEVERITY_INFO, "Set HOME to %.6f %.6f at %um", - (double)(new_home_loc.lat*1.0e-7f), - (double)(new_home_loc.lng*1.0e-7f), - (uint32_t)(new_home_loc.alt*0.01f)); - } - break; - } - - case MAV_CMD_DO_AUTOTUNE_ENABLE: - // param1 : enable/disable - plane.autotune_enable(!is_zero(packet.param1)); - break; - - case MAV_CMD_DO_START_MAG_CAL: - case MAV_CMD_DO_ACCEPT_MAG_CAL: - case MAV_CMD_DO_CANCEL_MAG_CAL: - result = plane.compass.handle_mag_cal_command(packet); - 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: - plane.parachute.enabled(false); - break; - case PARACHUTE_ENABLE: - plane.parachute.enabled(true); - break; - case PARACHUTE_RELEASE: - // treat as a manual release which performs some additional check of altitude - if (plane.parachute.released()) { - plane.gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "Parachute already released"); - result = MAV_RESULT_FAILED; - } else if (!plane.parachute.enabled()) { - plane.gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "Parachute not enabled"); - result = MAV_RESULT_FAILED; - } else { - if (!plane.parachute_manual_release()) { - result = MAV_RESULT_FAILED; - } - } - break; - default: - result = MAV_RESULT_FAILED; - break; - } - break; -#endif - - default: - break; - } - - mavlink_msg_command_ack_send_buf( - msg, - chan, - packet.command, - result); - - break; - } - - case MAVLINK_MSG_ID_SET_MODE: - { - handle_set_mode(msg, FUNCTOR_BIND(&plane, &Plane::mavlink_set_mode, bool, uint8_t)); - 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: - { - handle_mission_request_list(plane.mission, msg); - break; - } - - - // XXX read a WP from EEPROM and send it to the GCS - case MAVLINK_MSG_ID_MISSION_REQUEST: - { - handle_mission_request(plane.mission, msg); - break; - } - - - case MAVLINK_MSG_ID_MISSION_ACK: - { - // nothing to do - break; - } - - case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: - { - // 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 - handle_param_request_list(msg); - break; - } - - case MAVLINK_MSG_ID_PARAM_REQUEST_READ: - { - handle_param_request_read(msg); - break; - } - - case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: - { - handle_mission_clear_all(plane.mission, msg); - break; - } - - case MAVLINK_MSG_ID_MISSION_SET_CURRENT: - { - // disable cross-track when user asks for WP change, to - // prevent unexpected flight paths - plane.auto_state.next_wp_no_crosstrack = true; - handle_mission_set_current(plane.mission, msg); - if (plane.control_mode == AUTO && plane.mission.state() == AP_Mission::MISSION_STOPPED) { - plane.mission.resume(); - } - 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: - { - handle_mission_count(plane.mission, msg); - break; - } - - case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST: - { - handle_mission_write_partial_list(plane.mission, msg); - break; - } - - // GCS has sent us a mission item, store to EEPROM - case MAVLINK_MSG_ID_MISSION_ITEM: - { - if (handle_mission_item(msg, plane.mission)) { - plane.DataFlash.Log_Write_EntireMission(plane.mission); - } - break; - } - -#if GEOFENCE_ENABLED == ENABLED - // receive a fence point from GCS and store in EEPROM - case MAVLINK_MSG_ID_FENCE_POINT: { - mavlink_fence_point_t packet; - mavlink_msg_fence_point_decode(msg, &packet); - if (plane.g.fence_action != FENCE_ACTION_NONE) { - send_text(MAV_SEVERITY_WARNING,"Fencing must be disabled"); - } else if (packet.count != plane.g.fence_total) { - send_text(MAV_SEVERITY_WARNING,"Bad fence point"); - } else if (fabsf(packet.lat) > 90.0f || fabsf(packet.lng) > 180.0f) { - send_text(MAV_SEVERITY_WARNING,"Invalid fence point, lat or lng too large"); - } else { - Vector2l point; - point.x = packet.lat*1.0e7f; - point.y = packet.lng*1.0e7f; - plane.set_fence_point_with_index(point, packet.idx); - } - break; - } - - // send a fence point to GCS - case MAVLINK_MSG_ID_FENCE_FETCH_POINT: { - mavlink_fence_fetch_point_t packet; - mavlink_msg_fence_fetch_point_decode(msg, &packet); - if (packet.idx >= plane.g.fence_total) { - send_text(MAV_SEVERITY_WARNING,"Bad fence point"); - } else { - Vector2l point = plane.get_fence_point_with_index(packet.idx); - mavlink_msg_fence_point_send_buf(msg, chan, msg->sysid, msg->compid, packet.idx, plane.g.fence_total, - point.x*1.0e-7f, point.y*1.0e-7f); - } - break; - } -#endif // GEOFENCE_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 >= plane.rally.get_rally_total() || - packet.idx >= plane.rally.get_rally_max()) { - send_text(MAV_SEVERITY_WARNING,"Bad rally point message ID"); - break; - } - - if (packet.count != plane.rally.get_rally_total()) { - send_text(MAV_SEVERITY_WARNING,"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; - plane.rally.set_rally_point_with_index(packet.idx, 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 > plane.rally.get_rally_total()) { - send_text(MAV_SEVERITY_WARNING, "Bad rally point index"); - break; - } - RallyLocation rally_point; - if (!plane.rally.get_rally_point_with_index(packet.idx, rally_point)) { - send_text(MAV_SEVERITY_WARNING, "Failed to set rally point"); - break; - } - - mavlink_msg_rally_point_send_buf(msg, - chan, msg->sysid, msg->compid, packet.idx, - plane.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; - } - - case MAVLINK_MSG_ID_PARAM_SET: - { - handle_param_set(msg, &plane.DataFlash); - break; - } - - case MAVLINK_MSG_ID_GIMBAL_REPORT: - { -#if MOUNT == ENABLED - handle_gimbal_report(plane.camera_mount, msg); -#endif - break; - } - - case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: - { - // allow override of RC channel values for HIL - // or for complete GCS control of switch position - // and RC PWM values. - if(msg->sysid != plane.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; - - if (hal.rcin->set_overrides(v, 8)) { - plane.failsafe.last_valid_rc_ms = AP_HAL::millis(); - } - - // a RC override message is consiered to be a 'heartbeat' from - // the ground station for failsafe purposes - plane.failsafe.last_heartbeat_ms = AP_HAL::millis(); - break; - } - - case MAVLINK_MSG_ID_HEARTBEAT: - { - // We keep track of the last time we received a heartbeat from - // our GCS for failsafe purposes - if (msg->sysid != plane.g.sysid_my_gcs) break; - plane.failsafe.last_heartbeat_ms = AP_HAL::millis(); - break; - } - - case MAVLINK_MSG_ID_HIL_STATE: - { -#if HIL_SUPPORT - if (plane.g.hil_mode != 1) { - break; - } - mavlink_hil_state_t packet; - mavlink_msg_hil_state_decode(msg, &packet); - - last_hil_state = packet; - - // set gps hil sensor - Location loc; - memset(&loc, 0, sizeof(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; - - // setup airspeed pressure based on 3D speed, no wind - plane.airspeed.setHIL(sq(vel.length()) / 2.0f + 2013); - - plane.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*0.001f; - accels.y = packet.yacc * GRAVITY_MSS*0.001f; - accels.z = packet.zacc * GRAVITY_MSS*0.001f; - - plane.ins.set_gyro(0, gyros); - plane.ins.set_accel(0, accels); - - plane.barometer.setHIL(packet.alt*0.001f); - plane.compass.setHIL(0, packet.roll, packet.pitch, packet.yaw); - plane.compass.setHIL(1, packet.roll, packet.pitch, packet.yaw); - - // cope with DCM getting badly off due to HIL lag - if (plane.g.hil_err_limit > 0 && - (fabsf(packet.roll - plane.ahrs.roll) > ToRad(plane.g.hil_err_limit) || - fabsf(packet.pitch - plane.ahrs.pitch) > ToRad(plane.g.hil_err_limit) || - wrap_PI(fabsf(packet.yaw - plane.ahrs.yaw)) > ToRad(plane.g.hil_err_limit))) { - plane.ahrs.reset_attitude(packet.roll, packet.pitch, packet.yaw); - } -#endif - break; - } - -#if CAMERA == ENABLED - //deprecated. Use MAV_CMD_DO_DIGICAM_CONFIGURE - case MAVLINK_MSG_ID_DIGICAM_CONFIGURE: - { - break; - } - - //deprecated. Use MAV_CMD_DO_DIGICAM_CONTROL - case MAVLINK_MSG_ID_DIGICAM_CONTROL: - { - plane.camera.control_msg(msg); - plane.log_picture(); - break; - } -#endif // CAMERA == ENABLED - -#if MOUNT == ENABLED - //deprecated. Use MAV_CMD_DO_MOUNT_CONFIGURE - case MAVLINK_MSG_ID_MOUNT_CONFIGURE: - { - plane.camera_mount.configure_msg(msg); - break; - } - - //deprecated. Use MAV_CMD_DO_MOUNT_CONTROL - case MAVLINK_MSG_ID_MOUNT_CONTROL: - { - plane.camera_mount.control_msg(msg); - break; - } -#endif // MOUNT == ENABLED - - case MAVLINK_MSG_ID_RADIO: - case MAVLINK_MSG_ID_RADIO_STATUS: - { - handle_radio_status(msg, plane.DataFlash, plane.should_log(MASK_LOG_PM)); - break; - } - - case MAVLINK_MSG_ID_LOG_REQUEST_DATA: - case MAVLINK_MSG_ID_LOG_ERASE: - plane.in_log_download = true; - /* no break */ - case MAVLINK_MSG_ID_LOG_REQUEST_LIST: - if (!plane.in_mavlink_delay) { - handle_log_message(msg, plane.DataFlash); - } - break; - case MAVLINK_MSG_ID_LOG_REQUEST_END: - plane.in_log_download = false; - if (!plane.in_mavlink_delay) { - handle_log_message(msg, plane.DataFlash); - } - break; - - case MAVLINK_MSG_ID_SERIAL_CONTROL: - handle_serial_control(msg, plane.gps); - break; - - case MAVLINK_MSG_ID_GPS_INJECT_DATA: - handle_gps_inject(msg, plane.gps); - break; - - case MAVLINK_MSG_ID_TERRAIN_DATA: - case MAVLINK_MSG_ID_TERRAIN_CHECK: -#if AP_TERRAIN_AVAILABLE - plane.terrain.handle_data(chan, msg); -#endif - break; - - case MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST: - plane.gcs[chan-MAVLINK_COMM_0].send_autopilot_version(FIRMWARE_VERSION); - break; - - case MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS: - plane.DataFlash.remote_log_block_status_msg(chan, 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)) { - // don't allow the 0,0 position - break; - } - // 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; - plane.ahrs.set_home(new_home_loc); - plane.home_is_set = HOME_SET_NOT_LOCKED; - plane.Log_Write_Home_And_Origin(); - GCS_MAVLINK::send_home_all(new_home_loc); - plane.gcs_send_text_fmt(MAV_SEVERITY_INFO, "Set HOME to %.6f %.6f at %um", - (double)(new_home_loc.lat*1.0e-7f), - (double)(new_home_loc.lng*1.0e-7f), - (uint32_t)(new_home_loc.alt*0.01f)); - break; - } - - case MAVLINK_MSG_ID_ADSB_VEHICLE: - plane.adsb.update_vehicle(msg); - 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 Plane::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_message(MSG_HEARTBEAT); - gcs_send_message(MSG_EXTENDED_STATUS1); - } - if (tnow - last_50hz > 20) { - last_50hz = tnow; - gcs_update(); - gcs_data_stream_send(); - 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 Plane::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); -#if LOGGING_ENABLED == ENABLED - DataFlash.Log_Write_Message(gcs[0].pending_status.text); -#endif - gcs[0].send_message(MSG_STATUSTEXT); - for (uint8_t i=1; i= - MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN) { - airspeed.log_mavlink_send((mavlink_channel_t)i, vg); - } - } - } -} - -/** - retry any deferred messages - */ -void Plane::gcs_retry_deferred(void) -{ - gcs_send_message(MSG_RETRY_DEFERRED); -} diff --git a/ArduSubsea/Log.cpp b/ArduSubsea/Log.cpp deleted file mode 100644 index c2b3505867..0000000000 --- a/ArduSubsea/Log.cpp +++ /dev/null @@ -1,595 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#include "Plane.h" - -#if LOGGING_ENABLED == ENABLED - -#if CLI_ENABLED == ENABLED -// Code to Write and Read packets from DataFlash.log memory -// Code to interact with the user to dump or erase logs - -// Creates a constant array of structs representing menu options -// and stores them in Flash memory, not RAM. -// User enters the string in the console to call the functions on the right. -// See class Menu in AP_Coommon for implementation details -static const struct Menu::command log_menu_commands[] = { - {"dump", MENU_FUNC(dump_log)}, - {"erase", MENU_FUNC(erase_logs)}, - {"enable", MENU_FUNC(select_logs)}, - {"disable", MENU_FUNC(select_logs)} -}; - -// A Macro to create the Menu -MENU2(log_menu, "Log", log_menu_commands, FUNCTOR_BIND(&plane, &Plane::print_log_menu, bool)); - -bool Plane::print_log_menu(void) -{ - cliSerial->println("logs enabled: "); - - if (0 == g.log_bitmask) { - cliSerial->println("none"); - }else{ - // 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 - // the bit being set and print the name of the log option to suit. - #define PLOG(_s) if (g.log_bitmask & MASK_LOG_ ## _s) cliSerial->printf(" %s", # _s) - PLOG(ATTITUDE_FAST); - PLOG(ATTITUDE_MED); - PLOG(GPS); - PLOG(PM); - PLOG(CTUN); - PLOG(NTUN); - PLOG(MODE); - PLOG(IMU); - PLOG(CMD); - PLOG(CURRENT); - PLOG(COMPASS); - PLOG(TECS); - PLOG(CAMERA); - PLOG(RC); - PLOG(SONAR); - #undef PLOG - } - - cliSerial->println(); - - DataFlash.ListAvailableLogs(cliSerial); - return(true); -} - -int8_t Plane::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; -} - -int8_t Plane::erase_logs(uint8_t argc, const Menu::arg *argv) -{ - in_mavlink_delay = true; - do_erase_logs(); - in_mavlink_delay = false; - return 0; -} - -int8_t Plane::select_logs(uint8_t argc, const Menu::arg *argv) -{ - uint32_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 = 0xFFFFFFFFUL; - } 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(MODE); - TARG(IMU); - TARG(CMD); - TARG(CURRENT); - TARG(COMPASS); - TARG(TECS); - TARG(CAMERA); - TARG(RC); - TARG(SONAR); - #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 Plane::process_logs(uint8_t argc, const Menu::arg *argv) -{ - log_menu.run(); - return 0; -} - -#endif // CLI_ENABLED == ENABLED - -void Plane::do_erase_logs(void) -{ - gcs_send_text(MAV_SEVERITY_INFO, "Erasing logs"); - DataFlash.EraseAll(); - gcs_send_text(MAV_SEVERITY_INFO, "Log erase complete"); -} - - -// Write an attitude packet -void Plane::Log_Write_Attitude(void) -{ - Vector3f targets; // Package up the targets into a vector for commonality with Copter usage of Log_Wrote_Attitude - targets.x = nav_roll_cd; - targets.y = nav_pitch_cd; - targets.z = 0; //Plane does not have the concept of navyaw. This is a placeholder. - - DataFlash.Log_Write_Attitude(ahrs, targets); - DataFlash.Log_Write_PID(LOG_PIDR_MSG, rollController.get_pid_info()); - DataFlash.Log_Write_PID(LOG_PIDP_MSG, pitchController.get_pid_info()); - DataFlash.Log_Write_PID(LOG_PIDY_MSG, yawController.get_pid_info()); - DataFlash.Log_Write_PID(LOG_PIDS_MSG, steerController.get_pid_info()); - -#if AP_AHRS_NAVEKF_AVAILABLE - #if OPTFLOW == ENABLED - DataFlash.Log_Write_EKF(ahrs,optflow.enabled()); - #else - DataFlash.Log_Write_EKF(ahrs,false); - #endif - DataFlash.Log_Write_AHRS2(ahrs); -#endif -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL - sitl.Log_Write_SIMSTATE(&DataFlash); -#endif - DataFlash.Log_Write_POS(ahrs); -} - - -struct PACKED log_Performance { - LOG_PACKET_HEADER; - uint64_t time_us; - uint32_t loop_time; - uint16_t main_loop_count; - uint32_t g_dt_max; - int16_t gyro_drift_x; - int16_t gyro_drift_y; - int16_t gyro_drift_z; - uint8_t i2c_lockup_count; - uint16_t ins_error_count; -}; - -// Write a performance monitoring packet. Total length : 19 bytes -void Plane::Log_Write_Performance() -{ - struct log_Performance pkt = { - LOG_PACKET_HEADER_INIT(LOG_PERFORMANCE_MSG), - time_us : AP_HAL::micros64(), - loop_time : millis() - perf_mon_timer, - main_loop_count : mainLoop_count, - g_dt_max : G_Dt_max, - gyro_drift_x : (int16_t)(ahrs.get_gyro_drift().x * 1000), - gyro_drift_y : (int16_t)(ahrs.get_gyro_drift().y * 1000), - gyro_drift_z : (int16_t)(ahrs.get_gyro_drift().z * 1000), - i2c_lockup_count: hal.i2c->lockup_count(), - ins_error_count : ins.error_count() - }; - DataFlash.WriteBlock(&pkt, sizeof(pkt)); -} - -struct PACKED log_Startup { - LOG_PACKET_HEADER; - uint64_t time_us; - uint8_t startup_type; - uint16_t command_total; -}; - -void Plane::Log_Write_Startup(uint8_t type) -{ - struct log_Startup pkt = { - LOG_PACKET_HEADER_INIT(LOG_STARTUP_MSG), - time_us : AP_HAL::micros64(), - startup_type : type, - command_total : mission.num_commands() - }; - DataFlash.WriteBlock(&pkt, sizeof(pkt)); -} - -struct PACKED log_Control_Tuning { - LOG_PACKET_HEADER; - uint64_t time_us; - int16_t nav_roll_cd; - int16_t roll; - int16_t nav_pitch_cd; - int16_t pitch; - int16_t throttle_out; - int16_t rudder_out; - float accel_y; -}; - -// Write a control tuning packet. Total length : 22 bytes -void Plane::Log_Write_Control_Tuning() -{ - Vector3f accel = ins.get_accel(); - struct log_Control_Tuning pkt = { - LOG_PACKET_HEADER_INIT(LOG_CTUN_MSG), - time_us : AP_HAL::micros64(), - nav_roll_cd : (int16_t)nav_roll_cd, - roll : (int16_t)ahrs.roll_sensor, - nav_pitch_cd : (int16_t)nav_pitch_cd, - pitch : (int16_t)ahrs.pitch_sensor, - throttle_out : (int16_t)channel_throttle->servo_out, - rudder_out : (int16_t)channel_rudder->servo_out, - accel_y : accel.y - }; - DataFlash.WriteBlock(&pkt, sizeof(pkt)); -} - -// Write a TECS tuning packet -void Plane::Log_Write_TECS_Tuning(void) -{ - SpdHgt_Controller->log_data(DataFlash, LOG_TECS_MSG); -} - -struct PACKED log_Nav_Tuning { - LOG_PACKET_HEADER; - uint64_t time_us; - uint16_t yaw; - float wp_distance; - int16_t target_bearing_cd; - int16_t nav_bearing_cd; - int16_t altitude_error_cm; - int16_t airspeed_cm; - float altitude; - uint32_t groundspeed_cm; -}; - -// Write a navigation tuning packe -void Plane::Log_Write_Nav_Tuning() -{ - struct log_Nav_Tuning pkt = { - LOG_PACKET_HEADER_INIT(LOG_NTUN_MSG), - time_us : AP_HAL::micros64(), - yaw : (uint16_t)ahrs.yaw_sensor, - wp_distance : auto_state.wp_distance, - target_bearing_cd : (int16_t)nav_controller->target_bearing_cd(), - nav_bearing_cd : (int16_t)nav_controller->nav_bearing_cd(), - altitude_error_cm : (int16_t)altitude_error_cm, - airspeed_cm : (int16_t)airspeed.get_airspeed_cm(), - altitude : barometer.get_altitude(), - groundspeed_cm : (uint32_t)(gps.ground_speed()*100) - }; - DataFlash.WriteBlock(&pkt, sizeof(pkt)); -} - -struct PACKED log_Status { - LOG_PACKET_HEADER; - uint64_t time_us; - uint8_t is_flying; - float is_flying_probability; - uint8_t armed; - uint8_t safety; - bool is_crashed; - bool is_still; - uint8_t stage; -}; - -void Plane::Log_Write_Status() -{ - struct log_Status pkt = { - LOG_PACKET_HEADER_INIT(LOG_STATUS_MSG) - ,time_us : AP_HAL::micros64() - ,is_flying : is_flying() - ,is_flying_probability : isFlyingProbability - ,armed : hal.util->get_soft_armed() - ,safety : hal.util->safety_switch_state() - ,is_crashed : crash_state.is_crashed - ,is_still : plane.ins.is_still() - ,stage : flight_stage - }; - - DataFlash.WriteBlock(&pkt, sizeof(pkt)); -} - -struct PACKED log_Sonar { - LOG_PACKET_HEADER; - uint64_t time_us; - uint16_t distance; - float voltage; - float baro_alt; - float groundspeed; - uint8_t throttle; - uint8_t count; - float correction; -}; - -// Write a sonar packet -void Plane::Log_Write_Sonar() -{ -#if RANGEFINDER_ENABLED == ENABLED - uint16_t distance = 0; - if (rangefinder.status() == RangeFinder::RangeFinder_Good) { - distance = rangefinder.distance_cm(); - } - - struct log_Sonar pkt = { - LOG_PACKET_HEADER_INIT(LOG_SONAR_MSG), - time_us : AP_HAL::micros64(), - distance : distance, - voltage : rangefinder.voltage_mv()*0.001f, - baro_alt : barometer.get_altitude(), - groundspeed : gps.ground_speed(), - throttle : (uint8_t)(100 * channel_throttle->norm_output()), - count : rangefinder_state.in_range_count, - correction : rangefinder_state.correction - }; - DataFlash.WriteBlock(&pkt, sizeof(pkt)); - - DataFlash.Log_Write_RFND(rangefinder); -#endif -} - -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; -}; - -#if OPTFLOW == ENABLED -// Write an optical flow packet -void Plane::Log_Write_Optflow() -{ - // 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 - -struct PACKED log_Arm_Disarm { - LOG_PACKET_HEADER; - uint64_t time_us; - uint8_t arm_state; - uint16_t arm_checks; -}; - -void Plane::Log_Write_Current() -{ - DataFlash.Log_Write_Current(battery, channel_throttle->control_in); - - // also write power status - DataFlash.Log_Write_Power(); -} - -void Plane::Log_Arm_Disarm() { - struct log_Arm_Disarm pkt = { - LOG_PACKET_HEADER_INIT(LOG_ARM_DISARM_MSG), - time_us : AP_HAL::micros64(), - arm_state : arming.is_armed(), - arm_checks : arming.get_enabled_checks() - }; - DataFlash.WriteBlock(&pkt, sizeof(pkt)); -} - -void Plane::Log_Write_GPS(uint8_t instance) -{ - DataFlash.Log_Write_GPS(gps, instance, current_loc.alt - ahrs.get_home().alt); -} - -void Plane::Log_Write_IMU() -{ - DataFlash.Log_Write_IMU(ins); -} - -void Plane::Log_Write_RC(void) -{ - DataFlash.Log_Write_RCIN(); - DataFlash.Log_Write_RCOUT(); - if (rssi.enabled()) { - DataFlash.Log_Write_RSSI(rssi); - } -} - -void Plane::Log_Write_Baro(void) -{ - DataFlash.Log_Write_Baro(barometer); -} - -// Write a AIRSPEED packet -void Plane::Log_Write_Airspeed(void) -{ - DataFlash.Log_Write_Airspeed(airspeed); -} - -// log ahrs home and EKF origin to dataflash -void Plane::Log_Write_Home_And_Origin() -{ -#if AP_AHRS_NAVEKF_AVAILABLE - // 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); - } -#endif - - // log ahrs home if set - if (home_is_set != HOME_UNSET) { - DataFlash.Log_Write_Origin(LogOriginType::ahrs_home, ahrs.get_home()); - } -} - -static const struct LogStructure log_structure[] = { - LOG_COMMON_STRUCTURES, - { LOG_PERFORMANCE_MSG, sizeof(log_Performance), - "PM", "QIHIhhhBH", "TimeUS,LTime,MLC,gDt,GDx,GDy,GDz,I2CErr,INSErr" }, - { LOG_STARTUP_MSG, sizeof(log_Startup), - "STRT", "QBH", "TimeUS,SType,CTot" }, - { LOG_CTUN_MSG, sizeof(log_Control_Tuning), - "CTUN", "Qcccchhf", "TimeUS,NavRoll,Roll,NavPitch,Pitch,ThrOut,RdrOut,AccY" }, - { LOG_NTUN_MSG, sizeof(log_Nav_Tuning), - "NTUN", "QCfccccfI", "TimeUS,Yaw,WpDist,TargBrg,NavBrg,AltErr,Arspd,Alt,GSpdCM" }, - { LOG_SONAR_MSG, sizeof(log_Sonar), - "SONR", "QHfffBBf", "TimeUS,DistCM,Volt,BaroAlt,GSpd,Thr,Cnt,Corr" }, - { LOG_ARM_DISARM_MSG, sizeof(log_Arm_Disarm), - "ARM", "QBH", "TimeUS,ArmState,ArmChecks" }, - { LOG_ATRP_MSG, sizeof(AP_AutoTune::log_ATRP), - "ATRP", "QBBcfff", "TimeUS,Type,State,Servo,Demanded,Achieved,P" }, - { LOG_STATUS_MSG, sizeof(log_Status), - "STAT", "QBfBBBBB", "TimeUS,isFlying,isFlyProb,Armed,Safety,Crash,Still,Stage" }, -#if OPTFLOW == ENABLED - { LOG_OPTFLOW_MSG, sizeof(log_Optflow), - "OF", "QBffff", "TimeUS,Qual,flowX,flowY,bodyX,bodyY" }, -#endif - TECS_LOG_FORMAT(LOG_TECS_MSG) -}; - -#if CLI_ENABLED == ENABLED -// Read the DataFlash.log memory : Packet Parser -void Plane::Log_Read(uint16_t list_entry, int16_t start_page, int16_t end_page) -{ - cliSerial->printf("\n" FIRMWARE_STRING - "\nFree RAM: %u\n", - (unsigned)hal.util->available_memory()); - - cliSerial->println(HAL_BOARD_NAME); - - DataFlash.LogReadProcess(list_entry, start_page, end_page, - FUNCTOR_BIND_MEMBER(&Plane::print_flight_mode, void, AP_HAL::BetterStream *, uint8_t), - cliSerial); -} -#endif // CLI_ENABLED - -void Plane::Log_Write_Vehicle_Startup_Messages() -{ - // only 200(?) bytes are guaranteed by DataFlash - Log_Write_Startup(TYPE_GROUNDSTART_MSG); - DataFlash.Log_Write_Mode(control_mode); -} - -// start a new log -void Plane::start_logging() -{ - DataFlash.set_mission(&mission); - DataFlash.setVehicle_Startup_Log_Writer( - FUNCTOR_BIND(&plane, &Plane::Log_Write_Vehicle_Startup_Messages, void) - ); - - DataFlash.StartNewLog(); -} - -/* - initialise logging subsystem - */ -void Plane::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 95% - // @Values: 0:Disabled, 1:Enabled - // @User: Advanced - GSCALAR(land_abort_throttle_enable, "LAND_ABORT_THR", 0), - - // @Param: NAV_CONTROLLER - // @DisplayName: Navigation controller selection - // @Description: Which navigation controller to enable. Currently the only navigation controller available is L1. From time to time other experimental controllers will be added which are selected using this parameter. - // @Values: 0:Default,1:L1Controller - // @User: Standard - GSCALAR(nav_controller, "NAV_CONTROLLER", AP_Navigation::CONTROLLER_L1), - - // @Param: ALT_MIX - // @DisplayName: GPS to Baro Mix - // @Description: The percent of mixing between GPS altitude and baro altitude. 0 = 100% gps, 1 = 100% baro. It is highly recommend that you not change this from the default of 1, as GPS altitude is notoriously unreliable. The only time I would recommend changing this is if you have a high altitude enabled GPS, and you are dropping a plane from a high altitude balloon many kilometers off the ground. - // @Units: Percent - // @Range: 0 1 - // @Increment: 0.1 - // @User: Advanced - GSCALAR(altitude_mix, "ALT_MIX", ALTITUDE_MIX), - - // @Param: ALT_CTRL_ALG - // @DisplayName: Altitude control algorithm - // @Description: This sets what algorithm will be used for altitude control. The default is zero, which selects the most appropriate algorithm for your airframe. Currently the default is to use TECS (total energy control system). From time to time we will add other experimental altitude control algorithms which will be selected using this parameter. - // @Values: 0:Automatic - // @User: Advanced - GSCALAR(alt_control_algorithm, "ALT_CTRL_ALG", ALT_CONTROL_DEFAULT), - - // @Param: ALT_OFFSET - // @DisplayName: Altitude offset - // @Description: This is added to the target altitude in automatic flight. It can be used to add a global altitude offset to a mission - // @Units: Meters - // @Range: -32767 32767 - // @Increment: 1 - // @User: Advanced - GSCALAR(alt_offset, "ALT_OFFSET", 0), - - // @Param: WP_RADIUS - // @DisplayName: Waypoint Radius - // @Description: Defines the maximum distance from a waypoint that when crossed indicates the waypoint may be complete. To avoid the aircraft looping around the waypoint in case it misses by more than the WP_RADIUS an additional check is made to see if the aircraft has crossed a "finish line" passing through the waypoint and perpendicular to the flight path from the previous waypoint. If that finish line is crossed then the waypoint is considered complete. Note that the navigation controller may decide to turn later than WP_RADIUS before a waypoint, based on how sharp the turn is and the speed of the aircraft. It is safe to set WP_RADIUS much larger than the usual turn radius of your aircraft and the navigation controller will work out when to turn. If you set WP_RADIUS too small then you will tend to overshoot the turns. - // @Units: Meters - // @Range: 1 32767 - // @Increment: 1 - // @User: Standard - GSCALAR(waypoint_radius, "WP_RADIUS", WP_RADIUS_DEFAULT), - - // @Param: WP_MAX_RADIUS - // @DisplayName: Waypoint Maximum Radius - // @Description: Sets the maximum distance to a waypoint for the waypoint to be considered complete. This overrides the "cross the finish line" logic that is normally used to consider a waypoint complete. For normal AUTO behaviour this parameter should be set to zero. Using a non-zero value is only recommended when it is critical that the aircraft does approach within the given radius, and should loop around until it has done so. This can cause the aircraft to loop forever if its turn radius is greater than the maximum radius set. - // @Units: Meters - // @Range: 0 32767 - // @Increment: 1 - // @User: Standard - GSCALAR(waypoint_max_radius, "WP_MAX_RADIUS", 0), - - // @Param: WP_LOITER_RAD - // @DisplayName: Waypoint Loiter Radius - // @Description: Defines the distance from the waypoint center, the plane will maintain during a loiter. If you set this value to a negative number then the default loiter direction will be counter-clockwise instead of clockwise. - // @Units: Meters - // @Range: -32767 32767 - // @Increment: 1 - // @User: Standard - GSCALAR(loiter_radius, "WP_LOITER_RAD", LOITER_RADIUS_DEFAULT), - -#if GEOFENCE_ENABLED == ENABLED - // @Param: FENCE_ACTION - // @DisplayName: Action on geofence breach - // @Description: What to do on fence breach. If this is set to 0 then no action is taken, and geofencing is disabled. If this is set to 1 then the plane will enter GUIDED mode, with the target waypoint as the fence return point. If this is set to 2 then the fence breach is reported to the ground station, but no other action is taken. If set to 3 then the plane enters guided mode but the pilot retains manual throttle control. - // @Values: 0:None,1:GuidedMode,2:ReportOnly,3:GuidedModeThrPass - // @User: Standard - GSCALAR(fence_action, "FENCE_ACTION", 0), - - // @Param: FENCE_TOTAL - // @DisplayName: Fence Total - // @Description: Number of geofence points currently loaded - // @User: Advanced - GSCALAR(fence_total, "FENCE_TOTAL", 0), - - // @Param: FENCE_CHANNEL - // @DisplayName: Fence Channel - // @Description: RC Channel to use to enable geofence. PWM input above 1750 enables the geofence - // @User: Standard - GSCALAR(fence_channel, "FENCE_CHANNEL", 0), - - // @Param: FENCE_MINALT - // @DisplayName: Fence Minimum Altitude - // @Description: Minimum altitude allowed before geofence triggers - // @Units: meters - // @Range: 0 32767 - // @Increment: 1 - // @User: Standard - GSCALAR(fence_minalt, "FENCE_MINALT", 0), - - // @Param: FENCE_MAXALT - // @DisplayName: Fence Maximum Altitude - // @Description: Maximum altitude allowed before geofence triggers - // @Units: meters - // @Range: 0 32767 - // @Increment: 1 - // @User: Standard - GSCALAR(fence_maxalt, "FENCE_MAXALT", 0), - - // @Param: FENCE_RETALT - // @DisplayName: Fence Return Altitude - // @Description: Altitude the aircraft will transit to when a fence breach occurs. If FENCE_RETALT is <= 0 then the midpoint between FENCE_MAXALT and FENCE_MINALT is used, unless FENCE_MAXALT < FENCE_MINALT. If FENCE_MAXALT < FENCE_MINALT AND FENCE_RETALT is <= 0 then ALT_HOLD_RTL is the altitude used on a fence breach. - // @Units: meters - // @Range: 0 32767 - // @Increment: 1 - // @User: Standard - GSCALAR(fence_retalt, "FENCE_RETALT", 0), - - // @Param: FENCE_AUTOENABLE - // @DisplayName: Fence automatic enable - // @Description: When set to 1, geofence automatically enables after an auto takeoff and automatically disables at the beginning of an auto landing. When on the ground before takeoff the fence is disabled. When set to 2, the fence autoenables after an auto takeoff, but only disables the fence floor during landing. It is highly recommended to not use this option for line of sight flying and use a fence enable channel instead. - // @Values: 0:NoAutoEnable,1:AutoEnable,2:AutoEnableDisableFloorOnly - // @User: Standard - GSCALAR(fence_autoenable, "FENCE_AUTOENABLE", 0), - - // @Param: FENCE_RET_RALLY - // @DisplayName: Fence Return to Rally - // @Description: When set to 1: on fence breach the plane will return to the nearest rally point rather than the fence return point. If no rally points have been defined the plane will return to the home point. - // @Values: 0:FenceReturnPoint,1:NearestRallyPoint - // @User: Standard - GSCALAR(fence_ret_rally, "FENCE_RET_RALLY", 0), -#endif - - // @Param: STALL_PREVENTION - // @DisplayName: Enable stall prevention - // @Description: This controls the use of stall prevention techniques, including roll limits at low speed and raising the minimum airspeed in turns. The limits are based on the aerodynamic load factor of a banked turn. This option relies on the correct ARSPD_FBW_MIN value being set correctly. Note that if you don't have an airspeed sensor then stall prevention will use an airspeed estimate based on the ground speed plus a wind estimate taken from the response of the autopilot banked turns. That synthetic airspeed estimate may be inaccurate, so you should not assume that stall prevention with no airspeed sensor will be effective. - // @Values: 0:Disabled,1:Enabled - // @User: Standard - ASCALAR(stall_prevention, "STALL_PREVENTION", 1), - - // @Param: ARSPD_FBW_MIN - // @DisplayName: Minimum Airspeed - // @Description: This is the minimum airspeed you want to fly at in modes where the autopilot controls the airspeed. This should be set to a value around 20% higher than the level flight stall speed for the airframe. This value is also used in the STALL_PREVENTION code. - // @Units: m/s - // @Range: 5 100 - // @Increment: 1 - // @User: Standard - ASCALAR(airspeed_min, "ARSPD_FBW_MIN", AIRSPEED_FBW_MIN), - - // @Param: ARSPD_FBW_MAX - // @DisplayName: Maximum Airspeed - // @Description: This is the maximum airspeed that you want to allow for your airframe in auto-throttle modes. You should ensure that this value is sufficiently above the ARSPD_FBW_MIN value to allow for a sufficient flight envelope to accurately control altitude using airspeed. A value at least 50% above ARSPD_FBW_MIN is recommended. - // @Units: m/s - // @Range: 5 100 - // @Increment: 1 - // @User: Standard - ASCALAR(airspeed_max, "ARSPD_FBW_MAX", AIRSPEED_FBW_MAX), - - // @Param: FBWB_ELEV_REV - // @DisplayName: Fly By Wire elevator reverse - // @Description: Reverse sense of elevator in FBWB and CRUISE modes. When set to 0 up elevator (pulling back on the stick) means to lower altitude. When set to 1, up elevator means to raise altitude. - // @Values: 0:Disabled,1:Enabled - // @User: Standard - GSCALAR(flybywire_elev_reverse, "FBWB_ELEV_REV", 0), - -#if AP_TERRAIN_AVAILABLE - // @Param: TERRAIN_FOLLOW - // @DisplayName: Use terrain following - // @Description: This enables terrain following for CRUISE mode, FBWB mode, RTL and for rally points. To use this option you also need to set TERRAIN_ENABLE to 1, which enables terrain data fetching from the GCS, and you need to have a GCS that supports sending terrain data to the aircraft. When terrain following is enabled then CRUISE and FBWB mode will hold height above terrain rather than height above home. In RTL the return to launch altitude will be considered to be a height above the terrain. Rally point altitudes will be taken as height above the terrain. This option does not affect mission items, which have a per-waypoint flag for whether they are height above home or height above the terrain. To use terrain following missions you need a ground station which can set the waypoint type to be a terrain height waypoint when creating the mission. - // @Values: 0:Disabled,1:Enabled - // @User: Standard - GSCALAR(terrain_follow, "TERRAIN_FOLLOW", 0), - - // @Param: TERRAIN_LOOKAHD - // @DisplayName: Terrain lookahead - // @Description: This controls how far ahead the terrain following code looks to ensure it stays above upcoming terrain. A value of zero means no lookahead, so the controller will track only the terrain directly below the aircraft. The lookahead will never extend beyond the next waypoint when in AUTO mode. - // @Range: 0 10000 - // @Units: meters - // @User: Standard - GSCALAR(terrain_lookahead, "TERRAIN_LOOKAHD", 2000), -#endif - - // @Param: FBWB_CLIMB_RATE - // @DisplayName: Fly By Wire B altitude change rate - // @Description: This sets the rate in m/s at which FBWB and CRUISE modes will change its target altitude for full elevator deflection. Note that the actual climb rate of the aircraft can be lower than this, depending on your airspeed and throttle control settings. If you have this parameter set to the default value of 2.0, then holding the elevator at maximum deflection for 10 seconds would change the target altitude by 20 meters. - // @Range: 1 10 - // @Increment: 0.1 - // @User: Standard - GSCALAR(flybywire_climb_rate, "FBWB_CLIMB_RATE", 2.0f), - - // @Param: THR_MIN - // @DisplayName: Minimum Throttle - // @Description: The minimum throttle setting (as a percentage) which the autopilot will apply. For the final stage of an automatic landing this is always zero. - // @Units: Percent - // @Range: 0 100 - // @Increment: 1 - // @User: Standard - ASCALAR(throttle_min, "THR_MIN", THROTTLE_MIN), - - // @Param: THR_MAX - // @DisplayName: Maximum Throttle - // @Description: The maximum throttle setting (as a percentage) which the autopilot will apply. - // @Units: Percent - // @Range: 0 100 - // @Increment: 1 - // @User: Standard - ASCALAR(throttle_max, "THR_MAX", THROTTLE_MAX), - - // @Param: TKOFF_THR_MAX - // @DisplayName: Maximum Throttle for takeoff - // @Description: The maximum throttle setting during automatic takeoff. If this is zero then THR_MAX is used for takeoff as well. - // @Units: Percent - // @Range: 0 100 - // @Increment: 1 - // @User: Advanced - ASCALAR(takeoff_throttle_max, "TKOFF_THR_MAX", 0), - - // @Param: THR_SLEWRATE - // @DisplayName: Throttle slew rate - // @Description: maximum percentage change in throttle per second. A setting of 10 means to not change the throttle by more than 10% of the full throttle range in one second. - // @Units: Percent - // @Range: 0 127 - // @Increment: 1 - // @User: Standard - ASCALAR(throttle_slewrate, "THR_SLEWRATE", 100), - - // @Param: FLAP_SLEWRATE - // @DisplayName: Flap slew rate - // @Description: maximum percentage change in flap output per second. A setting of 25 means to not change the flap by more than 25% of the full flap range in one second. A value of 0 means no rate limiting. - // @Units: Percent - // @Range: 0 100 - // @Increment: 1 - // @User: Advanced - GSCALAR(flap_slewrate, "FLAP_SLEWRATE", 75), - - // @Param: THR_SUPP_MAN - // @DisplayName: Throttle suppress manual passthru - // @Description: When throttle is suppressed in auto mode it is normally forced to zero. If you enable this option, then while suppressed it will be manual throttle. This is useful on petrol engines to hold the idle throttle manually while waiting for takeoff - // @Values: 0:Disabled,1:Enabled - // @User: Advanced - GSCALAR(throttle_suppress_manual,"THR_SUPP_MAN", 0), - - // @Param: THR_PASS_STAB - // @DisplayName: Throttle passthru in stabilize - // @Description: If this is set then when in STABILIZE, FBWA or ACRO modes the throttle is a direct passthru from the transmitter. This means the THR_MIN and THR_MAX settings are not used in these modes. This is useful for petrol engines where you setup a throttle cut switch that suppresses the throttle below the normal minimum. - // @Values: 0:Disabled,1:Enabled - // @User: Advanced - GSCALAR(throttle_passthru_stabilize,"THR_PASS_STAB", 0), - - // @Param: THR_FAILSAFE - // @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 - // @User: Standard - GSCALAR(throttle_fs_enabled, "THR_FAILSAFE", THROTTLE_FAILSAFE), - - - // @Param: THR_FS_VALUE - // @DisplayName: Throttle Failsafe Value - // @Description: The PWM level on channel 3 below which throttle failsafe triggers - // @Range: 925 1100 - // @Increment: 1 - // @User: Standard - GSCALAR(throttle_fs_value, "THR_FS_VALUE", THROTTLE_FS_VALUE), - - // @Param: TRIM_THROTTLE - // @DisplayName: Throttle cruise percentage - // @Description: The target percentage of throttle to apply for normal flight - // @Units: Percent - // @Range: 0 100 - // @Increment: 1 - // @User: Standard - ASCALAR(throttle_cruise, "TRIM_THROTTLE", THROTTLE_CRUISE), - - // @Param: THROTTLE_NUDGE - // @DisplayName: Throttle nudge enable - // @Description: When enabled, this uses the throttle input in auto-throttle modes to 'nudge' the throttle or airspeed to higher or lower values. When you have an airspeed sensor the nudge affects the target airspeed, so that throttle inputs above 50% will increase the target airspeed from TRIM_ARSPD_CM up to a maximum of ARSPD_FBW_MAX. When no airspeed sensor is enabled the throttle nudge will push up the target throttle for throttle inputs above 50%. - // @Values: 0:Disabled,1:Enabled - // @User: Standard - // @User: Standard - GSCALAR(throttle_nudge, "THROTTLE_NUDGE", 1), - - // @Param: FS_SHORT_ACTN - // @DisplayName: Short failsafe action - // @Description: The action to take on a short (FS_SHORT_TIMEOUT) failsafe event. A short failsafe even can be triggered either by loss of RC control (see THR_FS_VALUE) or by loss of GCS control (see FS_GCS_ENABL). If in CIRCLE or RTL mode this parameter is ignored. A short failsafe event in stabilization and manual modes will cause an change to CIRCLE mode if FS_SHORT_ACTN is 0 or 1, and a change to FBWA mode if FS_SHORT_ACTN is 2. In all other modes (AUTO, GUIDED and LOITER) a short failsafe event will cause no mode change is FS_SHORT_ACTN is set to 0, will cause a change to CIRCLE mode if set to 1 and will change to FBWA mode if set to 2. Please see the documentation for FS_LONG_ACTN for the behaviour after FS_LONG_TIMEOUT seconds of failsafe. - // @Values: 0:CIRCLE/no change(if already in AUTO|GUIDED|LOITER),1:CIRCLE,2:FBWA - // @User: Standard - GSCALAR(short_fs_action, "FS_SHORT_ACTN", SHORT_FAILSAFE_ACTION), - - // @Param: FS_SHORT_TIMEOUT - // @DisplayName: Short failsafe timeout - // @Description: The time in seconds that a failsafe condition has to persist before a short failsafe event will occur. This defaults to 1.5 seconds - // @Units: seconds - // @Range: 1 100 - // @Increment: 0.5 - // @User: Standard - GSCALAR(short_fs_timeout, "FS_SHORT_TIMEOUT", 1.5f), - - // @Param: FS_LONG_ACTN - // @DisplayName: Long failsafe action - // @Description: The action to take on a long (FS_LONG_TIMEOUT seconds) failsafe event. If the aircraft was in a stabilization or manual mode when failsafe started and a long failsafe occurs then it will change to RTL mode if FS_LONG_ACTN is 0 or 1, and will change to FBWA if FS_LONG_ACTN is set to 2. If the aircraft was in an auto mode (such as AUTO or GUIDED) when the failsafe started then it will continue in the auto mode if FS_LONG_ACTN is set to 0, will change to RTL mode if FS_LONG_ACTN is set to 1 and will change to FBWA mode if FS_LONG_ACTN is set to 2. - // @Values: 0:Continue,1:ReturnToLaunch,2:Glide - // @User: Standard - GSCALAR(long_fs_action, "FS_LONG_ACTN", LONG_FAILSAFE_ACTION), - - // @Param: FS_LONG_TIMEOUT - // @DisplayName: Long failsafe timeout - // @Description: The time in seconds that a failsafe condition has to persist before a long failsafe event will occur. This defaults to 5 seconds. - // @Units: seconds - // @Range: 1 300 - // @Increment: 0.5 - // @User: Standard - GSCALAR(long_fs_timeout, "FS_LONG_TIMEOUT", 5), - - // @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 continuously for 10 seconds then the plane will switch to RTL mode. - // @Units: Volts - // @Increment: 0.1 - // @User: Standard - GSCALAR(fs_batt_voltage, "FS_BATT_VOLTAGE", 0), - - // @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 plane will switch to RTL mode immediately. - // @Units: mAh - // @Increment: 50 - // @User: Standard - GSCALAR(fs_batt_mah, "FS_BATT_MAH", 0), - - // @Param: FS_GCS_ENABL - // @DisplayName: GCS failsafe enable - // @Description: Enable ground control station telemetry failsafe. Failsafe will trigger after FS_LONG_TIMEOUT seconds of no MAVLink heartbeat messages. There are two possible enabled settings. Seeing FS_GCS_ENABL to 1 means that GCS failsafe will be triggered when the aircraft has not received a MAVLink HEARTBEAT message. Setting FS_GCS_ENABL to 2 means that GCS failsafe will be triggered on either a loss of HEARTBEAT messages, or a RADIO_STATUS message from a MAVLink enabled 3DR radio indicating that the ground station is not receiving status updates from the aircraft, which is indicated by the RADIO_STATUS.remrssi field being zero (this may happen if you have a one way link due to asymmetric noise on the ground station and aircraft radios).Setting FS_GCS_ENABL to 3 means that GCS failsafe will be triggered by Heartbeat(like option one), but only in AUTO mode. WARNING: Enabling this option opens up the possibility of your plane going into failsafe mode and running the motor on the ground it it loses contact with your ground station. If this option is enabled on an electric plane then you should enable ARMING_REQUIRED. - // @Values: 0:Disabled,1:Heartbeat,2:HeartbeatAndREMRSSI,3:HeartbeatAndAUTO - // @User: Standard - GSCALAR(gcs_heartbeat_fs_enabled, "FS_GCS_ENABL", GCS_FAILSAFE_OFF), - - // @Param: FLTMODE_CH - // @DisplayName: Flightmode channel - // @Description: RC Channel to use for flight mode control - // @User: Advanced - GSCALAR(flight_mode_channel, "FLTMODE_CH", FLIGHT_MODE_CHANNEL), - - // @Param: FLTMODE1 - // @DisplayName: FlightMode1 - // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,15:Guided - // @User: Standard - // @Description: Flight mode for switch position 1 (910 to 1230 and above 2049) - GSCALAR(flight_mode1, "FLTMODE1", FLIGHT_MODE_1), - - // @Param: FLTMODE2 - // @DisplayName: FlightMode2 - // @Description: Flight mode for switch position 2 (1231 to 1360) - // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,15:Guided - // @User: Standard - GSCALAR(flight_mode2, "FLTMODE2", FLIGHT_MODE_2), - - // @Param: FLTMODE3 - // @DisplayName: FlightMode3 - // @Description: Flight mode for switch position 3 (1361 to 1490) - // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,15:Guided - // @User: Standard - GSCALAR(flight_mode3, "FLTMODE3", FLIGHT_MODE_3), - - // @Param: FLTMODE4 - // @DisplayName: FlightMode4 - // @Description: Flight mode for switch position 4 (1491 to 1620) - // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,15:Guided - // @User: Standard - GSCALAR(flight_mode4, "FLTMODE4", FLIGHT_MODE_4), - - // @Param: FLTMODE5 - // @DisplayName: FlightMode5 - // @Description: Flight mode for switch position 5 (1621 to 1749) - // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,15:Guided - // @User: Standard - GSCALAR(flight_mode5, "FLTMODE5", FLIGHT_MODE_5), - - // @Param: FLTMODE6 - // @DisplayName: FlightMode6 - // @Description: Flight mode for switch position 6 (1750 to 2049) - // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,15:Guided - // @User: Standard - GSCALAR(flight_mode6, "FLTMODE6", FLIGHT_MODE_6), - - // @Param: INITIAL_MODE - // @DisplayName: Initial flight mode - // @Description: This selects the mode to start in on boot. This is useful for when you want to start in AUTO mode on boot without a receiver. - // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,15:Guided - // @User: Advanced - GSCALAR(initial_mode, "INITIAL_MODE", MANUAL), - - // @Param: LIM_ROLL_CD - // @DisplayName: Maximum Bank Angle - // @Description: The maximum commanded bank angle in either direction - // @Units: centi-Degrees - // @Range: 0 9000 - // @Increment: 1 - // @User: Standard - GSCALAR(roll_limit_cd, "LIM_ROLL_CD", HEAD_MAX_CENTIDEGREE), - - // @Param: LIM_PITCH_MAX - // @DisplayName: Maximum Pitch Angle - // @Description: The maximum commanded pitch up angle - // @Units: centi-Degrees - // @Range: 0 9000 - // @Increment: 1 - // @User: Standard - ASCALAR(pitch_limit_max_cd, "LIM_PITCH_MAX", PITCH_MAX_CENTIDEGREE), - - // @Param: LIM_PITCH_MIN - // @DisplayName: Minimum Pitch Angle - // @Description: The minimum commanded pitch down angle - // @Units: centi-Degrees - // @Range: -9000 0 - // @Increment: 1 - // @User: Standard - ASCALAR(pitch_limit_min_cd, "LIM_PITCH_MIN", PITCH_MIN_CENTIDEGREE), - - // @Param: ACRO_ROLL_RATE - // @DisplayName: ACRO mode roll rate - // @Description: The maximum roll rate at full stick deflection in ACRO mode - // @Units: degrees/second - // @Range: 10 500 - // @Increment: 1 - // @User: Standard - GSCALAR(acro_roll_rate, "ACRO_ROLL_RATE", 180), - - // @Param: ACRO_PITCH_RATE - // @DisplayName: ACRO mode pitch rate - // @Description: The maximum pitch rate at full stick deflection in ACRO mode - // @Units: degrees/second - // @Range: 10 500 - // @Increment: 1 - // @User: Standard - GSCALAR(acro_pitch_rate, "ACRO_PITCH_RATE", 180), - - // @Param: ACRO_LOCKING - // @DisplayName: ACRO mode attitude locking - // @Description: Enable attitude locking when sticks are released - // @Values: 0:Disabled,1:Enabled - // @User: Standard - GSCALAR(acro_locking, "ACRO_LOCKING", 0), - - // @Param: GROUND_STEER_ALT - // @DisplayName: Ground steer altitude - // @Description: Altitude at which to use the ground steering controller on the rudder. If non-zero then the STEER2SRV controller will be used to control the rudder for altitudes within this limit of the home altitude. - // @Units: Meters - // @Range: -100 100 - // @Increment: 0.1 - // @User: Standard - GSCALAR(ground_steer_alt, "GROUND_STEER_ALT", 0), - - // @Param: GROUND_STEER_DPS - // @DisplayName: Ground steer rate - // @Description: Ground steering rate in degrees per second for full rudder stick deflection - // @Units: degrees/second - // @Range: 10 360 - // @Increment: 1 - // @User: Advanced - GSCALAR(ground_steer_dps, "GROUND_STEER_DPS", 90), - - // @Param: TRIM_AUTO - // @DisplayName: Automatic trim adjustment - // @Description: Set RC trim PWM levels to current levels when switching away from manual mode. When this option is enabled and you change from MANUAL to any other mode then the APM will take the current position of the control sticks as the trim values for aileron, elevator and rudder. It will use those to set RC1_TRIM, RC2_TRIM and RC4_TRIM. This option is disabled by default as if a pilot is not aware of this option and changes from MANUAL to another mode while control inputs are not centered then the trim could be changed to a dangerously bad value. You can enable this option to assist with trimming your plane, by enabling it before takeoff then switching briefly to MANUAL in flight, and seeing how the plane reacts. You can then switch back to FBWA, trim the surfaces then again test MANUAL mode. Each time you switch from MANUAL the APM will take your control inputs as the new trim. After you have good trim on your aircraft you can disable TRIM_AUTO for future flights. - // @Values: 0:Disabled,1:Enabled - // @User: Standard - GSCALAR(auto_trim, "TRIM_AUTO", AUTO_TRIM), - - // @Param: ELEVON_MIXING - // @DisplayName: Elevon mixing - // @Description: This enables an older form of elevon mixing which is now deprecated. Please see the ELEVON_OUTPUT option for setting up elevons. The ELEVON_MIXING option should be set to 0 for elevon planes except for backwards compatibility with older setups. - // @Values: 0:Disabled,1:Enabled - // @User: User - GSCALAR(mix_mode, "ELEVON_MIXING", ELEVON_MIXING), - - // @Param: ELEVON_REVERSE - // @DisplayName: Elevon reverse - // @Description: Reverse elevon mixing - // @Values: 0:Disabled,1:Enabled - // @User: User - GSCALAR(reverse_elevons, "ELEVON_REVERSE", ELEVON_REVERSE), - - - // @Param: ELEVON_CH1_REV - // @DisplayName: Elevon reverse - // @Description: Reverse elevon channel 1 - // @Values: -1:Disabled,1:Enabled - // @User: User - GSCALAR(reverse_ch1_elevon, "ELEVON_CH1_REV", ELEVON_CH1_REVERSE), - - // @Param: ELEVON_CH2_REV - // @DisplayName: Elevon reverse - // @Description: Reverse elevon channel 2 - // @Values: -1:Disabled,1:Enabled - // @User: User - GSCALAR(reverse_ch2_elevon, "ELEVON_CH2_REV", ELEVON_CH2_REVERSE), - - // @Param: VTAIL_OUTPUT - // @DisplayName: VTail output - // @Description: Enable VTail output in software. If enabled then the APM will provide software VTail mixing on the elevator and rudder channels. There are 4 different mixing modes available, which refer to the 4 ways the elevator can be mapped to the two VTail servos. Note that you must not use VTail output mixing with hardware pass-through of RC values, such as with channel 8 manual control on an APM1. So if you use an APM1 then set FLTMODE_CH to something other than 8 before you enable VTAIL_OUTPUT. Please also see the MIXING_GAIN parameter for the output gain of the mixer. - // @Values: 0:Disabled,1:UpUp,2:UpDown,3:DownUp,4:DownDown - // @User: User - GSCALAR(vtail_output, "VTAIL_OUTPUT", 0), - - // @Param: ELEVON_OUTPUT - // @DisplayName: Elevon output - // @Description: Enable software elevon output mixer. If enabled then the APM will provide software elevon mixing on the aileron and elevator channels. There are 4 different mixing modes available, which refer to the 4 ways the elevator can be mapped to the two elevon servos. Note that you must not use elevon output mixing with hardware pass-through of RC values, such as with channel 8 manual control on an APM1. So if you use an APM1 then set FLTMODE_CH to something other than 8 before you enable ELEVON_OUTPUT. Please also see the MIXING_GAIN parameter for the output gain of the mixer. - // @Values: 0:Disabled,1:UpUp,2:UpDown,3:DownUp,4:DownDown - // @User: User - GSCALAR(elevon_output, "ELEVON_OUTPUT", 0), - - // @Param: MIXING_GAIN - // @DisplayName: Mixing Gain - // @Description: The gain for the Vtail and elevon output mixers. The default is 0.5, which ensures that the mixer doesn't saturate, allowing both input channels to go to extremes while retaining control over the output. Hardware mixers often have a 1.0 gain, which gives more servo throw, but can saturate. If you don't have enough throw on your servos with VTAIL_OUTPUT or ELEVON_OUTPUT enabled then you can raise the gain using MIXING_GAIN. The mixer allows outputs in the range 900 to 2100 microseconds. - // @Range: 0.5 1.2 - // @User: User - GSCALAR(mixing_gain, "MIXING_GAIN", 0.5f), - - // @Param: RUDDER_ONLY - // @DisplayName: Rudder only aircraft - // @Description: Enable rudder only mode. The rudder will control attitude in attitude controlled modes (such as FBWA). You should setup your transmitter to send roll stick inputs to the RCMAP_YAW channel (normally channel 4). The rudder servo should be attached to the RCMAP_YAW channel as well. Note that automatic ground steering will be disabled for rudder only aircraft. You should also set KFF_RDDRMIX to 1.0. You will also need to setup the YAW2SRV_DAMP yaw damping appropriately for your aircraft. A value of 0.5 for YAW2SRV_DAMP is a good starting point. - // @Values: 0:Disabled,1:Enabled - // @User: User - GSCALAR(rudder_only, "RUDDER_ONLY", 0), - - // @Param: SYS_NUM_RESETS - // @DisplayName: Num Resets - // @Description: Number of APM board resets - // @User: Advanced - GSCALAR(num_resets, "SYS_NUM_RESETS", 0), - - // @Param: LOG_BITMASK - // @DisplayName: Log bitmask - // @Description: Bitmap of what log types to enable in dataflash. This values is made up of the sum of each of the log types you want to be saved on dataflash. On a PX4 or Pixhawk the large storage size of a microSD card means it is usually best just to enable all log types by setting this to 65535. On APM2 the smaller 4 MByte dataflash means you need to be more selective in your logging or you may run out of log space while flying (in which case it will wrap and overwrite the start of the log). The individual bits are ATTITUDE_FAST=1, ATTITUDE_MEDIUM=2, GPS=4, PerformanceMonitoring=8, ControlTuning=16, NavigationTuning=32, Mode=64, IMU=128, Commands=256, Battery=512, Compass=1024, TECS=2048, Camera=4096, RCandServo=8192, Sonar=16384, Arming=32768, LogWhenDisarmed=65536, FullLogsArmedOnly=65535, FullLogsWhenDisarmed=131071 - // @Values: 0:Disabled,5190:APM2-Default,65535:PX4/Pixhawk-Default - // @Bitmask: 0:ATTITUDE_FAST,1:ATTITUDE_MED,2:GPS,3:PM,4:CTUN,5:NTUN,6:MODE,7:IMU,8:CMD,9:CURRENT,10:COMPASS,11:TECS,12:CAMERA,13:RC,14:SONAR,15:ARM/DISARM,16:WHEN_DISARMED,19:IMU_RAW - // @User: Advanced - GSCALAR(log_bitmask, "LOG_BITMASK", DEFAULT_LOG_BITMASK), - - // @Param: RST_SWITCH_CH - // @DisplayName: Reset Switch Channel - // @Description: RC channel to use to reset to last flight mode after geofence takeover. - // @User: Advanced - GSCALAR(reset_switch_chan, "RST_SWITCH_CH", 0), - - // @Param: RST_MISSION_CH - // @DisplayName: Reset Mission Channel - // @Description: RC channel to use to reset the mission to the first waypoint. When this channel goes above 1750 the mission is reset. Set RST_MISSION_CH to 0 to disable. - // @User: Advanced - GSCALAR(reset_mission_chan, "RST_MISSION_CH", 0), - - // @Param: TRIM_ARSPD_CM - // @DisplayName: Target airspeed - // @Description: Airspeed in cm/s to aim for when airspeed is enabled in auto mode. This is a calibrated (apparent) airspeed. - // @Units: cm/s - // @User: User - GSCALAR(airspeed_cruise_cm, "TRIM_ARSPD_CM", AIRSPEED_CRUISE_CM), - - // @Param: SCALING_SPEED - // @DisplayName: speed used for speed scaling calculations - // @Description: Airspeed in m/s to use when calculating surface speed scaling. Note that changing this value will affect all PID values - // @Units: m/s - // @User: Advanced - GSCALAR(scaling_speed, "SCALING_SPEED", SCALING_SPEED), - - // @Param: MIN_GNDSPD_CM - // @DisplayName: Minimum ground speed - // @Description: Minimum ground speed in cm/s when under airspeed control - // @Units: cm/s - // @User: Advanced - GSCALAR(min_gndspeed_cm, "MIN_GNDSPD_CM", MIN_GNDSPEED_CM), - - // @Param: TRIM_PITCH_CD - // @DisplayName: Pitch angle offset - // @Description: offset to add to pitch - used for in-flight pitch trimming. It is recommended that instead of using this parameter you level your plane correctly on the ground for good flight attitude. - // @Units: centi-Degrees - // @User: Advanced - GSCALAR(pitch_trim_cd, "TRIM_PITCH_CD", 0), - - // @Param: ALT_HOLD_RTL - // @DisplayName: RTL altitude - // @Description: Return to launch target altitude. This is the relative altitude the plane will aim for and loiter at when returning home. If this is negative (usually -1) then the plane will use the current altitude at the time of entering RTL. Note that when transiting to a Rally Point the altitude of the Rally Point is used instead of ALT_HOLD_RTL. - // @Units: centimeters - // @User: User - GSCALAR(RTL_altitude_cm, "ALT_HOLD_RTL", ALT_HOLD_HOME_CM), - - // @Param: ALT_HOLD_FBWCM - // @DisplayName: Minimum altitude for FBWB mode - // @Description: This is the minimum altitude in centimeters that FBWB and CRUISE modes will allow. If you attempt to descend below this altitude then the plane will level off. A value of zero means no limit. - // @Units: centimeters - // @User: User - GSCALAR(FBWB_min_altitude_cm, "ALT_HOLD_FBWCM", ALT_HOLD_FBW_CM), - - // @Param: MAG_ENABLE - // @DisplayName: Enable Compass - // @Description: Setting this to Enabled(1) will enable the compass. Setting this to Disabled(0) will disable the compass. Note that this is separate from COMPASS_USE. This will enable the low level senor, and will enable logging of magnetometer data. To use the compass for navigation you must also set COMPASS_USE to 1. - // @Values: 0:Disabled,1:Enabled - // @User: Standard - GSCALAR(compass_enabled, "MAG_ENABLE", 1), - - // @Param: FLAP_IN_CHANNEL - // @DisplayName: Flap input channel - // @Description: An RC input channel to use for flaps control. If this is set to a RC channel number then that channel will be used for manual flaps control. When enabled, the percentage of flaps is taken as the percentage travel from the TRIM value of the channel to the MIN value of the channel. A value above the TRIM values will give inverse flaps (spoilers). This option needs to be enabled in conjunction with a FUNCTION setting on an output channel to one of the flap functions. When a FLAP_IN_CHANNEL is combined with auto-flaps the higher of the two flap percentages is taken. You must also enable a FLAPERON_OUTPUT flaperon mixer setting if using flaperons. - // @User: User - GSCALAR(flapin_channel, "FLAP_IN_CHANNEL", 0), - - // @Param: FLAPERON_OUTPUT - // @DisplayName: Flaperon output - // @Description: Enable flaperon output in software. If enabled then the APM will provide software flaperon mixing on the FLAPERON1 and FLAPERON2 output channels specified using the FUNCTION on two auxiliary channels. There are 4 different mixing modes available, which refer to the 4 ways the flap and aileron outputs can be mapped to the two flaperon servos. Note that you must not use flaperon output mixing with hardware pass-through of RC values, such as with channel 8 manual control on an APM1. So if you use an APM1 then set FLTMODE_CH to something other than 8 before you enable FLAPERON_OUTPUT. Please also see the MIXING_GAIN parameter for the output gain of the mixer. FLAPERON_OUTPUT cannot be combined with ELEVON_OUTPUT or ELEVON_MIXING. - // @Values: 0:Disabled,1:UpUp,2:UpDown,3:DownUp,4:DownDown - // @User: User - GSCALAR(flaperon_output, "FLAPERON_OUTPUT", 0), - - // @Param: FLAP_1_PERCNT - // @DisplayName: Flap 1 percentage - // @Description: The percentage change in flap position when FLAP_1_SPEED is reached. Use zero to disable flaps - // @Range: 0 100 - // @Units: Percent - // @User: Advanced - GSCALAR(flap_1_percent, "FLAP_1_PERCNT", FLAP_1_PERCENT), - - // @Param: FLAP_1_SPEED - // @DisplayName: Flap 1 speed - // @Description: The speed in meters per second at which to engage FLAP_1_PERCENT of flaps. Note that FLAP_1_SPEED should be greater than or equal to FLAP_2_SPEED - // @Range: 0 100 - // @Increment: 1 - // @Units: m/s - // @User: Advanced - GSCALAR(flap_1_speed, "FLAP_1_SPEED", FLAP_1_SPEED), - - // @Param: FLAP_2_PERCNT - // @DisplayName: Flap 2 percentage - // @Description: The percentage change in flap position when FLAP_2_SPEED is reached. Use zero to disable flaps - // @Range: 0 100 - // @Units: Percent - // @User: Advanced - GSCALAR(flap_2_percent, "FLAP_2_PERCNT", FLAP_2_PERCENT), - - // @Param: FLAP_2_SPEED - // @DisplayName: Flap 2 speed - // @Description: The speed in meters per second at which to engage FLAP_2_PERCENT of flaps. Note that FLAP_1_SPEED should be greater than or equal to FLAP_2_SPEED - // @Range: 0 100 - // @Units: m/s - // @Increment: 1 - // @User: Advanced - GSCALAR(flap_2_speed, "FLAP_2_SPEED", FLAP_2_SPEED), - - // @Param: LAND_FLAP_PERCNT - // @DisplayName: Landing flap percentage - // @Description: The amount of flaps (as a percentage) to apply in the landing approach and flare of an automatic landing - // @Range: 0 100 - // @Units: Percent - // @User: Advanced - GSCALAR(land_flap_percent, "LAND_FLAP_PERCNT", 0), - -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 - // @Param: OVERRIDE_CHAN - // @DisplayName: PX4IO override channel - // @Description: If set to a non-zero value then this is an RC input channel number to use for testing manual control in case the main FMU microcontroller on a PX4 or Pixhawk fails. When this RC input channel goes above 1750 the FMU will stop sending servo controls to the PX4IO board, which will trigger the PX4IO board to start using its failsafe override behaviour, which should give you manual control of the aircraft. That allows you to test for correct manual behaviour without actually crashing the FMU. This parameter is normally only set to a non-zero value for ground testing purposes. When the override channel is used it also forces the PX4 safety switch into an armed state. This allows it to be used as a way to re-arm a plane after an in-flight reboot. Use in that way is considered a developer option, for people testing unstable developer code. Note that you may set OVERRIDE_CHAN to the same channel as FLTMODE_CH to get PX4IO based override when in flight mode 6. Note that when override is triggered the 6 auxiliary output channels on Pixhawk will no longer be updated, so all the flight controls you need must be assigned to the first 8 channels. - // @User: Advanced - GSCALAR(override_channel, "OVERRIDE_CHAN", 0), -#endif - - // @Param: INVERTEDFLT_CH - // @DisplayName: Inverted flight channel - // @Description: A RC input channel number to enable inverted flight. If this is non-zero then the APM will monitor the corresponding RC input channel and will enable inverted flight when the channel goes above 1750. - // @Values: 0:Disabled,1:Channel1,2:Channel2,3:Channel3,4:Channel4,5:Channel5,6:Channel6,7:Channel7,8:Channel8 - // @User: Standard - GSCALAR(inverted_flight_ch, "INVERTEDFLT_CH", 0), - -#if HIL_SUPPORT - // @Param: HIL_MODE - // @DisplayName: HIL mode enable - // @Description: This enables and disables hardware in the loop mode. If HIL_MODE is 1 then on the next reboot all sensors are replaced with HIL sensors which come from the GCS. - // @Values: 0:Disabled,1:Enabled - // @User: Advanced - GSCALAR(hil_mode, "HIL_MODE", 0), -#endif - - // @Param: HIL_SERVOS - // @DisplayName: HIL Servos enable - // @Description: This controls whether real servo controls are used in HIL mode. If you enable this then the APM will control the real servos in HIL mode. If disabled it will report servo values, but will not output to the real servos. Be careful that your motor and propeller are not connected if you enable this option. - // @Values: 0:Disabled,1:Enabled - // @User: Advanced - GSCALAR(hil_servos, "HIL_SERVOS", 0), - - // @Param: HIL_ERR_LIMIT - // @DisplayName: Limit of error in HIL attitude before reset - // @Description: This controls the maximum error in degrees on any axis before HIL will reset the DCM attitude to match the HIL_STATE attitude. This limit will prevent poor timing on HIL from causing a major attitude error. If the value is zero then no limit applies. - // @Units: degrees - // @Range: 0 90 - // @Increment: 0.1 - // @User: Advanced - GSCALAR(hil_err_limit, "HIL_ERR_LIMIT", 5), - - // @Param: RTL_AUTOLAND - // @DisplayName: RTL auto land - // @Description: Automatically begin landing sequence after arriving at RTL location. This requires the addition of a DO_LAND_START mission item, which acts as a marker for the start of a landing sequence. The closest landing sequence will be chosen to the current location. - // @Values: 0:Disable,1:Enable - go HOME then land,2:Enable - go directly to landing sequence - // @User: Standard - GSCALAR(rtl_autoland, "RTL_AUTOLAND", 0), - - // @Param: RC_TRIM_AT_START - // @DisplayName: RC Trims auto set at start. - // @Description: Automatically set roll/pitch trim from Tx at ground start. This makes the assumption that the RC transmitter has not been altered since trims were last captured. - // @Values: 0:Disable,1:Enable - // @User: Standard - GSCALAR(trim_rc_at_start, "TRIM_RC_AT_START", 0), - - // @Param: CRASH_DETECT - // @DisplayName: Crash Detection - // @Description: Automatically detect a crash during AUTO flight and perform the bitmask selected action(s). Disarm will turn off motor for saftey and to help against burning out ESC and motor. Setting the mode to manual will help save the servos from burning out by overexerting if the aircraft crashed in an odd orientation such as upsidedown. - // @Values: 0:Disabled,1:Disarm - // @Bitmask: 0:Disarm - // @User: Advanced - GSCALAR(crash_detection_enable, "CRASH_DETECT", 0), - - // barometer ground calibration. The GND_ prefix is chosen for - // compatibility with previous releases of ArduPlane - // @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), - -#if CAMERA == ENABLED - // @Group: CAM_ - // @Path: ../libraries/AP_Camera/AP_Camera.cpp - GOBJECT(camera, "CAM_", AP_Camera), -#endif - - // @Group: ARMING_ - // @Path: arming_checks.cpp,../libraries/AP_Arming/AP_Arming.cpp - GOBJECT(arming, "ARMING_", AP_Arming_Plane), - - // @Group: RELAY_ - // @Path: ../libraries/AP_Relay/AP_Relay.cpp - GOBJECT(relay, "RELAY_", AP_Relay), - -#if PARACHUTE == ENABLED - // @Group: CHUTE_ - // @Path: ../libraries/AP_Parachute/AP_Parachute.cpp - GOBJECT(parachute, "CHUTE_", AP_Parachute), - - // @Param: CHUTE_CHAN - // @DisplayName: Parachute release channel - // @Description: If set to a non-zero value then this is an RC input channel number to use for manually releasing the parachute. When this channel goes above 1700 the parachute will be released - // @User: Advanced - GSCALAR(parachute_channel, "CHUTE_CHAN", 0), -#endif - -#if RANGEFINDER_ENABLED == ENABLED - // @Group: RNGFND - // @Path: ../libraries/AP_RangeFinder/RangeFinder.cpp - GOBJECT(rangefinder, "RNGFND", RangeFinder), -#endif - - // @Param: RNGFND_LANDING - // @DisplayName: Enable rangefinder for landing - // @Description: This enables the use of a rangefinder for automatic landing. The rangefinder will be used both on the landing approach and for final flare - // @Values: 0:Disabled,1:Enabled - // @User: Standard - GSCALAR(rangefinder_landing, "RNGFND_LANDING", 0), - -#if AP_TERRAIN_AVAILABLE - // @Group: TERRAIN_ - // @Path: ../libraries/AP_Terrain/AP_Terrain.cpp - GOBJECT(terrain, "TERRAIN_", AP_Terrain), -#endif - - // @Group: ADSB_ - // @Path: ../libraries/AP_ADSB/AP_ADSB.cpp - GOBJECT(adsb, "ADSB_", AP_ADSB), - - // 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), - - // @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), - - // @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 - - // @Group: RLL2SRV_ - // @Path: ../libraries/APM_Control/AP_RollController.cpp - GOBJECT(rollController, "RLL2SRV_", AP_RollController), - - // @Group: PTCH2SRV_ - // @Path: ../libraries/APM_Control/AP_PitchController.cpp - GOBJECT(pitchController, "PTCH2SRV_", AP_PitchController), - - // @Group: YAW2SRV_ - // @Path: ../libraries/APM_Control/AP_YawController.cpp - GOBJECT(yawController, "YAW2SRV_", AP_YawController), - - // @Group: STEER2SRV_ - // @Path: ../libraries/APM_Control/AP_SteerController.cpp - GOBJECT(steerController, "STEER2SRV_", AP_SteerController), - - // variables not in the g class which contain EEPROM saved variables - - // @Group: COMPASS_ - // @Path: ../libraries/AP_Compass/Compass.cpp - GOBJECT(compass, "COMPASS_", Compass), - - // @Group: SCHED_ - // @Path: ../libraries/AP_Scheduler/AP_Scheduler.cpp - GOBJECT(scheduler, "SCHED_", AP_Scheduler), - - // @Group: RCMAP_ - // @Path: ../libraries/AP_RCMapper/AP_RCMapper.cpp - GOBJECT(rcmap, "RCMAP_", RCMapper), - - // @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: INS_ - // @Path: ../libraries/AP_InertialSensor/AP_InertialSensor.cpp - GOBJECT(ins, "INS_", AP_InertialSensor), - - // @Group: AHRS_ - // @Path: ../libraries/AP_AHRS/AP_AHRS.cpp - GOBJECT(ahrs, "AHRS_", AP_AHRS), - - // @Group: ARSPD_ - // @Path: ../libraries/AP_Airspeed/AP_Airspeed.cpp - GOBJECT(airspeed, "ARSPD_", AP_Airspeed), - - // @Group: NAVL1_ - // @Path: ../libraries/AP_L1_Control/AP_L1_Control.cpp - GOBJECT(L1_controller, "NAVL1_", AP_L1_Control), - - // @Group: TECS_ - // @Path: ../libraries/AP_TECS/AP_TECS.cpp - GOBJECT(TECS_controller, "TECS_", AP_TECS), - -#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 CONFIG_HAL_BOARD == HAL_BOARD_SITL - // @Group: SIM_ - // @Path: ../libraries/SITL/SITL.cpp - GOBJECT(sitl, "SIM_", SITL::SITL), -#endif - -#if OBC_FAILSAFE == ENABLED - // @Group: AFS_ - // @Path: ../libraries/APM_OBC/APM_OBC.cpp - GOBJECT(obc, "AFS_", APM_OBC), -#endif - -#if OPTFLOW == ENABLED - // @Group: FLOW - // @Path: ../libraries/AP_OpticalFlow/OpticalFlow.cpp - GOBJECT(optflow, "FLOW", OpticalFlow), -#endif - - // @Group: MIS_ - // @Path: ../libraries/AP_Mission/AP_Mission.cpp - GOBJECT(mission, "MIS_", AP_Mission), - - // @Group: RALLY_ - // @Path: ../libraries/AP_Rally/AP_Rally.cpp - GOBJECT(rally, "RALLY_", AP_Rally), - -#if AP_AHRS_NAVEKF_AVAILABLE - // @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), -#endif - - // @Group: RPM - // @Path: ../libraries/AP_RPM/AP_RPM.cpp - GOBJECT(rpm_sensor, "RPM", AP_RPM), - - // @Group: RSSI_ - // @Path: ../libraries/AP_RSSI/AP_RSSI.cpp - GOBJECT(rssi, "RSSI_", AP_RSSI), - - 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_pidServoRoll, 0, AP_PARAM_FLOAT, "RLL2SRV_P" }, - { Parameters::k_param_pidServoRoll, 1, AP_PARAM_FLOAT, "RLL2SRV_I" }, - { Parameters::k_param_pidServoRoll, 2, AP_PARAM_FLOAT, "RLL2SRV_D" }, - { Parameters::k_param_pidServoRoll, 3, AP_PARAM_FLOAT, "RLL2SRV_IMAX" }, - - { Parameters::k_param_pidServoPitch, 0, AP_PARAM_FLOAT, "PTCH2SRV_P" }, - { Parameters::k_param_pidServoPitch, 1, AP_PARAM_FLOAT, "PTCH2SRV_I" }, - { Parameters::k_param_pidServoPitch, 2, AP_PARAM_FLOAT, "PTCH2SRV_D" }, - { Parameters::k_param_pidServoPitch, 3, AP_PARAM_FLOAT, "PTCH2SRV_IMAX" }, - - { 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_curr_amp_offset, 0, AP_PARAM_FLOAT, "BATT_AMP_OFFSET" }, - { 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_rally_limit_km_old, 0, AP_PARAM_FLOAT, "RALLY_LIMIT_KM" }, - { Parameters::k_param_rally_total_old, 0, AP_PARAM_INT8, "RALLY_TOTAL" }, - { 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 Plane::load_parameters(void) -{ - if (!AP_Param::check_var_info()) { - cliSerial->printf("Bad parameter table\n"); - AP_HAL::panic("Bad parameter table"); - } - 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/ArduSubsea/Parameters.h b/ArduSubsea/Parameters.h deleted file mode 100644 index 8cdc2af4bf..0000000000 --- a/ArduSubsea/Parameters.h +++ /dev/null @@ -1,541 +0,0 @@ -// -*- 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 value of k_format_version determines whether the existing - * eeprom data is considered valid. You should only change this - * value under the following circumstances: - * - * 1) the meaning of an existing eeprom parameter changes - * - * 2) the value of an existing k_param_* enum value changes - * - * Adding a new parameter should _not_ require a change to - * k_format_version except under special circumstances. If you - * change it anyway then all ArduPlane users will need to reload all - * their parameters. We want that to be an extremely rare - * thing. Please do not just change it "just in case". - * - * To determine if a k_param_* value has changed, use the rules of - * C++ enums to work out the value of the neighboring enum - * values. If you don't know the C++ enum rules then please ask for - * help. - */ - - ////////////////////////////////////////////////////////////////// - // STOP!!! DO NOT CHANGE THIS VALUE UNTIL YOU FULLY UNDERSTAND THE - // COMMENTS ABOVE. IF UNSURE, ASK ANOTHER DEVELOPER!!! - static const uint16_t k_format_version = 13; - ////////////////////////////////////////////////////////////////// - - - // 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 = 0; // 0 for APM trunk - - enum { - // Layout version number, always key zero. - // - k_param_format_version = 0, - k_param_software_type, - k_param_num_resets, - - // Misc - // - k_param_auto_trim = 10, - k_param_log_bitmask_old, // unused - k_param_pitch_trim_cd, - k_param_mix_mode, - k_param_reverse_elevons, - k_param_reverse_ch1_elevon, - k_param_reverse_ch2_elevon, - k_param_flap_1_percent, - k_param_flap_1_speed, - k_param_flap_2_percent, - k_param_flap_2_speed, - k_param_reset_switch_chan, - k_param_manual_level, // unused - k_param_land_pitch_cd, - k_param_ins_old, // *** Deprecated, remove with next eeprom number change - k_param_stick_mixing, - k_param_reset_mission_chan, - k_param_land_flare_alt, - k_param_land_flare_sec, - k_param_crosstrack_min_distance, // unused - k_param_rudder_steer, // unused - k_param_throttle_nudge, - k_param_alt_offset, - k_param_ins, // libraries/AP_InertialSensor variables - k_param_takeoff_throttle_min_speed, - k_param_takeoff_throttle_min_accel, - k_param_takeoff_heading_hold, // unused - k_param_level_roll_limit, - k_param_hil_servos, - k_param_vtail_output, - k_param_nav_controller, - k_param_elevon_output, - k_param_att_controller, - k_param_mixing_gain, - k_param_scheduler, - k_param_relay, - k_param_takeoff_throttle_delay, - k_param_skip_gyro_cal, // unused - k_param_auto_fbw_steer, - k_param_waypoint_max_radius, - k_param_ground_steer_alt, - k_param_ground_steer_dps, - k_param_rally_limit_km_old, //unused anymore -- just holding this index - k_param_hil_err_limit, - k_param_sonar_old, // unused - k_param_log_bitmask, - k_param_BoardConfig, - k_param_rssi_range, // unused, replaced by rssi_ library parameters - k_param_flapin_channel, - k_param_flaperon_output, - k_param_gps, - k_param_autotune_level, - k_param_rally, - k_param_serial0_baud, // deprecated - k_param_serial1_baud, // deprecated - k_param_serial2_baud, // deprecated - k_param_takeoff_tdrag_elevator, - k_param_takeoff_tdrag_speed1, - k_param_takeoff_rotate_speed, - k_param_takeoff_throttle_slewrate, - k_param_takeoff_throttle_max, - k_param_rangefinder, - k_param_terrain, - k_param_terrain_follow, - k_param_stab_pitch_down_cd_old, // deprecated - k_param_glide_slope_min, - k_param_stab_pitch_down, - k_param_terrain_lookahead, - k_param_fbwa_tdrag_chan, - k_param_rangefinder_landing, - k_param_land_flap_percent, - k_param_takeoff_flap_percent, - k_param_flap_slewrate, - k_param_rtl_autoland, - k_param_override_channel, - k_param_stall_prevention, - k_param_optflow, - k_param_cli_enabled, - k_param_trim_rc_at_start, - k_param_hil_mode, - k_param_land_disarm_delay, - k_param_glide_slope_threshold, - k_param_rudder_only, - k_param_gcs3, // 93 - k_param_gcs_pid_mask, - k_param_crash_detection_enable, - k_param_land_abort_throttle_enable, - k_param_rssi = 97, - k_param_rpm_sensor, - k_param_parachute, - k_param_arming = 100, - k_param_parachute_channel, - - // 105: Extra parameters - k_param_fence_retalt = 105, - k_param_fence_autoenable, - k_param_fence_ret_rally, - - // 110: Telemetry control - // - k_param_gcs0 = 110, // stream rates for uartA - k_param_gcs1, // stream rates for uartC - k_param_sysid_this_mav, - k_param_sysid_my_gcs, - k_param_serial1_baud_old, // deprecated - k_param_telem_delay, - k_param_serial0_baud_old, // deprecated - k_param_gcs2, // stream rates for uartD - k_param_serial2_baud_old, // deprecated - k_param_serial2_protocol, // deprecated - - // 120: Fly-by-wire control - // - k_param_airspeed_min = 120, - k_param_airspeed_max, - k_param_FBWB_min_altitude_cm, // 0=disabled, minimum value for altitude in cm (for first time try 30 meters = 3000 cm) - k_param_flybywire_elev_reverse, - k_param_alt_control_algorithm, - k_param_flybywire_climb_rate, - k_param_acro_roll_rate, - k_param_acro_pitch_rate, - k_param_acro_locking, - - // - // 130: Sensor parameters - // - k_param_imu = 130, // unused - k_param_altitude_mix, - - k_param_compass_enabled, - k_param_compass, - k_param_battery_monitoring, // unused - k_param_volt_div_ratio, // unused - k_param_curr_amp_per_volt, // unused - k_param_input_voltage, // deprecated, can be deleted - k_param_pack_capacity, // unused - k_param_sonar_enabled_old, // unused - k_param_ahrs, // AHRS group - k_param_barometer, // barometer ground calibration - k_param_airspeed, // AP_Airspeed parameters - k_param_curr_amp_offset, - k_param_NavEKF, // Extended Kalman Filter Inertial Navigation Group - k_param_mission, // mission library - k_param_serial_manager, // serial manager library - k_param_NavEKF2, // EKF2 - - // - // 150: Navigation parameters - // - k_param_crosstrack_gain = 150, // unused - k_param_crosstrack_entry_angle, // unused - k_param_roll_limit_cd, - k_param_pitch_limit_max_cd, - k_param_pitch_limit_min_cd, - k_param_airspeed_cruise_cm, - k_param_RTL_altitude_cm, - k_param_inverted_flight_ch, - k_param_min_gndspeed_cm, - k_param_crosstrack_use_wind, // unused - - - // - // Camera and mount parameters - // - k_param_camera = 160, - k_param_camera_mount, - k_param_camera_mount2, // unused - k_param_adsb, - - // - // Battery monitoring parameters - // - k_param_battery = 166, - k_param_rssi_pin, // unused, replaced by rssi_ library parameters - 167 - k_param_battery_volt_pin, // unused - 168 - k_param_battery_curr_pin, // unused - 169 - - // - // 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_9, - k_param_rc_10, - k_param_rc_11, - - k_param_throttle_min, - k_param_throttle_max, - k_param_throttle_fs_enabled, - k_param_throttle_fs_value, - k_param_throttle_cruise, - - k_param_short_fs_action, - k_param_long_fs_action, - k_param_gcs_heartbeat_fs_enabled, - k_param_throttle_slewrate, - k_param_throttle_suppress_manual, - k_param_throttle_passthru_stabilize, - k_param_rc_12, - k_param_fs_batt_voltage, - k_param_fs_batt_mah, - k_param_short_fs_timeout, - k_param_long_fs_timeout, - k_param_rc_13, - k_param_rc_14, - - // - // 200: Feed-forward gains - // - k_param_kff_pitch_compensation = 200, // unused - k_param_kff_rudder_mix, - k_param_kff_pitch_to_throttle, // unused - k_param_kff_throttle_to_pitch, - k_param_scaling_speed, - - // - // 210: flight modes - // - k_param_flight_mode_channel = 210, - k_param_flight_mode1, - k_param_flight_mode2, - k_param_flight_mode3, - k_param_flight_mode4, - k_param_flight_mode5, - k_param_flight_mode6, - k_param_initial_mode, - - // - // 220: Waypoint data - // - k_param_waypoint_mode = 220, - k_param_command_total, // unused - k_param_command_index, // unused - k_param_waypoint_radius, - k_param_loiter_radius, - k_param_fence_action, - k_param_fence_total, - k_param_fence_channel, - k_param_fence_minalt, - k_param_fence_maxalt, - - // other objects - k_param_sitl = 230, - k_param_obc, - k_param_rollController, - k_param_pitchController, - k_param_yawController, - k_param_L1_controller, - k_param_rcmap, - k_param_TECS_controller, - k_param_rally_total_old, //unused - k_param_steerController, - - // - // 240: PID Controllers - k_param_pidNavRoll = 240, // unused - k_param_pidServoRoll, // unused - k_param_pidServoPitch, // unused - k_param_pidNavPitchAirspeed, // unused - k_param_pidServoRudder, // unused - k_param_pidTeThrottle, // unused - k_param_pidNavPitchAltitude, // unused - k_param_pidWheelSteer, // unused - - k_param_DataFlash = 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 hil_err_limit; - - AP_Int8 rtl_autoland; - - AP_Int8 trim_rc_at_start; - AP_Int8 crash_detection_enable; - - // Feed-forward gains - // - AP_Float kff_rudder_mix; - AP_Float kff_pitch_to_throttle; - AP_Float kff_throttle_to_pitch; - AP_Float ground_steer_alt; - AP_Int16 ground_steer_dps; - AP_Float stab_pitch_down; - - // speed used for speed scaling - AP_Float scaling_speed; - - // navigation controller type. See AP_Navigation::ControllerType - AP_Int8 nav_controller; - - // attitude controller type. - AP_Int8 att_controller; - - AP_Int8 auto_fbw_steer; - - // Estimation - // - AP_Float altitude_mix; - AP_Int8 alt_control_algorithm; - - // Waypoints - // - AP_Int8 waypoint_mode; - AP_Int16 waypoint_radius; - AP_Int16 waypoint_max_radius; - AP_Int16 loiter_radius; - -#if GEOFENCE_ENABLED == ENABLED - AP_Int8 fence_action; - AP_Int8 fence_total; - AP_Int8 fence_channel; - AP_Int16 fence_minalt; // meters - AP_Int16 fence_maxalt; // meters - AP_Int16 fence_retalt; // meters - AP_Int8 fence_autoenable; - AP_Int8 fence_ret_rally; -#endif - - // Fly-by-wire - // - AP_Int8 flybywire_elev_reverse; - AP_Int8 flybywire_climb_rate; - - // Throttle - // - AP_Int8 throttle_suppress_manual; - AP_Int8 throttle_passthru_stabilize; - AP_Int8 throttle_fs_enabled; - AP_Int16 throttle_fs_value; - AP_Int8 throttle_nudge; - - // Failsafe - AP_Int8 short_fs_action; - AP_Int8 long_fs_action; - AP_Float short_fs_timeout; - AP_Float long_fs_timeout; - AP_Int8 gcs_heartbeat_fs_enabled; - AP_Float fs_batt_voltage; - AP_Float fs_batt_mah; - - // Flight modes - // - AP_Int8 flight_mode_channel; - 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 initial_mode; - - // Navigational maneuvering limits - // - AP_Int16 roll_limit_cd; - AP_Int16 alt_offset; - AP_Int16 acro_roll_rate; - AP_Int16 acro_pitch_rate; - AP_Int8 acro_locking; - - // Misc - // - AP_Int8 auto_trim; - AP_Int8 mix_mode; - AP_Int8 vtail_output; - AP_Int8 elevon_output; - AP_Int8 rudder_only; - AP_Float mixing_gain; - AP_Int8 reverse_elevons; - AP_Int8 reverse_ch1_elevon; - AP_Int8 reverse_ch2_elevon; - AP_Int16 num_resets; - AP_Int32 log_bitmask; - AP_Int8 reset_switch_chan; - AP_Int8 reset_mission_chan; - AP_Int32 airspeed_cruise_cm; - AP_Int32 RTL_altitude_cm; - AP_Float land_flare_alt; - AP_Int8 land_disarm_delay; - AP_Int8 land_abort_throttle_enable; - AP_Int32 min_gndspeed_cm; - AP_Int16 pitch_trim_cd; - AP_Int16 FBWB_min_altitude_cm; - AP_Int8 hil_servos; -#if HIL_SUPPORT - AP_Int8 hil_mode; -#endif - - AP_Int8 compass_enabled; - AP_Int8 flap_1_percent; - AP_Int8 flap_1_speed; - AP_Int8 flap_2_percent; - AP_Int8 flap_2_speed; - AP_Int8 land_flap_percent; - AP_Int8 takeoff_flap_percent; - AP_Int8 inverted_flight_ch; // 0=disabled, 1-8 is channel for inverted flight trigger - AP_Int8 stick_mixing; - AP_Float takeoff_throttle_min_speed; - AP_Float takeoff_throttle_min_accel; - AP_Int8 takeoff_throttle_delay; - AP_Int8 takeoff_tdrag_elevator; - AP_Float takeoff_tdrag_speed1; - AP_Float takeoff_rotate_speed; - AP_Int8 takeoff_throttle_slewrate; - AP_Int8 level_roll_limit; - AP_Int8 flapin_channel; - AP_Int8 flaperon_output; -#if AP_TERRAIN_AVAILABLE - AP_Int8 terrain_follow; - AP_Int16 terrain_lookahead; -#endif - AP_Int16 glide_slope_min; - AP_Float glide_slope_threshold; - AP_Int8 fbwa_tdrag_chan; - AP_Int8 rangefinder_landing; - AP_Int8 flap_slewrate; -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 - AP_Int8 override_channel; -#endif - AP_Int16 gcs_pid_mask; - AP_Int8 parachute_channel; - - // 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; - RC_Channel_aux rc_10; - RC_Channel_aux rc_11; - RC_Channel_aux rc_12; - RC_Channel_aux rc_13; - RC_Channel_aux rc_14; -#endif - uint8_t _dummy; - - Parameters() : - // variable default - //---------------------------------------- - 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), - rc_10 (CH_10), - rc_11 (CH_11), - rc_12 (CH_12), - rc_13 (CH_13), - rc_14 (CH_14), -#endif - _dummy(0) - {} -}; - -extern const AP_Param::Info var_info[]; - -#endif // PARAMETERS_H diff --git a/ArduSubsea/Parameters.pde b/ArduSubsea/Parameters.pde deleted file mode 100644 index 10d157b86c..0000000000 --- a/ArduSubsea/Parameters.pde +++ /dev/null @@ -1,3 +0,0 @@ -/* - blank file. This is needed to help with upgrades of old versions if MissionPlanner - */ diff --git a/ArduSubsea/Plane.cpp b/ArduSubsea/Plane.cpp deleted file mode 100644 index 2529166341..0000000000 --- a/ArduSubsea/Plane.cpp +++ /dev/null @@ -1,33 +0,0 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#include "Plane.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 Plane class - */ - -const AP_HAL::HAL& hal = AP_HAL::get_HAL(); - -Plane::Plane(void) -{ - // C++11 doesn't allow in-class initialisation of bitfields - auto_state.takeoff_complete = true; - auto_state.next_wp_no_crosstrack = true; - auto_state.no_crosstrack = true; -} - -Plane plane; diff --git a/ArduSubsea/Plane.h b/ArduSubsea/Plane.h deleted file mode 100644 index e210870728..0000000000 --- a/ArduSubsea/Plane.h +++ /dev/null @@ -1,1033 +0,0 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#ifndef _PLANE_H -#define _PLANE_H - -#define THISFIRMWARE "ArduPlane V3.4.1dev" -#define FIRMWARE_VERSION 3,4,1,FIRMWARE_VERSION_TYPE_DEV - -/* - Lead developer: Andrew Tridgell - - Authors: Doug Weibel, Jose Julio, Jordi Munoz, Jason Short, Randy Mackay, Pat Hickey, John Arne Birkeland, Olivier Adler, Amilcar Lucas, Gregory Fletcher, Paul Riseborough, Brandon Jones, Jon Challinger - Thanks to: Chris Anderson, Michael Oborne, Paul Mather, Bill Premerlani, James Cohen, JB from rotorFX, Automatik, Fefenin, Peter Meister, Remzibi, Yury Smirnov, Sandro Benigno, Max Levine, Roberto Navoni, Lorenz Meier, Yury MonZon - - Please contribute your ideas! See http://dev.ardupilot.com for details - - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . - */ - -//////////////////////////////////////////////////////////////////////////////// -// Header includes -//////////////////////////////////////////////////////////////////////////////// - -#include -#include -#include - -#include -#include -#include -#include -#include -#include // ArduPilot GPS library -#include // ArduPilot barometer library -#include // ArduPilot Mega Magnetometer Library -#include // ArduPilot Mega Vector/Matrix math Library -#include // ArduPilot Mega Analog to Digital Converter Library -#include // Inertial Sensor Library -#include // interface and maths for accelerometer calibration -#include // ArduPilot Mega DCM Library -#include // RC Channel Library -#include // Range finder library -#include // Filter library -#include // APM FIFO Buffer -#include // APM relay -#include // Photo or video camera -#include -#include -#include - -#include -#include -#include -#include -#include // MAVLink GCS definitions -#include // Serial manager library -#include // Camera/Antenna mount -#include // ArduPilot Mega Declination Helper Library -#include -#include // main loop scheduler - -#include -#include -#include // RC input mapping library - -#include -#include -#include -#include -#include -#include // Mission command library - -#include // Notify library -#include // Battery monitor library - -#include -#include -#include -#include - -#include - -#include // Optical Flow library -#include // RSSI Library -#include -#include - -// Configuration -#include "config.h" - -// Local modules -#include "defines.h" - -#include "Parameters.h" - -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL -#include -#endif - -/* - a plane specific arming class - */ -class AP_Arming_Plane : public AP_Arming -{ -public: - AP_Arming_Plane(const AP_AHRS &ahrs_ref, const AP_Baro &baro, Compass &compass, - const enum HomeState &home_set) : - AP_Arming(ahrs_ref, baro, compass, home_set) { - AP_Param::setup_object_defaults(this, var_info); - } - bool pre_arm_checks(bool report); - - // var_info for holding Parameter information - static const struct AP_Param::GroupInfo var_info[]; -protected: - bool ins_checks(bool report); -}; - -/* - main APM:Plane class - */ -class Plane : public AP_HAL::HAL::Callbacks { -public: - friend class GCS_MAVLINK; - friend class Parameters; - friend class AP_Arming_Plane; - - Plane(void); - - // HAL::Callbacks implementation. - void setup() override; - void loop() override; - -private: - // key aircraft parameters passed to multiple libraries - AP_Vehicle::FixedWing aparm; - AP_HAL::BetterStream* cliSerial; - - // Global parameters are all contained within the 'g' class. - Parameters g; - - // main loop scheduler - AP_Scheduler scheduler; - - // mapping between input channels - RCMapper rcmap; - - // board specific config - AP_BoardConfig BoardConfig; - - // primary control channels - RC_Channel *channel_roll; - RC_Channel *channel_pitch; - RC_Channel *channel_throttle; - RC_Channel *channel_rudder; - - // notification object for LEDs, buzzers etc (parameter set to false disables external leds) - AP_Notify notify; - - DataFlash_Class DataFlash{FIRMWARE_STRING}; - - // has a log download started? - bool in_log_download; - - // scaled roll limit based on pitch - int32_t roll_limit_cd; - int32_t pitch_limit_min_cd; - - // Sensors - AP_GPS gps; - - // flight modes convenience array - AP_Int8 *flight_modes = &g.flight_mode1; - - AP_Baro barometer; - Compass compass; - - AP_InertialSensor ins; - -#if RANGEFINDER_ENABLED == ENABLED - // rangefinder - RangeFinder rangefinder {serial_manager}; - - struct { - bool in_range:1; - bool have_initial_reading:1; - bool in_use:1; - float initial_range; - float correction; - float initial_correction; - uint32_t last_correction_time_ms; - uint8_t in_range_count; - } rangefinder_state; -#endif - - AP_RPM rpm_sensor; - -// Inertial Navigation EKF -#if AP_AHRS_NAVEKF_AVAILABLE - NavEKF EKF{&ahrs, barometer, rangefinder}; - NavEKF2 EKF2{&ahrs, barometer, rangefinder}; - AP_AHRS_NavEKF ahrs {ins, barometer, gps, rangefinder, EKF, EKF2}; -#else - AP_AHRS_DCM ahrs {ins, barometer, gps}; -#endif - - AP_L1_Control L1_controller {ahrs}; - AP_TECS TECS_controller {ahrs, aparm}; - - // Attitude to servo controllers - AP_RollController rollController {ahrs, aparm, DataFlash}; - AP_PitchController pitchController {ahrs, aparm, DataFlash}; - AP_YawController yawController {ahrs, aparm}; - AP_SteerController steerController {ahrs}; - -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL - SITL::SITL sitl; -#endif - - // Training mode - bool training_manual_roll; // user has manual roll control - bool training_manual_pitch; // user has manual pitch control - - /* - keep steering and rudder control separated until we update servos, - to allow for a separate wheel servo from rudder servo - */ - struct { - bool ground_steering; // are we doing ground steering? - int16_t steering; // value for nose/tail wheel - int16_t rudder; // value for rudder - } steering_control; - - // should throttle be pass-thru in guided? - bool guided_throttle_passthru; - - // are we doing calibration? This is used to allow heartbeat to - // external failsafe boards during baro and airspeed calibration - bool in_calibration; - - - // GCS selection - AP_SerialManager serial_manager; - const uint8_t num_gcs = MAVLINK_COMM_NUM_BUFFERS; - GCS_MAVLINK gcs[MAVLINK_COMM_NUM_BUFFERS]; - - // selected navigation controller - AP_Navigation *nav_controller = &L1_controller; - - // selected navigation controller - AP_SpdHgtControl *SpdHgt_Controller = &TECS_controller; - - // Relay - AP_Relay relay; - - // handle servo and relay events - AP_ServoRelayEvents ServoRelayEvents {relay}; - - // Camera -#if CAMERA == ENABLED - AP_Camera camera {&relay}; -#endif - -#if OPTFLOW == ENABLED - // Optical flow sensor - OpticalFlow optflow{ahrs}; -#endif - - // Rally Ponints - AP_Rally rally {ahrs}; - - // RSSI - AP_RSSI rssi; - - // remember if USB is connected, so we can adjust baud rate - bool usb_connected; - - // This is the state of the flight control system - // There are multiple states defined such as MANUAL, FBW-A, AUTO - enum FlightMode control_mode = INITIALISING; - enum FlightMode previous_mode = INITIALISING; - - // Used to maintain the state of the previous control switch position - // This is set to 254 when we need to re-read the switch - uint8_t oldSwitchPosition = 254; - - // This is used to enable the inverted flight feature - bool inverted_flight; - - // This is used to enable the PX4IO override for testing - bool px4io_override_enabled; - - struct { - // These are trim values used for elevon control - // For elevons radio_in[CH_ROLL] and radio_in[CH_PITCH] are - // equivalent aileron and elevator, not left and right elevon - uint16_t trim1; - uint16_t trim2; - // These are used in the calculation of elevon1_trim and elevon2_trim - uint16_t ch1_temp; - uint16_t ch2_temp; - } elevon { 1500, 1500, 1500, 1500 }; - - // Failsafe - struct { - // Used to track if the value on channel 3 (throtttle) has fallen below the failsafe threshold - // RC receiver should be set up to output a low throttle value when signal is lost - uint8_t ch3_failsafe:1; - - // has the saved mode for failsafe been set? - uint8_t saved_mode_set:1; - - // flag to hold whether battery low voltage threshold has been breached - uint8_t low_battery:1; - - // saved flight mode - enum FlightMode saved_mode; - - // A tracking variable for type of failsafe active - // Used for failsafe based on loss of RC signal or GCS signal - int16_t state; - - // number of low ch3 values - uint8_t ch3_counter; - - // the time when the last HEARTBEAT message arrived from a GCS - uint32_t last_heartbeat_ms; - - // A timer used to track how long we have been in a "short failsafe" condition due to loss of RC signal - uint32_t ch3_timer_ms; - - uint32_t last_valid_rc_ms; - } failsafe; - - // A counter used to count down valid gps fixes to allow the gps estimate to settle - // before recording our home position (and executing a ground start if we booted with an air start) - uint8_t ground_start_count = 5; - - // true if we have a position estimate from AHRS - bool have_position; - - // Airspeed - // The calculated airspeed to use in FBW-B. Also used in higher modes for insuring min ground speed is met. - // Also used for flap deployment criteria. Centimeters per second. - int32_t target_airspeed_cm; - - // The difference between current and desired airspeed. Used in the pitch controller. Centimeters per second. - float airspeed_error_cm; - - // An amount that the airspeed should be increased in auto modes based on the user positioning the - // throttle stick in the top half of the range. Centimeters per second. - int16_t airspeed_nudge_cm; - - // Similar to airspeed_nudge, but used when no airspeed sensor. - // 0-(throttle_max - throttle_cruise) : throttle nudge in Auto mode using top 1/2 of throttle stick travel - int16_t throttle_nudge; - - // receiver RSSI - uint8_t receiver_rssi; - - // Ground speed - // The amount current ground speed is below min ground speed. Centimeters per second - int32_t groundspeed_undershoot; - - // Difference between current altitude and desired altitude. Centimeters - int32_t altitude_error_cm; - - // Battery Sensors - AP_BattMonitor battery; - -#if FRSKY_TELEM_ENABLED == ENABLED - // FrSky telemetry support - AP_Frsky_Telem frsky_telemetry {ahrs, battery}; -#endif - - // Airspeed Sensors - AP_Airspeed airspeed {aparm}; - - // ACRO controller state - struct { - bool locked_roll; - bool locked_pitch; - float locked_roll_err; - int32_t locked_pitch_cd; - } acro_state; - - // CRUISE controller state - struct { - bool locked_heading; - int32_t locked_heading_cd; - uint32_t lock_timer_ms; - } cruise_state; - - // ground steering controller state - struct { - // Direction held during phases of takeoff and landing centidegrees - // A value of -1 indicates the course has not been set/is not in use - // this is a 0..36000 value, or -1 for disabled - int32_t hold_course_cd; - - // locked_course and locked_course_cd are used in stabilize mode - // when ground steering is active, and for steering in auto-takeoff - bool locked_course; - float locked_course_err; - } steer_state { -1, false, 0 }; - - // flight mode specific - struct { - // Flag for using gps ground course instead of INS yaw. Set false when takeoff command in process. - bool takeoff_complete:1; - - // Flag to indicate if we have landed. - // Set land_complete if we are within 2 seconds distance or within 3 meters altitude of touchdown - bool land_complete:1; - - // should we fly inverted? - bool inverted_flight:1; - - // should we disable cross-tracking for the next waypoint? - bool next_wp_no_crosstrack:1; - - // should we use cross-tracking for this waypoint? - bool no_crosstrack:1; - - // in FBWA taildragger takeoff mode - bool fbwa_tdrag_takeoff_mode:1; - - // have we checked for an auto-land? - bool checked_for_autoland:1; - - // denotes if a go-around has been commanded for landing - bool commanded_go_around:1; - - // Altitude threshold to complete a takeoff command in autonomous modes. Centimeters - // are we in idle mode? used for balloon launch to stop servo - // movement until altitude is reached - bool idle_mode:1; - - // used to 'wiggle' servos in idle mode to prevent them freezing - // at high altitudes - uint8_t idle_wiggle_stage; - - // Altitude threshold to complete a takeoff command in autonomous - // modes. Centimeters above home - int32_t takeoff_altitude_rel_cm; - - // Minimum pitch to hold during takeoff command execution. Hundredths of a degree - int16_t takeoff_pitch_cd; - - // the highest airspeed we have reached since entering AUTO. Used - // to control ground takeoff - float highest_airspeed; - - // initial pitch. Used to detect if nose is rising in a tail dragger - int16_t initial_pitch_cd; - - // turn angle for next leg of mission - float next_turn_angle {90}; - - // filtered sink rate for landing - float sink_rate; - - // time when we first pass min GPS speed on takeoff - uint32_t takeoff_speed_time_ms; - - // distance to next waypoint - float wp_distance; - - // proportion to next waypoint - float wp_proportion; - - // last time is_flying() returned true in milliseconds - uint32_t last_flying_ms; - - // once landed, post some landing statistics to the GCS - bool post_landing_stats; - - // time stamp of when we start flying while in auto mode in milliseconds - uint32_t started_flying_in_auto_ms; - - // barometric altitude at start of takeoff - float baro_takeoff_alt; - } auto_state; - - struct { - // on hard landings, only check once after directly a landing so you - // don't trigger a crash when picking up the aircraft - bool checkHardLanding:1; - - // crash detection. True when we are crashed - bool is_crashed:1; - - // debounce timer - uint32_t debounce_timer_ms; - } crash_state; - - // true if we are in an auto-throttle mode, which means - // we need to run the speed/height controller - bool auto_throttle_mode; - - // this controls throttle suppression in auto modes - bool throttle_suppressed; - - AP_SpdHgtControl::FlightStage flight_stage = AP_SpdHgtControl::FLIGHT_NORMAL; - - // probability of aircraft is currently in flight. range from 0 to - // 1 where 1 is 100% sure we're in flight - float isFlyingProbability; - - // previous value of is_flying() - bool previous_is_flying; - - // time since started flying in any mode in milliseconds - uint32_t started_flying_ms; - - // Navigation control variables - // The instantaneous desired bank angle. Hundredths of a degree - int32_t nav_roll_cd; - - // The instantaneous desired pitch angle. Hundredths of a degree - int32_t nav_pitch_cd; - - // we separate out rudder input to allow for RUDDER_ONLY=1 - int16_t rudder_input; - - // the aerodymamic load factor. This is calculated from the demanded - // roll before the roll is clipped, using 1/sqrt(cos(nav_roll)) - float aerodynamic_load_factor = 1.0f; - - // a smoothed airspeed estimate, used for limiting roll angle - float smoothed_airspeed; - - // Mission library - AP_Mission mission {ahrs, - FUNCTOR_BIND_MEMBER(&Plane::start_command_callback, bool, const AP_Mission::Mission_Command &), - FUNCTOR_BIND_MEMBER(&Plane::verify_command_callback, bool, const AP_Mission::Mission_Command &), - FUNCTOR_BIND_MEMBER(&Plane::exit_mission_callback, void)}; - - -#if PARACHUTE == ENABLED - AP_Parachute parachute {relay}; -#endif - - // terrain handling -#if AP_TERRAIN_AVAILABLE - AP_Terrain terrain {ahrs, mission, rally}; -#endif - - AP_ADSB adsb {ahrs}; - struct { - - // for Loiter_and_descend behavior, keeps track of rate changes - uint32_t time_last_alt_change_ms; - - // previous wp to restore to when switching between modes back to AUTO - Location prev_wp; - } adsb_state; - - - // Outback Challenge Failsafe Support -#if OBC_FAILSAFE == ENABLED - APM_OBC obc {mission, barometer, gps, rcmap}; -#endif - - /* - meta data to support counting the number of circles in a loiter - */ - struct { - // previous target bearing, used to update sum_cd - int32_t old_target_bearing_cd; - - // Total desired rotation in a loiter. Used for Loiter Turns commands. - int32_t total_cd; - - // total angle completed in the loiter so far - int32_t sum_cd; - - // Direction for loiter. 1 for clockwise, -1 for counter-clockwise - int8_t direction; - - // start time of the loiter. Milliseconds. - uint32_t start_time_ms; - - // The amount of time we should stay in a loiter for the Loiter Time command. Milliseconds. - uint32_t time_max_ms; - } loiter; - - - // Conditional command - // A value used in condition commands (eg delay, change alt, etc.) - // For example in a change altitude command, it is the altitude to change to. - int32_t condition_value; - - // Sometimes there is a second condition required: - int32_t condition_value2; - // A starting value used to check the status of a conditional command. - // For example in a delay command the condition_start records that start time for the delay - uint32_t condition_start; - // A value used in condition commands. For example the rate at which to change altitude. - int16_t condition_rate; - - // 3D Location vectors - // Location structure defined in AP_Common - const struct Location &home = ahrs.get_home(); - - // Flag for if we have g_gps lock and have set the home location in AHRS - enum HomeState home_is_set = HOME_UNSET; - - // The location of the previous waypoint. Used for track following and altitude ramp calculations - Location prev_WP_loc {}; - - // The plane's current location - struct Location current_loc {}; - - // The location of the current/active waypoint. Used for altitude ramp, track following and loiter calculations. - Location next_WP_loc {}; - - // The location of the active waypoint in Guided mode. - struct Location guided_WP_loc {}; - - // special purpose command used only after mission completed to return vehicle to home or rally point - struct AP_Mission::Mission_Command auto_rtl_command; - - // Altitude control - struct { - // target altitude above sea level in cm. Used for barometric - // altitude navigation - int32_t amsl_cm; - - // Altitude difference between previous and current waypoint in - // centimeters. Used for glide slope handling - int32_t offset_cm; - -#if AP_TERRAIN_AVAILABLE - // are we trying to follow terrain? - bool terrain_following; - - // target altitude above terrain in cm, valid if terrain_following - // is set - int32_t terrain_alt_cm; - - // lookahead value for height error reporting - float lookahead; -#endif - } target_altitude {}; - - // INS variables - // The main loop execution time. Seconds - // This is the time between calls to the DCM algorithm and is the Integration time for the gyros. - float G_Dt = 0.02f; - - // Performance monitoring - // Timer used to accrue data and trigger recording of the performanc monitoring log message - uint32_t perf_mon_timer = 0; - - // The maximum and minimum main loop execution time recorded in the current performance monitoring interval - uint32_t G_Dt_max = 0; - uint32_t G_Dt_min = 0; - - // System Timers - // Time in microseconds of start of main control loop - uint32_t fast_loopTimer_us = 0; - - // Number of milliseconds used in last main loop cycle - uint32_t delta_us_fast_loop = 0; - - // Counter of main loop executions. Used for performance monitoring and failsafe processing - uint16_t mainLoop_count = 0; - - // 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 {ahrs, current_loc}; -#endif - - // Arming/Disarming mangement class - AP_Arming_Plane arming {ahrs, barometer, compass, home_is_set }; - - AP_Param param_loader {var_info}; - - static const AP_Scheduler::Task scheduler_tasks[]; - static const AP_Param::Info var_info[]; - - bool demoing_servos = false; - - // use this to prevent recursion during sensor init - bool in_mavlink_delay = false; - - // true if we are out of time in our event timeslice - bool gcs_out_of_time = false; - - // time that rudder arming has been running - uint32_t rudder_arm_timer; - - - void demo_servos(uint8_t i); - void adjust_nav_pitch_throttle(void); - void update_load_factor(void); - void send_heartbeat(mavlink_channel_t chan); - void send_attitude(mavlink_channel_t chan); - void send_fence_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_servo_out(mavlink_channel_t chan); - void send_radio_out(mavlink_channel_t chan); - void send_vfr_hud(mavlink_channel_t chan); - void send_simstate(mavlink_channel_t chan); - void send_hwstatus(mavlink_channel_t chan); - void send_wind(mavlink_channel_t chan); - void send_pid_tuning(mavlink_channel_t chan); - void send_rpm(mavlink_channel_t chan); - void send_rangefinder(mavlink_channel_t chan); - void send_current_waypoint(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_update(void); - void gcs_send_text(MAV_SEVERITY severity, const char *str); - void gcs_send_airspeed_calibration(const Vector3f &vg); - void gcs_retry_deferred(void); - - void do_erase_logs(void); - void Log_Write_Attitude(void); - void Log_Write_Performance(); - void Log_Write_Startup(uint8_t type); - void Log_Write_Control_Tuning(); - void Log_Write_TECS_Tuning(void); - void Log_Write_Nav_Tuning(); - void Log_Write_Status(); - void Log_Write_Sonar(); - void Log_Write_Optflow(); - void Log_Write_Current(); - void Log_Arm_Disarm(); - void Log_Write_GPS(uint8_t instance); - void Log_Write_IMU(); - void Log_Write_RC(void); - void Log_Write_Baro(void); - void Log_Write_Airspeed(void); - void Log_Write_Home_And_Origin(); - void Log_Write_Vehicle_Startup_Messages(); - void Log_Read(uint16_t log_num, int16_t start_page, int16_t end_page); - void start_logging(); - - void load_parameters(void); - void adjust_altitude_target(); - void setup_glide_slope(void); - int32_t get_RTL_altitude(); - float relative_altitude(void); - int32_t relative_altitude_abs_cm(void); - void set_target_altitude_current(void); - void set_target_altitude_current_adjusted(void); - void set_target_altitude_location(const Location &loc); - int32_t relative_target_altitude_cm(void); - void change_target_altitude(int32_t change_cm); - void set_target_altitude_proportion(const Location &loc, float proportion); - void constrain_target_altitude_location(const Location &loc1, const Location &loc2); - int32_t calc_altitude_error_cm(void); - void check_minimum_altitude(void); - void reset_offset_altitude(void); - void set_offset_altitude_location(const Location &loc); - bool above_location_current(const Location &loc); - void setup_terrain_target_alt(Location &loc); - int32_t adjusted_altitude_cm(void); - int32_t adjusted_relative_altitude_cm(void); - float height_above_target(void); - float lookahead_adjustment(void); - float rangefinder_correction(void); - void rangefinder_height_update(void); - void set_next_WP(const struct Location &loc); - void set_guided_WP(void); - void init_home(); - void update_home(); - void do_RTL(void); - bool verify_takeoff(); - bool verify_loiter_unlim(); - bool verify_loiter_time(); - bool verify_loiter_turns(); - bool verify_loiter_to_alt(); - bool verify_RTL(); - bool verify_continue_and_change_alt(); - bool verify_wait_delay(); - bool verify_change_alt(); - bool verify_within_distance(); - bool verify_altitude_wait(const AP_Mission::Mission_Command &cmd); - void do_loiter_at_location(); - void do_take_picture(); - void log_picture(); - void exit_mission_callback(); - void update_commands(void); - void mavlink_delay(uint32_t ms); - void read_control_switch(); - uint8_t readSwitch(void); - void reset_control_switch(); - void autotune_start(void); - void autotune_restore(void); - void autotune_enable(bool enable); - bool fly_inverted(void); - void failsafe_short_on_event(enum failsafe_state fstype); - void failsafe_long_on_event(enum failsafe_state fstype); - void failsafe_short_off_event(); - void low_battery_event(void); - void update_events(void); - uint8_t max_fencepoints(void); - Vector2l get_fence_point_with_index(unsigned i); - void set_fence_point_with_index(Vector2l &point, unsigned i); - void geofence_load(void); - bool geofence_present(void); - void geofence_update_pwm_enabled_state(); - bool geofence_set_enabled(bool enable, GeofenceEnableReason r); - bool geofence_enabled(void); - bool geofence_set_floor_enabled(bool floor_enable); - bool geofence_check_minalt(void); - bool geofence_check_maxalt(void); - void geofence_check(bool altitude_check_only); - bool geofence_stickmixing(void); - void geofence_send_status(mavlink_channel_t chan); - bool geofence_breached(void); - bool verify_land(); - void disarm_if_autoland_complete(); - void setup_landing_glide_slope(void); - bool jump_to_landing_sequence(void); - float tecs_hgt_afe(void); - void set_nav_controller(void); - void loiter_angle_reset(void); - void loiter_angle_update(void); - void navigate(); - void calc_airspeed_errors(); - void calc_gndspeed_undershoot(); - void update_loiter(); - void update_cruise(); - void update_fbwb_speed_height(void); - void setup_turn_angle(void); - bool print_buffer(char *&buf, uint16_t &buf_size, const char *fmt, ...); - bool create_mixer(char *buf, uint16_t buf_size, const char *filename); - bool setup_failsafe_mixing(void); - void set_control_channels(void); - void init_rc_in(); - void init_rc_out(); - void rudder_arm_disarm_check(); - void read_radio(); - void control_failsafe(uint16_t pwm); - void trim_control_surfaces(); - void trim_radio(); - bool rc_failsafe_active(void); - void init_barometer(void); - void init_rangefinder(void); - void read_rangefinder(void); - void read_airspeed(void); - void zero_airspeed(bool in_startup); - void read_battery(void); - void read_receiver_rssi(void); - void rpm_update(void); - void report_radio(); - void report_ins(); - void report_compass(); - void print_radio_values(); - void print_done(); - void print_blanks(int16_t num); - void print_divider(void); - void zero_eeprom(void); - void print_enabled(bool b); - void print_accel_offsets_and_scaling(void); - void print_gyro_offsets(void); - void init_ardupilot(); - void startup_ground(void); - enum FlightMode get_previous_mode(); - void set_mode(enum FlightMode mode); - bool mavlink_set_mode(uint8_t mode); - void exit_mode(enum FlightMode mode); - void check_long_failsafe(); - void check_short_failsafe(); - void startup_INS_ground(void); - void update_notify(); - void resetPerfData(void); - void check_usb_mux(void); - void print_comma(void); - void servo_write(uint8_t ch, uint16_t pwm); - bool should_log(uint32_t mask); - void frsky_telemetry_send(void); - uint8_t throttle_percentage(void); - void change_arm_state(void); - bool disarm_motors(void); - bool arm_motors(AP_Arming::ArmingMethod method); - bool auto_takeoff_check(void); - void takeoff_calc_roll(void); - void takeoff_calc_pitch(void); - int8_t takeoff_tail_hold(void); - void print_hit_enter(); - void ahrs_update(); - void update_speed_height(void); - void update_GPS_50Hz(void); - void update_GPS_10Hz(void); - void update_compass(void); - void update_alt(void); - void obc_fs_check(void); - void compass_accumulate(void); - void compass_cal_update(); - void barometer_accumulate(void); - void update_optical_flow(void); - void one_second_loop(void); - void airspeed_ratio_update(void); - void update_mount(void); - void log_perf_info(void); - void compass_save(void); - void update_logging1(void); - void update_logging2(void); - void terrain_update(void); - void adsb_update(void); - void adsb_handle_vehicle_threats(void); - void update_flight_mode(void); - void stabilize(); - void set_servos_idle(void); - void set_servos(); - void update_aux(); - void update_is_flying_5Hz(void); - void crash_detection_update(void); - void gcs_send_text_fmt(MAV_SEVERITY severity, const char *fmt, ...); - void handle_auto_mode(void); - void calc_throttle(); - void calc_nav_roll(); - void calc_nav_pitch(); - void update_flight_stage(); - void update_navigation(); - void set_flight_stage(AP_SpdHgtControl::FlightStage fs); - bool is_flying(void); - float get_speed_scaler(void); - bool stick_mixing_enabled(void); - void stabilize_roll(float speed_scaler); - void stabilize_pitch(float speed_scaler); - void stick_mix_channel(RC_Channel *channel, int16_t &servo_out); - void stabilize_stick_mixing_direct(); - void stabilize_stick_mixing_fbw(); - void stabilize_yaw(float speed_scaler); - void stabilize_training(float speed_scaler); - void stabilize_acro(float speed_scaler); - void calc_nav_yaw_coordinated(float speed_scaler); - void calc_nav_yaw_course(void); - void calc_nav_yaw_ground(void); - void throttle_slew_limit(int16_t last_throttle); - void flap_slew_limit(int8_t &last_value, int8_t &new_value); - bool suppress_throttle(void); - void channel_output_mixer(uint8_t mixing_type, int16_t &chan1_out, int16_t &chan2_out); - void flaperon_update(int8_t flap_percent); - bool start_command(const AP_Mission::Mission_Command& cmd); - bool verify_command(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 loiter_set_direction_wp(const AP_Mission::Mission_Command& cmd); - void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd); - void do_loiter_turns(const AP_Mission::Mission_Command& cmd); - void do_loiter_time(const AP_Mission::Mission_Command& cmd); - void do_altitude_wait(const AP_Mission::Mission_Command& cmd); - void do_continue_and_change_alt(const AP_Mission::Mission_Command& cmd); - void do_loiter_to_alt(const AP_Mission::Mission_Command& cmd); - bool verify_nav_wp(const AP_Mission::Mission_Command& cmd); - void do_wait_delay(const AP_Mission::Mission_Command& cmd); - void do_change_alt(const AP_Mission::Mission_Command& cmd); - void do_within_distance(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_digicam_configure(const AP_Mission::Mission_Command& cmd); - void do_digicam_control(const AP_Mission::Mission_Command& cmd); - bool start_command_callback(const AP_Mission::Mission_Command &cmd); - bool verify_command_callback(const AP_Mission::Mission_Command& cmd); - void print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode); - void run_cli(AP_HAL::UARTDriver *port); - bool restart_landing_sequence(); - void log_init(); - void init_capabilities(void); - void dataflash_periodic(void); - uint16_t throttle_min(void) const; - void do_parachute(const AP_Mission::Mission_Command& cmd); - void parachute_check(); - void parachute_release(); - bool parachute_manual_release(); - void accel_cal_update(void); - -public: - void mavlink_delay_cb(); - void failsafe_check(void); - bool print_log_menu(void); - 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); - int8_t process_logs(uint8_t argc, const Menu::arg *argv); - 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_erase(uint8_t argc, const Menu::arg *argv); - int8_t test_mode(uint8_t argc, const Menu::arg *argv); - int8_t reboot_board(uint8_t argc, const Menu::arg *argv); - int8_t main_menu_help(uint8_t argc, const Menu::arg *argv); - int8_t test_radio_pwm(uint8_t argc, const Menu::arg *argv); - int8_t test_passthru(uint8_t argc, const Menu::arg *argv); - int8_t test_radio(uint8_t argc, const Menu::arg *argv); - int8_t test_failsafe(uint8_t argc, const Menu::arg *argv); - int8_t test_relay(uint8_t argc, const Menu::arg *argv); - int8_t test_wp(uint8_t argc, const Menu::arg *argv); - void test_wp_print(const AP_Mission::Mission_Command& cmd); - int8_t test_xbee(uint8_t argc, const Menu::arg *argv); - int8_t test_modeswitch(uint8_t argc, const Menu::arg *argv); - int8_t test_logging(uint8_t argc, const Menu::arg *argv); - int8_t test_gps(uint8_t argc, const Menu::arg *argv); - int8_t test_ins(uint8_t argc, const Menu::arg *argv); - int8_t test_mag(uint8_t argc, const Menu::arg *argv); - int8_t test_airspeed(uint8_t argc, const Menu::arg *argv); - int8_t test_pressure(uint8_t argc, const Menu::arg *argv); - int8_t test_shell(uint8_t argc, const Menu::arg *argv); -}; - -#define MENU_FUNC(func) FUNCTOR_BIND(&plane, &Plane::func, int8_t, uint8_t, const Menu::arg *) - -extern const AP_HAL::HAL& hal; -extern Plane plane; - -using AP_HAL::millis; -using AP_HAL::micros; - -#endif // _PLANE_H_ diff --git a/ArduSubsea/adsb.cpp b/ArduSubsea/adsb.cpp deleted file mode 100644 index 8fbf0cb4fd..0000000000 --- a/ArduSubsea/adsb.cpp +++ /dev/null @@ -1,101 +0,0 @@ -/// -*- 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 "Plane.h" - -/* - * this module deals with ADS-B handling for ArduPlane - * 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 Plane::adsb_update(void) -{ - adsb.update(); - adsb_handle_vehicle_threats(); -} - -/* - * Handle ADS-B based threats which are platform dependent - */ -void Plane::adsb_handle_vehicle_threats(void) -{ - uint32_t now = millis(); - AP_ADSB::ADSB_BEHAVIOR behavior = adsb.get_behavior(); - - switch (control_mode) { - case AUTO: - if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_TAKEOFF) { - // for testing purposes ignore ADS-B traffic until we get into the air so we don't screw up the sim takeoff - break; - } - switch(behavior) { - case AP_ADSB::ADSB_BEHAVIOR_NONE: - default: - break; - - case AP_ADSB::ADSB_BEHAVIOR_LOITER: - case AP_ADSB::ADSB_BEHAVIOR_LOITER_AND_DESCEND: - if (adsb.get_another_vehicle_within_radius() && !adsb.get_is_evading_threat()) { - adsb.set_is_evading_threat(true); - gcs_send_text(MAV_SEVERITY_CRITICAL, "ADS-B threat found, performing LOITER"); - adsb_state.prev_wp = prev_WP_loc; - set_mode(LOITER); - if (behavior == AP_ADSB::ADSB_BEHAVIOR_LOITER_AND_DESCEND) { - adsb_state.time_last_alt_change_ms = now; - } - } - } // switch behavior - break; // case auto - - case LOITER: - switch(behavior) { - case AP_ADSB::ADSB_BEHAVIOR_NONE: - // TODO: recover from this - default: - break; - - case AP_ADSB::ADSB_BEHAVIOR_LOITER: - case AP_ADSB::ADSB_BEHAVIOR_LOITER_AND_DESCEND: - if (adsb.get_is_evading_threat()) { - if (!adsb.get_another_vehicle_within_radius()) { - adsb.set_is_evading_threat(false); - gcs_send_text(MAV_SEVERITY_CRITICAL, "ADS-B threat gone, continuing mission"); - set_mode(AUTO); - prev_WP_loc = adsb_state.prev_wp; - auto_state.no_crosstrack = false; - } else if (behavior == AP_ADSB::ADSB_BEHAVIOR_LOITER_AND_DESCEND && - now - adsb_state.time_last_alt_change_ms >= 1000) { - // slowly reduce altitude 1m/s while loitering. Drive into the ground if threat persists - adsb_state.time_last_alt_change_ms = now; - next_WP_loc.alt -= 100; - } // get_another_vehicle_within_radius - } // get_is_evading_threat - } // switch behavior - break; // case LOITER - - default: - break; - } // switch control_mode -} - diff --git a/ArduSubsea/altitude.cpp b/ArduSubsea/altitude.cpp deleted file mode 100644 index 2b517def70..0000000000 --- a/ArduSubsea/altitude.cpp +++ /dev/null @@ -1,621 +0,0 @@ -// -*- 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 . - */ - -#include "Plane.h" - -/* - altitude handling routines. These cope with both barometric control - and terrain following control - */ - -/* - adjust altitude target depending on mode - */ -void Plane::adjust_altitude_target() -{ - if (control_mode == FLY_BY_WIRE_B || - control_mode == CRUISE) { - return; - } - if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL) { - // in land final TECS uses TECS_LAND_SINK as a target sink - // rate, and ignores the target altitude - set_target_altitude_location(next_WP_loc); - } else if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH) { - setup_landing_glide_slope(); - } else if (nav_controller->reached_loiter_target()) { - // once we reach a loiter target then lock to the final - // altitude target - set_target_altitude_location(next_WP_loc); - } else if (target_altitude.offset_cm != 0 && - !location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) { - // control climb/descent rate - set_target_altitude_proportion(next_WP_loc, 1.0f-auto_state.wp_proportion); - - // stay within the range of the start and end locations in altitude - constrain_target_altitude_location(next_WP_loc, prev_WP_loc); - } else if (mission.get_current_do_cmd().id != MAV_CMD_CONDITION_CHANGE_ALT) { - set_target_altitude_location(next_WP_loc); - } - - altitude_error_cm = calc_altitude_error_cm(); -} - -/* - setup for a gradual glide slope to the next waypoint, if appropriate - */ -void Plane::setup_glide_slope(void) -{ - // establish the distance we are travelling to the next waypoint, - // for calculating out rate of change of altitude - auto_state.wp_distance = get_distance(current_loc, next_WP_loc); - auto_state.wp_proportion = location_path_proportion(current_loc, - prev_WP_loc, next_WP_loc); - - /* - work out if we will gradually change altitude, or try to get to - the new altitude as quickly as possible. - */ - switch (control_mode) { - case RTL: - case GUIDED: - /* glide down slowly if above target altitude, but ascend more - rapidly if below it. See - https://github.com/diydrones/ardupilot/issues/39 - */ - if (above_location_current(next_WP_loc)) { - set_offset_altitude_location(next_WP_loc); - } else { - reset_offset_altitude(); - } - break; - - case AUTO: - // we only do glide slide handling in AUTO when above 20m or - // when descending. The 20 meter threshold is arbitrary, and - // is basically to prevent situations where we try to slowly - // gain height at low altitudes, potentially hitting - // obstacles. - if (adjusted_relative_altitude_cm() > 2000 || above_location_current(next_WP_loc)) { - set_offset_altitude_location(next_WP_loc); - } else { - reset_offset_altitude(); - } - break; - default: - reset_offset_altitude(); - break; - } -} - -/* - return RTL altitude as AMSL altitude - */ -int32_t Plane::get_RTL_altitude() -{ - if (g.RTL_altitude_cm < 0) { - return current_loc.alt; - } - return g.RTL_altitude_cm + home.alt; -} - - -/* - return relative altitude in meters (relative to home) - */ -float Plane::relative_altitude(void) -{ - return (current_loc.alt - home.alt) * 0.01f; -} - -/* - return relative altitude in centimeters, absolute value - */ -int32_t Plane::relative_altitude_abs_cm(void) -{ - return labs(current_loc.alt - home.alt); -} - - -/* - set the target altitude to the current altitude. This is used when - setting up for altitude hold, such as when releasing elevator in - CRUISE mode. - */ -void Plane::set_target_altitude_current(void) -{ - // record altitude above sea level at the current time as our - // target altitude - target_altitude.amsl_cm = current_loc.alt; - - // reset any glide slope offset - reset_offset_altitude(); - -#if AP_TERRAIN_AVAILABLE - // also record the terrain altitude if possible - float terrain_altitude; - if (g.terrain_follow && terrain.height_above_terrain(terrain_altitude, true)) { - target_altitude.terrain_following = true; - target_altitude.terrain_alt_cm = terrain_altitude*100; - } else { - // if terrain following is disabled, or we don't know our - // terrain altitude when we set the altitude then don't - // terrain follow - target_altitude.terrain_following = false; - } -#endif -} - -/* - set the target altitude to the current altitude, with ALT_OFFSET adjustment - */ -void Plane::set_target_altitude_current_adjusted(void) -{ - set_target_altitude_current(); - - // use adjusted_altitude_cm() to take account of ALTITUDE_OFFSET - target_altitude.amsl_cm = adjusted_altitude_cm(); -} - -/* - set target altitude based on a location structure - */ -void Plane::set_target_altitude_location(const Location &loc) -{ - target_altitude.amsl_cm = loc.alt; - if (loc.flags.relative_alt) { - target_altitude.amsl_cm += home.alt; - } -#if AP_TERRAIN_AVAILABLE - /* - if this location has the terrain_alt flag set and we know the - terrain altitude of our current location then treat it as a - terrain altitude - */ - float height; - if (loc.flags.terrain_alt && terrain.height_above_terrain(height, true)) { - target_altitude.terrain_following = true; - target_altitude.terrain_alt_cm = loc.alt; - if (!loc.flags.relative_alt) { - // it has home added, remove it - target_altitude.terrain_alt_cm -= home.alt; - } - } else { - target_altitude.terrain_following = false; - } -#endif -} - -/* - return relative to home target altitude in centimeters. Used for - altitude control libraries - */ -int32_t Plane::relative_target_altitude_cm(void) -{ -#if AP_TERRAIN_AVAILABLE - float relative_home_height; - if (target_altitude.terrain_following && - terrain.height_relative_home_equivalent(target_altitude.terrain_alt_cm*0.01f, - relative_home_height, true)) { - // add lookahead adjustment the target altitude - target_altitude.lookahead = lookahead_adjustment(); - relative_home_height += target_altitude.lookahead; - - // correct for rangefinder data - relative_home_height += rangefinder_correction(); - - // we are following terrain, and have terrain data for the - // current location. Use it. - return relative_home_height*100; - } -#endif - int32_t relative_alt = target_altitude.amsl_cm - home.alt; - relative_alt += int32_t(g.alt_offset)*100; - relative_alt += rangefinder_correction() * 100; - return relative_alt; -} - -/* - change the current target altitude by an amount in centimeters. Used - to cope with changes due to elevator in CRUISE or FBWB - */ -void Plane::change_target_altitude(int32_t change_cm) -{ - target_altitude.amsl_cm += change_cm; -#if AP_TERRAIN_AVAILABLE - if (target_altitude.terrain_following) { - target_altitude.terrain_alt_cm += change_cm; - } -#endif -} - -/* - change target altitude by a proportion of the target altitude offset - (difference in height to next WP from previous WP). proportion - should be between 0 and 1. - - When proportion is zero we have reached the destination. When - proportion is 1 we are at the starting waypoint. - - Note that target_altitude is setup initially based on the - destination waypoint - */ -void Plane::set_target_altitude_proportion(const Location &loc, float proportion) -{ - set_target_altitude_location(loc); - proportion = constrain_float(proportion, 0.0f, 1.0f); - change_target_altitude(-target_altitude.offset_cm*proportion); - //rebuild the glide slope if we are above it and supposed to be climbing - if(g.glide_slope_threshold > 0) { - if(target_altitude.offset_cm > 0 && calc_altitude_error_cm() < -100 * g.glide_slope_threshold) { - set_target_altitude_location(loc); - set_offset_altitude_location(loc); - change_target_altitude(-target_altitude.offset_cm*proportion); - //adjust the new target offset altitude to reflect that we are partially already done - if(proportion > 0.0f) - target_altitude.offset_cm = ((float)target_altitude.offset_cm)/proportion; - } - } -} - -/* - constrain target altitude to be between two locations. Used to - ensure we stay within two waypoints in altitude - */ -void Plane::constrain_target_altitude_location(const Location &loc1, const Location &loc2) -{ - if (loc1.alt > loc2.alt) { - target_altitude.amsl_cm = constrain_int32(target_altitude.amsl_cm, loc2.alt, loc1.alt); - } else { - target_altitude.amsl_cm = constrain_int32(target_altitude.amsl_cm, loc1.alt, loc2.alt); - } -} - -/* - return error between target altitude and current altitude - */ -int32_t Plane::calc_altitude_error_cm(void) -{ -#if AP_TERRAIN_AVAILABLE - float terrain_height; - if (target_altitude.terrain_following && - terrain.height_above_terrain(terrain_height, true)) { - return target_altitude.lookahead*100 + target_altitude.terrain_alt_cm - (terrain_height*100); - } -#endif - return target_altitude.amsl_cm - adjusted_altitude_cm(); -} - -/* - check for FBWB_min_altitude_cm violation - */ -void Plane::check_minimum_altitude(void) -{ - if (g.FBWB_min_altitude_cm == 0) { - return; - } - -#if AP_TERRAIN_AVAILABLE - if (target_altitude.terrain_following) { - // set our target terrain height to be at least the min set - if (target_altitude.terrain_alt_cm < g.FBWB_min_altitude_cm) { - target_altitude.terrain_alt_cm = g.FBWB_min_altitude_cm; - } - return; - } -#endif - - if (target_altitude.amsl_cm < home.alt + g.FBWB_min_altitude_cm) { - target_altitude.amsl_cm = home.alt + g.FBWB_min_altitude_cm; - } -} - -/* - reset the altitude offset used for glide slopes - */ -void Plane::reset_offset_altitude(void) -{ - target_altitude.offset_cm = 0; -} - - -/* - reset the altitude offset used for glide slopes, based on difference - between altitude at a destination and current altitude. If - destination is above the current altitude then the result is - positive. - */ -void Plane::set_offset_altitude_location(const Location &loc) -{ - target_altitude.offset_cm = loc.alt - current_loc.alt; - -#if AP_TERRAIN_AVAILABLE - /* - if this location has the terrain_alt flag set and we know the - terrain altitude of our current location then treat it as a - terrain altitude - */ - float height; - if (loc.flags.terrain_alt && - target_altitude.terrain_following && - terrain.height_above_terrain(height, true)) { - target_altitude.offset_cm = target_altitude.terrain_alt_cm - (height * 100); - } -#endif - - if (flight_stage != AP_SpdHgtControl::FLIGHT_LAND_APPROACH && - flight_stage != AP_SpdHgtControl::FLIGHT_LAND_FINAL) { - // if we are within GLIDE_SLOPE_MIN meters of the target altitude - // then reset the offset to not use a glide slope. This allows for - // more accurate flight of missions where the aircraft may lose or - // gain a bit of altitude near waypoint turn points due to local - // terrain changes - if (g.glide_slope_min <= 0 || - labs(target_altitude.offset_cm)*0.01f < g.glide_slope_min) { - target_altitude.offset_cm = 0; - } - } -} - -/* - return true if current_loc is above loc. Used for glide slope - calculations. - - "above" is simple if we are not terrain following, as it just means - the pressure altitude of one is above the other. - - When in terrain following mode "above" means the over-the-terrain - current altitude is above the over-the-terrain alt of loc. It is - quite possible for current_loc to be "above" loc when it is at a - lower pressure altitude, if current_loc is in a low part of the - terrain - */ -bool Plane::above_location_current(const Location &loc) -{ -#if AP_TERRAIN_AVAILABLE - float terrain_alt; - if (loc.flags.terrain_alt && - terrain.height_above_terrain(terrain_alt, true)) { - float loc_alt = loc.alt*0.01f; - if (!loc.flags.relative_alt) { - loc_alt -= home.alt*0.01f; - } - return terrain_alt > loc_alt; - } -#endif - - float loc_alt_cm = loc.alt; - if (loc.flags.relative_alt) { - loc_alt_cm += home.alt; - } - return current_loc.alt > loc_alt_cm; -} - -/* - modify a destination to be setup for terrain following if - TERRAIN_FOLLOW is enabled - */ -void Plane::setup_terrain_target_alt(Location &loc) -{ -#if AP_TERRAIN_AVAILABLE - if (g.terrain_follow) { - loc.flags.terrain_alt = true; - } -#endif -} - -/* - return current_loc.alt adjusted for ALT_OFFSET - This is useful during long flights to account for barometer changes - from the GCS, or to adjust the flying height of a long mission - */ -int32_t Plane::adjusted_altitude_cm(void) -{ - return current_loc.alt - (g.alt_offset*100); -} - -/* - return home-relative altitude adjusted for ALT_OFFSET This is useful - during long flights to account for barometer changes from the GCS, - or to adjust the flying height of a long mission - */ -int32_t Plane::adjusted_relative_altitude_cm(void) -{ - return adjusted_altitude_cm() - home.alt; -} - -/* - return the height in meters above the next_WP_loc altitude - */ -float Plane::height_above_target(void) -{ - float target_alt = next_WP_loc.alt*0.01; - if (!next_WP_loc.flags.relative_alt) { - target_alt -= ahrs.get_home().alt*0.01f; - } - -#if AP_TERRAIN_AVAILABLE - // also record the terrain altitude if possible - float terrain_altitude; - if (next_WP_loc.flags.terrain_alt && - terrain.height_above_terrain(terrain_altitude, true)) { - return terrain_altitude - target_alt; - } -#endif - - return (adjusted_altitude_cm()*0.01f - ahrs.get_home().alt*0.01f) - target_alt; -} - -/* - work out target altitude adjustment from terrain lookahead - */ -float Plane::lookahead_adjustment(void) -{ -#if AP_TERRAIN_AVAILABLE - int32_t bearing_cd; - int16_t distance; - // work out distance and bearing to target - if (control_mode == FLY_BY_WIRE_B) { - // there is no target waypoint in FBWB, so use yaw as an approximation - bearing_cd = ahrs.yaw_sensor; - distance = g.terrain_lookahead; - } else if (!nav_controller->reached_loiter_target()) { - bearing_cd = nav_controller->target_bearing_cd(); - distance = constrain_float(auto_state.wp_distance, 0, g.terrain_lookahead); - } else { - // no lookahead when loitering - bearing_cd = 0; - distance = 0; - } - if (distance <= 0) { - // no lookahead - return 0; - } - - - float groundspeed = ahrs.groundspeed(); - if (groundspeed < 1) { - // we're not moving - return 0; - } - // we need to know the climb ratio. We use 50% of the maximum - // climb rate so we are not constantly at 100% throttle and to - // give a bit more margin on terrain - float climb_ratio = 0.5f * SpdHgt_Controller->get_max_climbrate() / groundspeed; - - if (climb_ratio <= 0) { - // lookahead makes no sense for negative climb rates - return 0; - } - - // ask the terrain code for the lookahead altitude change - float lookahead = terrain.lookahead(bearing_cd*0.01f, distance, climb_ratio); - - if (target_altitude.offset_cm < 0) { - // we are heading down to the waypoint, so we don't need to - // climb as much - lookahead += target_altitude.offset_cm*0.01f; - } - - // constrain lookahead to a reasonable limit - return constrain_float(lookahead, 0, 1000.0f); -#else - return 0; -#endif -} - - -/* - correct target altitude using rangefinder data. Returns offset in - meters to correct target altitude. A positive number means we need - to ask the speed/height controller to fly higher - */ -float Plane::rangefinder_correction(void) -{ -#if RANGEFINDER_ENABLED == ENABLED - if (millis() - rangefinder_state.last_correction_time_ms > 5000) { - // we haven't had any rangefinder data for 5s - don't use it - return 0; - } - - // for now we only support the rangefinder for landing - bool using_rangefinder = (g.rangefinder_landing && - control_mode == AUTO && - (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH || - flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL)); - if (!using_rangefinder) { - return 0; - } - - return rangefinder_state.correction; -#else - return 0; -#endif -} - -#if RANGEFINDER_ENABLED == ENABLED -/* - update the offset between rangefinder height and terrain height - */ -void Plane::rangefinder_height_update(void) -{ - float distance = rangefinder.distance_cm()*0.01f; - float height_estimate = 0; - if ((rangefinder.status() == RangeFinder::RangeFinder_Good) && home_is_set != HOME_UNSET) { - if (!rangefinder_state.have_initial_reading) { - rangefinder_state.have_initial_reading = true; - rangefinder_state.initial_range = distance; - } - // correct the range for attitude (multiply by DCM.c.z, which - // is cos(roll)*cos(pitch)) - height_estimate = distance * ahrs.get_rotation_body_to_ned().c.z; - - // we consider ourselves to be fully in range when we have 10 - // good samples (0.2s) that are different by 5% of the maximum - // range from the initial range we see. The 5% change is to - // catch Lidars that are giving a constant range, either due - // to misconfiguration or a faulty sensor - if (rangefinder_state.in_range_count < 10) { - if (fabsf(rangefinder_state.initial_range - distance) > 0.05f * rangefinder.max_distance_cm()*0.01f) { - rangefinder_state.in_range_count++; - } - } else { - rangefinder_state.in_range = true; - if (!rangefinder_state.in_use && - flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH && - g.rangefinder_landing) { - rangefinder_state.in_use = true; - gcs_send_text_fmt(MAV_SEVERITY_INFO, "Rangefinder engaged at %.2fm", (double)height_estimate); - } - } - } else { - rangefinder_state.in_range_count = 0; - rangefinder_state.in_range = false; - } - - if (rangefinder_state.in_range) { - // base correction is the difference between baro altitude and - // rangefinder estimate - float correction = relative_altitude() - height_estimate; - -#if AP_TERRAIN_AVAILABLE - // if we are terrain following then correction is based on terrain data - float terrain_altitude; - if ((target_altitude.terrain_following || g.terrain_follow) && - terrain.height_above_terrain(terrain_altitude, true)) { - correction = terrain_altitude - height_estimate; - } -#endif - - // remember the last correction. Use a low pass filter unless - // the old data is more than 5 seconds old - if (millis() - rangefinder_state.last_correction_time_ms > 5000) { - rangefinder_state.correction = correction; - rangefinder_state.initial_correction = correction; - } else { - rangefinder_state.correction = 0.8f*rangefinder_state.correction + 0.2f*correction; - if (fabsf(rangefinder_state.correction - rangefinder_state.initial_correction) > 30) { - // the correction has changed by more than 30m, reset use of Lidar. We may have a bad lidar - if (rangefinder_state.in_use) { - gcs_send_text_fmt(MAV_SEVERITY_INFO, "Rangefinder disengaged at %.2fm", (double)height_estimate); - } - memset(&rangefinder_state, 0, sizeof(rangefinder_state)); - } - } - rangefinder_state.last_correction_time_ms = millis(); - } -} -#endif diff --git a/ArduSubsea/arming_checks.cpp b/ArduSubsea/arming_checks.cpp deleted file mode 100644 index f22be6f78d..0000000000 --- a/ArduSubsea/arming_checks.cpp +++ /dev/null @@ -1,91 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -/* - additional arming checks for plane - */ -#include "Plane.h" - -const AP_Param::GroupInfo AP_Arming_Plane::var_info[] = { - // variables from parent vehicle - AP_NESTEDGROUPINFO(AP_Arming, 0), - - // @Param: RUDDER - // @DisplayName: Rudder Arming - // @Description: Control arm/disarm by rudder input. When enabled arming is done with right rudder, disarming with left rudder. Rudder arming only works in manual throttle modes with throttle at zero - // @Values: 0:Disabled,1:ArmingOnly,2:ArmOrDisarm - // @User: Advanced - AP_GROUPINFO("RUDDER", 3, AP_Arming_Plane, rudder_arming_value, ARMING_RUDDER_ARMONLY), - - AP_GROUPEND -}; - - -/* - additional arming checks for plane - */ -bool AP_Arming_Plane::pre_arm_checks(bool report) -{ - // call parent class checks - bool ret = AP_Arming::pre_arm_checks(report); - - // Check airspeed sensor - ret &= AP_Arming::airspeed_checks(report); - - if (plane.g.roll_limit_cd < 300) { - if (report) { - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: LIM_ROLL_CD too small (%u)", plane.g.roll_limit_cd); - } - ret = false; - } - - if (plane.aparm.pitch_limit_max_cd < 300) { - if (report) { - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: LIM_PITCH_MAX too small (%u)", plane.aparm.pitch_limit_max_cd); - } - ret = false; - } - - if (plane.aparm.pitch_limit_min_cd > -300) { - if (report) { - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: LIM_PITCH_MIN too large (%u)", plane.aparm.pitch_limit_min_cd); - } - ret = false; - } - - if (plane.channel_throttle->get_reverse() && - plane.g.throttle_fs_enabled && - plane.g.throttle_fs_value < - plane.channel_throttle->radio_max) { - if (report) { - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: Invalid THR_FS_VALUE for rev throttle"); - } - ret = false; - } - - return ret; -} - -bool AP_Arming_Plane::ins_checks(bool report) -{ - // call parent class checks - if (!AP_Arming::ins_checks(report)) { - return false; - } - - // additional plane specific checks - if ((checks_to_perform & ARMING_CHECK_ALL) || - (checks_to_perform & ARMING_CHECK_INS)) { - if (!ahrs.healthy()) { - if (report) { - const char *reason = ahrs.prearm_failure_reason(); - if (reason) { - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: %s", reason); - } else { - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: AHRS not healthy"); - } - } - return false; - } - } - - return true; -} diff --git a/ArduSubsea/capabilities.cpp b/ArduSubsea/capabilities.cpp deleted file mode 100644 index fd9ab981be..0000000000 --- a/ArduSubsea/capabilities.cpp +++ /dev/null @@ -1,9 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#include "Plane.h" - -void Plane::init_capabilities(void) -{ - hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT); - hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT); -} diff --git a/ArduSubsea/commands.cpp b/ArduSubsea/commands.cpp deleted file mode 100644 index e4d8b9acd3..0000000000 --- a/ArduSubsea/commands.cpp +++ /dev/null @@ -1,133 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -/* - * logic for dealing with the current command in the mission and home location - */ - -#include "Plane.h" - -/* - * set_next_WP - sets the target location the vehicle should fly to - */ -void Plane::set_next_WP(const struct Location &loc) -{ - if (auto_state.next_wp_no_crosstrack) { - // we should not try to cross-track for this waypoint - prev_WP_loc = current_loc; - // use cross-track for the next waypoint - auto_state.next_wp_no_crosstrack = false; - auto_state.no_crosstrack = true; - } else { - // copy the current WP into the OldWP slot - prev_WP_loc = next_WP_loc; - auto_state.no_crosstrack = false; - } - - // Load the next_WP slot - // --------------------- - next_WP_loc = loc; - - // if lat and lon is zero, then use current lat/lon - // this allows a mission to contain a "loiter on the spot" - // command - if (next_WP_loc.lat == 0 && next_WP_loc.lng == 0) { - next_WP_loc.lat = current_loc.lat; - next_WP_loc.lng = current_loc.lng; - // additionally treat zero altitude as current altitude - if (next_WP_loc.alt == 0) { - next_WP_loc.alt = current_loc.alt; - next_WP_loc.flags.relative_alt = false; - next_WP_loc.flags.terrain_alt = false; - } - } - - // convert relative alt to absolute alt - if (next_WP_loc.flags.relative_alt) { - next_WP_loc.flags.relative_alt = false; - next_WP_loc.alt += home.alt; - } - - // are we already past the waypoint? This happens when we jump - // waypoints, and it can cause us to skip a waypoint. If we are - // past the waypoint when we start on a leg, then use the current - // location as the previous waypoint, to prevent immediately - // considering the waypoint complete - if (location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) { - gcs_send_text(MAV_SEVERITY_NOTICE, "Resetting previous waypoint"); - prev_WP_loc = current_loc; - } - - // used to control FBW and limit the rate of climb - // ----------------------------------------------- - set_target_altitude_location(next_WP_loc); - - // zero out our loiter vals to watch for missed waypoints - loiter_angle_reset(); - - setup_glide_slope(); - setup_turn_angle(); - - loiter_angle_reset(); -} - -void Plane::set_guided_WP(void) -{ - if (g.loiter_radius < 0) { - loiter.direction = -1; - } else { - loiter.direction = 1; - } - - // copy the current location into the OldWP slot - // --------------------------------------- - prev_WP_loc = current_loc; - - // Load the next_WP slot - // --------------------- - next_WP_loc = guided_WP_loc; - - // used to control FBW and limit the rate of climb - // ----------------------------------------------- - set_target_altitude_current(); - - update_flight_stage(); - setup_glide_slope(); - setup_turn_angle(); - - loiter_angle_reset(); -} - -// run this at setup on the ground -// ------------------------------- -void Plane::init_home() -{ - gcs_send_text(MAV_SEVERITY_INFO, "Init HOME"); - - ahrs.set_home(gps.location()); - home_is_set = HOME_SET_NOT_LOCKED; - Log_Write_Home_And_Origin(); - GCS_MAVLINK::send_home_all(gps.location()); - - gcs_send_text_fmt(MAV_SEVERITY_INFO, "GPS alt: %lu", (unsigned long)home.alt); - - // Save Home to EEPROM - mission.write_home_to_storage(); - - // Save prev loc - // ------------- - next_WP_loc = prev_WP_loc = home; -} - -/* - update home location from GPS - this is called as long as we have 3D lock and the arming switch is - not pushed -*/ -void Plane::update_home() -{ - if (home_is_set == HOME_SET_NOT_LOCKED) { - ahrs.set_home(gps.location()); - Log_Write_Home_And_Origin(); - GCS_MAVLINK::send_home_all(gps.location()); - } - barometer.update_calibration(); -} diff --git a/ArduSubsea/commands_logic.cpp b/ArduSubsea/commands_logic.cpp deleted file mode 100644 index c795ab8b2e..0000000000 --- a/ArduSubsea/commands_logic.cpp +++ /dev/null @@ -1,996 +0,0 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#include "Plane.h" - -/********************************************************************************/ -// Command Event Handlers -/********************************************************************************/ -bool Plane::start_command(const AP_Mission::Mission_Command& cmd) -{ - // log when new commands start - if (should_log(MASK_LOG_CMD)) { - DataFlash.Log_Write_Mission_Cmd(mission, cmd); - } - - // special handling for nav vs non-nav commands - if (AP_Mission::is_nav_cmd(cmd)) { - // set land_complete to false to stop us zeroing the throttle - auto_state.land_complete = false; - auto_state.sink_rate = 0; - - // set takeoff_complete to true so we don't add extra evevator - // except in a takeoff - auto_state.takeoff_complete = true; - - // if a go around had been commanded, clear it now. - auto_state.commanded_go_around = false; - - // start non-idle - auto_state.idle_mode = false; - - // once landed, post some landing statistics to the GCS - auto_state.post_landing_stats = false; - - // reset loiter start time. New command is a new loiter - loiter.start_time_ms = 0; - - gcs_send_text_fmt(MAV_SEVERITY_INFO, "Executing nav command ID #%i",cmd.id); - } else { - gcs_send_text_fmt(MAV_SEVERITY_INFO, "Executing command ID #%i",cmd.id); - } - - switch(cmd.id) { - - case MAV_CMD_NAV_TAKEOFF: - crash_state.is_crashed = false; - do_takeoff(cmd); - break; - - case MAV_CMD_NAV_WAYPOINT: // Navigate to Waypoint - do_nav_wp(cmd); - break; - - case MAV_CMD_NAV_LAND: // LAND to Waypoint - do_land(cmd); - break; - - case MAV_CMD_NAV_LOITER_UNLIM: // Loiter indefinitely - do_loiter_unlimited(cmd); - break; - - case MAV_CMD_NAV_LOITER_TURNS: // Loiter N Times - do_loiter_turns(cmd); - break; - - case MAV_CMD_NAV_LOITER_TIME: - do_loiter_time(cmd); - break; - - case MAV_CMD_NAV_LOITER_TO_ALT: - do_loiter_to_alt(cmd); - break; - - case MAV_CMD_NAV_RETURN_TO_LAUNCH: - set_mode(RTL); - break; - - case MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT: - do_continue_and_change_alt(cmd); - break; - - case MAV_CMD_NAV_ALTITUDE_WAIT: - do_altitude_wait(cmd); - break; - - // Conditional commands - - case MAV_CMD_CONDITION_DELAY: - do_wait_delay(cmd); - break; - - case MAV_CMD_CONDITION_DISTANCE: - do_within_distance(cmd); - break; - - case MAV_CMD_CONDITION_CHANGE_ALT: - do_change_alt(cmd); - break; - - // Do commands - - case MAV_CMD_DO_CHANGE_SPEED: - do_change_speed(cmd); - break; - - case MAV_CMD_DO_SET_HOME: - 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_INVERTED_FLIGHT: - if (cmd.p1 == 0 || cmd.p1 == 1) { - auto_state.inverted_flight = (bool)cmd.p1; - gcs_send_text_fmt(MAV_SEVERITY_INFO, "Set inverted %u", cmd.p1); - } - break; - - case MAV_CMD_DO_LAND_START: - //ensure go around hasn't been set - auto_state.commanded_go_around = false; - break; - - case MAV_CMD_DO_FENCE_ENABLE: -#if GEOFENCE_ENABLED == ENABLED - if (cmd.p1 != 2) { - if (!geofence_set_enabled((bool) cmd.p1, AUTO_TOGGLED)) { - gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Unable to set fence. Enabled state to %u", cmd.p1); - } else { - gcs_send_text_fmt(MAV_SEVERITY_INFO, "Set fence enabled state to %u", cmd.p1); - } - } else { //commanding to only disable floor - if (! geofence_set_floor_enabled(false)) { - gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Unabled to disable fence floor"); - } else { - gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Fence floor disabled"); - } - } -#endif - break; - - case MAV_CMD_DO_AUTOTUNE_ENABLE: - autotune_enable(cmd.p1); - 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 Send Digicam Control message with the camera library - 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: - do_parachute(cmd); - break; -#endif - -#if MOUNT == ENABLED - // Sets the region of interest (ROI) for a sensor set or the - // vehicle itself. This can then be used by the vehicles control - // system to control the vehicle attitude and the attitude of various - // devices such as cameras. - // |Region of interest mode. (see MAV_ROI enum)| Waypoint index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple cameras etc.)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| - case MAV_CMD_DO_SET_ROI: - if (cmd.content.location.alt == 0 && cmd.content.location.lat == 0 && cmd.content.location.lng == 0) { - // switch off the camera tracking if enabled - if (camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) { - camera_mount.set_mode_to_default(); - } - } else { - // set mount's target location - camera_mount.set_roi_target(cmd.content.location); - } - break; - - case MAV_CMD_DO_MOUNT_CONTROL: // 205 - // point the camera to a specified angle - camera_mount.set_angle_targets(cmd.content.mount_control.roll, - cmd.content.mount_control.pitch, - cmd.content.mount_control.yaw); - break; -#endif - } - - return true; -} - -/******************************************************************************* -Verify command Handlers - -Each type of mission element has a "verify" operation. The verify -operation returns true when the mission element has completed and we -should move onto the next mission element. -*******************************************************************************/ - -bool Plane::verify_command(const AP_Mission::Mission_Command& cmd) // Returns true if command complete -{ - switch(cmd.id) { - - case MAV_CMD_NAV_TAKEOFF: - return verify_takeoff(); - - case MAV_CMD_NAV_LAND: - return verify_land(); - - case MAV_CMD_NAV_WAYPOINT: - return verify_nav_wp(cmd); - - case MAV_CMD_NAV_LOITER_UNLIM: - return verify_loiter_unlim(); - - case MAV_CMD_NAV_LOITER_TURNS: - return verify_loiter_turns(); - - case MAV_CMD_NAV_LOITER_TIME: - return verify_loiter_time(); - - case MAV_CMD_NAV_LOITER_TO_ALT: - return verify_loiter_to_alt(); - - case MAV_CMD_NAV_RETURN_TO_LAUNCH: - return verify_RTL(); - - case MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT: - return verify_continue_and_change_alt(); - - case MAV_CMD_NAV_ALTITUDE_WAIT: - return verify_altitude_wait(cmd); - - // 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(); - -#if PARACHUTE == ENABLED - case MAV_CMD_DO_PARACHUTE: - // assume parachute was released successfully - return true; - break; -#endif - - // do commands (always return true) - case MAV_CMD_DO_CHANGE_SPEED: - case MAV_CMD_DO_SET_HOME: - case MAV_CMD_DO_SET_SERVO: - case MAV_CMD_DO_SET_RELAY: - case MAV_CMD_DO_REPEAT_SERVO: - case MAV_CMD_DO_REPEAT_RELAY: - case MAV_CMD_DO_CONTROL_VIDEO: - case MAV_CMD_DO_DIGICAM_CONFIGURE: - case MAV_CMD_DO_DIGICAM_CONTROL: - case MAV_CMD_DO_SET_CAM_TRIGG_DIST: - case MAV_CMD_NAV_ROI: - case MAV_CMD_DO_MOUNT_CONFIGURE: - case MAV_CMD_DO_INVERTED_FLIGHT: - case MAV_CMD_DO_LAND_START: - case MAV_CMD_DO_FENCE_ENABLE: - case MAV_CMD_DO_AUTOTUNE_ENABLE: - return true; - - default: - // error message - if (AP_Mission::is_nav_cmd(cmd)) { - gcs_send_text(MAV_SEVERITY_WARNING,"Verify nav. Invalid or no current nav cmd"); - }else{ - gcs_send_text(MAV_SEVERITY_WARNING,"Verify conditon. Invalid or no current condition cmd"); - } - // return true so that we do not get stuck at this command - return true; - } -} - -/********************************************************************************/ -// Nav (Must) commands -/********************************************************************************/ - -void Plane::do_RTL(void) -{ - auto_state.next_wp_no_crosstrack = true; - auto_state.no_crosstrack = true; - prev_WP_loc = current_loc; - next_WP_loc = rally.calc_best_rally_or_home_location(current_loc, get_RTL_altitude()); - setup_terrain_target_alt(next_WP_loc); - set_target_altitude_location(next_WP_loc); - - if (g.loiter_radius < 0) { - loiter.direction = -1; - } else { - loiter.direction = 1; - } - - update_flight_stage(); - setup_glide_slope(); - setup_turn_angle(); - - if (should_log(MASK_LOG_MODE)) - DataFlash.Log_Write_Mode(control_mode); -} - -void Plane::do_takeoff(const AP_Mission::Mission_Command& cmd) -{ - prev_WP_loc = current_loc; - set_next_WP(cmd.content.location); - // pitch in deg, airspeed m/s, throttle %, track WP 1 or 0 - auto_state.takeoff_pitch_cd = (int16_t)cmd.p1 * 100; - if (auto_state.takeoff_pitch_cd <= 0) { - // if the mission doesn't specify a pitch use 4 degrees - auto_state.takeoff_pitch_cd = 400; - } - auto_state.takeoff_altitude_rel_cm = next_WP_loc.alt - home.alt; - next_WP_loc.lat = home.lat + 10; - next_WP_loc.lng = home.lng + 10; - auto_state.takeoff_speed_time_ms = 0; - auto_state.takeoff_complete = false; // set flag to use gps ground course during TO. IMU will be doing yaw drift correction - // Flag also used to override "on the ground" throttle disable - - // zero locked course - steer_state.locked_course_err = 0; - steer_state.hold_course_cd = -1; - auto_state.baro_takeoff_alt = barometer.get_altitude(); -} - -void Plane::do_nav_wp(const AP_Mission::Mission_Command& cmd) -{ - set_next_WP(cmd.content.location); -} - -void Plane::do_land(const AP_Mission::Mission_Command& cmd) -{ - auto_state.commanded_go_around = false; - set_next_WP(cmd.content.location); - - // configure abort altitude and pitch - // if NAV_LAND has an abort altitude then use it, else use last takeoff, else use 50m - if (cmd.p1 > 0) { - auto_state.takeoff_altitude_rel_cm = (int16_t)cmd.p1 * 100; - } else if (auto_state.takeoff_altitude_rel_cm <= 0) { - auto_state.takeoff_altitude_rel_cm = 3000; - } - - if (auto_state.takeoff_pitch_cd <= 0) { - // If no takeoff command has ever been used, default to a conservative 10deg - auto_state.takeoff_pitch_cd = 1000; - } - -#if RANGEFINDER_ENABLED == ENABLED - // zero rangefinder state, start to accumulate good samples now - memset(&rangefinder_state, 0, sizeof(rangefinder_state)); -#endif -} - -void Plane::loiter_set_direction_wp(const AP_Mission::Mission_Command& cmd) -{ - if (cmd.content.location.flags.loiter_ccw) { - loiter.direction = -1; - } else { - loiter.direction = 1; - } -} - -void Plane::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd) -{ - set_next_WP(cmd.content.location); - loiter_set_direction_wp(cmd); -} - -void Plane::do_loiter_turns(const AP_Mission::Mission_Command& cmd) -{ - set_next_WP(cmd.content.location); - loiter.total_cd = (uint32_t)(LOWBYTE(cmd.p1)) * 36000UL; - loiter_set_direction_wp(cmd); -} - -void Plane::do_loiter_time(const AP_Mission::Mission_Command& cmd) -{ - set_next_WP(cmd.content.location); - // we set start_time_ms when we reach the waypoint - loiter.time_max_ms = cmd.p1 * (uint32_t)1000; // units are seconds - loiter_set_direction_wp(cmd); -} - -void Plane::do_continue_and_change_alt(const AP_Mission::Mission_Command& cmd) -{ - // select heading method. Either mission, gps bearing projection or yaw based - // If prev_WP_loc and next_WP_loc are different then an accurate wp based bearing can - // be computed. However, if we had just changed modes before this, such as an aborted landing - // via mode change, the prev and next wps are the same. - float bearing; - if (!locations_are_same(prev_WP_loc, next_WP_loc)) { - // use waypoint based bearing, this is the usual case - steer_state.hold_course_cd = -1; - } else if (ahrs.get_gps().status() >= AP_GPS::GPS_OK_FIX_2D) { - // use gps ground course based bearing hold - steer_state.hold_course_cd = -1; - bearing = ahrs.get_gps().ground_course_cd() * 0.01f; - location_update(next_WP_loc, bearing, 1000); // push it out 1km - } else { - // use yaw based bearing hold - steer_state.hold_course_cd = wrap_360_cd(ahrs.yaw_sensor); - bearing = ahrs.yaw_sensor * 0.01f; - location_update(next_WP_loc, bearing, 1000); // push it out 1km - } - - next_WP_loc.alt = cmd.content.location.alt + home.alt; - condition_value = cmd.p1; - reset_offset_altitude(); -} - -void Plane::do_altitude_wait(const AP_Mission::Mission_Command& cmd) -{ - // set all servos to trim until we reach altitude or descent speed - auto_state.idle_mode = true; -} - -void Plane::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd) -{ - //set target alt - next_WP_loc.alt = cmd.content.location.alt; - - // convert relative alt to absolute alt - if (cmd.content.location.flags.relative_alt) { - next_WP_loc.flags.relative_alt = false; - next_WP_loc.alt += home.alt; - } - - //I know I'm storing this twice -- I'm doing that on purpose -- - //see verify_loiter_alt() function - condition_value = next_WP_loc.alt; - - //are lat and lon 0? if so, don't change the current wp lat/lon - if (cmd.content.location.lat != 0 || cmd.content.location.lng != 0) { - set_next_WP(cmd.content.location); - } - //set loiter direction - loiter_set_direction_wp(cmd); - - //must the plane be heading towards the next waypoint before breaking? - condition_value2 = LOWBYTE(cmd.p1); -} - -/********************************************************************************/ -// Verify Nav (Must) commands -/********************************************************************************/ -bool Plane::verify_takeoff() -{ - if (ahrs.yaw_initialised() && steer_state.hold_course_cd == -1) { - const float min_gps_speed = 5; - if (auto_state.takeoff_speed_time_ms == 0 && - gps.status() >= AP_GPS::GPS_OK_FIX_3D && - gps.ground_speed() > min_gps_speed) { - auto_state.takeoff_speed_time_ms = millis(); - } - if (auto_state.takeoff_speed_time_ms != 0 && - millis() - auto_state.takeoff_speed_time_ms >= 2000) { - // once we reach sufficient speed for good GPS course - // estimation we save our current GPS ground course - // corrected for summed yaw to set the take off - // course. This keeps wings level until we are ready to - // rotate, and also allows us to cope with arbitary - // compass errors for auto takeoff - float takeoff_course = wrap_PI(radians(gps.ground_course_cd()*0.01f)) - steer_state.locked_course_err; - takeoff_course = wrap_PI(takeoff_course); - steer_state.hold_course_cd = wrap_360_cd(degrees(takeoff_course)*100); - gcs_send_text_fmt(MAV_SEVERITY_INFO, "Holding course %ld at %.1fm/s (%.1f)", - steer_state.hold_course_cd, - (double)gps.ground_speed(), - (double)degrees(steer_state.locked_course_err)); - } - } - - if (steer_state.hold_course_cd != -1) { - // call navigation controller for heading hold - nav_controller->update_heading_hold(steer_state.hold_course_cd); - } else { - nav_controller->update_level_flight(); - } - - // see if we have reached takeoff altitude - int32_t relative_alt_cm = adjusted_relative_altitude_cm(); - if (relative_alt_cm > auto_state.takeoff_altitude_rel_cm) { - gcs_send_text_fmt(MAV_SEVERITY_INFO, "Takeoff complete at %.2fm", - (double)(relative_alt_cm*0.01f)); - steer_state.hold_course_cd = -1; - auto_state.takeoff_complete = true; - next_WP_loc = prev_WP_loc = current_loc; - -#if GEOFENCE_ENABLED == ENABLED - if (g.fence_autoenable > 0) { - if (! geofence_set_enabled(true, AUTO_TOGGLED)) { - gcs_send_text(MAV_SEVERITY_NOTICE, "Enable fence failed (cannot autoenable"); - } else { - gcs_send_text(MAV_SEVERITY_INFO, "Fence enabled (autoenabled)"); - } - } -#endif - - // don't cross-track on completion of takeoff, as otherwise we - // can end up doing too sharp a turn - auto_state.next_wp_no_crosstrack = true; - return true; - } else { - return false; - } -} - -/* - update navigation for normal mission waypoints. Return true when the - waypoint is complete - */ -bool Plane::verify_nav_wp(const AP_Mission::Mission_Command& cmd) -{ - steer_state.hold_course_cd = -1; - - if (auto_state.no_crosstrack) { - nav_controller->update_waypoint(current_loc, next_WP_loc); - } else { - nav_controller->update_waypoint(prev_WP_loc, next_WP_loc); - } - - // see if the user has specified a maximum distance to waypoint - if (g.waypoint_max_radius > 0 && - auto_state.wp_distance > (uint16_t)g.waypoint_max_radius) { - if (location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) { - // this is needed to ensure completion of the waypoint - prev_WP_loc = current_loc; - } - return false; - } - - float acceptance_distance = nav_controller->turn_distance(g.waypoint_radius, auto_state.next_turn_angle); - if (cmd.p1 > 0) { - // allow user to override acceptance radius - acceptance_distance = cmd.p1; - } - - if (auto_state.wp_distance <= acceptance_distance) { - gcs_send_text_fmt(MAV_SEVERITY_INFO, "Reached waypoint #%i dist %um", - (unsigned)mission.get_current_nav_cmd().index, - (unsigned)get_distance(current_loc, next_WP_loc)); - return true; - } - - // have we flown past the waypoint? - if (location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) { - gcs_send_text_fmt(MAV_SEVERITY_INFO, "Passed waypoint #%i dist %um", - (unsigned)mission.get_current_nav_cmd().index, - (unsigned)get_distance(current_loc, next_WP_loc)); - return true; - } - - return false; -} - -bool Plane::verify_loiter_unlim() -{ - update_loiter(); - return false; -} - -bool Plane::verify_loiter_time() -{ - update_loiter(); - if (loiter.start_time_ms == 0) { - if (nav_controller->reached_loiter_target()) { - // we've reached the target, start the timer - loiter.start_time_ms = millis(); - } - } else if ((millis() - loiter.start_time_ms) > loiter.time_max_ms) { - gcs_send_text(MAV_SEVERITY_WARNING,"Verify nav: LOITER time complete"); - return true; - } - return false; -} - -bool Plane::verify_loiter_turns() -{ - update_loiter(); - if (loiter.sum_cd > loiter.total_cd) { - loiter.total_cd = 0; - gcs_send_text(MAV_SEVERITY_WARNING,"Verify nav: LOITER orbits complete"); - // clear the command queue; - return true; - } - return false; -} - -/* - verify a LOITER_TO_ALT command. This involves checking we have - reached both the desired altitude and desired heading. The desired - altitude only needs to be reached once. - */ -bool Plane::verify_loiter_to_alt() -{ - update_loiter(); - - //has target altitude been reached? - if (condition_value != 0) { - if (labs(condition_value - current_loc.alt) < 500) { - //Only have to reach the altitude once -- that's why I need - //this global condition variable. - // - //This is in case of altitude oscillation when still trying - //to reach the target heading. - condition_value = 0; - } else { - return false; - } - } - - //has target heading been reached? - if (condition_value2 != 0) { - //Get the lat/lon of next Nav waypoint after this one: - AP_Mission::Mission_Command next_nav_cmd; - if (! mission.get_next_nav_cmd(mission.get_current_nav_index() + 1, - next_nav_cmd)) { - //no next waypoint to shoot for -- go ahead and break out of loiter - return true; - } - - if (get_distance(next_WP_loc, next_nav_cmd.content.location) < labs(g.loiter_radius)) { - /* Whenever next waypoint is within the loiter radius, - maintaining loiter would prevent us from ever pointing toward the next waypoint. - Hence break out of loiter immediately - */ - return true; - } - - // Bearing in radians - int32_t bearing_cd = get_bearing_cd(current_loc,next_nav_cmd.content.location); - - // get current heading. We should probably be using the ground - // course instead to improve the accuracy in wind - int32_t heading_cd = ahrs.yaw_sensor; - - int32_t heading_err_cd = wrap_180_cd(bearing_cd - heading_cd); - - /* - Check to see if the the plane is heading toward the land - waypoint - We use 10 degrees of slop so that we can handle 100 - degrees/second of yaw - */ - if (labs(heading_err_cd) <= 1000) { - //Want to head in a straight line from _here_ to the next waypoint. - //DON'T want to head in a line from the center of the loiter to - //the next waypoint. - //Therefore: mark the "last" (next_wp_loc is about to be updated) - //wp lat/lon as the current location. - next_WP_loc = current_loc; - return true; - } else { - return false; - } - } - - return true; -} - -bool Plane::verify_RTL() -{ - update_loiter(); - if (auto_state.wp_distance <= (uint32_t)MAX(g.waypoint_radius,0) || - nav_controller->reached_loiter_target()) { - gcs_send_text(MAV_SEVERITY_INFO,"Reached HOME"); - return true; - } else { - return false; - } -} - -bool Plane::verify_continue_and_change_alt() -{ - // is waypoint info not available and heading hold is? - if (locations_are_same(prev_WP_loc, next_WP_loc) && - steer_state.hold_course_cd != -1) { - //keep flying the same course with fixed steering heading computed at start if cmd - nav_controller->update_heading_hold(steer_state.hold_course_cd); - } - else { - // Is the next_WP less than 200 m away? - if (get_distance(current_loc, next_WP_loc) < 200.0f) { - //push another 300 m down the line - int32_t next_wp_bearing_cd = get_bearing_cd(prev_WP_loc, next_WP_loc); - location_update(next_WP_loc, next_wp_bearing_cd * 0.01f, 300.0f); - } - - //keep flying the same course - nav_controller->update_waypoint(prev_WP_loc, next_WP_loc); - } - - //climbing? - if (condition_value == 1 && adjusted_altitude_cm() >= next_WP_loc.alt) { - return true; - } - //descending? - else if (condition_value == 2 && - adjusted_altitude_cm() <= next_WP_loc.alt) { - return true; - } - //don't care if we're climbing or descending - else if (labs(adjusted_altitude_cm() - next_WP_loc.alt) <= 500) { - return true; - } - - return false; -} - -/* - see if we have reached altitude or descent speed - */ -bool Plane::verify_altitude_wait(const AP_Mission::Mission_Command &cmd) -{ - if (current_loc.alt > cmd.content.altitude_wait.altitude*100.0f) { - gcs_send_text(MAV_SEVERITY_INFO,"Reached altitude"); - return true; - } - if (auto_state.sink_rate > cmd.content.altitude_wait.descent_rate) { - gcs_send_text_fmt(MAV_SEVERITY_INFO, "Reached descent rate %.1f m/s", (double)auto_state.sink_rate); - return true; - } - - // if requested, wiggle servos - if (cmd.content.altitude_wait.wiggle_time != 0) { - static uint32_t last_wiggle_ms; - if (auto_state.idle_wiggle_stage == 0 && - AP_HAL::millis() - last_wiggle_ms > cmd.content.altitude_wait.wiggle_time*1000) { - auto_state.idle_wiggle_stage = 1; - last_wiggle_ms = AP_HAL::millis(); - } - // idle_wiggle_stage is updated in set_servos_idle() - } - - return false; -} - -/********************************************************************************/ -// Condition (May) commands -/********************************************************************************/ - -void Plane::do_wait_delay(const AP_Mission::Mission_Command& cmd) -{ - condition_start = millis(); - condition_value = cmd.content.delay.seconds * 1000; // convert seconds to milliseconds -} - -/* - process a DO_CHANGE_ALT request - */ -void Plane::do_change_alt(const AP_Mission::Mission_Command& cmd) -{ - condition_rate = labs((int)cmd.content.location.lat); // climb rate in cm/s - condition_value = cmd.content.location.alt; // To-Do: ensure this altitude is an absolute altitude? - if (condition_value < adjusted_altitude_cm()) { - condition_rate = -condition_rate; - } - set_target_altitude_current_adjusted(); - change_target_altitude(condition_rate/10); - next_WP_loc.alt = condition_value; // For future nav calculations - reset_offset_altitude(); - setup_glide_slope(); -} - -void Plane::do_within_distance(const AP_Mission::Mission_Command& cmd) -{ - condition_value = cmd.content.distance.meters; -} - -/********************************************************************************/ -// Verify Condition (May) commands -/********************************************************************************/ - -bool Plane::verify_wait_delay() -{ - if ((unsigned)(millis() - condition_start) > (unsigned)condition_value) { - condition_value = 0; - return true; - } - return false; -} - -bool Plane::verify_change_alt() -{ - if( (condition_rate>=0 && adjusted_altitude_cm() >= condition_value) || - (condition_rate<=0 && adjusted_altitude_cm() <= condition_value)) { - condition_value = 0; - return true; - } - // condition_rate is climb rate in cm/s. - // We divide by 10 because this function is called at 10hz - change_target_altitude(condition_rate/10); - return false; -} - -bool Plane::verify_within_distance() -{ - if (auto_state.wp_distance < MAX(condition_value,0)) { - condition_value = 0; - return true; - } - return false; -} - -/********************************************************************************/ -// Do (Now) commands -/********************************************************************************/ - -void Plane::do_loiter_at_location() -{ - if (g.loiter_radius < 0) { - loiter.direction = -1; - } else { - loiter.direction = 1; - } - next_WP_loc = current_loc; -} - -void Plane::do_change_speed(const AP_Mission::Mission_Command& cmd) -{ - switch (cmd.content.speed.speed_type) - { - case 0: // Airspeed - if (cmd.content.speed.target_ms > 0) { - g.airspeed_cruise_cm.set(cmd.content.speed.target_ms * 100); - gcs_send_text_fmt(MAV_SEVERITY_INFO, "Set airspeed %u m/s", (unsigned)cmd.content.speed.target_ms); - } - break; - case 1: // Ground speed - gcs_send_text_fmt(MAV_SEVERITY_INFO, "Set groundspeed %u", (unsigned)cmd.content.speed.target_ms); - g.min_gndspeed_cm.set(cmd.content.speed.target_ms * 100); - break; - } - - if (cmd.content.speed.throttle_pct > 0 && cmd.content.speed.throttle_pct <= 100) { - gcs_send_text_fmt(MAV_SEVERITY_INFO, "Set throttle %u", (unsigned)cmd.content.speed.throttle_pct); - aparm.throttle_cruise.set(cmd.content.speed.throttle_pct); - } -} - -void Plane::do_set_home(const AP_Mission::Mission_Command& cmd) -{ - if (cmd.p1 == 1 && gps.status() >= AP_GPS::GPS_OK_FIX_3D) { - init_home(); - } else { - ahrs.set_home(cmd.content.location); - home_is_set = HOME_SET_NOT_LOCKED; - Log_Write_Home_And_Origin(); - GCS_MAVLINK::send_home_all(cmd.content.location); - } -} - -// do_digicam_configure Send Digicam Configure message with the camera library -void Plane::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 Plane::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 Plane::do_take_picture() -{ -#if CAMERA == ENABLED - camera.trigger_pic(true); - log_picture(); -#endif -} - -#if PARACHUTE == ENABLED -// do_parachute - configure or release parachute -void Plane::do_parachute(const AP_Mission::Mission_Command& cmd) -{ - switch (cmd.p1) { - case PARACHUTE_DISABLE: - parachute.enabled(false); - break; - case PARACHUTE_ENABLE: - parachute.enabled(true); - break; - case PARACHUTE_RELEASE: - parachute_release(); - break; - default: - // do nothing - break; - } -} -#endif - -// log_picture - log picture taken and send feedback to GCS -void Plane::log_picture() -{ -#if CAMERA == ENABLED - gcs_send_message(MSG_CAMERA_FEEDBACK); - if (should_log(MASK_LOG_CAMERA)) { - DataFlash.Log_Write_Camera(ahrs, gps, current_loc); - } -#endif -} - -// start_command_callback - callback function called from ap-mission when it begins a new mission command -// 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 Plane::start_command_callback(const AP_Mission::Mission_Command &cmd) -{ - if (control_mode == AUTO) { - return start_command(cmd); - } - return true; -} - -// 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 Plane::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; -} - -// exit_mission_callback - callback function called from ap-mission when the mission has completed -// 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 -void Plane::exit_mission_callback() -{ - if (control_mode == AUTO) { - gcs_send_text_fmt(MAV_SEVERITY_INFO, "Returning to HOME"); - memset(&auto_rtl_command, 0, sizeof(auto_rtl_command)); - auto_rtl_command.content.location = - rally.calc_best_rally_or_home_location(current_loc, get_RTL_altitude()); - auto_rtl_command.id = MAV_CMD_NAV_LOITER_UNLIM; - setup_terrain_target_alt(auto_rtl_command.content.location); - update_flight_stage(); - setup_glide_slope(); - setup_turn_angle(); - start_command(auto_rtl_command); - } -} diff --git a/ArduSubsea/commands_process.cpp b/ArduSubsea/commands_process.cpp deleted file mode 100644 index eaefeefae2..0000000000 --- a/ArduSubsea/commands_process.cpp +++ /dev/null @@ -1,20 +0,0 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#include "Plane.h" - -// called by update navigation at 10Hz -// -------------------- -void Plane::update_commands(void) -{ - if(control_mode == AUTO) { - if (home_is_set != HOME_UNSET) { - if(mission.state() == AP_Mission::MISSION_RUNNING) { - mission.update(); - } else { - // auto_rtl_command should have been set to MAV_CMD_NAV_LOITER_UNLIM by exit_mission - verify_command(auto_rtl_command); - } - } - } -} - diff --git a/ArduSubsea/config.h b/ArduSubsea/config.h deleted file mode 100644 index 37e473b1d6..0000000000 --- a/ArduSubsea/config.h +++ /dev/null @@ -1,432 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -// -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// -// -// 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 -/// -/// DO NOT EDIT THIS INCLUDE - if you want to make a local change, make that -/// change in your local copy of APM_Config.h. -/// - -// Just so that it's completely clear... -#define ENABLED 1 -#define DISABLED 0 - -// this avoids a very common config error -#define ENABLE ENABLED -#define DISABLE DISABLED - -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// -// HARDWARE CONFIGURATION AND CONNECTIONS -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// - -#ifdef CONFIG_APM_HARDWARE -#error CONFIG_APM_HARDWARE option is depreated! use CONFIG_HAL_BOARD instead. -#endif - -#ifndef MAV_SYSTEM_ID - # define MAV_SYSTEM_ID 1 -#endif - -////////////////////////////////////////////////////////////////////////////// -// FrSky telemetry support -// - -#ifndef FRSKY_TELEM_ENABLED -#define FRSKY_TELEM_ENABLED ENABLED -#endif - -////////////////////////////////////////////////////////////////////////////// -// Optical flow sensor support -// - -#ifndef OPTFLOW -#if AP_AHRS_NAVEKF_AVAILABLE - # define OPTFLOW ENABLED -#else - # define OPTFLOW DISABLED -#endif -#endif - -#define RANGEFINDER_ENABLED ENABLED - -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// -// RADIO CONFIGURATION -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// - - -////////////////////////////////////////////////////////////////////////////// -// Radio channel limits -// -// Note that these are not called out in APM_Config.h.reference. -// -#ifndef CH5_MIN - # define CH5_MIN 1000 -#endif -#ifndef CH5_MAX - # define CH5_MAX 2000 -#endif -#ifndef CH6_MIN - # define CH6_MIN 1000 -#endif -#ifndef CH6_MAX - # define CH6_MAX 2000 -#endif -#ifndef CH7_MIN - # define CH7_MIN 1000 -#endif -#ifndef CH7_MAX - # define CH7_MAX 2000 -#endif -#ifndef CH8_MIN - # define CH8_MIN 1000 -#endif -#ifndef CH8_MAX - # define CH8_MAX 2000 -#endif - - -#ifndef FLAP_1_PERCENT - # define FLAP_1_PERCENT 0 -#endif -#ifndef FLAP_1_SPEED - # define FLAP_1_SPEED 0 -#endif -#ifndef FLAP_2_PERCENT - # define FLAP_2_PERCENT 0 -#endif -#ifndef FLAP_2_SPEED - # define FLAP_2_SPEED 0 -#endif -////////////////////////////////////////////////////////////////////////////// -// FLIGHT_MODE -// FLIGHT_MODE_CHANNEL -// -#ifndef FLIGHT_MODE_CHANNEL - # define FLIGHT_MODE_CHANNEL 8 -#endif -#if (FLIGHT_MODE_CHANNEL != 5) && (FLIGHT_MODE_CHANNEL != 6) && (FLIGHT_MODE_CHANNEL != 7) && (FLIGHT_MODE_CHANNEL != 8) - # error XXX - # error XXX You must set FLIGHT_MODE_CHANNEL to 5, 6, 7 or 8 - # error XXX -#endif - -#if !defined(FLIGHT_MODE_1) - # define FLIGHT_MODE_1 RTL -#endif -#if !defined(FLIGHT_MODE_2) - # define FLIGHT_MODE_2 RTL -#endif -#if !defined(FLIGHT_MODE_3) - # define FLIGHT_MODE_3 FLY_BY_WIRE_A -#endif -#if !defined(FLIGHT_MODE_4) - # define FLIGHT_MODE_4 FLY_BY_WIRE_A -#endif -#if !defined(FLIGHT_MODE_5) - # define FLIGHT_MODE_5 MANUAL -#endif -#if !defined(FLIGHT_MODE_6) - # define FLIGHT_MODE_6 MANUAL -#endif - - -////////////////////////////////////////////////////////////////////////////// -// THROTTLE_FAILSAFE -// THROTTLE_FS_VALUE -// SHORT_FAILSAFE_ACTION -// LONG_FAILSAFE_ACTION -#ifndef THROTTLE_FAILSAFE - # define THROTTLE_FAILSAFE ENABLED -#endif -#ifndef THROTTLE_FS_VALUE - # define THROTTLE_FS_VALUE 950 -#endif -#ifndef SHORT_FAILSAFE_ACTION - # define SHORT_FAILSAFE_ACTION 0 -#endif -#ifndef LONG_FAILSAFE_ACTION - # define LONG_FAILSAFE_ACTION 0 -#endif - -////////////////////////////////////////////////////////////////////////////// -// AUTO_TRIM -// -#ifndef AUTO_TRIM - # define AUTO_TRIM DISABLED -#endif - - -////////////////////////////////////////////////////////////////////////////// -// THROTTLE_OUT -// -#ifndef THROTTE_OUT - # define THROTTLE_OUT ENABLED -#endif - - -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// -// STARTUP BEHAVIOUR -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// - - -////////////////////////////////////////////////////////////////////////////// -// GROUND_START_DELAY -// -#ifndef GROUND_START_DELAY - # define GROUND_START_DELAY 0 -#endif - -////////////////////////////////////////////////////////////////////////////// -// ENABLE ELEVON_MIXING -// -#ifndef ELEVON_MIXING - # define ELEVON_MIXING DISABLED -#endif -#ifndef ELEVON_REVERSE - # define ELEVON_REVERSE DISABLED -#endif -#ifndef ELEVON_CH1_REVERSE - # define ELEVON_CH1_REVERSE DISABLED -#endif -#ifndef ELEVON_CH2_REVERSE - # define ELEVON_CH2_REVERSE DISABLED -#endif - -////////////////////////////////////////////////////////////////////////////// -// CAMERA TRIGGER AND CONTROL -// -// uses 1182 bytes of memory -#ifndef CAMERA - # define CAMERA ENABLED -#endif - -////////////////////////////////////////////////////////////////////////////// -// MOUNT (ANTENNA OR CAMERA) -// -// uses 7726 bytes of memory on 2560 chips (all options are enabled) -#ifndef MOUNT -#define MOUNT ENABLED -#endif - -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// -// FLIGHT AND NAVIGATION CONTROL -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////////////////////////// -// Altitude measurement and control. -// -#ifndef ALTITUDE_MIX - # define ALTITUDE_MIX 1 -#endif - - -////////////////////////////////////////////////////////////////////////////// -// AIRSPEED_CRUISE -// -#ifndef AIRSPEED_CRUISE - # define AIRSPEED_CRUISE 12 // 12 m/s -#endif -#define AIRSPEED_CRUISE_CM AIRSPEED_CRUISE*100 - - -////////////////////////////////////////////////////////////////////////////// -// MIN_GNDSPEED -// -#ifndef MIN_GNDSPEED - # define MIN_GNDSPEED 0 // m/s (0 disables) -#endif -#define MIN_GNDSPEED_CM MIN_GNDSPEED*100 - - -////////////////////////////////////////////////////////////////////////////// -// FLY_BY_WIRE_B airspeed control -// -#ifndef AIRSPEED_FBW_MIN - # define AIRSPEED_FBW_MIN 9 -#endif -#ifndef AIRSPEED_FBW_MAX - # define AIRSPEED_FBW_MAX 22 -#endif - -#ifndef ALT_HOLD_FBW - # define ALT_HOLD_FBW 0 -#endif -#define ALT_HOLD_FBW_CM ALT_HOLD_FBW*100 - - -////////////////////////////////////////////////////////////////////////////// -// Servo Mapping -// -#ifndef THROTTLE_MIN - # define THROTTLE_MIN 0 // percent -#endif -#ifndef THROTTLE_CRUISE - # define THROTTLE_CRUISE 45 -#endif -#ifndef THROTTLE_MAX - # define THROTTLE_MAX 75 -#endif - -////////////////////////////////////////////////////////////////////////////// -// Autopilot control limits -// -#ifndef HEAD_MAX - # define HEAD_MAX 45 -#endif -#ifndef PITCH_MAX - # define PITCH_MAX 20 -#endif -#ifndef PITCH_MIN - # define PITCH_MIN -25 -#endif -#define HEAD_MAX_CENTIDEGREE HEAD_MAX * 100 -#define PITCH_MAX_CENTIDEGREE PITCH_MAX * 100 -#define PITCH_MIN_CENTIDEGREE PITCH_MIN * 100 - -#ifndef RUDDER_MIX - # define RUDDER_MIX 0.5f -#endif - - -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// -// DEBUGGING -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////////////////////////// -// Dataflash logging control -// - -#ifndef LOGGING_ENABLED - # define LOGGING_ENABLED ENABLED -#endif - -#define DEFAULT_LOG_BITMASK 0xffff - - -////////////////////////////////////////////////////////////////////////////// -// Navigation defaults -// -#ifndef WP_RADIUS_DEFAULT - # define WP_RADIUS_DEFAULT 90 -#endif - -#ifndef LOITER_RADIUS_DEFAULT - # define LOITER_RADIUS_DEFAULT 60 -#endif - -#ifndef ALT_HOLD_HOME - # define ALT_HOLD_HOME 100 -#endif -#define ALT_HOLD_HOME_CM ALT_HOLD_HOME*100 - -#ifndef USE_CURRENT_ALT - # define USE_CURRENT_ALT FALSE -#endif - -#ifndef INVERTED_FLIGHT_PWM - # define INVERTED_FLIGHT_PWM 1750 -#endif - -#ifndef PX4IO_OVERRIDE_PWM - # define PX4IO_OVERRIDE_PWM 1750 -#endif - -////////////////////////////////////////////////////////////////////////////// -// Developer Items -// - -#ifndef SCALING_SPEED - # define SCALING_SPEED 15.0 -#endif - -// use this to completely disable the CLI. We now default the CLI to -// off on smaller boards. -#ifndef CLI_ENABLED -#define CLI_ENABLED ENABLED -#endif - -// use this to disable geo-fencing -#ifndef GEOFENCE_ENABLED - # define GEOFENCE_ENABLED ENABLED -#endif - -// pwm value on FENCE_CHANNEL to use to enable fenced mode -#ifndef FENCE_ENABLE_PWM - # define FENCE_ENABLE_PWM 1750 -#endif - -// a digital pin to set high when the geo-fence triggers. Defaults -// to -1, which means don't activate a pin -#ifndef FENCE_TRIGGERED_PIN - # define FENCE_TRIGGERED_PIN -1 -#endif - -// if RESET_SWITCH_CH is not zero, then this is the PWM value on -// that channel where we reset the control mode to the current switch -// position (to for example return to switched mode after failsafe or -// fence breach) -#ifndef RESET_SWITCH_CHAN_PWM - # define RESET_SWITCH_CHAN_PWM 1750 -#endif - -// OBC Failsafe enable -#ifndef OBC_FAILSAFE -#define OBC_FAILSAFE ENABLED -#endif - -#define HIL_SUPPORT ENABLED - -////////////////////////////////////////////////////////////////////////////// -// Parachute release -#ifndef PARACHUTE -#define PARACHUTE 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 diff --git a/ArduSubsea/control_modes.cpp b/ArduSubsea/control_modes.cpp deleted file mode 100644 index 43e0d8e4b1..0000000000 --- a/ArduSubsea/control_modes.cpp +++ /dev/null @@ -1,164 +0,0 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#include "Plane.h" - -void Plane::read_control_switch() -{ - static bool switch_debouncer; - uint8_t switchPosition = readSwitch(); - - // If switchPosition = 255 this indicates that the mode control channel input was out of range - // If we get this value we do not want to change modes. - if(switchPosition == 255) return; - - if (failsafe.ch3_failsafe || failsafe.ch3_counter > 0) { - // when we are in ch3_failsafe mode then RC input is not - // working, and we need to ignore the mode switch channel - return; - } - - if (millis() - failsafe.last_valid_rc_ms > 100) { - // only use signals that are less than 0.1s old. - return; - } - - // we look for changes in the switch position. If the - // RST_SWITCH_CH parameter is set, then it is a switch that can be - // used to force re-reading of the control switch. This is useful - // when returning to the previous mode after a failsafe or fence - // breach. This channel is best used on a momentary switch (such - // as a spring loaded trainer switch). - if (oldSwitchPosition != switchPosition || - (g.reset_switch_chan != 0 && - hal.rcin->read(g.reset_switch_chan-1) > RESET_SWITCH_CHAN_PWM)) { - - if (switch_debouncer == false) { - // this ensures that mode switches only happen if the - // switch changes for 2 reads. This prevents momentary - // spikes in the mode control channel from causing a mode - // switch - switch_debouncer = true; - return; - } - - set_mode((enum FlightMode)(flight_modes[switchPosition].get())); - - oldSwitchPosition = switchPosition; - } - - if (g.reset_mission_chan != 0 && - hal.rcin->read(g.reset_mission_chan-1) > RESET_SWITCH_CHAN_PWM) { - mission.start(); - prev_WP_loc = current_loc; - } - - switch_debouncer = false; - - if (g.inverted_flight_ch != 0) { - // if the user has configured an inverted flight channel, then - // fly upside down when that channel goes above INVERTED_FLIGHT_PWM - inverted_flight = (control_mode != MANUAL && hal.rcin->read(g.inverted_flight_ch-1) > INVERTED_FLIGHT_PWM); - } - - if (g.parachute_channel > 0) { - if (hal.rcin->read(g.parachute_channel-1) >= 1700) { - parachute_manual_release(); - } - } - -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 - if (g.override_channel > 0) { - // if the user has configured an override channel then check it - bool override = (hal.rcin->read(g.override_channel-1) >= PX4IO_OVERRIDE_PWM); - if (override && !px4io_override_enabled) { - // we only update the mixer if we are not armed. This is - // important as otherwise we will need to temporarily - // disarm to change the mixer - if (hal.util->get_soft_armed() || setup_failsafe_mixing()) { - px4io_override_enabled = true; - // disable output channels to force PX4IO override - gcs_send_text(MAV_SEVERITY_WARNING, "PX4IO override enabled"); - } else { - // we'll try again next loop. The PX4IO code sometimes - // rejects a mixer, probably due to it being busy in - // some way? - gcs_send_text(MAV_SEVERITY_WARNING, "PX4IO override enable failed"); - } - } else if (!override && px4io_override_enabled) { - px4io_override_enabled = false; - RC_Channel_aux::enable_aux_servos(); - gcs_send_text(MAV_SEVERITY_WARNING, "PX4IO override disabled"); - } - if (px4io_override_enabled && - hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_ARMED) { - // we force safety off, so that if this override is used - // with a in-flight reboot it gives a way for the pilot to - // re-arm and take manual control - hal.rcout->force_safety_off(); - } - } -#endif // CONFIG_HAL_BOARD -} - -uint8_t Plane::readSwitch(void) -{ - uint16_t pulsewidth = hal.rcin->read(g.flight_mode_channel - 1); - if (pulsewidth <= 900 || pulsewidth >= 2200) return 255; // This is an error condition - if (pulsewidth > 1230 && pulsewidth <= 1360) return 1; - if (pulsewidth > 1360 && pulsewidth <= 1490) return 2; - if (pulsewidth > 1490 && pulsewidth <= 1620) return 3; - if (pulsewidth > 1620 && pulsewidth <= 1749) return 4; // Software Manual - if (pulsewidth >= 1750) return 5; // Hardware Manual - return 0; -} - -void Plane::reset_control_switch() -{ - oldSwitchPosition = 254; - read_control_switch(); -} - -/* - called when entering autotune - */ -void Plane::autotune_start(void) -{ - rollController.autotune_start(); - pitchController.autotune_start(); -} - -/* - called when exiting autotune - */ -void Plane::autotune_restore(void) -{ - rollController.autotune_restore(); - pitchController.autotune_restore(); -} - -/* - enable/disable autotune for AUTO modes - */ -void Plane::autotune_enable(bool enable) -{ - if (enable) { - autotune_start(); - } else { - autotune_restore(); - } -} - -/* - are we flying inverted? - */ -bool Plane::fly_inverted(void) -{ - if (g.inverted_flight_ch != 0 && inverted_flight) { - // controlled with INVERTED_FLIGHT_CH - return true; - } - if (control_mode == AUTO && auto_state.inverted_flight) { - return true; - } - return false; -} diff --git a/ArduSubsea/createTags b/ArduSubsea/createTags deleted file mode 100755 index 01bfa7ad5e..0000000000 --- a/ArduSubsea/createTags +++ /dev/null @@ -1,67 +0,0 @@ -#!/bin/bash -#" Autocompletion enabled vim for arduino pde's - -ctags -RV --language-force=C++ --c++-kinds=+p --fields=+iaS --extra=+q \ - . \ - ../libraries/* - -# sample vimrc file -# you'll need to have omnicpp plugin for vim - -#"set compatible - -#" Vim5 and later versions support syntax highlighting. Uncommenting the next -#" line enables syntax highlighting by default. -#syntax on -#filetype on -#au BufNewFile,BufRead *.pde set filetype=cpp - -#" If using a dark background within the editing area and syntax highlighting -#" turn on this option as well -#"set background=dark - -#" Uncomment the following to have Vim jump to the last position when -#" reopening a file -#if has("autocmd") - #au BufReadPost * if line("'\"") > 1 && line("'\"") <= line("$") | exe "normal! g'\"" | endif -#endif - -#" Uncomment the following to have Vim load indentation rules and plugins -#" according to the detected filetype. -#if has("autocmd") - #filetype plugin indent on -#endif - -#" The following are commented out as they cause vim to behave a lot -#" differently from regular Vi. They are highly recommended though. -#set showcmd " Show (partial) command in status line. -#set showmatch " Show matching brackets. -#set ignorecase " Do case insensitive matching -#set smartcase " Do smart case matching -#set incsearch " Incremental search -#set autowrite " Automatically save before commands like :next and :make -#set hidden " Hide buffers when they are abandoned -#set mouse=a " Enable mouse usage (all modes) -#set vb - -#" build tags of your own project with Ctrl-F12 -#map :!ctags -R --sort=yes --c++-kinds=+p --fields=+iaS --extra=+q . - -#" OmniCppComplete -#let OmniCpp_NamespaceSearch = 1 -#let OmniCpp_GlobalScopeSearch = 1 -#let OmniCpp_ShowAccess = 1 -#let OmniCpp_ShowPrototypeInAbbr = 1 " show function parameters -#let OmniCpp_MayCompleteDot = 1 " autocomplete after . -#let OmniCpp_MayCompleteArrow = 1 " autocomplete after -> -#let OmniCpp_MayCompleteScope = 1 " autocomplete after :: -#let OmniCpp_DefaultNamespaces = ["std", "_GLIBCXX_STD"] - -#" arduino syntax -#au BufNewFile,BufRead *.pde setf arduino - -#" automatically open and close the popup menu / preview window -#"au CursorMovedI,InsertLeave * if pumvisible() == 0|silent! pclose|endif -#"set completeopt=menuone,menu,longest,preview -#set ts=4 -#set sw=4 diff --git a/ArduSubsea/defines.h b/ArduSubsea/defines.h deleted file mode 100644 index 438a8f886b..0000000000 --- a/ArduSubsea/defines.h +++ /dev/null @@ -1,193 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#ifndef _DEFINES_H -#define _DEFINES_H - -// Internal defines, don't edit and expect things to work -// ------------------------------------------------------- - -#define TRUE 1 -#define FALSE 0 -#define ToRad(x) radians(x) // *pi/180 -#define ToDeg(x) degrees(x) // *180/pi - -#define DEBUG 0 -#define LOITER_RANGE 60 // for calculating power outside of loiter radius -#define SERVO_MAX 4500 // This value represents 45 degrees and is just an - // arbitrary representation of servo max travel. - -// failsafe -// ---------------------- -enum failsafe_state { - FAILSAFE_NONE=0, - FAILSAFE_SHORT=1, - FAILSAFE_LONG=2, - FAILSAFE_GCS=3 -}; - - -// GCS failsafe types for FS_GCS_ENABL parameter -enum gcs_failsafe { - GCS_FAILSAFE_OFF = 0, // no GCS failsafe - GCS_FAILSAFE_HEARTBEAT = 1, // failsafe if we stop receiving heartbeat - GCS_FAILSAFE_HB_RSSI = 2, // failsafe if we stop receiving - // heartbeat or if RADIO.remrssi - // drops to 0 - GCS_FAILSAFE_HB_AUTO = 3 // failsafe if we stop receiving heartbeat - // while in AUTO mode -}; - - -// active altitude sensor -// ---------------------- -#define SONAR 0 -#define BARO 1 - -#define PITOT_SOURCE_ADC 1 -#define PITOT_SOURCE_ANALOG_PIN 2 - -#define T6 1000000 -#define T7 10000000 - -enum FlightMode { - MANUAL = 0, - CIRCLE = 1, - STABILIZE = 2, - TRAINING = 3, - ACRO = 4, - FLY_BY_WIRE_A = 5, - FLY_BY_WIRE_B = 6, - CRUISE = 7, - AUTOTUNE = 8, - AUTO = 10, - RTL = 11, - LOITER = 12, - GUIDED = 15, - INITIALISING = 16 -}; - -// type of stick mixing enabled -enum StickMixing { - STICK_MIXING_DISABLED = 0, - STICK_MIXING_FBW = 1, - STICK_MIXING_DIRECT = 2 -}; - -enum ChannelMixing { - MIXING_DISABLED = 0, - MIXING_UPUP = 1, - MIXING_UPDN = 2, - MIXING_DNUP = 3, - MIXING_DNDN = 4 -}; - -/* - * The cause for the most recent fence enable - */ -typedef enum GeofenceEnableReason { - NOT_ENABLED = 0, //The fence is not enabled - PWM_TOGGLED, //Fence enabled/disabled by PWM signal - AUTO_TOGGLED, //Fence auto enabled/disabled at takeoff. - GCS_TOGGLED //Fence enabled/disabled by the GCS via Mavlink -} GeofenceEnableReason; - -//repeating events -#define NO_REPEAT 0 -#define CH_5_TOGGLE 1 -#define CH_6_TOGGLE 2 -#define CH_7_TOGGLE 3 -#define CH_8_TOGGLE 4 -#define RELAY_TOGGLE 5 -#define STOP_REPEAT 10 - - -// Logging message types -enum log_messages { - LOG_CTUN_MSG, - LOG_NTUN_MSG, - LOG_PERFORMANCE_MSG, - LOG_STARTUP_MSG, - TYPE_AIRSTART_MSG, - TYPE_GROUNDSTART_MSG, - LOG_TECS_MSG, - LOG_RC_MSG, - LOG_SONAR_MSG, - LOG_ARM_DISARM_MSG, - LOG_STATUS_MSG -#if OPTFLOW == ENABLED - ,LOG_OPTFLOW_MSG -#endif -}; - -#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_MODE (1<<6) -#define MASK_LOG_IMU (1<<7) -#define MASK_LOG_CMD (1<<8) -#define MASK_LOG_CURRENT (1<<9) -#define MASK_LOG_COMPASS (1<<10) -#define MASK_LOG_TECS (1<<11) -#define MASK_LOG_CAMERA (1<<12) -#define MASK_LOG_RC (1<<13) -#define MASK_LOG_SONAR (1<<14) -#define MASK_LOG_ARM_DISARM (1<<15) -#define MASK_LOG_WHEN_DISARMED (1UL<<16) -#define MASK_LOG_IMU_RAW (1UL<<19) - -// Waypoint Modes -// ---------------- -#define ABS_WP 0 -#define REL_WP 1 - -// Command Queues -// --------------- -#define COMMAND_MUST 0 -#define COMMAND_MAY 1 -#define COMMAND_NOW 2 - -// Events -// ------ -#define EVENT_WILL_REACH_WAYPOINT 1 -#define EVENT_SET_NEW_COMMAND_INDEX 2 -#define EVENT_LOADED_WAYPOINT 3 -#define EVENT_LOOP 4 - -// Climb rate calculations -#define ALTITUDE_HISTORY_LENGTH 8 //Number of (time,altitude) points to - // regress a climb rate from - -#define AN4 4 -#define AN5 5 - -#define SPEEDFILT 400 // centimeters/second; the speed below - // which a groundstart will be - // triggered - -// convert a boolean (0 or 1) to a sign for multiplying (0 maps to 1, 1 maps -// to -1) -#define BOOL_TO_SIGN(bvalue) ((bvalue) ? -1 : 1) - -// altitude control algorithms -enum { - ALT_CONTROL_DEFAULT = 0, - ALT_CONTROL_NON_AIRSPEED = 1, - ALT_CONTROL_TECS = 2, - ALT_CONTROL_AIRSPEED = 3 -}; - -// attitude controller choice -enum { - ATT_CONTROL_PID = 0, - ATT_CONTROL_APMCONTROL = 1 -}; - -enum { - CRASH_DETECT_ACTION_BITMASK_DISABLED = 0, - CRASH_DETECT_ACTION_BITMASK_DISARM = (1<<0), - // note: next enum will be (1<<1), then (1<<2), then (1<<3) -}; -#endif // _DEFINES_H diff --git a/ArduSubsea/events.cpp b/ArduSubsea/events.cpp deleted file mode 100644 index c0989bf390..0000000000 --- a/ArduSubsea/events.cpp +++ /dev/null @@ -1,130 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#include "Plane.h" - -void Plane::failsafe_short_on_event(enum failsafe_state fstype) -{ - // This is how to handle a short loss of control signal failsafe. - failsafe.state = fstype; - failsafe.ch3_timer_ms = millis(); - gcs_send_text(MAV_SEVERITY_WARNING, "Failsafe. Short event on, "); - switch(control_mode) - { - case MANUAL: - case STABILIZE: - case ACRO: - case FLY_BY_WIRE_A: - case AUTOTUNE: - case FLY_BY_WIRE_B: - case CRUISE: - case TRAINING: - failsafe.saved_mode = control_mode; - failsafe.saved_mode_set = 1; - if(g.short_fs_action == 2) { - set_mode(FLY_BY_WIRE_A); - } else { - set_mode(CIRCLE); - } - break; - - case AUTO: - case GUIDED: - case LOITER: - if(g.short_fs_action != 0) { - failsafe.saved_mode = control_mode; - failsafe.saved_mode_set = 1; - if(g.short_fs_action == 2) { - set_mode(FLY_BY_WIRE_A); - } else { - set_mode(CIRCLE); - } - } - break; - - case CIRCLE: - case RTL: - default: - break; - } - gcs_send_text_fmt(MAV_SEVERITY_INFO, "Flight mode = %u", (unsigned)control_mode); -} - -void Plane::failsafe_long_on_event(enum failsafe_state fstype) -{ - // This is how to handle a long loss of control signal failsafe. - gcs_send_text(MAV_SEVERITY_WARNING, "Failsafe. Long event on, "); - // If the GCS is locked up we allow control to revert to RC - hal.rcin->clear_overrides(); - failsafe.state = fstype; - switch(control_mode) - { - case MANUAL: - case STABILIZE: - case ACRO: - case FLY_BY_WIRE_A: - case AUTOTUNE: - case FLY_BY_WIRE_B: - case CRUISE: - case TRAINING: - case CIRCLE: - if(g.long_fs_action == 2) { - set_mode(FLY_BY_WIRE_A); - } else { - set_mode(RTL); - } - break; - - case AUTO: - case GUIDED: - case LOITER: - if(g.long_fs_action == 2) { - set_mode(FLY_BY_WIRE_A); - } else if (g.long_fs_action == 1) { - set_mode(RTL); - } - break; - - case RTL: - default: - break; - } - if (fstype == FAILSAFE_GCS) { - gcs_send_text(MAV_SEVERITY_CRITICAL, "No GCS heartbeat"); - } - gcs_send_text_fmt(MAV_SEVERITY_INFO, "Flight mode = %u", (unsigned)control_mode); -} - -void Plane::failsafe_short_off_event() -{ - // We're back in radio contact - gcs_send_text(MAV_SEVERITY_WARNING, "Failsafe. Short event off"); - failsafe.state = FAILSAFE_NONE; - - // re-read the switch so we can return to our preferred mode - // -------------------------------------------------------- - if (control_mode == CIRCLE && failsafe.saved_mode_set) { - failsafe.saved_mode_set = 0; - set_mode(failsafe.saved_mode); - } -} - -void Plane::low_battery_event(void) -{ - if (failsafe.low_battery) { - return; - } - gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Low battery %.2fV used %.0f mAh", - (double)battery.voltage(), (double)battery.current_total_mah()); - if (flight_stage != AP_SpdHgtControl::FLIGHT_LAND_FINAL && - flight_stage != AP_SpdHgtControl::FLIGHT_LAND_APPROACH) { - set_mode(RTL); - aparm.throttle_cruise.load(); - } - failsafe.low_battery = true; - AP_Notify::flags.failsafe_battery = true; -} - -void Plane::update_events(void) -{ - ServoRelayEvents.update_events(); -} diff --git a/ArduSubsea/failsafe.cpp b/ArduSubsea/failsafe.cpp deleted file mode 100644 index 98df797977..0000000000 --- a/ArduSubsea/failsafe.cpp +++ /dev/null @@ -1,111 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#include "Plane.h" - -/* - * failsafe support - * Andrew Tridgell, December 2011 - */ - -/* - * our failsafe strategy is to detect main loop lockup and switch to - * passing inputs straight from the RC inputs to RC outputs. - */ - -/* - * this failsafe_check function is called from the core timer interrupt - * at 1kHz. - */ -void Plane::failsafe_check(void) -{ - static uint16_t last_mainLoop_count; - static uint32_t last_timestamp; - static bool in_failsafe; - uint32_t tnow = micros(); - - if (mainLoop_count != last_mainLoop_count) { - // the main loop is running, all is OK - last_mainLoop_count = mainLoop_count; - last_timestamp = tnow; - in_failsafe = false; - return; - } - - if (tnow - last_timestamp > 200000) { - // we have gone at least 0.2 seconds since the main loop - // ran. That means we're in trouble, or perhaps are in - // an initialisation routine or log erase. Start passing RC - // inputs through to outputs - in_failsafe = true; - } - - if (in_failsafe && tnow - last_timestamp > 20000) { - last_timestamp = tnow; - -#if OBC_FAILSAFE == ENABLED - if (in_calibration) { - // tell the failsafe system that we are calibrating - // sensors, so don't trigger failsafe - obc.heartbeat(); - } -#endif - - if (hal.rcin->num_channels() < 5) { - // we don't have any RC input to pass through - return; - } - - // pass RC inputs to outputs every 20ms - hal.rcin->clear_overrides(); - channel_roll->radio_out = channel_roll->read(); - channel_pitch->radio_out = channel_pitch->read(); - if (hal.util->get_soft_armed()) { - channel_throttle->radio_out = channel_throttle->read(); - } - channel_rudder->radio_out = channel_rudder->read(); - - int16_t roll = channel_roll->pwm_to_angle_dz(0); - int16_t pitch = channel_pitch->pwm_to_angle_dz(0); - int16_t rudder = channel_rudder->pwm_to_angle_dz(0); - - // setup secondary output channels that don't have - // corresponding input channels - RC_Channel_aux::set_servo_out(RC_Channel_aux::k_aileron, roll); - RC_Channel_aux::set_servo_out(RC_Channel_aux::k_elevator, pitch); - RC_Channel_aux::set_servo_out(RC_Channel_aux::k_rudder, rudder); - RC_Channel_aux::set_servo_out(RC_Channel_aux::k_steering, rudder); - - if (g.vtail_output != MIXING_DISABLED) { - channel_output_mixer(g.vtail_output, channel_pitch->radio_out, channel_rudder->radio_out); - } else if (g.elevon_output != MIXING_DISABLED) { - channel_output_mixer(g.elevon_output, channel_pitch->radio_out, channel_roll->radio_out); - } - -#if OBC_FAILSAFE == ENABLED - // this is to allow the failsafe module to deliberately crash - // the plane. Only used in extreme circumstances to meet the - // OBC rules - obc.check_crash_plane(); -#endif - - if (!demoing_servos) { - channel_roll->output(); - channel_pitch->output(); - } - channel_throttle->output(); - if (g.rudder_only == 0) { - channel_rudder->output(); - } - - // setup secondary output channels that do have - // corresponding input channels - RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_manual, true); - RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_aileron_with_input, true); - RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_elevator_with_input, true); - RC_Channel_aux::set_servo_out(RC_Channel_aux::k_flap, 0); - RC_Channel_aux::set_servo_out(RC_Channel_aux::k_flap_auto, 0); - - // setup flaperons - flaperon_update(0); - } -} diff --git a/ArduSubsea/geofence.cpp b/ArduSubsea/geofence.cpp deleted file mode 100644 index bed7c3970c..0000000000 --- a/ArduSubsea/geofence.cpp +++ /dev/null @@ -1,487 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -/* - * geo-fencing support - * Andrew Tridgell, December 2011 - */ - -#include "Plane.h" - -#if GEOFENCE_ENABLED == ENABLED - -#define MIN_GEOFENCE_POINTS 5 // 3 to define a minimal polygon (triangle) - // + 1 for return point and +1 for last - // pt (same as first) - -/* - * The state of geo-fencing. This structure is dynamically allocated - * the first time it is used. This means we only pay for the pointer - * and not the structure on systems where geo-fencing is not being - * used. - * - * We store a copy of the boundary in memory as we need to access it - * very quickly at runtime - */ -static struct GeofenceState { - uint8_t num_points; - bool boundary_uptodate; - bool fence_triggered; - bool is_pwm_enabled; //true if above FENCE_ENABLE_PWM threshold - bool previous_is_pwm_enabled; //true if above FENCE_ENALBE_PWM threshold - // last time we checked - bool is_enabled; - GeofenceEnableReason enable_reason; - bool floor_enabled; //typically used for landing - uint16_t breach_count; - uint8_t breach_type; - uint32_t breach_time; - uint8_t old_switch_position; - int32_t guided_lat; - int32_t guided_lng; - /* point 0 is the return point */ - Vector2l *boundary; -} *geofence_state; - - -static const StorageAccess fence_storage(StorageManager::StorageFence); - -/* - maximum number of fencepoints - */ -uint8_t Plane::max_fencepoints(void) -{ - return MIN(255U, fence_storage.size() / sizeof(Vector2l)); -} - -/* - * fence boundaries fetch/store - */ -Vector2l Plane::get_fence_point_with_index(unsigned i) -{ - Vector2l ret; - - if (i > (unsigned)g.fence_total || i >= max_fencepoints()) { - return Vector2l(0,0); - } - - // read fence point - ret.x = fence_storage.read_uint32(i * sizeof(Vector2l)); - ret.y = fence_storage.read_uint32(i * sizeof(Vector2l) + 4); - - return ret; -} - -// save a fence point -void Plane::set_fence_point_with_index(Vector2l &point, unsigned i) -{ - if (i >= (unsigned)g.fence_total.get() || i >= max_fencepoints()) { - // not allowed - return; - } - - fence_storage.write_uint32(i * sizeof(Vector2l), point.x); - fence_storage.write_uint32(i * sizeof(Vector2l)+4, point.y); - - if (geofence_state != NULL) { - geofence_state->boundary_uptodate = false; - } -} - -/* - * allocate and fill the geofence state structure - */ -void Plane::geofence_load(void) -{ - uint8_t i; - - if (geofence_state == NULL) { - uint16_t boundary_size = sizeof(Vector2l) * max_fencepoints(); - if (hal.util->available_memory() < 100 + boundary_size + sizeof(struct GeofenceState)) { - // too risky to enable as we could run out of stack - goto failed; - } - geofence_state = (struct GeofenceState *)calloc(1, sizeof(struct GeofenceState)); - if (geofence_state == NULL) { - // not much we can do here except disable it - goto failed; - } - - geofence_state->boundary = (Vector2l *)calloc(1, boundary_size); - if (geofence_state->boundary == NULL) { - free(geofence_state); - geofence_state = NULL; - goto failed; - } - - geofence_state->old_switch_position = 254; - } - - if (g.fence_total <= 0) { - g.fence_total.set(0); - return; - } - - for (i=0; iboundary[i] = get_fence_point_with_index(i); - } - geofence_state->num_points = i; - - if (!Polygon_complete(&geofence_state->boundary[1], geofence_state->num_points-1)) { - // first point and last point must be the same - goto failed; - } - if (Polygon_outside(geofence_state->boundary[0], &geofence_state->boundary[1], geofence_state->num_points-1)) { - // return point needs to be inside the fence - goto failed; - } - - geofence_state->boundary_uptodate = true; - geofence_state->fence_triggered = false; - - gcs_send_text(MAV_SEVERITY_INFO,"Geofence loaded"); - gcs_send_message(MSG_FENCE_STATUS); - return; - -failed: - g.fence_action.set(FENCE_ACTION_NONE); - gcs_send_text(MAV_SEVERITY_WARNING,"Geofence setup error"); -} - -/* - * return true if a geo-fence has been uploaded and - * FENCE_ACTION is 1 (not necessarily enabled) - */ -bool Plane::geofence_present(void) -{ - //require at least a return point and a triangle - //to define a geofence area: - if (g.fence_action == FENCE_ACTION_NONE || g.fence_total < MIN_GEOFENCE_POINTS) { - return false; - } - return true; -} - -/* - check FENCE_CHANNEL and update the is_pwm_enabled state - */ -void Plane::geofence_update_pwm_enabled_state() -{ - bool is_pwm_enabled; - if (g.fence_channel == 0) { - is_pwm_enabled = false; - } else { - is_pwm_enabled = (hal.rcin->read(g.fence_channel-1) > FENCE_ENABLE_PWM); - } - if (is_pwm_enabled && geofence_state == NULL) { - // we need to load the fence - geofence_load(); - return; - } - - if (geofence_state == NULL) { - // not loaded - return; - } - - geofence_state->previous_is_pwm_enabled = geofence_state->is_pwm_enabled; - geofence_state->is_pwm_enabled = is_pwm_enabled; - - if (geofence_state->is_pwm_enabled != geofence_state->previous_is_pwm_enabled) { - geofence_set_enabled(geofence_state->is_pwm_enabled, PWM_TOGGLED); - } -} - -//return true on success, false on failure -bool Plane::geofence_set_enabled(bool enable, GeofenceEnableReason r) -{ - if (geofence_state == NULL && enable) { - geofence_load(); - } - if (geofence_state == NULL) { - return false; - } - - geofence_state->is_enabled = enable; - if (enable == true) { - //turn the floor back on if it had been off - geofence_set_floor_enabled(true); - } - geofence_state->enable_reason = r; - - return true; -} - -/* - * return true if geo-fencing is enabled - */ -bool Plane::geofence_enabled(void) -{ - geofence_update_pwm_enabled_state(); - - if (geofence_state == NULL) { - return false; - } - - if (g.fence_action == FENCE_ACTION_NONE || - !geofence_present() || - (g.fence_action != FENCE_ACTION_REPORT && !geofence_state->is_enabled)) { - // geo-fencing is disabled - // re-arm for when the channel trigger is switched on - geofence_state->fence_triggered = false; - return false; - } - - return true; -} - -/* - * Set floor state IF the fence is present. - * Return false on failure to set floor state. - */ -bool Plane::geofence_set_floor_enabled(bool floor_enable) { - if (geofence_state == NULL) { - return false; - } - - geofence_state->floor_enabled = floor_enable; - return true; -} - -/* - * return true if we have breached the geo-fence minimum altiude - */ -bool Plane::geofence_check_minalt(void) -{ - if (g.fence_maxalt <= g.fence_minalt) { - return false; - } - if (g.fence_minalt == 0) { - return false; - } - return (adjusted_altitude_cm() < (g.fence_minalt*100.0f) + home.alt); -} - -/* - * return true if we have breached the geo-fence maximum altiude - */ -bool Plane::geofence_check_maxalt(void) -{ - if (g.fence_maxalt <= g.fence_minalt) { - return false; - } - if (g.fence_maxalt == 0) { - return false; - } - return (adjusted_altitude_cm() > (g.fence_maxalt*100.0f) + home.alt); -} - - -/* - * check if we have breached the geo-fence - */ -void Plane::geofence_check(bool altitude_check_only) -{ - if (!geofence_enabled()) { - // switch back to the chosen control mode if still in - // GUIDED to the return point - if (geofence_state != NULL && - (g.fence_action == FENCE_ACTION_GUIDED || g.fence_action == FENCE_ACTION_GUIDED_THR_PASS) && - control_mode == GUIDED && - geofence_present() && - geofence_state->boundary_uptodate && - geofence_state->old_switch_position == oldSwitchPosition && - guided_WP_loc.lat == geofence_state->guided_lat && - guided_WP_loc.lng == geofence_state->guided_lng) { - geofence_state->old_switch_position = 254; - set_mode(get_previous_mode()); - } - return; - } - - /* allocate the geo-fence state if need be */ - if (geofence_state == NULL || !geofence_state->boundary_uptodate) { - geofence_load(); - if (!geofence_enabled()) { - // may have been disabled by load - return; - } - } - - bool outside = false; - uint8_t breach_type = FENCE_BREACH_NONE; - struct Location loc; - - // Never trigger a fence breach in the final stage of landing - if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL) { - return; - } - - if (geofence_state->floor_enabled && geofence_check_minalt()) { - outside = true; - breach_type = FENCE_BREACH_MINALT; - } else if (geofence_check_maxalt()) { - outside = true; - breach_type = FENCE_BREACH_MAXALT; - } else if (!altitude_check_only && ahrs.get_position(loc)) { - Vector2l location; - location.x = loc.lat; - location.y = loc.lng; - outside = Polygon_outside(location, &geofence_state->boundary[1], geofence_state->num_points-1); - if (outside) { - breach_type = FENCE_BREACH_BOUNDARY; - } - } - - if (!outside) { - if (geofence_state->fence_triggered && !altitude_check_only) { - // we have moved back inside the fence - geofence_state->fence_triggered = false; - gcs_send_text(MAV_SEVERITY_INFO,"Geofence OK"); - #if FENCE_TRIGGERED_PIN > 0 - hal.gpio->pinMode(FENCE_TRIGGERED_PIN, HAL_GPIO_OUTPUT); - hal.gpio->write(FENCE_TRIGGERED_PIN, 0); - #endif - gcs_send_message(MSG_FENCE_STATUS); - } - // we're inside, all is good with the world - return; - } - - // we are outside the fence - if (geofence_state->fence_triggered && - (control_mode == GUIDED || g.fence_action == FENCE_ACTION_REPORT)) { - // we have already triggered, don't trigger again until the - // user disables/re-enables using the fence channel switch - return; - } - - // we are outside, and have not previously triggered. - geofence_state->fence_triggered = true; - geofence_state->breach_count++; - geofence_state->breach_time = millis(); - geofence_state->breach_type = breach_type; - - #if FENCE_TRIGGERED_PIN > 0 - hal.gpio->pinMode(FENCE_TRIGGERED_PIN, HAL_GPIO_OUTPUT); - hal.gpio->write(FENCE_TRIGGERED_PIN, 1); - #endif - - gcs_send_text(MAV_SEVERITY_NOTICE,"Geofence triggered"); - gcs_send_message(MSG_FENCE_STATUS); - - // see what action the user wants - switch (g.fence_action) { - case FENCE_ACTION_REPORT: - break; - - case FENCE_ACTION_GUIDED: - case FENCE_ACTION_GUIDED_THR_PASS: - // make sure we don't auto trim the surfaces on this mode change - int8_t saved_auto_trim = g.auto_trim; - g.auto_trim.set(0); - set_mode(GUIDED); - g.auto_trim.set(saved_auto_trim); - - if (g.fence_ret_rally != 0) { //return to a rally point - guided_WP_loc = rally.calc_best_rally_or_home_location(current_loc, get_RTL_altitude()); - - } else { //return to fence return point, not a rally point - if (g.fence_retalt > 0) { - //fly to the return point using fence_retalt - guided_WP_loc.alt = home.alt + 100.0f*g.fence_retalt; - } else if (g.fence_minalt >= g.fence_maxalt) { - // invalid min/max, use RTL_altitude - guided_WP_loc.alt = home.alt + g.RTL_altitude_cm; - } else { - // fly to the return point, with an altitude half way between - // min and max - guided_WP_loc.alt = home.alt + 100.0f*(g.fence_minalt + g.fence_maxalt)/2; - } - guided_WP_loc.options = 0; - guided_WP_loc.lat = geofence_state->boundary[0].x; - guided_WP_loc.lng = geofence_state->boundary[0].y; - } - geofence_state->guided_lat = guided_WP_loc.lat; - geofence_state->guided_lng = guided_WP_loc.lng; - geofence_state->old_switch_position = oldSwitchPosition; - - setup_terrain_target_alt(guided_WP_loc); - - set_guided_WP(); - - if (g.fence_action == FENCE_ACTION_GUIDED_THR_PASS) { - guided_throttle_passthru = true; - } - break; - } - -} - -/* - * return true if geofencing allows stick mixing. When we have - * triggered failsafe and are in GUIDED mode then stick mixing is - * disabled. Otherwise the aircraft may not be able to recover from - * a breach of the geo-fence - */ -bool Plane::geofence_stickmixing(void) { - if (geofence_enabled() && - geofence_state != NULL && - geofence_state->fence_triggered && - control_mode == GUIDED) { - // don't mix in user input - return false; - } - // normal mixing rules - return true; -} - -/* - * - */ -void Plane::geofence_send_status(mavlink_channel_t chan) -{ - if (geofence_enabled() && geofence_state != NULL) { - mavlink_msg_fence_status_send(chan, - (int8_t)geofence_state->fence_triggered, - geofence_state->breach_count, - geofence_state->breach_type, - geofence_state->breach_time); - } -} - -/* - return true if geofence has been breached - */ -bool Plane::geofence_breached(void) -{ - return geofence_state ? geofence_state->fence_triggered : false; -} - - -#else // GEOFENCE_ENABLED - -void Plane::geofence_check(bool altitude_check_only) { -} -bool Plane::geofence_stickmixing(void) { - return true; -} -bool Plane::geofence_enabled(void) { - return false; -} - -bool Plane::geofence_present(void) { - return false; -} - -bool Plane::geofence_set_enabled(bool enable, GeofenceEnableReason r) { - return false; -} - -bool Plane::geofence_set_floor_enabled(bool floor_enable) { - return false; -} - -bool geofence_breached(void) { - return false; -} - -#endif // GEOFENCE_ENABLED diff --git a/ArduSubsea/is_flying.cpp b/ArduSubsea/is_flying.cpp deleted file mode 100644 index 49b34ae714..0000000000 --- a/ArduSubsea/is_flying.cpp +++ /dev/null @@ -1,245 +0,0 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#include "Plane.h" - -/* - is_flying and crash detection logic - */ - -/* - Do we think we are flying? - Probabilistic method where a bool is low-passed and considered a probability. -*/ -void Plane::update_is_flying_5Hz(void) -{ - float aspeed; - bool is_flying_bool; - uint32_t now_ms = AP_HAL::millis(); - - uint32_t ground_speed_thresh_cm = (g.min_gndspeed_cm > 0) ? ((uint32_t)(g.min_gndspeed_cm*0.9f)) : 500; - bool gps_confirmed_movement = (gps.status() >= AP_GPS::GPS_OK_FIX_3D) && - (gps.ground_speed_cm() >= ground_speed_thresh_cm); - - // airspeed at least 75% of stall speed? - bool airspeed_movement = ahrs.airspeed_estimate(&aspeed) && (aspeed >= (aparm.airspeed_min*0.75f)); - - if(arming.is_armed()) { - // when armed assuming flying and we need overwhelming evidence that we ARE NOT flying - - // short drop-outs of GPS are common during flight due to banking which points the antenna in different directions - bool gps_lost_recently = (gps.last_fix_time_ms() > 0) && // we have locked to GPS before - (gps.status() < AP_GPS::GPS_OK_FIX_2D) && // and it's lost now - (now_ms - gps.last_fix_time_ms() < 5000); // but it wasn't that long ago (<5s) - - if ((auto_state.last_flying_ms > 0) && gps_lost_recently) { - // we've flown before, remove GPS constraints temporarily and only use airspeed - is_flying_bool = airspeed_movement; // moving through the air - } else { - // we've never flown yet, require good GPS movement - is_flying_bool = airspeed_movement || // moving through the air - gps_confirmed_movement; // locked and we're moving - } - - if (control_mode == AUTO) { - /* - make is_flying() more accurate during various auto modes - */ - - switch (flight_stage) - { - case AP_SpdHgtControl::FLIGHT_TAKEOFF: - // while on the ground, an uncalibrated airspeed sensor can drift to 7m/s so - // ensure we aren't showing a false positive. If the throttle is suppressed - // we are definitely not flying, or at least for not much longer! - if (throttle_suppressed) { - is_flying_bool = false; - crash_state.is_crashed = false; - } - break; - - case AP_SpdHgtControl::FLIGHT_LAND_ABORT: - case AP_SpdHgtControl::FLIGHT_NORMAL: - // TODO: detect ground impacts - break; - - case AP_SpdHgtControl::FLIGHT_LAND_APPROACH: - // TODO: detect ground impacts - if (fabsf(auto_state.sink_rate) > 0.2f) { - is_flying_bool = true; - } - break; - - case AP_SpdHgtControl::FLIGHT_LAND_FINAL: - break; - } // switch - } - } else { - // when disarmed assume not flying and need overwhelming evidence that we ARE flying - is_flying_bool = airspeed_movement && gps_confirmed_movement; - - if ((control_mode == AUTO) && - ((flight_stage == AP_SpdHgtControl::FLIGHT_TAKEOFF) || - (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL)) ) { - is_flying_bool = false; - } - } - - // low-pass the result. - // coef=0.15f @ 5Hz takes 3.0s to go from 100% down to 10% (or 0% up to 90%) - isFlyingProbability = (0.85f * isFlyingProbability) + (0.15f * (float)is_flying_bool); - - /* - update last_flying_ms so we always know how long we have not - been flying for. This helps for crash detection and auto-disarm - */ - bool new_is_flying = is_flying(); - - // we are flying, note the time - if (new_is_flying) { - - auto_state.last_flying_ms = now_ms; - - if (!previous_is_flying) { - // just started flying in any mode - started_flying_ms = now_ms; - } - - if ((control_mode == AUTO) && - ((auto_state.started_flying_in_auto_ms == 0) || !previous_is_flying) ) { - - // We just started flying, note that time also - auto_state.started_flying_in_auto_ms = now_ms; - } - } - previous_is_flying = new_is_flying; - - if (should_log(MASK_LOG_MODE)) { - Log_Write_Status(); - } -} - -/* - return true if we think we are flying. This is a probabilistic - estimate, and needs to be used very carefully. Each use case needs - to be thought about individually. - */ -bool Plane::is_flying(void) -{ - if (hal.util->get_soft_armed()) { - // when armed, assume we're flying unless we probably aren't - return (isFlyingProbability >= 0.1f); - } - - // when disarmed, assume we're not flying unless we probably are - return (isFlyingProbability >= 0.9f); -} - -/* - * Determine if we have crashed - */ -void Plane::crash_detection_update(void) -{ - if (control_mode != AUTO) - { - // crash detection is only available in AUTO mode - crash_state.debounce_timer_ms = 0; - return; - } - - uint32_t now_ms = AP_HAL::millis(); - bool auto_launch_detected; - bool crashed_near_land_waypoint = false; - bool crashed = false; - bool been_auto_flying = (auto_state.started_flying_in_auto_ms > 0) && - (now_ms - auto_state.started_flying_in_auto_ms >= 2500); - - if (!is_flying()) - { - switch (flight_stage) - { - case AP_SpdHgtControl::FLIGHT_TAKEOFF: - auto_launch_detected = !throttle_suppressed && (g.takeoff_throttle_min_accel > 0); - - if (been_auto_flying || // failed hand launch - auto_launch_detected) { // threshold of been_auto_flying may not be met on auto-launches - - // has launched but is no longer flying. That's a crash on takeoff. - crashed = true; - } - break; - - case AP_SpdHgtControl::FLIGHT_LAND_ABORT: - case AP_SpdHgtControl::FLIGHT_NORMAL: - if (been_auto_flying) { - crashed = true; - } - // TODO: handle auto missions without NAV_TAKEOFF mission cmd - break; - - case AP_SpdHgtControl::FLIGHT_LAND_APPROACH: - if (been_auto_flying) { - crashed = true; - } - // when altitude gets low, we automatically progress to FLIGHT_LAND_FINAL - // so ground crashes most likely can not be triggered from here. However, - // a crash into a tree, for example, would be. - break; - - case AP_SpdHgtControl::FLIGHT_LAND_FINAL: - // We should be nice and level-ish in this flight stage. If not, we most - // likely had a crazy landing. Throttle is inhibited already at the flare - // but go ahead and notify GCS and perform any additional post-crash actions. - // Declare a crash if we are oriented more that 60deg in pitch or roll - if (been_auto_flying && - !crash_state.checkHardLanding && // only check once - (fabsf(ahrs.roll_sensor) > 6000 || fabsf(ahrs.pitch_sensor) > 6000)) { - crashed = true; - - // did we "crash" within 75m of the landing location? Probably just a hard landing - crashed_near_land_waypoint = - get_distance(current_loc, mission.get_current_nav_cmd().content.location) < 75; - - // trigger hard landing event right away, or never again. This inhibits a false hard landing - // event when, for example, a minute after a good landing you pick the plane up and - // this logic is still running and detects the plane is on its side as you carry it. - crash_state.debounce_timer_ms = now_ms + 2500; - } - crash_state.checkHardLanding = true; - break; - } // switch - } else { - crash_state.checkHardLanding = false; - } - - if (!crashed) { - // reset timer - crash_state.debounce_timer_ms = 0; - - } else if (crash_state.debounce_timer_ms == 0) { - // start timer - crash_state.debounce_timer_ms = now_ms; - - } else if ((now_ms - crash_state.debounce_timer_ms >= 2500) && !crash_state.is_crashed) { - crash_state.is_crashed = true; - - if (g.crash_detection_enable == CRASH_DETECT_ACTION_BITMASK_DISABLED) { - if (crashed_near_land_waypoint) { - gcs_send_text(MAV_SEVERITY_CRITICAL, "Hard landing detected. No action taken"); - } else { - gcs_send_text(MAV_SEVERITY_EMERGENCY, "Crash detected. No action taken"); - } - } - else { - if (g.crash_detection_enable & CRASH_DETECT_ACTION_BITMASK_DISARM) { - disarm_motors(); - } - auto_state.land_complete = true; - if (crashed_near_land_waypoint) { - gcs_send_text(MAV_SEVERITY_CRITICAL, "Hard landing detected"); - } else { - gcs_send_text(MAV_SEVERITY_EMERGENCY, "Crash detected"); - } - } - } -} - diff --git a/ArduSubsea/landing.cpp b/ArduSubsea/landing.cpp deleted file mode 100644 index f08a359c4a..0000000000 --- a/ArduSubsea/landing.cpp +++ /dev/null @@ -1,335 +0,0 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#include "Plane.h" - -/* - landing logic - */ - -/* - update navigation for landing. Called when on landing approach or - final flare - */ -bool Plane::verify_land() -{ - // we don't 'verify' landing in the sense that it never completes, - // so we don't verify command completion. Instead we use this to - // adjust final landing parameters - - // If a go around has been commanded, we are done landing. This will send - // the mission to the next mission item, which presumably is a mission - // segment with operations to perform when a landing is called off. - // If there are no commands after the land waypoint mission item then - // the plane will proceed to loiter about its home point. - if (auto_state.commanded_go_around) { - return true; - } - - // when aborting a landing, mimic the verify_takeoff with steering hold. Once - // the altitude has been reached, restart the landing sequence - if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT) { - - throttle_suppressed = false; - auto_state.land_complete = false; - nav_controller->update_heading_hold(get_bearing_cd(prev_WP_loc, next_WP_loc)); - - // see if we have reached abort altitude - if (adjusted_relative_altitude_cm() > auto_state.takeoff_altitude_rel_cm) { - next_WP_loc = current_loc; - mission.stop(); - bool success = restart_landing_sequence(); - mission.resume(); - if (!success) { - // on a restart failure lets RTL or else the plane may fly away with nowhere to go! - set_mode(RTL); - } - // make sure to return false so it leaves the mission index alone - } - return false; - } - - float height = height_above_target(); - - // use rangefinder to correct if possible - height -= rangefinder_correction(); - - /* Set land_complete (which starts the flare) under 3 conditions: - 1) we are within LAND_FLARE_ALT meters of the landing altitude - 2) we are within LAND_FLARE_SEC of the landing point vertically - by the calculated sink rate (if LAND_FLARE_SEC != 0) - 3) we have gone past the landing point and don't have - rangefinder data (to prevent us keeping throttle on - after landing if we've had positive baro drift) - */ -#if RANGEFINDER_ENABLED == ENABLED - bool rangefinder_in_range = rangefinder_state.in_range; -#else - bool rangefinder_in_range = false; -#endif - if (height <= g.land_flare_alt || - (aparm.land_flare_sec > 0 && height <= auto_state.sink_rate * aparm.land_flare_sec) || - (!rangefinder_in_range && location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) || - (fabsf(auto_state.sink_rate) < 0.2f && !is_flying())) { - - if (!auto_state.land_complete) { - auto_state.post_landing_stats = true; - if (!is_flying() && (millis()-auto_state.last_flying_ms) > 3000) { - gcs_send_text_fmt(MAV_SEVERITY_CRITICAL, "Flare crash detected: speed=%.1f", (double)gps.ground_speed()); - } else { - gcs_send_text_fmt(MAV_SEVERITY_INFO, "Flare %.1fm sink=%.2f speed=%.1f dist=%.1f", - (double)height, (double)auto_state.sink_rate, - (double)gps.ground_speed(), - (double)get_distance(current_loc, next_WP_loc)); - } - } - auto_state.land_complete = true; - - if (gps.ground_speed() < 3) { - // reload any airspeed or groundspeed parameters that may have - // been set for landing. We don't do this till ground - // speed drops below 3.0 m/s as otherwise we will change - // target speeds too early. - g.airspeed_cruise_cm.load(); - g.min_gndspeed_cm.load(); - aparm.throttle_cruise.load(); - } - } - - /* - when landing we keep the L1 navigation waypoint 200m ahead. This - prevents sudden turns if we overshoot the landing point - */ - struct Location land_WP_loc = next_WP_loc; - int32_t land_bearing_cd = get_bearing_cd(prev_WP_loc, next_WP_loc); - location_update(land_WP_loc, - land_bearing_cd*0.01f, - get_distance(prev_WP_loc, current_loc) + 200); - nav_controller->update_waypoint(prev_WP_loc, land_WP_loc); - - // once landed and stationary, post some statistics - // this is done before disarm_if_autoland_complete() so that it happens on the next loop after the disarm - if (auto_state.post_landing_stats && !arming.is_armed()) { - auto_state.post_landing_stats = false; - gcs_send_text_fmt(MAV_SEVERITY_INFO, "Distance from LAND point=%.2fm", (double)get_distance(current_loc, next_WP_loc)); - } - - // check if we should auto-disarm after a confirmed landing - disarm_if_autoland_complete(); - - /* - we return false as a landing mission item never completes - - we stay on this waypoint unless the GCS commands us to change - mission item, reset the mission, command a go-around or finish - a land_abort procedure. - */ - return false; -} - -/* - If land_DisarmDelay is enabled (non-zero), check for a landing then auto-disarm after time expires - */ -void Plane::disarm_if_autoland_complete() -{ - if (g.land_disarm_delay > 0 && - auto_state.land_complete && - !is_flying() && - arming.arming_required() != AP_Arming::NO && - arming.is_armed()) { - /* we have auto disarm enabled. See if enough time has passed */ - if (millis() - auto_state.last_flying_ms >= g.land_disarm_delay*1000UL) { - if (disarm_motors()) { - gcs_send_text(MAV_SEVERITY_INFO,"Auto disarmed"); - } - } - } -} - - - -/* - a special glide slope calculation for the landing approach - - During the land approach use a linear glide slope to a point - projected through the landing point. We don't use the landing point - itself as that leads to discontinuities close to the landing point, - which can lead to erratic pitch control - */ -void Plane::setup_landing_glide_slope(void) -{ - Location loc = next_WP_loc; - - // project a point 500 meters past the landing point, passing - // through the landing point - const float land_projection = 500; - int32_t land_bearing_cd = get_bearing_cd(prev_WP_loc, next_WP_loc); - float total_distance = get_distance(prev_WP_loc, next_WP_loc); - - // If someone mistakenly puts all 0's in their LAND command then total_distance - // will be calculated as 0 and cause a divide by 0 error below. Lets avoid that. - if (total_distance < 1) { - total_distance = 1; - } - - // height we need to sink for this WP - float sink_height = (prev_WP_loc.alt - next_WP_loc.alt)*0.01f; - - // current ground speed - float groundspeed = ahrs.groundspeed(); - if (groundspeed < 0.5f) { - groundspeed = 0.5f; - } - - // calculate time to lose the needed altitude - float sink_time = total_distance / groundspeed; - if (sink_time < 0.5f) { - sink_time = 0.5f; - } - - // find the sink rate needed for the target location - float sink_rate = sink_height / sink_time; - - // the height we aim for is the one to give us the right flare point - float aim_height = aparm.land_flare_sec * sink_rate; - if (aim_height <= 0) { - aim_height = g.land_flare_alt; - } - - // don't allow the aim height to be too far above LAND_FLARE_ALT - if (g.land_flare_alt > 0 && aim_height > g.land_flare_alt*2) { - aim_height = g.land_flare_alt*2; - } - - // time before landing that we will flare - float flare_time = aim_height / SpdHgt_Controller->get_land_sinkrate(); - - // distance to flare is based on ground speed, adjusted as we - // get closer. This takes into account the wind - float flare_distance = groundspeed * flare_time; - - // don't allow the flare before half way along the final leg - if (flare_distance > total_distance/2) { - flare_distance = total_distance/2; - } - - // now calculate our aim point, which is before the landing - // point and above it - location_update(loc, land_bearing_cd*0.01f, -flare_distance); - loc.alt += aim_height*100; - - // calculate slope to landing point - float land_slope = (sink_height - aim_height) / total_distance; - - // calculate point along that slope 500m ahead - location_update(loc, land_bearing_cd*0.01f, land_projection); - loc.alt -= land_slope * land_projection * 100; - - // setup the offset_cm for set_target_altitude_proportion() - target_altitude.offset_cm = loc.alt - prev_WP_loc.alt; - - // calculate the proportion we are to the target - float land_proportion = location_path_proportion(current_loc, prev_WP_loc, loc); - - // now setup the glide slope for landing - set_target_altitude_proportion(loc, 1.0f - land_proportion); - - // stay within the range of the start and end locations in altitude - constrain_target_altitude_location(loc, prev_WP_loc); -} - -/* - Restart a landing by first checking for a DO_LAND_START and - jump there. Otherwise decrement waypoint so we would re-start - from the top with same glide slope. Return true if successful. - */ -bool Plane::restart_landing_sequence() -{ - if (mission.get_current_nav_cmd().id != MAV_CMD_NAV_LAND) { - return false; - } - - uint16_t do_land_start_index = mission.get_landing_sequence_start(); - uint16_t prev_cmd_with_wp_index = mission.get_prev_nav_cmd_with_wp_index(); - bool success = false; - uint16_t current_index = mission.get_current_nav_index(); - AP_Mission::Mission_Command cmd; - - if (mission.read_cmd_from_storage(current_index+1,cmd) && - cmd.id == MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT && - (cmd.p1 == 0 || cmd.p1 == 1) && - mission.set_current_cmd(current_index+1)) - { - // if the next immediate command is MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT to climb, do it - gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "Restarted landing sequence. Climbing to %dm", cmd.content.location.alt/100); - success = true; - } - else if (do_land_start_index != 0 && - mission.set_current_cmd(do_land_start_index)) - { - // look for a DO_LAND_START and use that index - gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "Restarted landing via DO_LAND_START: %d",do_land_start_index); - success = true; - } - else if (prev_cmd_with_wp_index != AP_MISSION_CMD_INDEX_NONE && - mission.set_current_cmd(prev_cmd_with_wp_index)) - { - // if a suitable navigation waypoint was just executed, one that contains lat/lng/alt, then - // repeat that cmd to restart the landing from the top of approach to repeat intended glide slope - gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "Restarted landing sequence at waypoint %d", prev_cmd_with_wp_index); - success = true; - } else { - gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Unable to restart landing sequence"); - success = false; - } - return success; -} - -/* - find the nearest landing sequence starting point (DO_LAND_START) and - switch to that mission item. Returns false if no DO_LAND_START - available. - */ -bool Plane::jump_to_landing_sequence(void) -{ - uint16_t land_idx = mission.get_landing_sequence_start(); - if (land_idx != 0) { - if (mission.set_current_cmd(land_idx)) { - set_mode(AUTO); - - //if the mission has ended it has to be restarted - if (mission.state() == AP_Mission::MISSION_STOPPED) { - mission.resume(); - } - - gcs_send_text(MAV_SEVERITY_INFO, "Landing sequence start"); - return true; - } - } - - gcs_send_text(MAV_SEVERITY_WARNING, "Unable to start landing sequence"); - return false; -} - -/* - the height above field elevation that we pass to TECS - */ -float Plane::tecs_hgt_afe(void) -{ - /* - pass the height above field elevation as the height above - the ground when in landing, which means that TECS gets the - rangefinder information and thus can know when the flare is - coming. - */ - float hgt_afe; - if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL || - flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH) { - hgt_afe = height_above_target(); - hgt_afe -= rangefinder_correction(); - } else { - // when in normal flight we pass the hgt_afe as relative - // altitude to home - hgt_afe = relative_altitude(); - } - return hgt_afe; -} diff --git a/ArduSubsea/make.inc b/ArduSubsea/make.inc deleted file mode 100644 index 8dbc87585d..0000000000 --- a/ArduSubsea/make.inc +++ /dev/null @@ -1,52 +0,0 @@ -LIBRARIES += AP_Common -LIBRARIES += AP_Menu -LIBRARIES += AP_Param -LIBRARIES += StorageManager -LIBRARIES += AP_GPS -LIBRARIES += AP_Baro -LIBRARIES += AP_Compass -LIBRARIES += AP_Math -LIBRARIES += AP_ADC -LIBRARIES += AP_InertialSensor -LIBRARIES += AP_AccelCal -LIBRARIES += AP_AHRS -LIBRARIES += RC_Channel -LIBRARIES += AP_RangeFinder -LIBRARIES += Filter -LIBRARIES += AP_Buffer -LIBRARIES += AP_Relay -LIBRARIES += AP_Camera -LIBRARIES += AP_Airspeed -LIBRARIES += AP_Terrain -LIBRARIES += APM_OBC -LIBRARIES += APM_Control -LIBRARIES += AP_AutoTune -LIBRARIES += GCS -LIBRARIES += GCS_MAVLink -LIBRARIES += AP_SerialManager -LIBRARIES += AP_Mount -LIBRARIES += AP_Declination -LIBRARIES += DataFlash -LIBRARIES += AP_Scheduler -LIBRARIES += AP_Navigation -LIBRARIES += AP_L1_Control -LIBRARIES += AP_RCMapper -LIBRARIES += AP_Vehicle -LIBRARIES += AP_SpdHgtControl -LIBRARIES += AP_TECS -LIBRARIES += AP_NavEKF -LIBRARIES += AP_NavEKF2 -LIBRARIES += AP_Mission -LIBRARIES += AP_Notify -LIBRARIES += AP_BattMonitor -LIBRARIES += AP_Arming -LIBRARIES += AP_BoardConfig -LIBRARIES += AP_Frsky_Telem -LIBRARIES += AP_ServoRelayEvents -LIBRARIES += AP_Rally -LIBRARIES += AP_OpticalFlow -LIBRARIES += AP_RSSI -LIBRARIES += AP_RPM -LIBRARIES += AP_Parachute -LIBRARIES += AP_ADSB - diff --git a/ArduSubsea/navigation.cpp b/ArduSubsea/navigation.cpp deleted file mode 100644 index 2bffc9060b..0000000000 --- a/ArduSubsea/navigation.cpp +++ /dev/null @@ -1,238 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#include "Plane.h" - -// set the nav_controller pointer to the right controller -void Plane::set_nav_controller(void) -{ - switch ((AP_Navigation::ControllerType)g.nav_controller.get()) { - case AP_Navigation::CONTROLLER_L1: - nav_controller = &L1_controller; - break; - } -} - -/* - reset the total loiter angle - */ -void Plane::loiter_angle_reset(void) -{ - loiter.sum_cd = 0; - loiter.total_cd = 0; -} - -/* - update the total angle we have covered in a loiter. Used to support - commands to do N circles of loiter - */ -void Plane::loiter_angle_update(void) -{ - int32_t target_bearing_cd = nav_controller->target_bearing_cd(); - int32_t loiter_delta_cd; - if (loiter.sum_cd == 0) { - // use 1 cd for initial delta - loiter_delta_cd = 1; - } else { - loiter_delta_cd = target_bearing_cd - loiter.old_target_bearing_cd; - } - loiter.old_target_bearing_cd = target_bearing_cd; - loiter_delta_cd = wrap_180_cd(loiter_delta_cd); - - loiter.sum_cd += loiter_delta_cd * loiter.direction; -} - -//**************************************************************** -// Function that will calculate the desired direction to fly and distance -//**************************************************************** -void Plane::navigate() -{ - // allow change of nav controller mid-flight - set_nav_controller(); - - // do not navigate with corrupt data - // --------------------------------- - if (!have_position) { - return; - } - - if (next_WP_loc.lat == 0) { - return; - } - - // waypoint distance from plane - // ---------------------------- - auto_state.wp_distance = get_distance(current_loc, next_WP_loc); - auto_state.wp_proportion = location_path_proportion(current_loc, - prev_WP_loc, next_WP_loc); - - // update total loiter angle - loiter_angle_update(); - - // control mode specific updates to navigation demands - // --------------------------------------------------- - update_navigation(); -} - -void Plane::calc_airspeed_errors() -{ - float aspeed_cm = airspeed.get_airspeed_cm(); - - // Normal airspeed target - target_airspeed_cm = g.airspeed_cruise_cm; - - // FBW_B airspeed target - if (control_mode == FLY_BY_WIRE_B || - control_mode == CRUISE) { - target_airspeed_cm = ((int32_t)(aparm.airspeed_max - - aparm.airspeed_min) * - channel_throttle->control_in) + - ((int32_t)aparm.airspeed_min * 100); - } - - // Set target to current airspeed + ground speed undershoot, - // but only when this is faster than the target airspeed commanded - // above. - if (control_mode >= FLY_BY_WIRE_B && (g.min_gndspeed_cm > 0)) { - int32_t min_gnd_target_airspeed = aspeed_cm + groundspeed_undershoot; - if (min_gnd_target_airspeed > target_airspeed_cm) - target_airspeed_cm = min_gnd_target_airspeed; - } - - // Bump up the target airspeed based on throttle nudging - if (control_mode >= AUTO && airspeed_nudge_cm > 0) { - target_airspeed_cm += airspeed_nudge_cm; - } - - // Apply airspeed limit - if (target_airspeed_cm > (aparm.airspeed_max * 100)) - target_airspeed_cm = (aparm.airspeed_max * 100); - - // use the TECS view of the target airspeed for reporting, to take - // account of the landing speed - airspeed_error_cm = SpdHgt_Controller->get_target_airspeed()*100 - aspeed_cm; -} - -void Plane::calc_gndspeed_undershoot() -{ - // Use the component of ground speed in the forward direction - // This prevents flyaway if wind takes plane backwards - if (gps.status() >= AP_GPS::GPS_OK_FIX_2D) { - Vector2f gndVel = ahrs.groundspeed_vector(); - const Matrix3f &rotMat = ahrs.get_rotation_body_to_ned(); - Vector2f yawVect = Vector2f(rotMat.a.x,rotMat.b.x); - yawVect.normalize(); - float gndSpdFwd = yawVect * gndVel; - groundspeed_undershoot = (g.min_gndspeed_cm > 0) ? (g.min_gndspeed_cm - gndSpdFwd*100) : 0; - } -} - -void Plane::update_loiter() -{ - int16_t radius = abs(g.loiter_radius); - - if (loiter.start_time_ms == 0 && - control_mode == AUTO && - !auto_state.no_crosstrack && - get_distance(current_loc, next_WP_loc) > radius*2) { - // if never reached loiter point and using crosstrack and somewhat far away from loiter point - // navigate to it like in auto-mode for normal crosstrack behavior - nav_controller->update_waypoint(prev_WP_loc, next_WP_loc); - } else { - nav_controller->update_loiter(next_WP_loc, radius, loiter.direction); - } - - if (loiter.start_time_ms == 0) { - if (nav_controller->reached_loiter_target()) { - // we've reached the target, start the timer - loiter.start_time_ms = millis(); - } - } -} - -/* - handle CRUISE mode, locking heading to GPS course when we have - sufficient ground speed, and no aileron or rudder input - */ -void Plane::update_cruise() -{ - if (!cruise_state.locked_heading && - channel_roll->control_in == 0 && - rudder_input == 0 && - gps.status() >= AP_GPS::GPS_OK_FIX_2D && - gps.ground_speed() >= 3 && - cruise_state.lock_timer_ms == 0) { - // user wants to lock the heading - start the timer - cruise_state.lock_timer_ms = millis(); - } - if (cruise_state.lock_timer_ms != 0 && - (millis() - cruise_state.lock_timer_ms) > 500) { - // lock the heading after 0.5 seconds of zero heading input - // from user - cruise_state.locked_heading = true; - cruise_state.lock_timer_ms = 0; - cruise_state.locked_heading_cd = gps.ground_course_cd(); - prev_WP_loc = current_loc; - } - if (cruise_state.locked_heading) { - next_WP_loc = prev_WP_loc; - // always look 1km ahead - location_update(next_WP_loc, - cruise_state.locked_heading_cd*0.01f, - get_distance(prev_WP_loc, current_loc) + 1000); - nav_controller->update_waypoint(prev_WP_loc, next_WP_loc); - } -} - - -/* - handle speed and height control in FBWB or CRUISE mode. - In this mode the elevator is used to change target altitude. The - throttle is used to change target airspeed or throttle - */ -void Plane::update_fbwb_speed_height(void) -{ - static float last_elevator_input; - float elevator_input; - elevator_input = channel_pitch->control_in / 4500.0f; - - if (g.flybywire_elev_reverse) { - elevator_input = -elevator_input; - } - - change_target_altitude(g.flybywire_climb_rate * elevator_input * delta_us_fast_loop * 0.0001f); - - if (is_zero(elevator_input) && !is_zero(last_elevator_input)) { - // the user has just released the elevator, lock in - // the current altitude - set_target_altitude_current(); - } - - // check for FBWB altitude limit - check_minimum_altitude(); - - altitude_error_cm = calc_altitude_error_cm(); - - last_elevator_input = elevator_input; - - calc_throttle(); - calc_nav_pitch(); -} - -/* - calculate the turn angle for the next leg of the mission - */ -void Plane::setup_turn_angle(void) -{ - int32_t next_ground_course_cd = mission.get_next_ground_course_cd(-1); - if (next_ground_course_cd == -1) { - // the mission library can't determine a turn angle, assume 90 degrees - auto_state.next_turn_angle = 90.0f; - } else { - // get the heading of the current leg - int32_t ground_course_cd = get_bearing_cd(prev_WP_loc, next_WP_loc); - - // work out the angle we need to turn through - auto_state.next_turn_angle = wrap_180_cd(next_ground_course_cd - ground_course_cd) * 0.01f; - } -} - diff --git a/ArduSubsea/parachute.cpp b/ArduSubsea/parachute.cpp deleted file mode 100644 index 7121674716..0000000000 --- a/ArduSubsea/parachute.cpp +++ /dev/null @@ -1,58 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#include "Plane.h" - - -/* - call parachute library update -*/ -void Plane::parachute_check() -{ - parachute.update(); -} - -/* - parachute_release - trigger the release of the parachute -*/ -void Plane::parachute_release() -{ - if (parachute.released()) { - return; - } - - // send message to gcs and dataflash - gcs_send_text(MAV_SEVERITY_CRITICAL,"Parachute: Released"); - - // 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 -*/ -bool Plane::parachute_manual_release() -{ - // exit immediately if parachute is not enabled - if (!parachute.enabled() || parachute.released()) { - return false; - } - - // do not release if vehicle is not flying - if (!is_flying()) { - // warn user of reason for failure - gcs_send_text(MAV_SEVERITY_WARNING,"Parachute: Not flying"); - return false; - } - - if (relative_altitude() < parachute.alt_min()) { - gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Parachute: Too low"); - return false; - } - - // if we get this far release parachute - parachute_release(); - - return true; -} diff --git a/ArduSubsea/px4_mixer.cpp b/ArduSubsea/px4_mixer.cpp deleted file mode 100644 index b821e480bf..0000000000 --- a/ArduSubsea/px4_mixer.cpp +++ /dev/null @@ -1,391 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#include "Plane.h" - -/* - handle creation of PX4 mixer file, for failover to direct RC control - on failure of FMU - - This will create APM/MIXER.MIX on the microSD card. The user may - also create APM/CUSTOM.MIX, and if it exists that will be used - instead. That allows the user to setup more complex failsafe mixes - that include flaps, landing gear, ignition cut etc - */ - -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 -#include -#include -#include -#include -#include -#include -#include -#include - -#define PX4_LIM_RC_MIN 900 -#define PX4_LIM_RC_MAX 2100 - -/* - formatted print to a buffer with buffer advance. Returns true on - success, false on fail - */ -bool Plane::print_buffer(char *&buf, uint16_t &buf_size, const char *fmt, ...) -{ - va_list arg_list; - va_start(arg_list, fmt); - int n = ::vsnprintf(buf, buf_size, fmt, arg_list); - va_end(arg_list); - if (n <= 0 || n >= buf_size) { - return false; - } - buf += n; - buf_size -= n; - return true; -} - -/* - create a PX4 mixer buffer given the current fixed wing parameters - */ -bool Plane::create_mixer(char *buf, uint16_t buf_size, const char *filename) -{ - char *buf0 = buf; - uint16_t buf_size0 = buf_size; - - /* - this is the equivalent of channel_output_mixer() - */ - const int8_t mixmul[5][2] = { { 0, 0 }, { 1, 1 }, { 1, -1 }, { -1, 1 }, { -1, -1 }}; - // these are the internal clipping limits. Use scale_max1 when - // clipping to user specified min/max is wanted. Use scale_max2 - // when no clipping is wanted (simulated by setting a very large - // clipping value) - const float scale_max1 = 10000; - const float scale_max2 = 1000000; - // range for mixers - const uint16_t mix_max = scale_max1 * g.mixing_gain; - // scaling factors used by PX4IO between pwm and internal values, - // as configured in setup_failsafe_mixing() below - const float pwm_min = PX4_LIM_RC_MIN; - const float pwm_max = PX4_LIM_RC_MAX; - const float pwm_scale = 2*scale_max1/(pwm_max - pwm_min); - - for (uint8_t i=0; i<8; i++) { - int32_t c1, c2, mix=0; - bool rev = false; - RC_Channel_aux::Aux_servo_function_t function = RC_Channel_aux::channel_function(i); - if (i == rcmap.pitch()-1 && g.vtail_output > MIXING_DISABLED && g.vtail_output <= MIXING_DNDN) { - // first channel of VTAIL mix - c1 = rcmap.yaw()-1; - c2 = i; - rev = false; - mix = -mix_max*mixmul[g.vtail_output][0]; - } else if (i == rcmap.yaw()-1 && g.vtail_output > MIXING_DISABLED && g.vtail_output <= MIXING_DNDN) { - // second channel of VTAIL mix - c1 = rcmap.pitch()-1; - c2 = i; - rev = true; - mix = mix_max*mixmul[g.vtail_output][1]; - } else if (i == rcmap.roll()-1 && g.elevon_output > MIXING_DISABLED && - g.elevon_output <= MIXING_DNDN && g.vtail_output == 0) { - // first channel of ELEVON mix - c1 = i; - c2 = rcmap.pitch()-1; - rev = true; - mix = mix_max*mixmul[g.elevon_output][1]; - } else if (i == rcmap.pitch()-1 && g.elevon_output > MIXING_DISABLED && - g.elevon_output <= MIXING_DNDN && g.vtail_output == 0) { - // second channel of ELEVON mix - c1 = i; - c2 = rcmap.roll()-1; - rev = false; - mix = mix_max*mixmul[g.elevon_output][0]; - } else if (function == RC_Channel_aux::k_aileron || - function == RC_Channel_aux::k_flaperon1 || - function == RC_Channel_aux::k_flaperon2) { - // a secondary aileron. We don't mix flap input in yet for flaperons - c1 = rcmap.roll()-1; - } else if (function == RC_Channel_aux::k_elevator) { - // a secondary elevator - c1 = rcmap.pitch()-1; - } else if (function == RC_Channel_aux::k_rudder || - function == RC_Channel_aux::k_steering) { - // a secondary rudder or wheel - c1 = rcmap.yaw()-1; - } else if (g.flapin_channel > 0 && - (function == RC_Channel_aux::k_flap || - function == RC_Channel_aux::k_flap_auto)) { - // a flap output channel, and we have a manual flap input channel - c1 = g.flapin_channel-1; - } else if (i < 4 || - function == RC_Channel_aux::k_elevator_with_input || - function == RC_Channel_aux::k_aileron_with_input || - function == RC_Channel_aux::k_manual) { - // a pass-thru channel - c1 = i; - } else { - // a empty output - if (!print_buffer(buf, buf_size, "Z:\n")) { - return false; - } - continue; - } - if (mix == 0) { - // pass thru channel, possibly with reversal. We also - // adjust the gain based on the range of input and output - // channels and adjust for trims - const RC_Channel *chan1 = RC_Channel::rc_channel(i); - const RC_Channel *chan2 = RC_Channel::rc_channel(c1); - int16_t chan1_trim = (i==rcmap.throttle()-1?1500:chan1->radio_trim); - int16_t chan2_trim = (c1==rcmap.throttle()-1?1500:chan2->radio_trim); - chan1_trim = constrain_int16(chan1_trim, PX4_LIM_RC_MIN+1, PX4_LIM_RC_MAX-1); - chan2_trim = constrain_int16(chan2_trim, PX4_LIM_RC_MIN+1, PX4_LIM_RC_MAX-1); - // if the input and output channels are the same then we - // apply clipping. This allows for direct pass-thru - int32_t limit = (c1==i?scale_max2:scale_max1); - int32_t in_scale_low; - if (chan2_trim <= chan2->radio_min) { - in_scale_low = scale_max1; - } else { - in_scale_low = scale_max1*(chan2_trim - pwm_min)/(float)(chan2_trim - chan2->radio_min); - } - int32_t in_scale_high; - if (chan2->radio_max <= chan2_trim) { - in_scale_high = scale_max1; - } else { - in_scale_high = scale_max1*(pwm_max - chan2_trim)/(float)(chan2->radio_max - chan2_trim); - } - if (chan1->get_reverse() != chan2->get_reverse()) { - in_scale_low = -in_scale_low; - in_scale_high = -in_scale_high; - } - if (!print_buffer(buf, buf_size, "M: 1\n") || - !print_buffer(buf, buf_size, "O: %d %d %d %d %d\n", - (int)(pwm_scale*(chan1_trim - chan1->radio_min)), - (int)(pwm_scale*(chan1->radio_max - chan1_trim)), - (int)(pwm_scale*(chan1_trim - 1500)), - (int)-scale_max2, (int)scale_max2) || - !print_buffer(buf, buf_size, "S: 0 %u %d %d %d %d %d\n", c1, - in_scale_low, - in_scale_high, - 0, - -limit, limit)) { - return false; - } - } else { - const RC_Channel *chan1 = RC_Channel::rc_channel(c1); - const RC_Channel *chan2 = RC_Channel::rc_channel(c2); - int16_t chan1_trim = (c1==rcmap.throttle()-1?1500:chan1->radio_trim); - int16_t chan2_trim = (c2==rcmap.throttle()-1?1500:chan2->radio_trim); - chan1_trim = constrain_int16(chan1_trim, PX4_LIM_RC_MIN+1, PX4_LIM_RC_MAX-1); - chan2_trim = constrain_int16(chan2_trim, PX4_LIM_RC_MIN+1, PX4_LIM_RC_MAX-1); - // mix of two input channels to give an output channel. To - // make the mixer match the behaviour of APM we need to - // scale and offset the input channels to undo the affects - // of the PX4IO input processing - if (!print_buffer(buf, buf_size, "M: 2\n") || - !print_buffer(buf, buf_size, "O: %d %d 0 %d %d\n", mix, mix, (int)-scale_max1, (int)scale_max1)) { - return false; - } - int32_t in_scale_low = pwm_scale*(chan1_trim - pwm_min); - int32_t in_scale_high = pwm_scale*(pwm_max - chan1_trim); - int32_t offset = pwm_scale*(chan1_trim - 1500); - if (!print_buffer(buf, buf_size, "S: 0 %u %d %d %d %d %d\n", - c1, in_scale_low, in_scale_high, offset, - (int)-scale_max2, (int)scale_max2)) { - return false; - } - in_scale_low = pwm_scale*(chan2_trim - pwm_min); - in_scale_high = pwm_scale*(pwm_max - chan2_trim); - offset = pwm_scale*(chan2_trim - 1500); - if (rev) { - if (!print_buffer(buf, buf_size, "S: 0 %u %d %d %d %d %d\n", - c2, in_scale_low, in_scale_high, offset, - (int)-scale_max2, (int)scale_max2)) { - return false; - } - } else { - if (!print_buffer(buf, buf_size, "S: 0 %u %d %d %d %d %d\n", - c2, -in_scale_low, -in_scale_high, -offset, - (int)-scale_max2, (int)scale_max2)) { - return false; - } - } - } - } - - /* - if possible, also write to a file for debugging purposes - */ - int mix_fd = open(filename, O_WRONLY|O_CREAT|O_TRUNC, 0644); - if (mix_fd != -1) { - write(mix_fd, buf0, buf_size0 - buf_size); - close(mix_fd); - } - return true; -} - - -/* - setup mixer on PX4 so that if FMU dies the pilot gets manual control - */ -bool Plane::setup_failsafe_mixing(void) -{ - const char *mixer_filename = "/fs/microsd/APM/MIXER.MIX"; - bool ret = false; - char *buf = NULL; - const uint16_t buf_size = 2048; - - buf = (char *)malloc(buf_size); - if (buf == NULL) { - return false; - } - - if (!create_mixer(buf, buf_size, mixer_filename)) { - hal.console->printf("Unable to create mixer\n"); - free(buf); - return false; - } - - enum AP_HAL::Util::safety_state old_state = hal.util->safety_switch_state(); - struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 8}; - - int px4io_fd = open("/dev/px4io", 0); - if (px4io_fd == -1) { - // px4io isn't started, no point in setting up a mixer - free(buf); - return false; - } - - if (old_state == AP_HAL::Util::SAFETY_ARMED) { - // make sure the throttle has a non-zero failsafe value before we - // disable safety. This prevents sending zero PWM during switch over - hal.rcout->set_safety_pwm(1UL<<(rcmap.throttle()-1), throttle_min()); - } - - // we need to force safety on to allow us to load a mixer. We call - // it twice as there have been reports that this call can fail - // with a small probability - hal.rcout->force_safety_on(); - hal.rcout->force_safety_on(); - - /* reset any existing mixer in px4io. This shouldn't be needed, - * but is good practice */ - if (ioctl(px4io_fd, MIXERIOCRESET, 0) != 0) { - hal.console->printf("Unable to reset mixer\n"); - goto failed; - } - - /* pass the buffer to the device */ - if (ioctl(px4io_fd, MIXERIOCLOADBUF, (unsigned long)buf) != 0) { - hal.console->printf("Unable to send mixer to IO\n"); - goto failed; - } - - // setup RC config for each channel based on user specified - // mix/max/trim. We only do the first 8 channels due to - // a RC config limitation in px4io.c limiting to PX4IO_RC_MAPPED_CONTROL_CHANNELS - for (uint8_t i=0; i<8; i++) { - RC_Channel *ch = RC_Channel::rc_channel(i); - if (ch == NULL) { - continue; - } - struct pwm_output_rc_config config; - /* - we use a min/max of 900/2100 to allow for pass-thru of - larger values than the RC min/max range. This mimics the APM - behaviour of pass-thru in manual, which allows for dual-rate - transmitter setups in manual mode to go beyond the ranges - used in stabilised modes - */ - config.channel = i; - config.rc_min = 900; - config.rc_max = 2100; - if (rcmap.throttle()-1 == i) { - // throttle uses a trim of 1500, so we don't get division - // by small numbers near RC3_MIN - config.rc_trim = 1500; - } else { - config.rc_trim = constrain_int16(ch->radio_trim, config.rc_min+1, config.rc_max-1); - } - config.rc_dz = 0; // zero for the purposes of manual takeover - - // we set reverse as false, as users of ArduPilot will have - // input reversed on transmitter, so from the point of view of - // the mixer the input is never reversed. The one exception is - // the 2nd channel, which is reversed inside the PX4IO code, - // so needs to be unreversed here to give sane behaviour. - if (i == 1) { - config.rc_reverse = true; - } else { - config.rc_reverse = false; - } - - if (i+1 == g.override_channel.get()) { - /* - This is an OVERRIDE_CHAN channel. We want IO to trigger - override with a channel input of over 1750. The px4io - code is setup for triggering below 10% of full range. To - map this to values above 1750 we need to reverse the - direction and set the rc range for this channel to 1000 - to 1833 (1833 = 1000 + 750/0.9) - */ - config.rc_assignment = PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH; - config.rc_reverse = true; - config.rc_max = 1833; - config.rc_min = 1000; - config.rc_trim = 1500; - } else { - config.rc_assignment = i; - } - - if (ioctl(px4io_fd, PWM_SERVO_SET_RC_CONFIG, (unsigned long)&config) != 0) { - hal.console->printf("SET_RC_CONFIG failed\n"); - goto failed; - } - } - - for (uint8_t i = 0; i < pwm_values.channel_count; i++) { - pwm_values.values[i] = 900; - } - if (ioctl(px4io_fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values) != 0) { - hal.console->printf("SET_MIN_PWM failed\n"); - goto failed; - } - - for (uint8_t i = 0; i < pwm_values.channel_count; i++) { - pwm_values.values[i] = 2100; - } - if (ioctl(px4io_fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values) != 0) { - hal.console->printf("SET_MAX_PWM failed\n"); - goto failed; - } - if (ioctl(px4io_fd, PWM_SERVO_SET_OVERRIDE_OK, 0) != 0) { - hal.console->printf("SET_OVERRIDE_OK failed\n"); - goto failed; - } - - // setup for immediate manual control if FMU dies - if (ioctl(px4io_fd, PWM_SERVO_SET_OVERRIDE_IMMEDIATE, 1) != 0) { - hal.console->printf("SET_OVERRIDE_IMMEDIATE failed\n"); - goto failed; - } - - ret = true; - -failed: - if (buf != NULL) { - free(buf); - } - if (px4io_fd != -1) { - close(px4io_fd); - } - // restore safety state if it was previously armed - if (old_state == AP_HAL::Util::SAFETY_ARMED) { - hal.rcout->force_safety_off(); - hal.rcout->force_safety_off(); - } - return ret; -} - - -#endif // CONFIG_HAL_BOARD diff --git a/ArduSubsea/radio.cpp b/ArduSubsea/radio.cpp deleted file mode 100644 index 4629b605bd..0000000000 --- a/ArduSubsea/radio.cpp +++ /dev/null @@ -1,347 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#include "Plane.h" - -//Function that will read the radio data, limit servos and trigger a failsafe -// ---------------------------------------------------------------------------- - -/* - allow for runtime change of control channel ordering - */ -void Plane::set_control_channels(void) -{ - if (g.rudder_only) { - // in rudder only mode the roll and rudder channels are the - // same. - channel_roll = RC_Channel::rc_channel(rcmap.yaw()-1); - } else { - 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_rudder = RC_Channel::rc_channel(rcmap.yaw()-1); - - // set rc channel ranges - channel_roll->set_angle(SERVO_MAX); - channel_pitch->set_angle(SERVO_MAX); - channel_rudder->set_angle(SERVO_MAX); - channel_throttle->set_range(0, 100); - - if (!arming.is_armed() && arming.arming_required() == AP_Arming::YES_MIN_PWM) { - hal.rcout->set_safety_pwm(1UL<<(rcmap.throttle()-1), throttle_min()); - } - - // 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); -} - -/* - initialise RC input channels - */ -void Plane::init_rc_in() -{ - // set rc dead zones - channel_roll->set_default_dead_zone(30); - channel_pitch->set_default_dead_zone(30); - channel_rudder->set_default_dead_zone(30); - channel_throttle->set_default_dead_zone(30); - - update_aux(); -} - -/* - initialise RC output channels - */ -void Plane::init_rc_out() -{ - channel_roll->enable_out(); - channel_pitch->enable_out(); - - /* - change throttle trim to minimum throttle. This prevents a - configuration error where the user sets CH3_TRIM incorrectly and - the motor may start on power up - */ - channel_throttle->radio_trim = throttle_min(); - - if (arming.arming_required() != AP_Arming::YES_ZERO_PWM) { - channel_throttle->enable_out(); - } - channel_rudder->enable_out(); - RC_Channel_aux::enable_aux_servos(); - - // Initialization of servo outputs - RC_Channel::output_trim_all(); - - // setup PWM values to send if the FMU firmware dies - RC_Channel::setup_failsafe_trim_all(); - - // setup PX4 to output the min throttle when safety off if arming - // is setup for min on disarm - if (arming.arming_required() == AP_Arming::YES_MIN_PWM) { - hal.rcout->set_safety_pwm(1UL<<(rcmap.throttle()-1), throttle_min()); - } -} - -/* - check for pilot input on rudder stick for arming/disarming -*/ -void Plane::rudder_arm_disarm_check() -{ - AP_Arming::ArmingRudder arming_rudder = arming.rudder_arming(); - - if (arming_rudder == AP_Arming::ARMING_RUDDER_DISABLED) { - //parameter disallows rudder arming/disabling - return; - } - - // if throttle is not down, then pilot cannot rudder arm/disarm - if (channel_throttle->control_in > 0) { - rudder_arm_timer = 0; - return; - } - - // if not in a manual throttle mode then disallow rudder arming/disarming - if (auto_throttle_mode ) { - rudder_arm_timer = 0; - return; - } - - if (!arming.is_armed()) { - // when not armed, full right rudder starts arming counter - if (channel_rudder->control_in > 4000) { - uint32_t now = millis(); - - if (rudder_arm_timer == 0 || - now - rudder_arm_timer < 3000) { - - if (rudder_arm_timer == 0) { - rudder_arm_timer = now; - } - } else { - //time to arm! - arm_motors(AP_Arming::RUDDER); - rudder_arm_timer = 0; - } - } else { - // not at full right rudder - rudder_arm_timer = 0; - } - } else if (arming_rudder == AP_Arming::ARMING_RUDDER_ARMDISARM && !is_flying()) { - // when armed and not flying, full left rudder starts disarming counter - if (channel_rudder->control_in < -4000) { - uint32_t now = millis(); - - if (rudder_arm_timer == 0 || - now - rudder_arm_timer < 3000) { - if (rudder_arm_timer == 0) { - rudder_arm_timer = now; - } - } else { - //time to disarm! - disarm_motors(); - rudder_arm_timer = 0; - } - } else { - // not at full left rudder - rudder_arm_timer = 0; - } - } -} - -void Plane::read_radio() -{ - if (!hal.rcin->new_input()) { - control_failsafe(channel_throttle->radio_in); - return; - } - - failsafe.last_valid_rc_ms = millis(); - - elevon.ch1_temp = channel_roll->read(); - elevon.ch2_temp = channel_pitch->read(); - uint16_t pwm_roll, pwm_pitch; - - if (g.mix_mode == 0) { - pwm_roll = elevon.ch1_temp; - pwm_pitch = elevon.ch2_temp; - }else{ - pwm_roll = BOOL_TO_SIGN(g.reverse_elevons) * (BOOL_TO_SIGN(g.reverse_ch2_elevon) * int16_t(elevon.ch2_temp - elevon.trim2) - BOOL_TO_SIGN(g.reverse_ch1_elevon) * int16_t(elevon.ch1_temp - elevon.trim1)) / 2 + 1500; - pwm_pitch = (BOOL_TO_SIGN(g.reverse_ch2_elevon) * int16_t(elevon.ch2_temp - elevon.trim2) + BOOL_TO_SIGN(g.reverse_ch1_elevon) * int16_t(elevon.ch1_temp - elevon.trim1)) / 2 + 1500; - } - - RC_Channel::set_pwm_all(); - - if (control_mode == TRAINING) { - // in training mode we don't want to use a deadzone, as we - // want manual pass through when not exceeding attitude limits - channel_roll->set_pwm_no_deadzone(pwm_roll); - channel_pitch->set_pwm_no_deadzone(pwm_pitch); - channel_throttle->set_pwm_no_deadzone(channel_throttle->read()); - channel_rudder->set_pwm_no_deadzone(channel_rudder->read()); - } else { - channel_roll->set_pwm(pwm_roll); - channel_pitch->set_pwm(pwm_pitch); - } - - control_failsafe(channel_throttle->radio_in); - - channel_throttle->servo_out = channel_throttle->control_in; - - if (g.throttle_nudge && channel_throttle->servo_out > 50) { - float nudge = (channel_throttle->servo_out - 50) * 0.02f; - if (ahrs.airspeed_sensor_enabled()) { - airspeed_nudge_cm = (aparm.airspeed_max * 100 - g.airspeed_cruise_cm) * nudge; - } else { - throttle_nudge = (aparm.throttle_max - aparm.throttle_cruise) * nudge; - } - } else { - airspeed_nudge_cm = 0; - throttle_nudge = 0; - } - - rudder_arm_disarm_check(); - - if (g.rudder_only != 0) { - // in rudder only mode we discard rudder input and get target - // attitude from the roll channel. - rudder_input = 0; - } else { - rudder_input = channel_rudder->control_in; - } -} - -void Plane::control_failsafe(uint16_t pwm) -{ - if (millis() - failsafe.last_valid_rc_ms > 1000 || rc_failsafe_active()) { - // we do not have valid RC input. Set all primary channel - // control inputs to the trim value and throttle to min - channel_roll->radio_in = channel_roll->radio_trim; - channel_pitch->radio_in = channel_pitch->radio_trim; - channel_rudder->radio_in = channel_rudder->radio_trim; - - // note that we don't set channel_throttle->radio_in to radio_trim, - // as that would cause throttle failsafe to not activate - - channel_roll->control_in = 0; - channel_pitch->control_in = 0; - channel_rudder->control_in = 0; - channel_throttle->control_in = 0; - } - - if(g.throttle_fs_enabled == 0) - return; - - if (g.throttle_fs_enabled) { - if (rc_failsafe_active()) { - // we detect a failsafe from radio - // throttle has dropped below the mark - failsafe.ch3_counter++; - if (failsafe.ch3_counter == 10) { - gcs_send_text_fmt(MAV_SEVERITY_WARNING, "MSG FS ON %u", (unsigned)pwm); - failsafe.ch3_failsafe = true; - AP_Notify::flags.failsafe_radio = true; - } - if (failsafe.ch3_counter > 10) { - failsafe.ch3_counter = 10; - } - - }else if(failsafe.ch3_counter > 0) { - // we are no longer in failsafe condition - // but we need to recover quickly - failsafe.ch3_counter--; - if (failsafe.ch3_counter > 3) { - failsafe.ch3_counter = 3; - } - if (failsafe.ch3_counter == 1) { - gcs_send_text_fmt(MAV_SEVERITY_WARNING, "MSG FS OFF %u", (unsigned)pwm); - } else if(failsafe.ch3_counter == 0) { - failsafe.ch3_failsafe = false; - AP_Notify::flags.failsafe_radio = false; - } - } - } -} - -void Plane::trim_control_surfaces() -{ - read_radio(); - int16_t trim_roll_range = (channel_roll->radio_max - channel_roll->radio_min)/5; - int16_t trim_pitch_range = (channel_pitch->radio_max - channel_pitch->radio_min)/5; - if (channel_roll->radio_in < channel_roll->radio_min+trim_roll_range || - channel_roll->radio_in > channel_roll->radio_max-trim_roll_range || - channel_pitch->radio_in < channel_pitch->radio_min+trim_pitch_range || - channel_pitch->radio_in > channel_pitch->radio_max-trim_pitch_range) { - // don't trim for extreme values - if we attempt to trim so - // there is less than 20 percent range left then assume the - // sticks are not properly centered. This also prevents - // problems with starting APM with the TX off - return; - } - - // Store control surface trim values - // --------------------------------- - if(g.mix_mode == 0) { - if (channel_roll->radio_in != 0) { - channel_roll->radio_trim = channel_roll->radio_in; - } - if (channel_pitch->radio_in != 0) { - channel_pitch->radio_trim = channel_pitch->radio_in; - } - - // the secondary aileron/elevator is trimmed only if it has a - // corresponding transmitter input channel, which k_aileron - // doesn't have - RC_Channel_aux::set_radio_trim(RC_Channel_aux::k_aileron_with_input); - RC_Channel_aux::set_radio_trim(RC_Channel_aux::k_elevator_with_input); - } else{ - if (elevon.ch1_temp != 0) { - elevon.trim1 = elevon.ch1_temp; - } - if (elevon.ch2_temp != 0) { - elevon.trim2 = elevon.ch2_temp; - } - //Recompute values here using new values for elevon1_trim and elevon2_trim - //We cannot use radio_in[CH_ROLL] and radio_in[CH_PITCH] values from read_radio() because the elevon trim values have changed - uint16_t center = 1500; - channel_roll->radio_trim = center; - channel_pitch->radio_trim = center; - } - if (channel_rudder->radio_in != 0) { - channel_rudder->radio_trim = channel_rudder->radio_in; - } - - // save to eeprom - channel_roll->save_eeprom(); - channel_pitch->save_eeprom(); - channel_rudder->save_eeprom(); -} - -void Plane::trim_radio() -{ - for (uint8_t y = 0; y < 30; y++) { - read_radio(); - } - - trim_control_surfaces(); -} - -/* - return true if throttle level is below throttle failsafe threshold - or RC input is invalid - */ -bool Plane::rc_failsafe_active(void) -{ - if (!g.throttle_fs_enabled) { - return false; - } - if (millis() - failsafe.last_valid_rc_ms > 1000) { - // we haven't had a valid RC frame for 1 seconds - return true; - } - if (channel_throttle->get_reverse()) { - return channel_throttle->radio_in >= g.throttle_fs_value; - } - return channel_throttle->radio_in <= g.throttle_fs_value; -} diff --git a/ArduSubsea/release-notes.txt b/ArduSubsea/release-notes.txt deleted file mode 100644 index 07aa1130f8..0000000000 --- a/ArduSubsea/release-notes.txt +++ /dev/null @@ -1,1294 +0,0 @@ -Release 3.4.0, 24th September 2015 ----------------------------------- - -The ArduPilot development team is proud to announce the release of -version 3.4.0 of APM:Plane. This is a major release with a lot of -changes so please read the notes carefully! - -First release with EKF by default - -This is the also the first release that enables the EKF (Extended -Kalman Filter) for attitude and position estimation by default. This -has been in development for a long time, and significantly improves -flight performance. You can still disable the EKF if you want to using -the AHRS_EKF_USE parameter, but it is strongly recommended that you -use the EKF. Note that if an issue is discovered with the EKF in -flight it will automatically be disabled and the older DCM system will -be used instead. That should be very rare. - -In order to use the EKF we need to be a bit more careful about the -setup of the aircraft. That is why in the last release we enabled -arming and pre-arm checks by default. Please don't disable the arming -checks, they are there for very good reasons. - -Last release with APM1/APM2 support - -This will be the last major release that supports the old APM1/APM2 -AVR based boards. We have finally run out of flash space and -memory. In the last few releases we spent quite a bit of time trying -to squeeze more and more into the small flash space of the APM1/APM2, -but it had to end someday if ArduPilot is to continue to develop. I am -open to the idea of someone else volunteering to keep doing -development of APM1/APM2 so if you have the skills and inclination do -please get in touch. Otherwise I will only do small point release -changes for major bugs. - -Even to get this release onto the APM1/APM2 we had to make sacrifices -in terms of functionality. The APM1/APM2 release is missing quite a -few features that are on the Pixhawk and other boards. For example: - - - no rangefinder support for landing - - no terrain following - - no EKF support - - no camera control - - no CLI support - - no advanced failsafe support - - no HIL support (sorry!) - - support for far fewer GPS types - -that is just the most obvious major features that are missing on -APM1/APM2. There are also numerous other smaller things where we need -to take shortcuts on the APM1/APM2. Some of these features were -available on older APM1/APM2 releases but needed to be removed to -allow us to squeeze the new release onto the board. So if you are -happy with a previous release on your APM2 and want a feature that is -in that older release and not in this one then perhaps you shouldn't -upgrade. - -PID Tuning - -While most people are happy with autotune to tune the PIDs for their -planes, it is nice also to be able to do fine tuning by hand. This -release includes new dataflash and mavlink messages to help with that -tuning. You can now see the individual contributions of the P, I and D -components of each PID in the logs, allowing you to get a much better -picture of the performance. - -A simple application of this new tuning is you can easily see if your -trim is off. If the Pitch I term is constantly contributing a -signifcant positive factor then you know that ArduPilot is having to -constantly apply up elevator, which means your plane is nose -heavy. The same goes for roll, and can also be used to help tune your -ground steering. - -Vibration Logging - -This release includes a lot more options for diagnosing vibration -issues. You will notice new VIBRATION messages in MAVLink and VIBE -messages in the dataflash logs. Those give you a good idea of your -(unfiltered) vibration levels. For really detailed analysis you can -setup your LOG_BITMASK to include raw logging, which gives you every -accel and gyro sample on your Pixhawk. You can then do a FFT on the -result and plot the distribution of vibration level with -frequency. That is great for finding the cause of vibration -issues. Note that you need a very fast microSD card for that to work! - -Rudder Disarm - -This is the first release that allows you to disarm using the rudder -if you want to. It isn't enabled by default (due to the slight risk of -accidentially disarming while doing aerobatics). You can enable it -with the ARMING_RUDDER parameter by setting it to 2. It will only -allow you to disarm if the autopilot thinks you are not flying at the -time (thanks to the "is_flying" heuristics from Tom Pittenger). - -More Sensors - -This release includes support for a bunch more sensors. It now supports -3 different interfaces for the LightWare range of Lidars (serial, I2C -and analog), and also supports the very nice Septentrio RTK -dual-frequency GPS (the first dual-frequency GPS we have support -for). It also supports the new "blue label" Lidar from Pulsed Light -(both on I2C and PWM). - -For the uBlox GPS, we now have a lot more configurability of the -driver, with the ability to set the GNSS mode for different -constellations. Also in the uBlox driver we support logging of the raw -carrier phase and pseudo range data, which allows for post-flight RTK -analysis with raw-capable receivers for really accurate photo -missions. - -Better Linux support - -This release includes a lot of improvements to the Linux based -autopilot boards, including the NavIO+, the PXF and ERLE boards and -the BBBMini and the new RasPilot board. If you like the idea of flying -with Linux then please try it out! - -On-board compass calibrator - -We also have a new on-board compass calibrator, which also adds calibration -for soft iron effects, allowing for much more accurate compass -calibration. Support for starting the compass calibration in the -various ground stations is still under development, but it looks like -this will be a big improvement to compass calibration. - -Lots of other changes! - -The above list is just a taste of the changes that have gone into this -release. Thousands of small changes have gone into this release with -dozens of people contributing. Many thanks to everyone who helped! - -Other key changes include: - - - fixed return point on geofence breach - - enable messages for MAVLink gimbal support - - use 64 bit timestamps in dataflash logs - - added realtime PID tuning messages and PID logging - - fixed a failure case for the px4 failsafe mixer - - added DSM binding support on Pixhawk - - added ALTITUDE_WAIT mission command - - added vibration level logging - - ignore low voltage failsafe while disarmed - - added delta velocity and delta angle logging - - fix LOITER_TO_ALT to verify headings towards waypoints within the loiter radius - - allow rudder disarm based on ARMING_RUDDER parameter - - fix default behaviour of flaps - - prevent mode switch changes changing WP tracking - - make TRAINING mode obey stall prevention roll limits - - disable TRIM_RC_AT_START by default - - fixed parameter documentation spelling errors - - send MISSION_ITEM_REACHED messages on waypoint completion - - fixed airspeed handling in SITL simulators - - enable EKF by default on plane - - Improve gyro bias learning rate for plane and rover - - Allow switching primary GPS instance with 1 sat difference - - added NSH over MAVLink support - - added support for mpu9250 on pixhawk and pixhawk2 - - Add support for logging ublox RXM-RAWX messages - - lots of updates to improve support for Linux based boards - - added ORGN message in dataflash - - added support for new "blue label" Lidar - - switched to real hdop in uBlox driver - - improved auto-config of uBlox - - raise accel discrepancy arming threshold to 0.75 - - improved support for tcp and udp connections on Linux - - switched to delta-velocity and delta-angles in DCM - - improved detection of which accel to use in EKF - - improved auto-detections of flow control on pixhawk UARTs - - Failsafe actions are not executed if already on final approach or land. - - Option to trigger GCS failsafe only in AUTO mode. - - added climb/descend parameter to CONTINUE_AND_CHANGE_ALT - - added HDOP to uavcan GPS driver - - improved sending of autopilot version - - prevent motor startup with bad throttle trim on reboot - - log zero rangefinder distance when unhealthy - - added PRU firmware files for BeagleBoneBlack port - - fix for recent STORM32 gimbal support - - changed sending of STATUSTEXT severity to use correct values - - added new RSSI library with PWM input support - - fixed MAVLink heading report for UAVCAN GPS - - support LightWare I2C rangefinder on Linux - - improved staging of parameters and formats on startup to dataflash - - added new on-board compass calibrator - - improved RCOutput code for NavIO port - - added support for Septentrio GPS receiver - - support DO_MOUNT_CONTROl via command-long interface - - added CAM_RELAY_ON parameter - - moved SKIP_GYRO_CAL functionality to INS_GYR_CAL - - added detection of bad lidar settings for landing - -Note that the documentation hasn't yet caught up with all the changes -in this release. We are still working on that, but meanwhile if you -see a feature that interests you and it isn't documented yet then -please ask. - - - -Release 3.3.0, 20th May 2015 ----------------------------- - -The ardupilot development team is proud to announce the release of -version 3.3.0 of APM:Plane. This is a major release with a lot of -changes. Please read the release notes carefully! - -The last stable release was 3 months ago, and since that time we have -applied over 1200 changes to the code. It has been a period of very -rapid development for ArduPilot. Explaining all of the changes that -have been made would take far too long, so I've chosen some key -changes to explain in detail, and listed the most important secondary -changes in a short form. Please ask for details if there is a change -you see listed that you want some more information on. - -Arming Changes --------------- - -This is the first release of APM:Plane where ARMING_CHECK and -ARMING_REQUIRE both default to enabled. That means when you upgrade if -you didn't previously have arming enabled you will need to learn about -arming your plane. - -Please see this page for more information on arming: - - http://plane.ardupilot.com/wiki/arming-your-plane/ - -I know many users will be tempted to disable the arming checks, but -please don't do that without careful thought. The arming checks are an -important part of ensuring the aircraft is ready to fly, and a common -cause of flight problems is to takeoff before ArduPilot is ready. - -Re-do Accelerometer Calibration -------------------------------- - -Due to a change in the maximum accelerometer range on the Pixhawk all -users must re-do their accelerometer calibration for this release. If -you don't then your plane will fail to arm with a message saying that -you have not calibrated the accelerometers. - -Only 3D accel calibration -------------------------- - -The old "1D" accelerometer calibration method has now been removed, so -you must use the 3D accelerometer calibration method. The old method -was removed because a significant number of users had poor flights due -to scaling and offset errors on their accelerometers when they used -the 1D method. My apologies for people with very large aircraft who -find the 3D method difficult. - -Note that you can do the accelerometer calibration with the autopilot -outside the aircraft which can make things easier for large aircraft. - -Auto-disarm ------------ - -After an auto-landing the autopilot will now by default disarm after -LAND_DISARMDELAY seconds (with a default of 20 seconds). This feature -is to prevent the motor from spinning up unexpectedly on the ground -after a landing. - -HIL_MODE parameter ------------------- - -It is now possible to configure your autopilot for hardware in the -loop simulation without loading a special firmware. Just set the -parameter HIL_MODE to 1 and this will enable HIL for any -autopilot. This is designed to make it easier for users to try HIL -without having to find a HIL firmware. - -SITL on Windows ---------------- - -The SITL software in the loop simulation system has been completely -rewritten for this release. A major change is to make it possible to -run SITL on native windows without needing a Linux virtual -machine. There should be a release of MissionPlanner for Windows soon -which will make it easy to launch a SITL instance. - -The SITL changes also include new backends, including the CRRCSim -flight simulator. This gives us a much wider range of aircraft we can -use for SITL. See http://dev.ardupilot.com/wiki/simulation-2/ for more -information. - -Throttle control on takeoff ---------------------------- - -A number of users had problems with pitch control on auto-takeoff, and -with the aircraft exceeding its target speed during takeoff. The -auto-takeoff code has now been changed to use the normal TECS throttle -control which should solve this problem. - -Rudder only support -------------------- - -There is a new RUDDER_ONLY parameter for aircraft without ailerons, -where roll is controlled by the rudder. Please see the documentation -for more information on flying with a rudder only aircraft: - - http://plane.ardupilot.com/wiki/arduplane-parameters/#rudder_only_aircraft_arduplanerudder_only - -APM1/APM2 Support ------------------ - -We have managed to keep support for the APM1 and APM2 in this release, -but in order to fit it in the limited flash space we had to disable -some more features when building for those boards. For this release -the AP_Mount code for controlling camera mounts is disabled on -APM1/APM2. - -At some point soon it will become impractical to keep supporting the -APM1/APM2 for planes. Please consider moving to a 32 bit autopilot -soon if you are still using an APM1 or APM2. - -New INS code ------------- - -There have been a lot of changes to the gyro and accelerometer -handling for this release. The accelerometer range on the Pixhawk has -been changed to 16g from 8g to prevent clipping on high vibration -aircraft, and the sampling rate on the lsm303d has been increased to -1600Hz. - -An important bug has also been fixed which caused aliasing in the -sampling process from the accelerometers. That bug could cause -attitude errors in high vibration environments. - -Numerous Landing Changes ------------------------- - -Once again there have been a lot of improvements to the automatic -landing support. Perhaps most important is the introduction of a -smooth transition from landing approach to the flare, which reduces -the tendency to pitch up too much on flare. - -There is also a new parameter TECS_LAND_PMAX which controls the -maximum pitch during landing. This defaults to 10 degrees, but for -many aircraft a smaller value may be appropriate. Reduce it to 5 -degrees if you find you still get too much pitch up during the flare. - -Other secondary changes in this release include: - - - a new SerialManager library which gives much more flexible management of serial port assignment - - changed the default FS_LONG_TIMEOUT to 5 seconds - - raised default IMAX for roll/pitch to 3000 - - lowered default L1 navigation period to 20 - - new BRD_SBUS_OUT parameter to enable SBUS output on Pixhawk - - large improvements to the internals of PX4Firmware/PX4NuttX for better performance - - auto-formatting of microSD cards if they can't be mounted on boot (PX4/Pixhawk only) - - a new PWM based driver for the PulsedLight Lidar to avoid issues with the I2C interface - - fixed throttle forcing to zero when disarmed - - only reset mission on disarm if not in AUTO mode - - much better handling of steep landings - - added smooth transition in landing flare - - added HIL_MODE parameter for HIL without a special firmware - - lowered default FS_LONG_TIMEOUT to 5 seconds - - mark old ELEVON_MIXING mode as deprecated - - fixed 50Hz MAVLink support - - support DO_SET_HOME MAVLink command - - fixed larger values of TKOFF_THR_DELAY - - allow PulsedLight Lidar to be disabled at a given height - - fixed bungee launch (long throttle delay) - - fixed a bug handling entering AUTO mode before we have GPS lock - - added CLI_ENABLED parameter - - removed 1D accel calibration - - added EKF_STATUS_REPORT MAVLink message - - added INITIAL_MODE parameter - - added TRIM_RC_AT_START parameter - - added auto-disarm after landing (LAND_DISARMDELAY) - - added LOCAL_POSITION_NED MAVLink message - - avoid triggering a fence breach in final stage of landing - - rebuild glide slope if we are above it and climbing - - use TECS to control throttle on takeoff - - added RUDDER_ONLY parameter to better support planes with no ailerons - - updated Piksi RTK GPS driver - - improved support for GPS data injection (for Piksi RTK GPS) - - added NAV_LOITER_TO_ALT mission item - - fixed landing approach without an airspeed sensor - - support RTL_AUTOLAND=2 for landing without coming to home first - - disabled camera mount support on APM1/APM2 - - added support for SToRM32 and Alexmos camera gimbals - - added support for Jaimes mavlink enabled gimbal - - improved EKF default tuning for planes - - updated support for NavIO and NavIO+ boards - - updated support for VRBrain boards - - fixes for realtime threads on Linux - - added simulated sensor lag for baro and mag in SITL - - made it possible to build SITL for native Windows - - switched to faster accel sampling on Pixhawk - - added coning corrections on Pixhawk - - set ARMING_CHECK to 1 by default - - disable NMEA and SiRF GPS on APM1/APM2 - - support MPU9255 IMU on Linux - - updates to BBBMINI port for Linux - - added TECS_LAND_PMAX parameter - - switched to synthetic clock in SITL - - support CRRCSim FDM backend in SITL - - new general purpose replay parsing code - - switched to 16g accel range in Pixhawk - - added FENCE_AUTOENABLE=2 for disabling just fence floor - - added POS dataflash log message - - changed GUIDED behaviour to match copter - - added support for a 4th MAVLink channel - - support setting AHRS_TRIM in preflight calibration - - fixed a PX4 mixer out of range error - -Best wishes to all APM:Plane users from the dev team, and happy -flying! - - -Release 3.2.2, February 10th 2015 ---------------------------------- - -The ardupilot development team has released version 3.2.2 of -APM:Plane. This is a bugfix release for some important bugs found by -users of the 3.2.1 release. - -The changes in this release are: - - - fixed a bug that could cause short term loss of RC control with - some receiver systems and configurations - - - allowed for shorter sync pulse widths for PPM-SUM receivers on - APM1 and APM2 - - - fixed HIL mode altitude - -The most important bug fix is the one for short term loss of RC -control. This is a very long standing bug which didn't have a -noticible impact for most people, but could cause loss of RC control -for around 1 or 2 seconds for some people in certain circumstances. - -The bug was in the the AP_HAL RCInput API. Each HAL backend has a flag -that says whether there is a new RC input frame available. That flag -was cleared by the read() method (typically hal.rcin->read()). Callers -would check for new input by checking the boolean -hal.rcin->new_input() function. - -The problem was that read() was called from multiple places. Normally -this is fine as reads from other than the main radio input loop happen -before the other reads, but if the timing of the new radio frame -exactly matched the loop frequency then a read from another place -could clear the new_input flag and we would not see the new RC input -frame. If that happened enough times we would go into a short term RC -failsafe and ignore RC inputs, even in manual mode. - -The fix was very simple - it is the new_input() function itself that -should clear the flag, not read(). - -Many thanks to MarkM for helping us track down this bug by providing -us with sufficient detail on how to reproduce it. In Marks case his -OpenLRSng configuration happened to produce exactly the worst case -timing needed to reproduce the issue. Once I copied his OpenLRS -settings to my TX/RX I was able to reproduce the problem and it was -easy to find and fix. - -A number of users have reported occasional glitches in manual control -where servos pause for short periods in past releases. It is likely -that some of those issues were caused by this bug. The dev team would -like to apologise for taking so long to track down this bug! - -The other main change was also related to RC input. Some receivers use -a PPM-SUM sync pulse width shorter than what the APM1/APM2 code was -setup to handle. The OpenLRSng default sync pulse width is 3000 -microseconds, but the APM1/APM2 code was written for a mininum sync -pulse width of 4000 microseconds. For this release I have changed the -APM1/APM2 driver to accept a sync pulse width down to 2700 -microseconds. - - -Release 3.2.1, February 5th 2015 --------------------------------- - -The ardupilot development team is proud to announce the release of -version 3.2.1 of APM:Plane. This is primarily a bugfix release, but -does have some new features. - -The major changes in this release are: - - - fixed a mission handling bug that could cause a crash if jump - commands form an infinite loop (thanks to Dellarb for reporting - this bug) - - improved support for in-kernel SPI handling on Linux (thanks to John Williams) - - support UAVCAN based ESCs and GPS modules on Pixhawk (thanks to - Pavel, Holger and and PX4 dev team) - - Multiple updates for the NavIO+ cape on RaspberryPi (thanks to - Emlid) - - multiple automatic landing fixes, including improvements in flare - detection, glide slope calculation and lag handling - - fixed a bug that could cause a change altitude MAVLink command - from causing a sudden descent - - re-enable CLI on non-APM1/APM2 boards - - Lots of EKF changes, including reducing impact of ground magnetic - interference, reducing the impact of a GPS outage and integrating - optical flow support - - added initial support for the PX4 optical flow sensor. Just - logging for this release. - - added support for MAVLink packet routing - - added detection and recovery from faulty gyro and accel sensors - - improved arming checks code to detect a lot more error conditions, - and change the ARMING_CHECK default value to check all error - conditions. - - added support for BBBMini Linux port - - increased number of AVR input channels from 8 to 11 - - auto-set system clock based on GPS in Linux ports - - added SBUS FrSky telemetry support (thanks to Mathias) - -Release 3.2.0, November 25th 2014 ---------------------------------- - -The ardupilot development team is proud to announce the release of -version 3.2.0 of APM:Plane. This is a major release with a lot of -new features. - -The changes span a lot of different areas of the code, but arguably -the most important changes are: - - - automatic stall prevention code - - PX4IO based RC override code on FMU failure - - I2C crash bugfix - - new autoland code from Michael Day - - compass independent auto takeoff - -I'll go into each of these changes in a bit more detail. - -Automatic Stall Prevention --------------------------- - -The automatic stall prevention code is code that uses the aerodynamic -load factor (calculated from demanded bank angle) to adjust both the -maximum roll angle and the minimum airspeed. You can enable/disable -this code with the STALL_PREVENTION parameter which defaults to -enabled. - -When in stabilised manual throttle modes this option has the effect of -limiting how much bank angle you can demand when close to the -configured minimum airspeed (from ARSPD_FBW_MIN). That means when in -FBWA mode if you try to turn hard while close to ARSPD_FBW_MIN it will -limit the bank angle to an amount that will keep the speed above -ARSPD_FBW_MIN times the aerodynamic load factor. It will always allow -you at bank at least 25 degrees however, to ensure you keep some -maneuverability if the airspeed estimate is incorrect. - -When in auto-throttle modes (such as AUTO, RTL, CRUISE etc) it will -additionally raise the minimum airspeed in proportion to the -aerodynamic load factor. That means if a mission demands a sharp turn -at low speed then initially the turn will be less sharp, and the TECS -controller will add power to bring the airspeed up to a level that can -handle the demanded turn. After the turn is complete the minimum -airspeed will drop back to the normal level. - -This change won't completely eliminate stalls of course, but it should -make them less likely if you properly configure ARSPD_FBW_MIN for your -aircraft. - -PX4IO based RC override code ----------------------------- - -This releases adds support for PX4IO based RC override. This is a -safety feature where the stm32 IO co-processor on the PX4 and Pixhawk -will give the pilot manual control if the main ArduPilot -micro-controller fails (or the autopilot code crashes). This is -particularly useful when testing new code that may not be stable. - -As part of this new RC override support we also have a new -OVERRIDE_CHAN parameter, which allows you to specify a RC input -channel which can be used to test the RC override support. See the -documentation on OVERRIDE_CHAN for details. - -I2C bugfix ----------- - -This release fixes another I2C bug in NuttX which could cause the -Pixhawk to lock up under high I2C load with noise on I2C cables. This -bug has caused at least two aircraft to crash, so it is an important -fix. I hope this will be the last I2C crash bug we find in NuttX! An -audit of the code was done to try to confirm that no more bugs of this -type are present. - -New Autoland code ------------------ - -This release incorporates some new autoland capabilities contributed -by Michael Day. The key new feature is the ability to trigger an -automatic landing when a RTL completes, which for the first time -allows a user to setup their aircraft to land using only transmitter -control. - -The way it works is there is a new parameter RTL_AUTOLAND. If that is -set to 1 and the aircraft reaches its target location in an RTL it -will look for DO_LAND_START mission item in the mission. If that is -found then the aircraft will switch to AUTO starting at that section -of the mission. The user sets up their land mission commands starting -with a DO_LAND_START mission item. - -There is more to do in this autoland support. We have been discussing -more advanced go-around capabilities and also better path planning for -landing. The code in this release is an important first step though, -and will be a good basis for future work. - -Compass independent takeoff code --------------------------------- - -The auto-takeoff code has been changed to make it more independent of -compass settings, allowing for reliable takeoff down a runway with -poor compass offsets. The new takeoff code uses the gyroscope as the -primary heading control for the first part of the takeoff, until the -aircraft gains enough speed for a GPS heading to be reliable. - -Many thanks to all the contributors, especially: - - - Paul and Jon for EKF and TECS updates - - Bret and Grant for stall prevention testing - - Michael for all his autoland work - - all the work on NavIO, PXF and Zynq by John, Victor, George and Siddarth - - The PX4 team for all the PX4 updates - -More complete list of changes: - - - allow GCS to enable/disable PX4 safety switch - - make auto-takeoff independent of compass errors - - report gyro unhealthy if calibration failed - - added support for MAV_CMD_DO_LAND_START - - added RTL_AUTOLAND parameter - - disable CLI by default in build - - new InertialSensor implementation - - added landing go around support - - enable PX4 failsafe RC override - - added OVERRIDE_CHAN parameter - - changed default AUTOTUNE level to 6 - - changed default I value for roll/pitch controllers - - added CAMERA_FEEDBACK mavlink messages - - use airspeed temperature for baro calibration if possible - - added STALL_PREVENTION parameter - - fixed handling of TKOFF_THR_MAX parameter - - added ARSPD_SKIP_CAL parameter - - fixed flaperon trim handling (WARNING: may need to retrim flaperons) - - EKF robustness improvements, especially for MAG handling - - lots of HAL_Linux updates - - support wider range of I2C Lidars - - fixed fallback to DCM in AHRS - - fixed I2C crash bug in NuttX - - TECS prevent throttle undershoot after a climb - - AP_Mount: added lead filter to improve servo gimbals - - Zynq and NavIO updates - - fixed preflight calibration to prevent losing 3D accel cal - - perform a gyro calibration when doing 3D accel cal - - added DO_CONTINUE_AND_CHANGE_ALT mission command - - added support for DO_FENCE_ENABLE mission command - - allow gyro calibration to take up to 30 seconds - - improved health checks in the EKF for DCM fallback - -Note: If you use flaperons you may need to re-trim them before you -fly due to the change in flaperon trim handling. - -I hope that everyone enjoys flying this new APM:Plane release as much -as we enjoyed producing it! It is a major milestone in the development -of the fixed wing code for APM, and I think puts us in a great -position for future development. - -Happy flying! - - - -Release 3.1.1, September 12th 2014 ----------------------------------- - -The ardupilot development team is proud to announce the release of -version 3.1.1 of APM:Plane. This is primarily a bugfix release with a -small number of new features. - -The main bug fixed in this release is a bug that could affect saving -parameters and mission items in the FRAM/eeprom storage of -PX4v1/Pixhawk/VRBrain. The bug has been in the code since January 2013 -and did not cause problems very often (which is why it hasn't been -noticed till now), but when it does happen it causes changes to -parameters or mission items not to be saved on a reboot. - -Other changes in this release: - - - support for using a Lidar for landing for terrain altitude (see - the RNGFND_LANDING parameter) - - - improvements in the landing approach code, especially the glide - slope calculation - - - added LAND_FLAP_PERCENT and TKOFF_FLAP_PCNT parameters, to control - the amount of flaps to use on takeoff and landing - - - the default WP_RADIUS has been raised from 30 to 90. Note that the - L1 controller may choose to turn after the WP_RADIUS is - reached. The WP_RADIUS only controls the earliest point at which - the turn can happen, so a larger WP_RADIUS usually leads to better - flight paths, especially for faster aircraft. - - - send gyro and accel status separately to the GCS (thanks to Randy) - - - support setting the acceptance radius in mission waypoints (in - parameter 2), which allows for better control of waypoints where - actions such as servo release will happen - - - fixed GPS time offset in HIL - - - added RELAY_DEFAULT parameter, allowing control of relay state on - boot - - - fixed sdcard logging on PX4v1 - - - added GPS_SBAS_MODE and GPS_MIN_ELEV parameters for better control - of the use of SBAS and the GPS elevation mask for advanced users - -Happy flying! - - -Release 3.1.0, August 26th 2014 -------------------------------- - -The ardupilot development team is proud to announce the release of -version 3.1.0 of APM:Plane. This is a major release with a lot of new -features and bug fixes. - -The biggest change in this release is the addition of automatic -terrain following. Terrain following allows the autopilot to guide the -aircraft over varying terrain at a constant height above the ground -using an on-board terrain database. - -Changes in this release: - - - added terrain following support. See - http://plane.ardupilot.com/wiki/common-terrain-following/ - - - added support for higher baudrates on telemetry ports, to make it - easier to use high rate telemetry to companion boards. Rates of up - to 1.5MBit are now supported to companion boards. - - - added new takeoff code, including new parameters: - TKOFF_TDRAG_ELEV, TKOFF_TDRAG_SPD1, TKOFF_ROTATE_SPD, - TKOFF_THR_SLEW and TKOFF_THR_MAX. - This gives fine grained control of auto takeoff for tail dragger aircraft. - - - overhauled glide slope code to fix glide slope handling in many - situations. This makes transitions between different altitudes - much smoother. - - - prevent early waypoint completion for straight ahead - waypoints. This makes for more accurate servo release at specific - locations, for applications such as dropping water bottles. - - - added MAV_CMD_DO_INVERTED_FLIGHT command in missions, to change - from normal to inverted flight in AUTO (thanks to Philip Rowse for - testing of this feature). - - - new Rangefinder code with support for a wider range of rangefinder - types including a range of Lidars (thanks to Allyson Kreft) - - - added support for FrSky telemetry via SERIAL2_PROTOCOL parameter - (thanks to Matthias Badaire) - - - added new STAB_PITCH_DOWN parameter to improve low throttle - behaviour in FBWA mode, making a stall less likely in FBWA mode - (thanks to Jack Pittar for the idea). - - - added GLIDE_SLOPE_MIN parameter for better handling of small - altitude deviations in AUTO. This makes for more accurate altitude - tracking in AUTO. - - - added support for Linux based autopilots, initially with the PXF - BeagleBoneBlack cape and the Erle robotics board. Support for more - boards is expected in future releases. Thanks to Victor, Sid and - Anuj for their great work on the Linux port. See - http://diydrones.com/profiles/blogs/first-flight-of-ardupilot-on-linux - for details. - - - prevent cross-tracking on some waypoint types, such as when - initially entering AUTO or when the user commands a change of - target waypoint. - - - fixed servo demo on startup (thanks to Klrill-ka) - - - added AFS (Advanced Failsafe) support on 32 bit boards by - default. See - http://plane.ardupilot.com/wiki/advanced-failsafe-configuration/ - - - added support for monitoring voltage of a 2nd battery via BATTERY2 - MAVLink message - - - added airspeed sensor support in HIL - - - fixed HIL on APM2. HIL should now work again on all boards. - - - added StorageManager library, which expands available FRAM storage - on Pixhawk to 16 kByte. This allows for 724 waypoints, 50 rally - points and 84 fence points on Pixhawk. - - - improved steering on landing, so the plane is actively steered - right through the landing. - - - improved reporting of magnetometer and barometer errors to the GCS - - - added FBWA_TDRAG_CHAN parameter, for easier FBWA takeoffs of tail - draggers, and better testing of steering tuning for auto takeoff. - - - fixed failsafe pass through with no RC input (thanks to Klrill-ka) - - - fixed a bug in automatic flow control detection for serial ports - in Pixhawk - - - fixed use of FMU servo pins as digital inputs on Pixhawk - - - imported latest updates for VRBrain boards (thanks to Emile - Castelnuovo and Luca Micheletti) - - - updates to the Piksi GPS support (thanks to Niels Joubert) - - - improved gyro estimate in DCM (thanks to Jon Challinger) - - - improved position projection in DCM in wind (thanks to Przemek - Lekston) - - - several updates to AP_NavEKF for more robust handling of errors - (thanks to Paul Riseborough) - - - improved simulation of rangefinders in SITL - - - lots of small code cleanups thanks to Daniel Frenzel - - - initial support for NavIO board from Mikhail Avkhimenia - - - fixed logging of RCOU for up to 12 channels (thanks to Emile - Castelnuovo) - - - code cleanups from Silvia Nunezrivero - - - improved parameter download speed on radio links with no flow - control - -Many thanks to everyone who contributed to this release, especially -our beta testers Marco, Paul, Philip and Iam. - -Happy flying! - - -Release 3.0.3, May 19th 2014 ----------------------------- - -The ardupilot development team is proud to announce the release of -version 3.0.3 of APM:Plane. This release contains some important bug -fixes for all supported boards. - -The key bug fixes in this release are: - - - fixed handling of filter divergance in the EKF filter - - fixed a glide slope calculation bug when entering AUTO mode - -The EKF fixes are the main focus of this release. During testing of -APM:Plane with the AHRS_EKF_USE enabled it was found that under some -circumstances the EKF could diverge, resulting in loss of attitude -estimate. Unless the pilot quickly took control in MANUAL this could -result in the aircraft crashing. - -The fix for this problem was in several parts. The main fix was to -prevent the divergance, but as a precuation against future bugs of -this type additional numerical checks were added to allow the EKF to -automatically reset in flight when the internal state shows -large gyro bias changes, which are the first sign of something going -wrong in the filter. If this happens again the EKF will automatically -disable itself for 10 seconds, allowing APM:Plane to fall back to the -old DCM code. The EKF will then reset itself using initial state based -on the DCM state. The aircraft will report the failure using the AHRS -health bit in the SYS_STATUS MAVLink message. - -The default EKF tuning parameters were also updated based on a number -of user supplied flight logs to increase the robustness of the filter. - -The second bug fixed in this release relates to the glide slope -calculation when the aircraft enters AUTO mode for the first time when -at an altitude above the altitude of the first waypoint in the -mission. The starting point for the glide slope was incorrectly -calculated using the home altitude, which resulted in the aircraft -descending below the first waypoint altitude before climbing again. In -some circumstances this could lead to a crash due to local terrain. - -Many thanks to everyone who tested this release. Special thanks to -Dellarb for reporting the glide slope bug and to Paul Riseborough for -all his work on the EKF code over the last few weeks. - -Happy flying! - - -Release 3.0.2, May 4th 2014 ---------------------------- - -The ardupilot development team is proud to announce the release of -version 3.0.2 of APM:Plane. This release combines some important bug -fixes with some new features. - -I2C bug fix ------------ - -The most important change for this release is a bug fix for an I2C bug -in the NuttX I2C driver code that could (under some rare -circumstances) cause a Pixhawk to crash. This bug fix is the primary -reason for doing a new release now. - -This bug was able to be reproduced by creating a 1.3m GPS cable -carrying both the I2C signals for a magnetometer and the UART signals -for the GPS. Interference between these two signals could cause the -I2C controller to give spurious data to the I2C driver. The I2C driver -did not have sufficient protection against these errors and could -crash the board. - -While we have not been able to reproduce this error with the normal -cables that come with a Pixhawk we cannot rule out the bug triggering -with shorter cables, so we are doing a fast release to fix the bug. - -Autotune --------- - -This release also includes an important new feature - automatic -roll/pitch tuning. While this feature is still considered experimental -we have had very positive feedback from beta testers and have decided -to include it in the release. - -Full documentation for how to use automatic tuning is available here: - - http://plane.ardupilot.com/wiki/automatic-tuning-with-autotune/ - -we hope that the automatic tuning will help users who have had -difficulty with the standard APM:Plane manual tuning procedure. We -plan on extending autotune to other aspects of fixed wing tuning in -future releases. - -Other changes -------------- - - - fixed a glide slope calculation error when very close to waypoints - - fixed a bug when swithing to another auto-throttle mode during auto - takeoff (thanks to Marco for finding this bug!) - - added MIS_AUTORESET parameter (thanks to Andrew Chapman) - - support compassmot calibration by supplying current measurments to the - compass driver (thanks to Jon Challinger) - - fixed a GPS driver bug that could cause GPS lag in case of lost GPS - samples (thanks to Jon Challinger) - - fixed a LOITER_TURNS bug in missions for counter-clockwise loiter - (thanks to Iskess for finding this bug) - - added support for OBC termination requirements to PX4IO - - added support for pressure altitude termination to OBC module - - fixed EKF wind estimation with no airspeed sensor (thanks to Paul - Riseborough) - - improved tuning of EKF for fixed wing aircraft (thanks to Paul - Riseborough) - - Converted rally point code to library AP_Rally (thanks to Andrew - Chapman) - - added SITL checking for numerical errors - -Thanks to testers! - -Many thanks to everyone who tested the beta versions of this release! -Special thanks to Marco, Paul, Jon, Iam, JNJO, sonicdahousecat and -Keeyen for providing testing of both existing features and the new -autotune code. - - -Release 3.0.1, April 9th 2014 ------------------------------ - -I've just released APM:Plane 3.0.1, a bug fix release for the 3.0.0 release. -This release fixes two bugs: - - throttle failsafe for aircraft using PWM level for failsafe detection - wind reporting with EKF enabled and no airspeed sensor - -The throttle failsafe fix is a critical bugfix, which is why I am -doing a new release so soon. The bug was found by Sam Tabor, and he -posted the bug report before the 3.0.0 release, but I didn't notice it -in the release preparations. The bug only affects systems using PWM -value as the sole method of detecting loss of RC control. I hadn't -noticed it myself as my planes all use receivers which stop sending -value PWM frames when the RC link is lost. In that case failsafe -worked correctly. Receivers that keep sending PWM frames but with low -throttle levels are common though, so this is a very important fix. -Many thanks to Sam for reporting the bug, and my apologies for not -noticing it in time for the 3.0.0 release. - - -Release 3.0.0, April 8th 2014 ------------------------------ - -The ardupilot development team is proud to announce the release of -version 3.0.0 of APM:Plane. This is a major release with a lot of new -features. - -For each release I try to highlight the two or 3 key new features that -have gone in since the last release. That is a more difficult task -this time around because there are just so many new things. Still, I -think the most important ones are the new Extended Kalman Filter (EKF) -for attitude/position estimation, the extensive dual sensors support -and the new AP_Mission library. - -We have also managed to still keep support for the APM1 and APM2, -although quite a few of the new features are not available on those -boards. We don't yet know for how long we'll be able to keep going on -supporting these old boards, so if you are thinking of getting a new -board then you should get a Pixhawk, and if you want the best -performance from the APM:Plane code then you should swap to a -Pixhawk now. It really is a big improvement. - -New Extended Kalman Filter --------------------------- - -The biggest change for the 3.0.0 release (and in fact the major reason -why we are calling it 3.0.0) is the new Extended Kalman Filter from -Paul Riseborough. Using an EKF for attitude and position estimation -was never an option on the APM2 as it didn't have the CPU power or -memory to handle it. The Pixhawk does have plenty of floating point -performance, and Paul has done a fantastic job of taking full -advantage of the faster board. - -As this is the first stable release with the EKF code we have decided -to not enable it by default. It does however run all the time in -parallel with the existing DCM code, and both attitude/position -solutions are logged both to the on-board SD card and over -MAVLink. You can enable the EKF code using the parameter -AHRS_EKF_USE=1, which can be set and unset while flying, allowing you -to experiment with using the EKF either by examining your logs with -the EKF disabled to see how it would have done or by enabling it while -flying. - -The main thing you will notice with the EKF enabled is more accurate -attitude estimation and better handling of sensor glitches. A Kalman -filter has an internal estimate of the reliability of each of its -sensor inputs, and is able to weight them accordingly. This means that -if your accelerometers start giving data that is inconsistent with -your other sensors then it can cope in a much more graceful way than -our old DCM code. - -The result is more accurate flying, particularly in turns. It also -makes it possible to use higher tuning gains, as the increased -accuracy of the attitude estimation means that you can push the -airframe harder without it becoming unstable. You may find you can use -a smaller value for NAVL1_PERIOD, giving tighter turns, and higher -gains on your roll and pitch attitude controllers. - -Paul has written up a more technical description of the new EKF code -here: - - http://plane.ardupilot.com/wiki/common-apm-navigation-extended-kalman-filter-overview/ - - -Dual Sensors ------------- - -The second really big change for this release is support for -dual-sensors. We now take full advantage of the dual accelerometers -and dual gyros in the Pixhawk, and can use dual-GPS for GPS -failover. We already had dual compass support, so the only main -sensors we don't support two of now are the barometer and the airspeed -sensor. I fully expect we will support dual baro and dual airspeed in -a future release. - -You might wonder why dual sensors is useful, so let me give you an -example. I fly a lot of nitro and petrol planes, and one of my planes -(a BigStik 60) had a strange problem where it would be flying -perfectly in AUTO mode, then when the throttle reached a very specific -level the pitch solution would go crazy (sometimes off by 90 -degrees). I managed to recover in MANUAL each time, but it certainly -was exciting! - -A careful analysis of the logs showed that the culprit was -accelerometer aliasing. At a very specific throttle level the Z -accelerometer got a DC offset of 11 m/s/s. So when the plane was -flying along nice and level the Z accelerometer would change from -10 -m/s/s to +1 m/s/s. That resulted in massive errors in the attitude -solution. - -This sort of error happens because of the way the accelerometer is -sampled. In the APM code the MPU6000 (used on both the APM2 and -Pixhawk) samples the acceleration at 1kHz. So if you have a strong -vibrational mode that is right on 1kHz then you are sampling the "top -of the sine wave", and get a DC offset. - -The normal way to fix this issue is to improve the physical -anti-vibration mounting in the aircraft, but I don't like to fix -problems like this by making changes to my aircraft, as if I fix my -aircraft it does nothing for the thousands of other people running the -same code. As the lead APM developer I instead like to fix things in -software, so that everyone benefits. - -The solution was to take advantage of the fact that the Pixhawk has -two accelerometers, one is a MPU6000, and the 2nd is a LSM303D. The -LSM303D is sampled at 800Hz, whereas the MPU6000 is sampled at -1kHz. It would be extremely unusual to have a vibration mode with -aliasing at both frequencies at once, which means that all we needed -to do was work out which accelerometer is accurate at any point in -time. For the DCM code that involved matching each accelerometer at -each time step to the combination of the GPS velocity vector and -current attitude, and for the EKF it was a matter of producing a -weighting for the two accelerometers based on the covariance matrix. - -The result is that the plane flew perfectly with the new dual -accelerometer code, automatically switching between accelerometers as -aliasing occurred. - -Since adding that code I have been on the lookout for signs of -aliasing in other logs that people send me, and it looks like it is -more common than we expected. It is rarely so dramatic as seen on my -BigStik, but often results in some pitch error in turns. I am hopeful -that with a Pixhawk and the 3.0 release of APM:Plane that these types -of problems will now be greatly reduced. - -For the dual gyro support we went with a much simpler solution and -just average the two gyros when both are healthy. That reduces noise, -and works well, but doesn't produce the dramatic improvements that the -dual accelerometer code resulted in. - -Dual GPS was also quite a large development effort. We now support -connecting a 2nd GPS to the serial4/5 port on the Pixhawk. This allows -you to protect against GPS glitches, and has also allowed us to get a -lot of logs showing that even with two identical GPS modules it is -quite common for one of the GPS modules to get a significant error -during a flight. The new code currently switches between the two GPS -modules based on the lock status and number of satellites, but we are -working on a more sophisticated switching mechanism. - -Supporting dual GPS has also made it easier to test new GPS -modules. This has enabled us to do more direct comparisons between the -Lea6 and the Neo7 for example, and found the Neo7 performs very -well. It also helps with developing completely new GPS drivers, such -as the Piksi driver (see notes below). - -New AP_Mission library ----------------------- - -Many months ago Brandon Jones re-worked our mission handling code to -be a library, making it much cleaner and fixing a number of long term -annoyances with the behaviour. For this release Randy built upon the -work that Brandon did and created the new AP_Mission library. - -The main feature of this library from the point of view of the -developers is that it has a much cleaner interface, but it also has -some new user-visible features. The one that many users will be glad -to hear is that it no longer needs a "dummy waypoint" after a -jump. That was always an annoyance when creating complex missions. - -The real advantage of AP_Mission will come in future releases though, -as it has the ability to look ahead in the mission to see what is -coming, allowing for more sophisticated navigation. The copter code -already takes advantage of this with the new spline waypoint feature, -and we expect to take similar advantage of this in APM:Plane in future -releases. - -New Piksi GPS driver --------------------- - -One of the most exciting things to happen in the world of GPS modules -in the last couple of years is the announcement by SwiftNav that they -would be producing a RTK capable GPS module called the Piksi at a -price that (while certainly expensive!) is within reach of more -dedicated hobbyists. It offers the possibility of decimeter and -possibly even centimetre level relative positioning, which has a lot -of potential for small aircraft, particularly for landing control and -more precise aerial mapping. - -This release of APM:Plane has the first driver for the Piksi. The new -driver is written by Niels Joubert, and he has done a great job. It is -only a start though, as this is a single point positioning driver. It -will allow you to use your new Piksi if you were part of the -kickstarter, but it doesn't yet let you use it in RTK mode. Niels and -the SwiftNav team are working on a full RTK driver which we hope will -be in the next release. - -Support for more RC channels ----------------------------- - -This release is the first to allow use of more than 8 RC input -channels. We now support up to 18 input channels on SBus on Pixhawk, -with up to 14 of them able to be assigned to functions using the -RCn_FUNCTION settings. For my own flying I now use a FrSky Taranis -with X8R and X6R receivers and they work very nicely. Many thanks to -the PX4 team, and especially to Holger and Lorenz for their great work -on improving the SBus code. - -Flaperon Support ----------------- - -This release is the first to have integrated flaperon support, and -also includes much improved flaps support in general. You can now set -a FLAP_IN_CHANNEL parameter to give an RC channel for manual flap -control, and setup a FLAPERON_OUTPUT to allow you to setup your -ailerons for both manual and automatic flaperon control. - -We don't yet have a full wiki page on setting up flaperons, but you -can read about the parameters here: - - http://plane.ardupilot.com/wiki/arduplane-parameters/#Flap_input_channel_ArduPlaneFLAP_IN_CHANNEL - -Geofence improvements ---------------------- - -Michael Day has made an number of significant improvements to the -geo-fencing support for this release. It is now possible to -enable/disable the geofence via MAVLink, allowing ground stations to -control the fence. - -There are also three new fence control parameters. One is -FENCE_RET_RALLY which when enabled tells APM to fly back to the -closest rally point on a fence breach, instead of flying to the center -of the fence area. That can be very useful for more precise control of -fence breach handling. - -The second new parameter is FENCE_AUTOENABLE, which allows you to -automatically enable a geofence on takeoff, and disable when doing an -automatic landing. That is very useful for fully automated missions. - -The third new geofence parameter is FENCE_RETALT, which allows you to -specify a return altitude on fence breach. This can be used to -override the default (half way between min and max fence altitude). - -Automatic Landing improvements ------------------------------- - -Michael has also been busy on the automatic landing code, with -improvements to the TECS speed/height control when landing and new -TECS_LAND_ARSPD and TECS_LAND_THR parameters to control airspeed and -throttle when landing. This is much simpler to setup than -DO_CHANGE_SPEED commands in a mission. - -Michael is also working on automatic path planning for landing, based -on the rally points code. We hope that will get into a release soon. - -Detailed Pixhawk Power Logging ------------------------------- - -One of the most common causes of issues with autopilots is power -handling, with poor power supplies leading to brownouts or sensor -malfunction. For this release we have enabled detailed logging of the -information available from the on-board power management system of the -Pixhawk, allowing us to log the status of 3 different power sources -(brick input, servo rail and USB) and log the voltage level of the -servo rail separately from the 5v peripheral rail on the FMU. - -This new logging should make it much easier for us to diagnose power -issues that users may run into. - -New SERIAL_CONTROL protocol ---------------------------- - -This release adds a new SERIAL_CONTROL MAVLink message which makes it -possible to remotely control a serial port on a Pixhawk from a ground -station. This makes it possible to do things like upgrade the firmware -on a 3DR radio without removing it from an aircraft, and will also -make it possible to attach to and control a GPS without removing it -from the plane. - -There is still work to be done in the ground station code to take full -advantage of this new feature and we hope to provide documentation -soon on how to use u-Blox uCenter to talk to and configure a GPS in an -aircraft and to offer an easy 3DR radio upgrade button via the Pixhawk -USB port. - -Lots of other changes! ----------------------- - -There have been a lot of other improvements in the code, but to stop -this turning into a book instead of a set of release notes I'll stop -the detailed description there. Instead here is a list of the more -important changes not mentioned above: - - - added LOG_WHEN_DISARMED flag in LOG_BITMASK - - raised default LIM_PITCH_MAX to 20 degrees - - support a separate steering channel from the rudder channel - - faster mission upload on USB - - new mavlink API for reduced memory usage - - fixes for the APM_OBC Outback Challenge module - - fixed accelerometer launch detection with no airspeed sensor - - greatly improved UART flow control on Pixhawk - - added BRD_SAFETYENABLE option to auto-enable the safety - switch on PX4 and Pixhawk on boot - - fixed pitot tube ordering bug and added ARSPD_TUBE_ORDER parameter - - fixed log corruption bug on PX4 and Pixhawk - - fixed repeated log download bug on PX4 and Pixhawk - - new Replay tool for detailed log replay and analysis - - flymaple updates from Mike McCauley - - fixed zero logs display in MAVLink log download - - fixed norm_input for cruise mode attitude control - - added RADIO_STATUS logging in aircraft logs - - added UBX monitor messages for detailed hardware logging of u-Blox status - - added MS4525 I2C airspeed sensor voltage compensation - - -I hope that everyone enjoys flying this new APM:Plane release as much -as we enjoyed producing it! It is a major milestone in the development -of the fixed wing code for APM, and I think puts us in a great -position for future development. - - -Happy flying! diff --git a/ArduSubsea/sensors.cpp b/ArduSubsea/sensors.cpp deleted file mode 100644 index 356264ea71..0000000000 --- a/ArduSubsea/sensors.cpp +++ /dev/null @@ -1,150 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#include "Plane.h" -#include - -void Plane::init_barometer(void) -{ - gcs_send_text(MAV_SEVERITY_INFO, "Calibrating barometer"); - barometer.calibrate(); - - gcs_send_text(MAV_SEVERITY_INFO, "Barometer calibration complete"); -} - -void Plane::init_rangefinder(void) -{ -#if RANGEFINDER_ENABLED == ENABLED - rangefinder.init(); -#endif -} - -/* - read the rangefinder and update height estimate - */ -void Plane::read_rangefinder(void) -{ -#if RANGEFINDER_ENABLED == ENABLED - - // notify the rangefinder of our approximate altitude above ground to allow it to power on - // during low-altitude flight when configured to power down during higher-altitude flight - float height; -#if AP_TERRAIN_AVAILABLE - if (terrain.status() == AP_Terrain::TerrainStatusOK && terrain.height_above_terrain(height, true)) { - rangefinder.set_estimated_terrain_height(height); - } else -#endif - { - // use the best available alt estimate via baro above home - if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH || - flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL) { - // ensure the rangefinder is powered-on when land alt is higher than home altitude. - // This is done using the target alt which we know is below us and we are sinking to it - height = height_above_target(); - } else { - // otherwise just use the best available baro estimate above home. - height = relative_altitude(); - } - rangefinder.set_estimated_terrain_height(height); - } - - rangefinder.update(); - - if (should_log(MASK_LOG_SONAR)) - Log_Write_Sonar(); - - rangefinder_height_update(); -#endif -} - -/* - calibrate compass -*/ -void Plane::compass_cal_update() { - if (!hal.util->get_soft_armed()) { - compass.compass_cal_update(); - } -} - -/* - Accel calibration -*/ -void Plane::accel_cal_update() { - if (hal.util->get_soft_armed()) { - return; - } - ins.acal_update(); - float trim_roll, trim_pitch; - if(ins.get_new_trim(trim_roll, trim_pitch)) { - ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0)); - } -} - -/* - ask airspeed sensor for a new value - */ -void Plane::read_airspeed(void) -{ - if (airspeed.enabled()) { - airspeed.read(); - if (should_log(MASK_LOG_IMU)) { - Log_Write_Airspeed(); - } - calc_airspeed_errors(); - - // supply a new temperature to the barometer from the digital - // airspeed sensor if we can - float temperature; - if (airspeed.get_temperature(temperature)) { - barometer.set_external_temperature(temperature); - } - } - - // update smoothed airspeed estimate - float aspeed; - if (ahrs.airspeed_estimate(&aspeed)) { - smoothed_airspeed = smoothed_airspeed * 0.8f + aspeed * 0.2f; - } -} - -void Plane::zero_airspeed(bool in_startup) -{ - airspeed.calibrate(in_startup); - read_airspeed(); - // update barometric calibration with new airspeed supplied temperature - barometer.update_calibration(); - gcs_send_text(MAV_SEVERITY_INFO,"Zero airspeed calibrated"); -} - -// read_battery - reads battery voltage and current and invokes failsafe -// should be called at 10hz -void Plane::read_battery(void) -{ - battery.read(); - compass.set_current(battery.current_amps()); - - if (!usb_connected && - hal.util->get_soft_armed() && - battery.exhausted(g.fs_batt_voltage, g.fs_batt_mah)) { - low_battery_event(); - } -} - -// read the receiver RSSI as an 8 bit number for MAVLink -// RC_CHANNELS_SCALED message -void Plane::read_receiver_rssi(void) -{ - receiver_rssi = rssi.read_receiver_rssi_uint8(); -} - -/* - update RPM sensors - */ -void Plane::rpm_update(void) -{ - rpm_sensor.update(); - if (rpm_sensor.healthy(0) || rpm_sensor.healthy(1)) { - if (should_log(MASK_LOG_RC)) { - DataFlash.Log_Write_RPM(rpm_sensor); - } - } -} diff --git a/ArduSubsea/setup.cpp b/ArduSubsea/setup.cpp deleted file mode 100644 index f57ff994c8..0000000000 --- a/ArduSubsea/setup.cpp +++ /dev/null @@ -1,81 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#include "Plane.h" - -#if CLI_ENABLED == ENABLED - -// Command/function table for the setup menu -static const struct Menu::command setup_menu_commands[] = { - // command function called - // ======= =============== - {"reset", MENU_FUNC(setup_factory)}, - {"erase", MENU_FUNC(setup_erase)} -}; - -// 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 Plane::setup_mode(uint8_t argc, const Menu::arg *argv) -{ - // Give the user some guidance - cliSerial->printf("Setup Mode\n" - "\n" - "IMPORTANT: if you have not previously set this system up, use the\n" - "'reset' command to initialize the EEPROM to sensible default values\n" - "and then the 'radio' command to configure for your radio.\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 Plane::setup_factory(uint8_t argc, const Menu::arg *argv) -{ - int c; - - cliSerial->printf("\nType 'Y' and hit Enter to perform factory reset, any other key to abort: "); - - do { - c = cliSerial->read(); - } while (-1 == c); - - if (('y' != c) && ('Y' != c)) - return(-1); - AP_Param::erase_all(); - cliSerial->printf("\nFACTORY RESET complete - please reset board to continue"); - - for (;; ) { - } - // note, cannot actually return here - return(0); -} - - -int8_t Plane::setup_erase(uint8_t argc, const Menu::arg *argv) -{ - int c; - - cliSerial->printf("\nType 'Y' and hit Enter to erase all waypoint and parameter data, any other key to abort: "); - - do { - c = cliSerial->read(); - } while (-1 == c); - - if (('y' != c) && ('Y' != c)) - return(-1); - zero_eeprom(); - return 0; -} - -void Plane::zero_eeprom(void) -{ - cliSerial->printf("\nErasing EEPROM\n"); - StorageManager::erase(); - cliSerial->printf("done\n"); -} - -#endif // CLI_ENABLED diff --git a/ArduSubsea/system.cpp b/ArduSubsea/system.cpp deleted file mode 100644 index 8718760532..0000000000 --- a/ArduSubsea/system.cpp +++ /dev/null @@ -1,779 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#include "Plane.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 Plane::main_menu_help(uint8_t argc, const Menu::arg *argv) -{ - cliSerial->printf("Commands:\n" - " logs log readback/setup mode\n" - " setup setup mode\n" - " test test mode\n" - " reboot reboot to flight mode\n" - "\n"); - return(0); -} - -// Command/function table for the top-level menu. -static 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 Plane::reboot_board(uint8_t argc, const Menu::arg *argv) -{ - hal.scheduler->reboot(false); - return 0; -} - -// the user wants the CLI. It never exits -void Plane::run_cli(AP_HAL::UARTDriver *port) -{ - // disable the failsafe code in the CLI - hal.scheduler->register_timer_failsafe(NULL,1); - - // disable the mavlink delay callback - hal.scheduler->register_delay_callback(NULL, 5); - - cliSerial = port; - Menu::set_port(port); - port->set_blocking_writes(true); - - while (1) { - main_menu.run(); - } -} - -#endif // CLI_ENABLED - - -static void mavlink_delay_cb_static() -{ - plane.mavlink_delay_cb(); -} - -static void failsafe_check_static() -{ - plane.failsafe_check(); -} - -void Plane::init_ardupilot() -{ - // initialise serial port - serial_manager.init_console(); - - cliSerial->printf("\n\nInit " FIRMWARE_STRING - "\n\nFree RAM: %u\n", - (unsigned)hal.util->available_memory()); - - - // - // Check the EEPROM format version before loading any parameters from EEPROM - // - load_parameters(); - -#if HIL_SUPPORT - if (g.hil_mode == 1) { - // set sensors to HIL mode - ins.set_hil_mode(); - compass.set_hil_mode(); - barometer.set_hil_mode(); - } -#endif - -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 - // this must be before BoardConfig.init() so if - // BRD_SAFETYENABLE==0 then we don't have safety off yet - for (uint8_t tries=0; tries<10; tries++) { - if (setup_failsafe_mixing()) { - break; - } - hal.scheduler->delay(10); - } -#endif - - BoardConfig.init(); - - // initialise serial ports - serial_manager.init(); - - // allow servo set on all channels except first 4 - ServoRelayEvents.set_channel_mask(0xFFF0); - - set_control_channels(); - - // keep a record of how many resets have happened. This can be - // used to detect in-flight resets - g.num_resets.set_and_save(g.num_resets+1); - - // init baro before we start the GCS, so that the CLI baro test works - barometer.init(); - - // initialise rangefinder - init_rangefinder(); - - // initialise battery monitoring - battery.init(); - - rpm_sensor.init(); - - // init the GCS - gcs[0].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_Console, 0); - - // we start by assuming USB connected, as we initialed the serial - // port with SERIAL0_BAUD. check_usb_mux() fixes this if need be. - usb_connected = true; - check_usb_mux(); - - // setup serial port for telem1 - 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); - - // setup frsky -#if FRSKY_TELEM_ENABLED == ENABLED - frsky_telemetry.init(serial_manager); -#endif - - mavlink_system.sysid = g.sysid_this_mav; - -#if LOGGING_ENABLED == ENABLED - log_init(); -#endif - - // initialise airspeed sensor - airspeed.init(); - - if (g.compass_enabled==true) { - bool compass_ok = compass.init() && compass.read(); -#if HIL_SUPPORT - if (!is_zero(g.hil_mode)) { - compass_ok = true; - } -#endif - if (!compass_ok) { - cliSerial->println("Compass initialisation failed!"); - g.compass_enabled = false; - } else { - ahrs.set_compass(&compass); - } - } - -#if OPTFLOW == ENABLED - // make optflow available to libraries - ahrs.set_optflow(&optflow); -#endif - - // Register mavlink_delay_cb, which will run anytime you have - // more than 5ms remaining in your call to hal.scheduler->delay - hal.scheduler->register_delay_callback(mavlink_delay_cb_static, 5); - - // give AHRS the airspeed sensor - ahrs.set_airspeed(&airspeed); - - // GPS Initialization - gps.init(&DataFlash, serial_manager); - - init_rc_in(); // sets up rc channels from radio - init_rc_out(); // sets up the timer libs - - relay.init(); - -#if MOUNT == ENABLED - // initialise camera mount - camera_mount.init(serial_manager); -#endif - -#if FENCE_TRIGGERED_PIN > 0 - hal.gpio->pinMode(FENCE_TRIGGERED_PIN, HAL_GPIO_OUTPUT); - hal.gpio->write(FENCE_TRIGGERED_PIN, 0); -#endif - - /* - * 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); - -#if CLI_ENABLED == ENABLED - if (g.cli_enabled == 1) { - 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 - - init_capabilities(); - - startup_ground(); - - // choose the nav controller - set_nav_controller(); - - set_mode((FlightMode)g.initial_mode.get()); - - // set the correct flight mode - // --------------------------- - reset_control_switch(); - - // initialise sensor -#if OPTFLOW == ENABLED - optflow.init(); -#endif - -} - -//******************************************************************************** -//This function does all the calibrations, etc. that we need during a ground start -//******************************************************************************** -void Plane::startup_ground(void) -{ - set_mode(INITIALISING); - - gcs_send_text(MAV_SEVERITY_INFO," Ground start"); - -#if (GROUND_START_DELAY > 0) - gcs_send_text(MAV_SEVERITY_NOTICE," With delay"); - delay(GROUND_START_DELAY * 1000); -#endif - - // Makes the servos wiggle - // step 1 = 1 wiggle - // ----------------------- - if (ins.gyro_calibration_timing() != AP_InertialSensor::GYRO_CAL_NEVER) { - demo_servos(1); - } - - //INS ground start - //------------------------ - // - startup_INS_ground(); - - // read the radio to set trims - // --------------------------- - if (g.trim_rc_at_start != 0) { - trim_radio(); - } - - // Save the settings for in-air restart - // ------------------------------------ - //save_EEPROM_groundstart(); - - // initialise mission library - mission.init(); - - // Makes the servos wiggle - 3 times signals ready to fly - // ----------------------- - if (ins.gyro_calibration_timing() != AP_InertialSensor::GYRO_CAL_NEVER) { - demo_servos(3); - } - - // reset last heartbeat time, so we don't trigger failsafe on slow - // startup - failsafe.last_heartbeat_ms = millis(); - - // 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); - - ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW)); - ins.set_dataflash(&DataFlash); - - gcs_send_text(MAV_SEVERITY_INFO,"Ready to fly"); -} - -enum FlightMode Plane::get_previous_mode() { - return previous_mode; -} - -void Plane::set_mode(enum FlightMode mode) -{ - if(control_mode == mode) { - // don't switch modes if we are already in the correct mode. - return; - } - if(g.auto_trim > 0 && control_mode == MANUAL) - trim_control_surfaces(); - - // perform any cleanup required for prev flight mode - exit_mode(control_mode); - - // cancel inverted flight - auto_state.inverted_flight = false; - - // don't cross-track when starting a mission - auto_state.next_wp_no_crosstrack = true; - - // reset landing check - auto_state.checked_for_autoland = false; - - // reset go around command - auto_state.commanded_go_around = false; - - // zero locked course - steer_state.locked_course_err = 0; - - // reset crash detection - crash_state.is_crashed = false; - - // set mode - previous_mode = control_mode; - control_mode = mode; - - if (previous_mode == AUTOTUNE && control_mode != AUTOTUNE) { - // restore last gains - autotune_restore(); - } - - // zero initial pitch and highest airspeed on mode change - auto_state.highest_airspeed = 0; - auto_state.initial_pitch_cd = ahrs.pitch_sensor; - - // disable taildrag takeoff on mode change - auto_state.fbwa_tdrag_takeoff_mode = false; - - // start with previous WP at current location - prev_WP_loc = current_loc; - - // new mode means new loiter - loiter.start_time_ms = 0; - - switch(control_mode) - { - case INITIALISING: - auto_throttle_mode = true; - break; - - case MANUAL: - case STABILIZE: - case TRAINING: - case FLY_BY_WIRE_A: - auto_throttle_mode = false; - break; - - case AUTOTUNE: - auto_throttle_mode = false; - autotune_start(); - break; - - case ACRO: - auto_throttle_mode = false; - acro_state.locked_roll = false; - acro_state.locked_pitch = false; - break; - - case CRUISE: - auto_throttle_mode = true; - cruise_state.locked_heading = false; - cruise_state.lock_timer_ms = 0; - set_target_altitude_current(); - break; - - case FLY_BY_WIRE_B: - auto_throttle_mode = true; - set_target_altitude_current(); - break; - - case CIRCLE: - // the altitude to circle at is taken from the current altitude - auto_throttle_mode = true; - next_WP_loc.alt = current_loc.alt; - break; - - case AUTO: - auto_throttle_mode = true; - next_WP_loc = prev_WP_loc = current_loc; - // start or resume the mission, based on MIS_AUTORESET - mission.start_or_resume(); - break; - - case RTL: - auto_throttle_mode = true; - prev_WP_loc = current_loc; - do_RTL(); - break; - - case LOITER: - auto_throttle_mode = true; - do_loiter_at_location(); - break; - - case GUIDED: - auto_throttle_mode = true; - guided_throttle_passthru = false; - /* - when entering guided mode we set the target as the current - location. This matches the behaviour of the copter code - */ - guided_WP_loc = current_loc; - set_guided_WP(); - break; - } - - // start with throttle suppressed in auto_throttle modes - throttle_suppressed = auto_throttle_mode; - - if (should_log(MASK_LOG_MODE)) - DataFlash.Log_Write_Mode(control_mode); - - // reset attitude integrators on mode change - rollController.reset_I(); - pitchController.reset_I(); - yawController.reset_I(); - steerController.reset_I(); -} - -/* - set_mode() wrapper for MAVLink SET_MODE - */ -bool Plane::mavlink_set_mode(uint8_t mode) -{ - switch (mode) { - case MANUAL: - case CIRCLE: - case STABILIZE: - case TRAINING: - case ACRO: - case FLY_BY_WIRE_A: - case AUTOTUNE: - case FLY_BY_WIRE_B: - case CRUISE: - case GUIDED: - case AUTO: - case RTL: - case LOITER: - set_mode((enum FlightMode)mode); - return true; - } - return false; -} - -// exit_mode - perform any cleanup required when leaving a flight mode -void Plane::exit_mode(enum FlightMode mode) -{ - // stop mission when we leave auto - if (mode == AUTO) { - if (mission.state() == AP_Mission::MISSION_RUNNING) { - mission.stop(); - - if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND) - { - restart_landing_sequence(); - } - } - auto_state.started_flying_in_auto_ms = 0; - } -} - -void Plane::check_long_failsafe() -{ - uint32_t tnow = millis(); - // only act on changes - // ------------------- - if(failsafe.state != FAILSAFE_LONG && failsafe.state != FAILSAFE_GCS && flight_stage != AP_SpdHgtControl::FLIGHT_LAND_FINAL && - flight_stage != AP_SpdHgtControl::FLIGHT_LAND_APPROACH) { - if (failsafe.state == FAILSAFE_SHORT && - (tnow - failsafe.ch3_timer_ms) > g.long_fs_timeout*1000) { - failsafe_long_on_event(FAILSAFE_LONG); - } else if (g.gcs_heartbeat_fs_enabled == GCS_FAILSAFE_HB_AUTO && control_mode == AUTO && - failsafe.last_heartbeat_ms != 0 && - (tnow - failsafe.last_heartbeat_ms) > g.long_fs_timeout*1000) { - failsafe_long_on_event(FAILSAFE_GCS); - } else if (g.gcs_heartbeat_fs_enabled == GCS_FAILSAFE_HEARTBEAT && - failsafe.last_heartbeat_ms != 0 && - (tnow - failsafe.last_heartbeat_ms) > g.long_fs_timeout*1000) { - failsafe_long_on_event(FAILSAFE_GCS); - } else if (g.gcs_heartbeat_fs_enabled == GCS_FAILSAFE_HB_RSSI && - gcs[0].last_radio_status_remrssi_ms != 0 && - (tnow - gcs[0].last_radio_status_remrssi_ms) > g.long_fs_timeout*1000) { - failsafe_long_on_event(FAILSAFE_GCS); - } - } else { - // We do not change state but allow for user to change mode - if (failsafe.state == FAILSAFE_GCS && - (tnow - failsafe.last_heartbeat_ms) < g.short_fs_timeout*1000) { - failsafe.state = FAILSAFE_NONE; - } else if (failsafe.state == FAILSAFE_LONG && - !failsafe.ch3_failsafe) { - failsafe.state = FAILSAFE_NONE; - } - } -} - -void Plane::check_short_failsafe() -{ - // only act on changes - // ------------------- - if(failsafe.state == FAILSAFE_NONE && (flight_stage != AP_SpdHgtControl::FLIGHT_LAND_FINAL && - flight_stage != AP_SpdHgtControl::FLIGHT_LAND_APPROACH)) { - if(failsafe.ch3_failsafe) { // The condition is checked and the flag ch3_failsafe is set in radio.pde - failsafe_short_on_event(FAILSAFE_SHORT); - } - } - - if(failsafe.state == FAILSAFE_SHORT) { - if(!failsafe.ch3_failsafe) { - failsafe_short_off_event(); - } - } -} - - -void Plane::startup_INS_ground(void) -{ -#if HIL_SUPPORT - if (g.hil_mode == 1) { - 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"); - hal.scheduler->delay(1000); - } - } -#endif - - if (ins.gyro_calibration_timing() != AP_InertialSensor::GYRO_CAL_NEVER) { - gcs_send_text(MAV_SEVERITY_ALERT, "Beginning INS calibration. Do not move plane"); - hal.scheduler->delay(100); - } - - ahrs.init(); - ahrs.set_fly_forward(true); - ahrs.set_vehicle_class(AHRS_VEHICLE_FIXED_WING); - ahrs.set_wind_estimation(true); - - ins.init(scheduler.get_loop_rate_hz()); - ahrs.reset(); - - // read Baro pressure at ground - //----------------------------- - init_barometer(); - - if (airspeed.enabled()) { - // initialize airspeed sensor - // -------------------------- - zero_airspeed(true); - } else { - gcs_send_text(MAV_SEVERITY_WARNING,"No airspeed"); - } -} - -// updates the status of the notify objects -// should be called at 50hz -void Plane::update_notify() -{ - notify.update(); -} - -void Plane::resetPerfData(void) -{ - mainLoop_count = 0; - G_Dt_max = 0; - G_Dt_min = 0; - perf_mon_timer = millis(); -} - - -void Plane::check_usb_mux(void) -{ - bool usb_check = hal.gpio->usb_connected(); - if (usb_check == usb_connected) { - return; - } - - // the user has switched to/from the telemetry port - usb_connected = usb_check; -} - - -void Plane::print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode) -{ - switch (mode) { - case MANUAL: - port->print("Manual"); - break; - case CIRCLE: - port->print("Circle"); - break; - case STABILIZE: - port->print("Stabilize"); - break; - case TRAINING: - port->print("Training"); - break; - case ACRO: - port->print("ACRO"); - break; - case FLY_BY_WIRE_A: - port->print("FBW_A"); - break; - case AUTOTUNE: - port->print("AUTOTUNE"); - break; - case FLY_BY_WIRE_B: - port->print("FBW_B"); - break; - case CRUISE: - port->print("CRUISE"); - break; - case AUTO: - port->print("AUTO"); - break; - case RTL: - port->print("RTL"); - break; - case LOITER: - port->print("Loiter"); - break; - case GUIDED: - port->print("Guided"); - break; - default: - port->printf("Mode(%u)", (unsigned)mode); - break; - } -} - -#if CLI_ENABLED == ENABLED -void Plane::print_comma(void) -{ - cliSerial->print(","); -} -#endif - -/* - write to a servo - */ -void Plane::servo_write(uint8_t ch, uint16_t pwm) -{ -#if HIL_SUPPORT - if (g.hil_mode==1 && !g.hil_servos) { - if (ch < 8) { - RC_Channel::rc_channel(ch)->radio_out = pwm; - } - return; - } -#endif - hal.rcout->enable_ch(ch); - hal.rcout->write(ch, pwm); -} - -/* - should we log a message type now? - */ -bool Plane::should_log(uint32_t mask) -{ -#if LOGGING_ENABLED == ENABLED - if (!(mask & g.log_bitmask) || in_mavlink_delay) { - return false; - } - bool ret = hal.util->get_soft_armed() || (g.log_bitmask & MASK_LOG_WHEN_DISARMED) != 0; - if (ret && !DataFlash.logging_started() && !in_log_download) { - // we have to set in_mavlink_delay to prevent logging while - // writing headers - start_logging(); - } - return ret; -#else - return false; -#endif -} - -/* - send FrSky telemetry. Should be called at 5Hz by scheduler - */ -#if FRSKY_TELEM_ENABLED == ENABLED -void Plane::frsky_telemetry_send(void) -{ - frsky_telemetry.send_frames((uint8_t)control_mode); -} -#endif - - -/* - return throttle percentage from 0 to 100 - */ -uint8_t Plane::throttle_percentage(void) -{ - // to get the real throttle we need to use norm_output() which - // returns a number from -1 to 1. - return constrain_int16(50*(channel_throttle->norm_output()+1), 0, 100); -} - -/* - update AHRS soft arm state and log as needed - */ -void Plane::change_arm_state(void) -{ - Log_Arm_Disarm(); - hal.util->set_soft_armed(arming.is_armed() && - hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED); -} - -/* - arm motors - */ -bool Plane::arm_motors(AP_Arming::ArmingMethod method) -{ - if (!arming.arm(method)) { - return false; - } - - // only log if arming was successful - channel_throttle->enable_out(); - - change_arm_state(); - return true; -} - -/* - disarm motors - */ -bool Plane::disarm_motors(void) -{ - if (!arming.disarm()) { - return false; - } - if (arming.arming_required() == AP_Arming::YES_ZERO_PWM) { - channel_throttle->disable_out(); - } - if (control_mode != AUTO) { - // reset the mission on disarm if we are not in auto - mission.reset(); - } - - // suppress the throttle in auto-throttle modes - throttle_suppressed = auto_throttle_mode; - - //only log if disarming was successful - change_arm_state(); - - return true; -} diff --git a/ArduSubsea/takeoff.cpp b/ArduSubsea/takeoff.cpp deleted file mode 100644 index 87beaf6e67..0000000000 --- a/ArduSubsea/takeoff.cpp +++ /dev/null @@ -1,187 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#include "Plane.h" - -/* Check for automatic takeoff conditions being met using the following sequence: - * 1) Check for adequate GPS lock - if not return false - * 2) Check the gravity compensated longitudinal acceleration against the threshold and start the timer if true - * 3) Wait until the timer has reached the specified value (increments of 0.1 sec) and then check the GPS speed against the threshold - * 4) If the GPS speed is above the threshold and the attitude is within limits then return true and reset the timer - * 5) If the GPS speed and attitude within limits has not been achieved after 2.5 seconds, return false and reset the timer - * 6) If the time lapsed since the last timecheck is greater than 0.2 seconds, return false and reset the timer - * NOTE : This function relies on the TECS 50Hz processing for its acceleration measure. - */ -bool Plane::auto_takeoff_check(void) -{ - // this is a more advanced check that relies on TECS - uint32_t now = millis(); - static bool launchTimerStarted; - static uint32_t last_tkoff_arm_time; - static uint32_t last_check_ms; - uint16_t wait_time_ms = MIN(uint16_t(g.takeoff_throttle_delay)*100,12700); - - // Reset states if process has been interrupted - if (last_check_ms && (now - last_check_ms) > 200) { - gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Timer interrupted AUTO"); - launchTimerStarted = false; - last_tkoff_arm_time = 0; - last_check_ms = now; - return false; - } - - last_check_ms = now; - - // Check for bad GPS - if (gps.status() < AP_GPS::GPS_OK_FIX_3D) { - // no auto takeoff without GPS lock - return false; - } - - // Check for launch acceleration or timer started. NOTE: relies on TECS 50Hz processing - if (!launchTimerStarted && - !is_zero(g.takeoff_throttle_min_accel) && - SpdHgt_Controller->get_VXdot() < g.takeoff_throttle_min_accel) { - goto no_launch; - } - - // we've reached the acceleration threshold, so start the timer - if (!launchTimerStarted) { - launchTimerStarted = true; - last_tkoff_arm_time = now; - gcs_send_text_fmt(MAV_SEVERITY_INFO, "Armed AUTO, xaccel = %.1f m/s/s, waiting %.1f sec", - (double)SpdHgt_Controller->get_VXdot(), (double)(wait_time_ms*0.001f)); - } - - // Only perform velocity check if not timed out - if ((now - last_tkoff_arm_time) > wait_time_ms+100U) { - gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Timeout AUTO"); - goto no_launch; - } - - // Check aircraft attitude for bad launch - if (ahrs.pitch_sensor <= -3000 || - ahrs.pitch_sensor >= 4500 || - labs(ahrs.roll_sensor) > 3000) { - gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Bad launch AUTO"); - goto no_launch; - } - - // Check ground speed and time delay - if (((gps.ground_speed() > g.takeoff_throttle_min_speed || is_zero(g.takeoff_throttle_min_speed))) && - ((now - last_tkoff_arm_time) >= wait_time_ms)) { - gcs_send_text_fmt(MAV_SEVERITY_INFO, "Triggered AUTO. GPS speed = %.1f", (double)gps.ground_speed()); - launchTimerStarted = false; - last_tkoff_arm_time = 0; - return true; - } - - // we're not launching yet, but the timer is still going - return false; - -no_launch: - launchTimerStarted = false; - last_tkoff_arm_time = 0; - return false; -} - -/* - calculate desired bank angle during takeoff, setting nav_roll_cd - */ -void Plane::takeoff_calc_roll(void) -{ - if (steer_state.hold_course_cd == -1) { - // we don't yet have a heading to hold - just level - // the wings until we get up enough speed to get a GPS heading - nav_roll_cd = 0; - return; - } - - calc_nav_roll(); - - // during takeoff use the level flight roll limit to prevent large - // wing strike. Slowly allow for more roll as we get higher above - // the takeoff altitude - float roll_limit = roll_limit_cd*0.01f; - float baro_alt = barometer.get_altitude(); - // below 5m use the LEVEL_ROLL_LIMIT - const float lim1 = 5; - // at 15m allow for full roll - const float lim2 = 15; - if (baro_alt < auto_state.baro_takeoff_alt+lim1) { - roll_limit = g.level_roll_limit; - } else if (baro_alt < auto_state.baro_takeoff_alt+lim2) { - float proportion = (baro_alt - (auto_state.baro_takeoff_alt+lim1)) / (lim2 - lim1); - roll_limit = (1-proportion) * g.level_roll_limit + proportion * roll_limit; - } - nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit*100UL, roll_limit*100UL); -} - - -/* - calculate desired pitch angle during takeoff, setting nav_pitch_cd - */ -void Plane::takeoff_calc_pitch(void) -{ - if (auto_state.highest_airspeed < g.takeoff_rotate_speed) { - // we have not reached rotate speed, use a target pitch of 5 - // degrees. This should be enough to get the tail off the - // ground, while making it unlikely that overshoot in the - // pitch controller will cause a prop strike - nav_pitch_cd = 500; - return; - } - - if (ahrs.airspeed_sensor_enabled()) { - calc_nav_pitch(); - if (nav_pitch_cd < auto_state.takeoff_pitch_cd) { - nav_pitch_cd = auto_state.takeoff_pitch_cd; - } - } else { - nav_pitch_cd = ((gps.ground_speed()*100) / (float)g.airspeed_cruise_cm) * auto_state.takeoff_pitch_cd; - nav_pitch_cd = constrain_int32(nav_pitch_cd, 500, auto_state.takeoff_pitch_cd); - } -} - -/* - return a tail hold percentage during initial takeoff for a tail - dragger - - This can be used either in auto-takeoff or in FBWA mode with - FBWA_TDRAG_CHAN enabled - */ -int8_t Plane::takeoff_tail_hold(void) -{ - bool in_takeoff = ((control_mode == AUTO && !auto_state.takeoff_complete) || - (control_mode == FLY_BY_WIRE_A && auto_state.fbwa_tdrag_takeoff_mode)); - if (!in_takeoff) { - // not in takeoff - return 0; - } - if (g.takeoff_tdrag_elevator == 0) { - // no takeoff elevator set - goto return_zero; - } - if (auto_state.highest_airspeed >= g.takeoff_tdrag_speed1) { - // we've passed speed1. We now raise the tail and aim for - // level pitch. Return 0 meaning no fixed elevator setting - goto return_zero; - } - if (ahrs.pitch_sensor > auto_state.initial_pitch_cd + 1000) { - // the pitch has gone up by more then 10 degrees over the - // initial pitch. This may mean the nose is coming up for an - // early liftoff, perhaps due to a bad setting of - // g.takeoff_tdrag_speed1. Go to level flight to prevent a - // stall - goto return_zero; - } - // we are holding the tail down - return g.takeoff_tdrag_elevator; - -return_zero: - if (auto_state.fbwa_tdrag_takeoff_mode) { - gcs_send_text(MAV_SEVERITY_NOTICE, "FBWA tdrag off"); - auto_state.fbwa_tdrag_takeoff_mode = false; - } - return 0; -} - diff --git a/ArduSubsea/test.cpp b/ArduSubsea/test.cpp deleted file mode 100644 index 05ea80c1a8..0000000000 --- a/ArduSubsea/test.cpp +++ /dev/null @@ -1,531 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#include "Plane.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_Common for implementation details -static const struct Menu::command test_menu_commands[] = { - {"pwm", MENU_FUNC(test_radio_pwm)}, - {"radio", MENU_FUNC(test_radio)}, - {"passthru", MENU_FUNC(test_passthru)}, - {"failsafe", MENU_FUNC(test_failsafe)}, - {"relay", MENU_FUNC(test_relay)}, - {"waypoints", MENU_FUNC(test_wp)}, - {"xbee", MENU_FUNC(test_xbee)}, - {"modeswitch", MENU_FUNC(test_modeswitch)}, - - // Tests below here are for hardware sensors only present - // when real sensors are attached or they are emulated - {"gps", MENU_FUNC(test_gps)}, - {"ins", MENU_FUNC(test_ins)}, - {"airspeed", MENU_FUNC(test_airspeed)}, - {"airpressure", MENU_FUNC(test_pressure)}, - {"compass", MENU_FUNC(test_mag)}, - {"logging", MENU_FUNC(test_logging)}, -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN - {"shell", MENU_FUNC(test_shell)}, -#endif - -}; - -// A Macro to create the Menu -MENU(test_menu, "test", test_menu_commands); - -int8_t Plane::test_mode(uint8_t argc, const Menu::arg *argv) -{ - cliSerial->printf("Test Mode\n\n"); - test_menu.run(); - return 0; -} - -void Plane::print_hit_enter() -{ - cliSerial->printf("Hit Enter to exit.\n\n"); -} - -int8_t Plane::test_radio_pwm(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - hal.scheduler->delay(1000); - - while(1) { - hal.scheduler->delay(20); - - // Filters radio input - adjust filters in the radio.pde file - // ---------------------------------------------------------- - read_radio(); - - cliSerial->printf("IN:\t1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n", - (int)channel_roll->radio_in, - (int)channel_pitch->radio_in, - (int)channel_throttle->radio_in, - (int)channel_rudder->radio_in, - (int)g.rc_5.radio_in, - (int)g.rc_6.radio_in, - (int)g.rc_7.radio_in, - (int)g.rc_8.radio_in); - - if(cliSerial->available() > 0) { - return (0); - } - } -} - - -int8_t Plane::test_passthru(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - hal.scheduler->delay(1000); - - while(1) { - hal.scheduler->delay(20); - - // New radio frame? (we could use also if((millis()- timer) > 20) - if (hal.rcin->new_input()) { - cliSerial->print("CH:"); - for(int16_t i = 0; i < 8; i++) { - cliSerial->print(hal.rcin->read(i)); // Print channel values - print_comma(); - servo_write(i, hal.rcin->read(i)); // Copy input to Servos - } - cliSerial->println(); - } - if (cliSerial->available() > 0) { - return (0); - } - } - return 0; -} - -int8_t Plane::test_radio(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - hal.scheduler->delay(1000); - - // read the radio to set trims - // --------------------------- - trim_radio(); - - while(1) { - hal.scheduler->delay(20); - read_radio(); - - channel_roll->calc_pwm(); - channel_pitch->calc_pwm(); - channel_throttle->calc_pwm(); - channel_rudder->calc_pwm(); - - // write out the servo PWM values - // ------------------------------ - set_servos(); - - cliSerial->printf("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n", - (int)channel_roll->control_in, - (int)channel_pitch->control_in, - (int)channel_throttle->control_in, - (int)channel_rudder->control_in, - (int)g.rc_5.control_in, - (int)g.rc_6.control_in, - (int)g.rc_7.control_in, - (int)g.rc_8.control_in); - - if(cliSerial->available() > 0) { - return (0); - } - } -} - -int8_t Plane::test_failsafe(uint8_t argc, const Menu::arg *argv) -{ - uint8_t fail_test; - print_hit_enter(); - for(int16_t i = 0; i < 50; i++) { - hal.scheduler->delay(20); - read_radio(); - } - - // read the radio to set trims - // --------------------------- - trim_radio(); - - oldSwitchPosition = readSwitch(); - - cliSerial->printf("Unplug battery, throttle in neutral, turn off radio.\n"); - while(channel_throttle->control_in > 0) { - hal.scheduler->delay(20); - read_radio(); - } - - while(1) { - hal.scheduler->delay(20); - read_radio(); - - if(channel_throttle->control_in > 0) { - cliSerial->printf("THROTTLE CHANGED %d \n", (int)channel_throttle->control_in); - fail_test++; - } - - if(oldSwitchPosition != readSwitch()) { - cliSerial->printf("CONTROL MODE CHANGED: "); - print_flight_mode(cliSerial, readSwitch()); - cliSerial->println(); - fail_test++; - } - - if(rc_failsafe_active()) { - cliSerial->printf("THROTTLE FAILSAFE ACTIVATED: %d, ", (int)channel_throttle->radio_in); - print_flight_mode(cliSerial, readSwitch()); - cliSerial->println(); - fail_test++; - } - - if(fail_test > 0) { - return (0); - } - if(cliSerial->available() > 0) { - cliSerial->printf("LOS caused no change in APM.\n"); - return (0); - } - } -} - -int8_t Plane::test_relay(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - hal.scheduler->delay(1000); - - while(1) { - cliSerial->printf("Relay on\n"); - relay.on(0); - hal.scheduler->delay(3000); - if(cliSerial->available() > 0) { - return (0); - } - - cliSerial->printf("Relay off\n"); - relay.off(0); - hal.scheduler->delay(3000); - if(cliSerial->available() > 0) { - return (0); - } - } -} - -int8_t Plane::test_wp(uint8_t argc, const Menu::arg *argv) -{ - hal.scheduler->delay(1000); - - // save the alitude above home option - if (g.RTL_altitude_cm < 0) { - cliSerial->printf("Hold current altitude\n"); - }else{ - cliSerial->printf("Hold altitude of %dm\n", (int)g.RTL_altitude_cm/100); - } - - cliSerial->printf("%d waypoints\n", (int)mission.num_commands()); - cliSerial->printf("Hit radius: %d\n", (int)g.waypoint_radius); - cliSerial->printf("Loiter radius: %d\n\n", (int)g.loiter_radius); - - for(uint8_t i = 0; i <= mission.num_commands(); i++) { - AP_Mission::Mission_Command temp_cmd; - if (mission.read_cmd_from_storage(i,temp_cmd)) { - test_wp_print(temp_cmd); - } - } - - return (0); -} - -void Plane::test_wp_print(const AP_Mission::Mission_Command& cmd) -{ - cliSerial->printf("command #: %d id:%d options:%d p1:%d p2:%ld p3:%ld p4:%ld \n", - (int)cmd.index, - (int)cmd.id, - (int)cmd.content.location.options, - (int)cmd.p1, - (long)cmd.content.location.alt, - (long)cmd.content.location.lat, - (long)cmd.content.location.lng); -} - -int8_t Plane::test_xbee(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - hal.scheduler->delay(1000); - cliSerial->printf("Begin XBee X-CTU Range and RSSI Test:\n"); - - while(1) { - - if (hal.uartC->available()) - hal.uartC->write(hal.uartC->read()); - - if(cliSerial->available() > 0) { - return (0); - } - } -} - - -int8_t Plane::test_modeswitch(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - hal.scheduler->delay(1000); - - cliSerial->printf("Control CH "); - - cliSerial->println(FLIGHT_MODE_CHANNEL, BASE_DEC); - - while(1) { - hal.scheduler->delay(20); - uint8_t switchPosition = readSwitch(); - if (oldSwitchPosition != switchPosition) { - cliSerial->printf("Position %d\n", (int)switchPosition); - oldSwitchPosition = switchPosition; - } - if(cliSerial->available() > 0) { - return (0); - } - } -} - -/* - * test the dataflash is working - */ -int8_t Plane::test_logging(uint8_t argc, const Menu::arg *argv) -{ - DataFlash.ShowDeviceInfo(cliSerial); - return 0; -} - -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN -/* - * run a debug shell - */ -int8_t Plane::test_shell(uint8_t argc, const Menu::arg *argv) -{ - hal.util->run_debug_shell(cliSerial); - return 0; -} -#endif - -//------------------------------------------------------------------------------------------- -// tests in this section are for real sensors or sensors that have been simulated - -int8_t Plane::test_gps(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - hal.scheduler->delay(1000); - - uint32_t last_message_time_ms = 0; - while(1) { - hal.scheduler->delay(100); - - gps.update(); - - if (gps.last_message_time_ms() != last_message_time_ms) { - last_message_time_ms = gps.last_message_time_ms(); - const Location &loc = gps.location(); - cliSerial->printf("Lat: %ld, Lon %ld, Alt: %ldm, #sats: %d\n", - (long)loc.lat, - (long)loc.lng, - (long)loc.alt/100, - (int)gps.num_sats()); - } else { - cliSerial->printf("."); - } - if(cliSerial->available() > 0) { - return (0); - } - } -} - -int8_t Plane::test_ins(uint8_t argc, const Menu::arg *argv) -{ - //cliSerial->printf("Calibrating."); - ahrs.init(); - ahrs.set_fly_forward(true); - ahrs.set_wind_estimation(true); - - ins.init(scheduler.get_loop_rate_hz()); - ahrs.reset(); - - print_hit_enter(); - hal.scheduler->delay(1000); - - uint8_t counter = 0; - - while(1) { - hal.scheduler->delay(20); - if (micros() - fast_loopTimer_us > 19000UL) { - fast_loopTimer_us = micros(); - - // INS - // --- - ahrs.update(); - - if(g.compass_enabled) { - counter++; - if(counter == 5) { - compass.read(); - counter = 0; - } - } - - // We are using the INS - // --------------------- - Vector3f gyros = ins.get_gyro(); - Vector3f accels = ins.get_accel(); - cliSerial->printf("r:%4d p:%4d y:%3d g=(%5.1f %5.1f %5.1f) a=(%5.1f %5.1f %5.1f)\n", - (int)ahrs.roll_sensor / 100, - (int)ahrs.pitch_sensor / 100, - (uint16_t)ahrs.yaw_sensor / 100, - (double)gyros.x, (double)gyros.y, (double)gyros.z, - (double)accels.x, (double)accels.y, (double)accels.z); - } - if(cliSerial->available() > 0) { - return (0); - } - } -} - - -int8_t Plane::test_mag(uint8_t argc, const Menu::arg *argv) -{ - 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_wind_estimation(true); - ahrs.set_compass(&compass); - - // we need the AHRS initialised for this test - ins.init(scheduler.get_loop_rate_hz()); - ahrs.reset(); - - uint16_t counter = 0; - float heading = 0; - - print_hit_enter(); - - while(1) { - hal.scheduler->delay(20); - if (micros() - fast_loopTimer_us > 19000UL) { - fast_loopTimer_us = micros(); - - // INS - // --- - ahrs.update(); - - if(counter % 5 == 0) { - if (compass.read()) { - // Calculate heading - const Matrix3f &m = ahrs.get_rotation_body_to_ned(); - heading = compass.calculate_heading(m); - compass.learn_offsets(); - } - } - - 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", - (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); -} - -//------------------------------------------------------------------------------------------- -// real sensors that have not been simulated yet go here - -int8_t Plane::test_airspeed(uint8_t argc, const Menu::arg *argv) -{ - if (!airspeed.enabled()) { - cliSerial->printf("airspeed: "); - print_enabled(false); - return (0); - }else{ - print_hit_enter(); - zero_airspeed(false); - cliSerial->printf("airspeed: "); - print_enabled(true); - - while(1) { - hal.scheduler->delay(20); - read_airspeed(); - cliSerial->printf("%.1f m/s\n", (double)airspeed.get_airspeed()); - - if(cliSerial->available() > 0) { - return (0); - } - } - } -} - - -int8_t Plane::test_pressure(uint8_t argc, const Menu::arg *argv) -{ - cliSerial->printf("Uncalibrated relative airpressure\n"); - print_hit_enter(); - - init_barometer(); - - while(1) { - hal.scheduler->delay(100); - barometer.update(); - - if (!barometer.healthy()) { - cliSerial->println("not healthy"); - } else { - cliSerial->printf("Alt: %0.2fm, Raw: %f Temperature: %.1f\n", - (double)barometer.get_altitude(), - (double)barometer.get_pressure(), - (double)barometer.get_temperature()); - } - - if(cliSerial->available() > 0) { - return (0); - } - } -} - -void Plane::print_enabled(bool b) -{ - if (b) { - cliSerial->printf("en"); - } else { - cliSerial->printf("dis"); - } - cliSerial->printf("abled\n"); -} - -#endif // CLI_ENABLED diff --git a/ArduSubsea/wscript b/ArduSubsea/wscript deleted file mode 100644 index f6e0b63e00..0000000000 --- a/ArduSubsea/wscript +++ /dev/null @@ -1,37 +0,0 @@ -#!/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 + [ - 'APM_Control', - 'APM_OBC', - 'AP_ADSB', - 'AP_Arming', - 'AP_Camera', - 'AP_Frsky_Telem', - 'AP_L1_Control', - 'AP_Menu', - 'AP_Mount', - 'AP_Navigation', - 'AP_Parachute', - 'AP_RCMapper', - 'AP_RPM', - 'AP_RSSI', - 'AP_Relay', - 'AP_ServoRelayEvents', - 'AP_SpdHgtControl', - 'AP_TECS', - ], - ) - - ardupilotwaf.program( - bld, - use=vehicle + '_libs', - ) diff --git a/modules/gbenchmark b/modules/gbenchmark index 006d23ccca..cd525ae85d 160000 --- a/modules/gbenchmark +++ b/modules/gbenchmark @@ -1 +1 @@ -Subproject commit 006d23ccca1375a973b7fae0cc351cedb41b812a +Subproject commit cd525ae85d4a46ecb2e3bdbdd1df101e48c5195e