aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Rebased.

Browse Source
This commit is contained in:
Rustom Jehangir 2016-01-08 21:57:01 -08:00
parent bbca4f723e
commit db3666c284
43 changed files with 5 additions and 16844 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
ArduSub/motors.cpp
View File

@ -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

10
ArduSubsea/APM_Config.h
View File

@ -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.

949
ArduSubsea/APM_Config.h.reference
View File

@ -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.

907
ArduSubsea/ArduPlane.cpp
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
#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<gps.num_sensors(); i++) {
if (gps.last_message_time_ms(i) != last_gps_reading[i]) {
last_gps_reading[i] = gps.last_message_time_ms(i);
if (should_log(MASK_LOG_GPS)) {
Log_Write_GPS(i);
}
}
}
}
/*
read update GPS position - 10Hz update
*/
void Plane::update_GPS_10Hz(void)
{
// get position from AHRS
have_position = ahrs.get_position(current_loc);
static uint32_t last_gps_msg_ms;
if (gps.last_message_time_ms() != last_gps_msg_ms && gps.status() >= 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);

1136
ArduSubsea/Attitude.cpp
View File

File diff suppressed because it is too large Load Diff

2061
ArduSubsea/GCS_Mavlink.cpp
View File

File diff suppressed because it is too large Load Diff

595
ArduSubsea/Log.cpp
View File

@ -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<num_gcs; i++) {
gcs[i].reset_cli_timeout();
}
}
arming.set_logging_available(DataFlash.CardInserted());
}
#else // LOGGING_ENABLED
#if CLI_ENABLED == ENABLED
bool Plane::print_log_menu(void) { return true; }
int8_t Plane::dump_log(uint8_t argc, const Menu::arg *argv) { return 0; }
int8_t Plane::erase_logs(uint8_t argc, const Menu::arg *argv) { return 0; }
int8_t Plane::select_logs(uint8_t argc, const Menu::arg *argv) { return 0; }
int8_t Plane::process_logs(uint8_t argc, const Menu::arg *argv) { return 0; }
#endif // CLI_ENABLED == ENABLED
void Plane::do_erase_logs(void) {}
void Plane::Log_Write_Attitude(void) {}
void Plane::Log_Write_Performance() {}
void Plane::Log_Write_Startup(uint8_t type) {}
void Plane::Log_Write_Control_Tuning() {}
void Plane::Log_Write_TECS_Tuning(void) {}
void Plane::Log_Write_Nav_Tuning() {}
void Plane::Log_Write_Status() {}
void Plane::Log_Write_Sonar() {}
#if OPTFLOW == ENABLED
void Plane::Log_Write_Optflow() {}
#endif
void Plane::Log_Write_Current() {}
void Plane::Log_Arm_Disarm() {}
void Plane::Log_Write_GPS(uint8_t instance) {}
void Plane::Log_Write_IMU() {}
void Plane::Log_Write_RC(void) {}
void Plane::Log_Write_Baro(void) {}
void Plane::Log_Write_Airspeed(void) {}
void Plane::Log_Write_Home_And_Origin() {}
#if CLI_ENABLED == ENABLED
void Plane::Log_Read(uint16_t log_num, int16_t start_page, int16_t end_page) {}
#endif // CLI_ENABLED
void Plane::start_logging() {}
void Plane::log_init(void) {}
#endif // LOGGING_ENABLED

2
ArduSubsea/Makefile
View File

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

2
ArduSubsea/Makefile.waf
View File

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

1282
ArduSubsea/Parameters.cpp
View File

File diff suppressed because it is too large Load Diff

541
ArduSubsea/Parameters.h
View File

@ -1,541 +0,0 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#ifndef PARAMETERS_H
#define PARAMETERS_H
#include <AP_Common/AP_Common.h>
// Global parameter class.
//
class Parameters {
public:
/*
* The 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

3
ArduSubsea/Parameters.pde
View File

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

33
ArduSubsea/Plane.cpp
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
/*
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;

1033
ArduSubsea/Plane.h
View File

File diff suppressed because it is too large Load Diff

101
ArduSubsea/adsb.cpp
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
#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
}

621
ArduSubsea/altitude.cpp
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
#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

91
ArduSubsea/arming_checks.cpp
View File

@ -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;
}

9
ArduSubsea/capabilities.cpp
View File

@ -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);
}

133
ArduSubsea/commands.cpp
View File

@ -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();
}

996
ArduSubsea/commands_logic.cpp
View File

@ -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);
}
}

20
ArduSubsea/commands_process.cpp
View File

@ -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);
}
}
}
}

432
ArduSubsea/config.h
View File

@ -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

164
ArduSubsea/control_modes.cpp
View File

@ -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;
}

67
ArduSubsea/createTags
View File

@ -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 <C-F12> :!ctags -R --sort=yes --c++-kinds=+p --fields=+iaS --extra=+q .<CR>
#" 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

193
ArduSubsea/defines.h
View File

@ -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

130
ArduSubsea/events.cpp
View File

@ -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();
}

111
ArduSubsea/failsafe.cpp
View File

@ -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);
}
}

487
ArduSubsea/geofence.cpp
View File

@ -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; i<g.fence_total; i++) {
geofence_state->boundary[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

245
ArduSubsea/is_flying.cpp
View File

@ -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");
}
}
}
}

335
ArduSubsea/landing.cpp
View File

@ -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;
}

52
ArduSubsea/make.inc
View File

@ -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

238
ArduSubsea/navigation.cpp
View File

@ -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;
}
}

58
ArduSubsea/parachute.cpp
View File

@ -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;
}

391
ArduSubsea/px4_mixer.cpp
View File

@ -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 <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <unistd.h>
#include <stdio.h>
#include <drivers/drv_pwm_output.h>
#include <systemlib/mixer/mixer.h>
#include <modules/px4iofirmware/protocol.h>
#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

347
ArduSubsea/radio.cpp
View File

@ -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;
}

1294
ArduSubsea/release-notes.txt
View File

File diff suppressed because it is too large Load Diff

150
ArduSubsea/sensors.cpp
View File

@ -1,150 +0,0 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Plane.h"
#include <AP_RSSI/AP_RSSI.h>
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);
}
}
}

81
ArduSubsea/setup.cpp
View File

@ -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

779
ArduSubsea/system.cpp
View File

@ -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,"<startup_ground> Ground start");
#if (GROUND_START_DELAY > 0)
gcs_send_text(MAV_SEVERITY_NOTICE,"<startup_ground> 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;
}

187
ArduSubsea/takeoff.cpp
View File

@ -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;
}

531
ArduSubsea/test.cpp
View File

@ -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

37
ArduSubsea/wscript
View File

@ -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',
)

2
modules/gbenchmark

@ -1 +1 @@
Subproject commit 006d23ccca1375a973b7fae0cc351cedb41b812a
Subproject commit cd525ae85d4a46ecb2e3bdbdd1df101e48c5195e