From 247e7ff399a772393beab9deee83375f3de48c87 Mon Sep 17 00:00:00 2001
From: Jean-Louis Naudin <jlnaudin@gmail.com>
Date: Mon, 30 Apr 2012 09:17:14 +0200
Subject: [PATCH] APMrover v2.0 - tested on Traxxas Monster Jam Grinder XL-5
3602
Signed-off-by: Jean-Louis Naudin <jlnaudin@gmail.com>
---
APMrover2/APM_Config.h | 47 +
APMrover2/APM_Config.h.org | 41 +
APMrover2/APM_Config.h.reference | 954 ++++++++++++
APMrover2/APM_Config_HILmode.h | 439 ++++++
APMrover2/APM_Config_Rover.h | 430 ++++++
APMrover2/APM_Config_mavlink_hil.h | 31 +
APMrover2/APMrover2.pde | 1135 +++++++++++++++
APMrover2/Attitude.pde | 208 +++
APMrover2/GCS.h | 178 +++
APMrover2/GCS_Mavlink.pde | 2181 ++++++++++++++++++++++++++++
APMrover2/Log.pde | 696 +++++++++
APMrover2/Makefile | 35 +
APMrover2/Parameters.h | 510 +++++++
APMrover2/Parameters.pde | 174 +++
APMrover2/command_description.txt | 85 ++
APMrover2/commands.pde | 284 ++++
APMrover2/commands_logic.pde | 519 +++++++
APMrover2/commands_process.pde | 146 ++
APMrover2/config.h | 873 +++++++++++
APMrover2/control_modes.pde | 147 ++
APMrover2/createTags | 68 +
APMrover2/defines.h | 264 ++++
APMrover2/events.pde | 114 ++
APMrover2/failsafe.pde | 46 +
APMrover2/geofence.pde | 325 +++++
APMrover2/navigation.pde | 195 +++
APMrover2/planner.pde | 56 +
APMrover2/radio.pde | 263 ++++
APMrover2/sensors.pde | 106 ++
APMrover2/setup.pde | 604 ++++++++
APMrover2/system.pde | 583 ++++++++
APMrover2/test.pde | 695 +++++++++
32 files changed, 12432 insertions(+)
create mode 100644 APMrover2/APM_Config.h
create mode 100644 APMrover2/APM_Config.h.org
create mode 100644 APMrover2/APM_Config.h.reference
create mode 100644 APMrover2/APM_Config_HILmode.h
create mode 100644 APMrover2/APM_Config_Rover.h
create mode 100644 APMrover2/APM_Config_mavlink_hil.h
create mode 100644 APMrover2/APMrover2.pde
create mode 100644 APMrover2/Attitude.pde
create mode 100644 APMrover2/GCS.h
create mode 100644 APMrover2/GCS_Mavlink.pde
create mode 100644 APMrover2/Log.pde
create mode 100644 APMrover2/Makefile
create mode 100644 APMrover2/Parameters.h
create mode 100644 APMrover2/Parameters.pde
create mode 100644 APMrover2/command_description.txt
create mode 100644 APMrover2/commands.pde
create mode 100644 APMrover2/commands_logic.pde
create mode 100644 APMrover2/commands_process.pde
create mode 100644 APMrover2/config.h
create mode 100644 APMrover2/control_modes.pde
create mode 100644 APMrover2/createTags
create mode 100644 APMrover2/defines.h
create mode 100644 APMrover2/events.pde
create mode 100644 APMrover2/failsafe.pde
create mode 100644 APMrover2/geofence.pde
create mode 100644 APMrover2/navigation.pde
create mode 100644 APMrover2/planner.pde
create mode 100644 APMrover2/radio.pde
create mode 100644 APMrover2/sensors.pde
create mode 100644 APMrover2/setup.pde
create mode 100644 APMrover2/system.pde
create mode 100644 APMrover2/test.pde
diff --git a/APMrover2/APM_Config.h b/APMrover2/APM_Config.h
new file mode 100644
index 000000000..3a73abca0
--- /dev/null
+++ b/APMrover2/APM_Config.h
@@ -0,0 +1,47 @@
+
+ // -*- 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.
+
+// For example if you wanted the Port 3 baud rate to be 38400 you would add a statement like the one below (uncommented)
+//#define SERIAL3_BAUD 38400
+//#define GCS_PROTOCOL GCS_PROTOCOL_NONE
+
+#define CONFIG_APM_HARDWARE APM_HARDWARE_APM1
+
+#define CLI_ENABLED ENABLED
+#define CLI_SLIDER_ENABLED DISABLED
+#define CLOSED_LOOP_NAV ENABLED
+#define AUTO_WP_RADIUS ENABLED
+
+//#include "APM_Config_HILmode.h" // for test in HIL mode with AeroSIM Rc 3.83
+#include "APM_Config_Rover.h" // to be used with the real Traxxas model Monster Jam Grinder
+
+// Radio setup:
+// APM INPUT (Rec = receiver)
+// Rec ch1: Roll
+// Rec ch2: Throttle
+// Rec ch3: Pitch
+// Rec ch4: Yaw
+// Rec ch5: not used
+// Rec ch6:
+// Rec ch7: Option channel to 2 positions switch
+// Rec ch8: Mode channel to 3 positions switch
+// APM OUTPUT
+// Ch1: Wheel servo (direction)
+// Ch2: not used
+// Ch3: to the motor ESC
+// Ch4: not used
+
+// The following are the recommended settings for Xplane simulation. Remove the leading "/* and trailing "*/" to enable:
+
+/*
+#define HIL_PROTOCOL HIL_PROTOCOL_MAVLINK
+#define HIL_MODE HIL_MODE_ATTITUDE
+#define HIL_PORT 0
+#define GCS_PROTOCOL GCS_PROTOCOL_MAVLINK
+#define GCS_PORT 3
+*/
+
+
diff --git a/APMrover2/APM_Config.h.org b/APMrover2/APM_Config.h.org
new file mode 100644
index 000000000..0b40f0e88
--- /dev/null
+++ b/APMrover2/APM_Config.h.org
@@ -0,0 +1,41 @@
+// -*- 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.
+
+// For example if you wanted the Port 3 baud rate to be 38400 you would add a statement like the one below (uncommented)
+//#define SERIAL3_BAUD 38400
+
+// # define CONFIG_APM_HARDWARE APM_HARDWARE_APM2
+// # define APM2_BETA_HARDWARE
+
+
+// You may also put an include statement here to point at another configuration file. This is convenient if you maintain
+// different configuration files for different aircraft or HIL simulation. See the examples below
+//#include "APM_Config_mavlink_hil.h"
+//#include "Skywalker.h"
+
+// The following are the recommended settings for Xplane simulation. Remove the leading "/* and trailing "*/" to enable:
+
+/*
+#define HIL_MODE HIL_MODE_ATTITUDE
+*/
+
+/*
+// HIL_MODE SELECTION
+//
+// Mavlink supports
+// 1. HIL_MODE_ATTITUDE : simulated position, airspeed, and attitude
+// 2. HIL_MODE_SENSORS: full sensor simulation
+#define HIL_MODE HIL_MODE_ATTITUDE
+
+// Sensors
+// All sensors are supported in all modes.
+// The magnetometer is not used in
+// HIL_MODE_ATTITUDE but you may leave it
+// enabled if you wish.
+#define AIRSPEED_SENSOR ENABLED
+#define MAGNETOMETER ENABLED
+#define AIRSPEED_CRUISE 25
+#define THROTTLE_FAILSAFE ENABLED
+*/
diff --git a/APMrover2/APM_Config.h.reference b/APMrover2/APM_Config.h.reference
new file mode 100644
index 000000000..fed8532b2
--- /dev/null
+++ b/APMrover2/APM_Config.h.reference
@@ -0,0 +1,954 @@
+//
+// 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_MTK16 MediaTek-based GPS running the DIYDrones firmware 1.6
+// 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
+// MAG_ORIENTATION OPTIONAL
+//
+// Set MAGNETOMETER to ENABLED if you have a magnetometer attached.
+// Set MAG_ORIENTATION to reflect the orientation you have the magnetometer mounted with respect to ArduPilotMega
+//
+// The default assumes that a magnetometer is not connected.
+//
+//#define MAGNETOMETER DISABLED
+//#define MAG_ORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD
+//
+
+//////////////////////////////////////////////////////////////////////////////
+// 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_ATTITUDE Simulator provides attitude and position information.
+// HIL_MODE_SENSORS Simulator provides raw sensor values.
+//
+// Note that currently HIL_PROTOCOL_XPLANE requires HIL_MODE_ATTITUDE.
+// 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 IMU
+// calibration during a ground start.
+//
+// Use this setting to give you time to position the aircraft horizontally
+// for the IMU calibration.
+//
+// The default is to begin IMU 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 IMU
+// 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 IMU performance monitoring info every 20 seconds.
+// Defaults to DISABLED.
+//
+// LOG_CTUN OPTIONAL
+//
+// Logs control loop tuning info at 10 Hz. This information is useful for tuning
+// servo control loop gain values. Defaults to DISABLED.
+//
+// LOG_NTUN OPTIONAL
+//
+// Logs navigation tuning info at 10 Hz. This information is useful for tuning
+// navigation control loop gain values. Defaults to DISABLED.
+//
+// LOG_ MODE OPTIONAL
+//
+// Logs changes to the flight mode upon occurrence. Defaults to ENABLED.
+//
+// LOG_RAW DEBUG
+//
+// Logs raw accelerometer and gyro data at 50 Hz (uses more space).
+// Defaults to DISABLED.
+//
+// LOG_CMD OPTIONAL
+//
+// Logs new commands when they process.
+// Defaults to ENABLED.
+//
+//#define LOG_ATTITUDE_FAST DISABLED
+//#define LOG_ATTITUDE_MED ENABLED
+//#define LOG_GPS ENABLED
+//#define LOG_PM ENABLED
+//#define LOG_CTUN DISABLED
+//#define LOG_NTUN DISABLED
+//#define LOG_MODE ENABLED
+//#define LOG_RAW DISABLED
+//#define LOG_CMD ENABLED
+//#define LOG_CUR DISABLED
+//
+
+//////////////////////////////////////////////////////////////////////////////
+// Navigation defaults
+//
+// WP_RADIUS_DEFAULT OPTIONAL
+//
+// When the user performs a factory reset on the APM, set the waypoint radius
+// (the radius from a target waypoint within which the APM will consider
+// itself to have arrived at the waypoint) to this value in meters. This is
+// mainly intended to allow users to start using the APM without running the
+// WaypointWriter first.
+//
+// LOITER_RADIUS_DEFAULT OPTIONAL
+//
+// When the user performs a factory reset on the APM, set the loiter radius
+// (the distance the APM will attempt to maintain from a waypoint while
+// loitering) to this value in meters. This is mainly intended to allow
+// users to start using the APM without running the WaypointWriter first.
+//
+// USE_CURRENT_ALT OPTIONAL
+// ALT_HOLD_HOME OPTIONAL
+//
+// When the user performs a factory reset on the APM, set the flag for weather
+// the current altitude or ALT_HOLD_HOME altitude should be used for Return To Launch.
+// Also, set the value of USE_CURRENT_ALT in meters. This is mainly intended to allow
+// users to start using the APM without running the WaypointWriter first.
+//
+//#define WP_RADIUS_DEFAULT 30
+//#define LOITER_RADIUS_DEFAULT 60
+//#define USE_CURRENT_ALT FALSE
+//#define ALT_HOLD_HOME 100
+//
+
+//////////////////////////////////////////////////////////////////////////////
+// Debugging interface
+//
+// DEBUG_PORT OPTIONAL
+//
+// The APM will periodically send messages reporting what it is doing; this
+// variable determines to which serial port they will be sent. Port 0 is the
+// USB serial port on the shield, port 3 is the telemetry port.
+//
+//#define DEBUG_PORT 0
+//
+
+
+//
+// Do not remove - this is to discourage users from copying this file
+// and using it as-is rather than editing their own.
+//
+#error You should not copy APM_Config.h.reference - make your own APM_Config.h file with just the options you need.
diff --git a/APMrover2/APM_Config_HILmode.h b/APMrover2/APM_Config_HILmode.h
new file mode 100644
index 000000000..90d924c5d
--- /dev/null
+++ b/APMrover2/APM_Config_HILmode.h
@@ -0,0 +1,439 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+// THIS IS A SAMPLE CONFIGURATION FILE FOR DOING HARDWARE IN THE LOOP TESTING USING THE ORIGINAL X-PLANE INTERFACE
+// IF YOU WANTED TO USE THIS YOU WOULD COPY THE CONTENTS INTO YOUR APM_Config.h FILE!
+
+
+#define FLIGHT_MODE_CHANNEL 8
+
+#define X_PLANE ENABLED
+
+//#define HIL_PROTOCOL HIL_PROTOCOL_XPLANE
+
+//#define GCS_PROTOCOL GCS_PROTOCOL_MAVLINK
+#define GCS_PROTOCOL GCS_PROTOCOL_NONE
+#define GCS_PORT 3
+
+#define HIL_PROTOCOL HIL_PROTOCOL_MAVLINK
+
+//#define HIL_MODE HIL_MODE_DISABLED
+#define HIL_MODE HIL_MODE_ATTITUDE
+#define HIL_PORT 0
+
+#define FLIGHT_MODE_1 AUTO // pos 0 ---
+#define FLIGHT_MODE_2 AUTO // pos 1
+#define FLIGHT_MODE_3 STABILIZE // pos 2
+#define FLIGHT_MODE_4 STABILIZE // pos 3 ---
+#define FLIGHT_MODE_5 MANUAL // pos 4
+#define FLIGHT_MODE_6 MANUAL // pos 5 ---
+
+#define AUTO_TRIM ENABLED
+#define THROTTLE_FAILSAFE DISABLED
+#define AIRSPEED_SENSOR ENABLED
+#define MAGNETOMETER DISABLED
+#define LOGGING_ENABLED DISABLED
+
+#define CH7_OPTION CH7_CH7_SAVE_WP
+
+#define TURN_GAIN 5
+
+#define CH7_OPTION CH7_SAVE_WP
+
+#define FLIGHT_MODE_1 AUTO // pos 0 ---
+#define FLIGHT_MODE_2 AUTO // pos 1
+#define FLIGHT_MODE_3 STABILIZE // pos 2
+#define FLIGHT_MODE_4 STABILIZE // pos 3 ---
+#define FLIGHT_MODE_5 MANUAL // pos 4
+#define FLIGHT_MODE_6 MANUAL // pos 5 ---
+
+#define ENABLE_AIR_START DISABLED
+
+#define MANUAL_LEVEL DISABLED
+
+#define CLOSED_LOOP_NAV ENABLED // set to ENABLED if closed loop navigation else set to DISABLED (Return To Lauch)
+
+#define MAX_DIST 50 //300 // max distance (in m) for the HEADALT mode
+#define SARSEC_BRANCH 50 // Long branch of the SARSEC pattern
+
+//////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////
+// FLIGHT AND NAVIGATION CONTROL
+//////////////////////////////////////////////////////////////////////////////
+
+//////////////////////////////////////////////////////////////////////////////
+// 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 3
+#define GSBOOST 0 // 60 // boost the throttle if ground speed is too low in case of windy conditions // 100
+#define NUDGE_OFFSET 0 //1603 // nudge_offset to get a good sustained speed in autonomous flight
+#define MIN_GNDSPEED 3
+
+//////////////////////////////////////////////////////////////////////////////
+// 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 35
+//
+
+//////////////////////////////////////////////////////////////////////////////
+// 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 1 // 40
+#define THROTTLE_MAX 100
+
+//////////////////////////////////////////////////////////////////////////////
+// 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 enable AUTO_TRIM.
+//
+#define AUTO_TRIM ENABLED
+#define THROTTLE_FAILSAFE DISABLED
+
+//#define ENABLE_AIR_START 0
+
+//////////////////////////////////////////////////////////////////////////////
+// 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 80
+#define PITCH_MAX 15
+#define PITCH_MIN -20 //-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). If you find this value is insufficient, consider adjusting the AOA
+// parameter.
+// 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.0
+#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.0
+#define SERVO_PITCH_I 0.0
+#define SERVO_PITCH_D 0.0
+#define SERVO_PITCH_INT_MAX 5
+#define PITCH_COMP 0.0
+#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.0
+//
+//////////////////////////////////////////////////////////////////////////////
+// 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.001
+#define NAV_ROLL_D 0.06
+#define NAV_ROLL_INT_MAX 5
+
+#define NAV_PITCH_ASP_P 0.0
+#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.0
+#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.
+// 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.1
+#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.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 20 // deg
+
+/////////////////////////////////////////////////////////////////////////////
+// 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 3 // meters
+#define LOITER_RADIUS_DEFAULT 10 // 60
+#define USE_CURRENT_ALT TRUE
+#define ALT_HOLD_HOME 0
+
+//////////////////////////////////////////////////////////////////////////////
+// INPUT_VOLTAGE OPTIONAL
+//
+// In order to have accurate pressure and battery voltage readings, this
+// value should be set to the voltage measured on the 5V rail on the oilpan.
+//
+// See the manual for more details. The default value should be close.
+//
+#define INPUT_VOLTAGE 5.2
+//
diff --git a/APMrover2/APM_Config_Rover.h b/APMrover2/APM_Config_Rover.h
new file mode 100644
index 000000000..db28b8eff
--- /dev/null
+++ b/APMrover2/APM_Config_Rover.h
@@ -0,0 +1,430 @@
+// CONFIG FILE FOR APM_Rover project by Jean-Louis Naudin
+//
+#define GCS_PORT 3
+#define GCS_PROTOCOL GCS_PROTOCOL_MAVLINK // QGroundControl protocol
+//#define GCS_PROTOCOL GCS_PROTOCOL_NONE // No GCS protocol to save memory
+#define HIL_MODE HIL_MODE_DISABLED
+
+#define MAV_SYSTEM_ID 1
+
+// Add a ground start delay in seconds
+//#define GROUND_START_DELAY 1
+
+#define AIRSPEED_SENSOR DISABLED
+#define MAGNETOMETER ENABLED
+#define LOGGING_ENABLED DISABLED
+
+//////////////////////////////////////////////////////////////////////////////
+// Serial port speeds.
+//
+#define SERIAL0_BAUD 115200
+#define SERIAL3_BAUD 115200
+
+//////////////////////////////////////////////////////////////////////////////
+// GPS_PROTOCOL
+#define GPS_PROTOCOL GPS_PROTOCOL_AUTO
+
+#define CH7_OPTION CH7_SAVE_WP
+#define TUNING_OPTION TUN_NONE
+
+#define FLIGHT_MODE_1 AUTO // pos 0 ---
+#define FLIGHT_MODE_2 AUTO // pos 1
+#define FLIGHT_MODE_3 STABILIZE // pos 2
+#define FLIGHT_MODE_4 STABILIZE // pos 3 ---
+#define FLIGHT_MODE_5 MANUAL // pos 4
+#define FLIGHT_MODE_6 MANUAL // pos 5 ---
+
+#define ENABLE_AIR_START DISABLED
+
+#define MANUAL_LEVEL DISABLED
+
+#define TURN_GAIN 5
+
+#define CLOSED_LOOP_NAV ENABLED // set to ENABLED if closed loop navigation else set to DISABLED (Return To Lauch)
+#define AUTO_WP_RADIUS ENABLED
+
+#define MAX_DIST 50 //300 // max distance (in m) for the HEADALT mode
+#define SARSEC_BRANCH 50 // Long branch of the SARSEC pattern
+
+//////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////
+// FLIGHT AND NAVIGATION CONTROL
+//////////////////////////////////////////////////////////////////////////////
+
+//////////////////////////////////////////////////////////////////////////////
+// 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 3
+#define GSBOOST 0
+#define NUDGE_OFFSET 0
+#define MIN_GNDSPEED 3
+
+//////////////////////////////////////////////////////////////////////////////
+// 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 35
+//
+
+//////////////////////////////////////////////////////////////////////////////
+// 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 3 // 40
+#define THROTTLE_MAX 100
+
+//////////////////////////////////////////////////////////////////////////////
+// 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 enable AUTO_TRIM.
+//
+#define AUTO_TRIM ENABLED
+#define THROTTLE_FAILSAFE DISABLED
+
+//#define ENABLE_AIR_START 0
+
+//////////////////////////////////////////////////////////////////////////////
+// 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 80
+#define PITCH_MAX 15
+#define PITCH_MIN -20 //-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). If you find this value is insufficient, consider adjusting the AOA
+// parameter.
+// 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.0
+#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.0
+#define SERVO_PITCH_I 0.0
+#define SERVO_PITCH_D 0.0
+#define SERVO_PITCH_INT_MAX 5
+#define PITCH_COMP 0.0
+#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.0
+//
+//////////////////////////////////////////////////////////////////////////////
+// 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.001
+#define NAV_ROLL_D 0.06
+#define NAV_ROLL_INT_MAX 5
+
+#define NAV_PITCH_ASP_P 0.0
+#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.0
+#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.
+// 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.1
+#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.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 20 // deg
+
+/////////////////////////////////////////////////////////////////////////////
+// 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 3 // meters
+#define LOITER_RADIUS_DEFAULT 10 // 60
+#define USE_CURRENT_ALT TRUE
+#define ALT_HOLD_HOME 0
+
+//////////////////////////////////////////////////////////////////////////////
+// INPUT_VOLTAGE OPTIONAL
+//
+// In order to have accurate pressure and battery voltage readings, this
+// value should be set to the voltage measured on the 5V rail on the oilpan.
+//
+// See the manual for more details. The default value should be close.
+//
+#define INPUT_VOLTAGE 5.2
+//
diff --git a/APMrover2/APM_Config_mavlink_hil.h b/APMrover2/APM_Config_mavlink_hil.h
new file mode 100644
index 000000000..38cff467b
--- /dev/null
+++ b/APMrover2/APM_Config_mavlink_hil.h
@@ -0,0 +1,31 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+// THIS IS A SAMPLE CONFIGURATION FILE FOR DOING HARDWARE IN THE LOOP TESTING USING THE MAVLINK HIL INTERFACE
+// IF YOU WANTED TO USE THIS YOU WOULD COPY THE CONTENTS INTO YOUR APM_Config.h FILE!
+
+
+// Enable Autopilot Flight Mode
+#define FLIGHT_MODE_CHANNEL 8
+#define FLIGHT_MODE_1 AUTO
+#define FLIGHT_MODE_2 RTL
+#define FLIGHT_MODE_3 FLY_BY_WIRE_A
+#define FLIGHT_MODE_4 FLY_BY_WIRE_B
+#define FLIGHT_MODE_5 STABILIZE
+#define FLIGHT_MODE_6 MANUAL
+
+// HIL_MODE SELECTION
+//
+// Mavlink supports
+// 1. HIL_MODE_ATTITUDE : simulated position, airspeed, and attitude
+// 2. HIL_MODE_SENSORS: full sensor simulation
+#define HIL_MODE HIL_MODE_ATTITUDE
+
+// Sensors
+// All sensors are supported in all modes.
+// The magnetometer is not used in
+// HIL_MODE_ATTITUDE but you may leave it
+// enabled if you wish.
+#define AIRSPEED_SENSOR ENABLED
+#define MAGNETOMETER ENABLED
+#define AIRSPEED_CRUISE 25
+#define THROTTLE_FAILSAFE ENABLED
diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde
new file mode 100644
index 000000000..765bcc2b4
--- /dev/null
+++ b/APMrover2/APMrover2.pde
@@ -0,0 +1,1135 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#define THISFIRMWARE "APMrover v2.0a JL NAUDIN" //New version of the APMrover for the APM v1 or APM v2 and magnetometer
+
+// This is a full version of Arduplane v2.32 specially adapted for a Rover by Jean-Louis Naudin (JLN)
+
+/*
+Authors: Doug Weibel, Jose Julio, Jordi Munoz, Jason Short, Andrew Tridgell, Randy Mackay, Pat Hickey, John Arne Birkeland, Olivier Adler
+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
+Please contribute your ideas!
+
+
+This firmware is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 2.1 of the License, or (at your option) any later version.
+
+//
+// JLN updates: last update 2012-04-30
+//
+// DOLIST:
+//
+//
+//-------------------------------------------------------------------------------------------------------------------------
+// Dev Startup : 2012-04-21
+//
+// 2012-04-30: Now a full version for APM v1 or APM v2 with magnetometer
+// 2012-04-27: Cosmetic changes
+// 2012-04-26: Only one PID (pidNavRoll) for steering the wheel with nav_roll
+// 2012-04-26: Added ground_speed and ground_course variables in Update_GPS
+// 2012-04-26: Set GPS to 10 Hz (updated in the AP_GPS lib)
+// 2012-04-22: Tested on Traxxas Monster Jam Grinder XL-5 3602
+// 2012-04-21: Roll set to wheels control and Throttle neutral to 50% (0 -100) - Forward>50, Backward<50
+//
+// Radio setup:
+// APM INPUT (Rec = receiver)
+// Rec ch1: Roll
+// Rec ch2: Throttle
+// Rec ch3: Pitch
+// Rec ch4: Yaw
+// Rec ch5: not used
+// Rec ch6: not used
+// Rec ch7: Option channel to 2 positions switch
+// Rec ch8: Mode channel to 3 positions switch
+// APM OUTPUT
+// Ch1: Wheel servo (direction)
+// Ch2: not used
+// Ch3: to the motor ESC
+// Ch4: not used
+//
+// more infos this experimental version: http://diydrones.com/profile/JeanLouisNaudin
+// =======================================================================================================
+*/
+
+////////////////////////////////////////////////////////////////////////////////
+// Header includes
+////////////////////////////////////////////////////////////////////////////////
+
+// AVR runtime
+#include <avr/io.h>
+#include <avr/eeprom.h>
+#include <avr/pgmspace.h>
+#include <math.h>
+
+// Libraries
+#include <FastSerial.h>
+#include <AP_Common.h>
+#include <Arduino_Mega_ISR_Registry.h>
+#include <APM_RC.h> // ArduPilot Mega RC Library
+#include <AP_GPS.h> // ArduPilot GPS library
+#include <I2C.h> // Wayne Truchsess I2C lib
+#include <SPI.h> // Arduino SPI lib
+#include <DataFlash.h> // ArduPilot Mega Flash Memory Library
+#include <AP_ADC.h> // ArduPilot Mega Analog to Digital Converter Library
+#include <AP_AnalogSource.h>// ArduPilot Mega polymorphic analog getter
+#include <AP_PeriodicProcess.h> // ArduPilot Mega TimerProcess
+#include <AP_Baro.h> // ArduPilot barometer library
+#include <AP_Compass.h> // ArduPilot Mega Magnetometer Library
+#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
+#include <AP_InertialSensor.h> // Inertial Sensor (uncalibated IMU) Library
+#include <AP_IMU.h> // ArduPilot Mega IMU Library
+#include <AP_AHRS.h> // ArduPilot Mega DCM Library
+#include <PID.h> // PID library
+#include <RC_Channel.h> // RC Channel Library
+#include <AP_RangeFinder.h> // Range finder library
+#include <Filter.h> // Filter library
+#include <ModeFilter.h> // Mode Filter from Filter library
+#include <AverageFilter.h> // Mode Filter from Filter library
+#include <AP_Relay.h> // APM relay
+#include <AP_Mount.h> // Camera/Antenna mount
+#include <GCS_MAVLink.h> // MAVLink GCS definitions
+#include <memcheck.h>
+
+// Configuration
+#include "config.h"
+
+// Local modules
+#include "defines.h"
+#include "Parameters.h"
+#include "GCS.h"
+
+#include <AP_Declination.h> // ArduPilot Mega Declination Helper Library
+
+////////////////////////////////////////////////////////////////////////////////
+// Serial ports
+////////////////////////////////////////////////////////////////////////////////
+//
+// Note that FastSerial port buffers are allocated at ::begin time,
+// so there is not much of a penalty to defining ports that we don't
+// use.
+//
+FastSerialPort0(Serial); // FTDI/console
+FastSerialPort1(Serial1); // GPS port
+FastSerialPort3(Serial3); // Telemetry port
+
+////////////////////////////////////////////////////////////////////////////////
+// ISR Registry
+////////////////////////////////////////////////////////////////////////////////
+Arduino_Mega_ISR_Registry isr_registry;
+
+
+////////////////////////////////////////////////////////////////////////////////
+// APM_RC_Class Instance
+////////////////////////////////////////////////////////////////////////////////
+#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
+ APM_RC_APM2 APM_RC;
+#else
+ APM_RC_APM1 APM_RC;
+#endif
+
+////////////////////////////////////////////////////////////////////////////////
+// Dataflash
+////////////////////////////////////////////////////////////////////////////////
+#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
+ DataFlash_APM2 DataFlash;
+#else
+ DataFlash_APM1 DataFlash;
+#endif
+
+
+////////////////////////////////////////////////////////////////////////////////
+// Parameters
+////////////////////////////////////////////////////////////////////////////////
+//
+// Global parameters are all contained within the 'g' class.
+//
+static Parameters g;
+
+
+////////////////////////////////////////////////////////////////////////////////
+// prototypes
+static void update_events(void);
+
+
+////////////////////////////////////////////////////////////////////////////////
+// Sensors
+////////////////////////////////////////////////////////////////////////////////
+//
+// There are three basic options related to flight sensor selection.
+//
+// - Normal flight mode. Real sensors are used.
+// - HIL Attitude mode. Most sensors are disabled, as the HIL
+// protocol supplies attitude information directly.
+// - HIL Sensors mode. Synthetic sensors are configured that
+// supply data from the simulation.
+//
+
+// All GPS access should be through this pointer.
+static GPS *g_gps;
+
+// flight modes convenience array
+static AP_Int8 *flight_modes = &g.flight_mode1;
+
+#if HIL_MODE == HIL_MODE_DISABLED
+
+// real sensors
+#if CONFIG_ADC == ENABLED
+static AP_ADC_ADS7844 adc;
+#endif
+
+#ifdef DESKTOP_BUILD
+AP_Baro_BMP085_HIL barometer;
+AP_Compass_HIL compass;
+#else
+
+#if CONFIG_BARO == AP_BARO_BMP085
+# if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
+static AP_Baro_BMP085 barometer(true);
+# else
+static AP_Baro_BMP085 barometer(false);
+# endif
+#elif CONFIG_BARO == AP_BARO_MS5611
+static AP_Baro_MS5611 barometer;
+#endif
+
+static AP_Compass_HMC5843 compass;
+#endif
+
+// real GPS selection
+#if GPS_PROTOCOL == GPS_PROTOCOL_AUTO
+AP_GPS_Auto g_gps_driver(&Serial1, &g_gps);
+
+#elif GPS_PROTOCOL == GPS_PROTOCOL_NMEA
+AP_GPS_NMEA g_gps_driver(&Serial1);
+
+#elif GPS_PROTOCOL == GPS_PROTOCOL_SIRF
+AP_GPS_SIRF g_gps_driver(&Serial1);
+
+#elif GPS_PROTOCOL == GPS_PROTOCOL_UBLOX
+AP_GPS_UBLOX g_gps_driver(&Serial1);
+
+#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK
+AP_GPS_MTK g_gps_driver(&Serial1);
+
+#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK16
+AP_GPS_MTK16 g_gps_driver(&Serial1);
+
+#elif GPS_PROTOCOL == GPS_PROTOCOL_NONE
+AP_GPS_None g_gps_driver(NULL);
+
+#else
+ #error Unrecognised GPS_PROTOCOL setting.
+#endif // GPS PROTOCOL
+
+# if CONFIG_IMU_TYPE == CONFIG_IMU_MPU6000
+ AP_InertialSensor_MPU6000 ins( CONFIG_MPU6000_CHIP_SELECT_PIN );
+# else
+ AP_InertialSensor_Oilpan ins( &adc );
+#endif // CONFIG_IMU_TYPE
+AP_IMU_INS imu( &ins );
+
+#if QUATERNION_ENABLE == ENABLED
+ AP_AHRS_Quaternion ahrs(&imu, g_gps);
+#else
+ AP_AHRS_DCM ahrs(&imu, g_gps);
+#endif
+
+#elif HIL_MODE == HIL_MODE_SENSORS
+// sensor emulators
+AP_ADC_HIL adc;
+AP_Baro_BMP085_HIL barometer;
+AP_Compass_HIL compass;
+AP_GPS_HIL g_gps_driver(NULL);
+AP_InertialSensor_Oilpan ins( &adc );
+AP_IMU_Shim imu;
+AP_AHRS_DCM ahrs(&imu, g_gps);
+
+#elif HIL_MODE == HIL_MODE_ATTITUDE
+AP_ADC_HIL adc;
+AP_IMU_Shim imu; // never used
+AP_AHRS_HIL ahrs(&imu, g_gps);
+AP_GPS_HIL g_gps_driver(NULL);
+AP_Compass_HIL compass; // never used
+AP_Baro_BMP085_HIL barometer;
+#else
+ #error Unrecognised HIL_MODE setting.
+#endif // HIL MODE
+
+// we always have a timer scheduler
+AP_TimerProcess timer_scheduler;
+
+////////////////////////////////////////////////////////////////////////////////
+// GCS selection
+////////////////////////////////////////////////////////////////////////////////
+//
+GCS_MAVLINK gcs0;
+GCS_MAVLINK gcs3;
+
+////////////////////////////////////////////////////////////////////////////////
+// PITOT selection
+////////////////////////////////////////////////////////////////////////////////
+//
+ModeFilterInt16_Size5 sonar_mode_filter(2);
+
+#if CONFIG_PITOT_SOURCE == PITOT_SOURCE_ADC
+AP_AnalogSource_ADC pitot_analog_source( &adc,
+ CONFIG_PITOT_SOURCE_ADC_CHANNEL, 1.0);
+#elif CONFIG_PITOT_SOURCE == PITOT_SOURCE_ANALOG_PIN
+AP_AnalogSource_Arduino pitot_analog_source(CONFIG_PITOT_SOURCE_ANALOG_PIN, 4.0);
+#endif
+
+#if SONAR_TYPE == MAX_SONAR_XL
+ AP_RangeFinder_MaxsonarXL sonar(&pitot_analog_source, &sonar_mode_filter);
+#elif SONAR_TYPE == MAX_SONAR_LV
+ // XXX honestly I think these output the same values
+ // If someone knows, can they confirm it?
+ AP_RangeFinder_MaxsonarXL sonar(&pitot_analog_source, &sonar_mode_filter);
+#endif
+
+// Barometer filter
+AverageFilterInt32_Size5 baro_filter; // filtered pitch acceleration
+
+AP_Relay relay;
+
+// Camera/Antenna mount tracking and stabilisation stuff
+// --------------------------------------
+#if MOUNT == ENABLED
+AP_Mount camera_mount(g_gps, &dcm);
+#endif
+
+////////////////////////////////////////////////////////////////////////////////
+// Global variables
+////////////////////////////////////////////////////////////////////////////////
+
+// APM2 only
+#if USB_MUX_PIN > 0
+static bool usb_connected;
+#endif
+
+static const char *comma = ",";
+
+static const char* flight_mode_strings[] = {
+ "Manual",
+ "Circle",
+ "Stabilize",
+ "",
+ "",
+ "FBW_A",
+ "FBW_B",
+ "",
+ "",
+ "",
+ "Auto",
+ "RTL",
+ "Loiter",
+ "",
+ "",
+ "",
+ "",
+ "",
+ "",
+ "",
+ "",
+ ""};
+
+/* Radio values
+ Channel assignments
+ 1 Ailerons (rudder if no ailerons)
+ 2 Elevator
+ 3 Throttle
+ 4 Rudder (if we have ailerons)
+ 5 Aux5
+ 6 Aux6
+ 7 Aux7
+ 8 Aux8/Mode
+ Each Aux channel can be configured to have any of the available auxiliary functions assigned to it.
+ See libraries/RC_Channel/RC_Channel_aux.h for more information
+*/
+
+////////////////////////////////////////////////////////////////////////////////
+// Radio
+////////////////////////////////////////////////////////////////////////////////
+// This is the state of the flight control system
+// There are multiple states defined such as MANUAL, FBW-A, AUTO
+byte control_mode = INITIALISING;
+// Used to maintain the state of the previous control switch position
+// This is set to -1 when we need to re-read the switch
+byte oldSwitchPosition;
+// This is used to enable the inverted flight feature
+bool inverted_flight = false;
+// These are trim values used for elevon control
+// For elevons radio_in[CH_ROLL] and radio_in[CH_PITCH] are equivalent aileron and elevator, not left and right elevon
+static uint16_t elevon1_trim = 1500;
+static uint16_t elevon2_trim = 1500;
+// These are used in the calculation of elevon1_trim and elevon2_trim
+static uint16_t ch1_temp = 1500;
+static uint16_t ch2_temp = 1500;
+// These are values received from the GCS if the user is using GCS joystick
+// control and are substituted for the values coming from the RC radio
+static int16_t rc_override[8] = {0,0,0,0,0,0,0,0};
+// A flag if GCS joystick control is in use
+static bool rc_override_active = false;
+
+////////////////////////////////////////////////////////////////////////////////
+// Failsafe
+////////////////////////////////////////////////////////////////////////////////
+// A tracking variable for type of failsafe active
+// Used for failsafe based on loss of RC signal or GCS signal
+static int failsafe;
+// Used to track if the value on channel 3 (throtttle) has fallen below the failsafe threshold
+// RC receiver should be set up to output a low throttle value when signal is lost
+static bool ch3_failsafe;
+// A timer used to help recovery from unusual attitudes. If we enter an unusual attitude
+// while in autonomous flight this variable is used to hold roll at 0 for a recovery period
+static byte crash_timer;
+// A timer used to track how long since we have received the last GCS heartbeat or rc override message
+static uint32_t rc_override_fs_timer = 0;
+// A timer used to track how long we have been in a "short failsafe" condition due to loss of RC signal
+static uint32_t ch3_failsafe_timer = 0;
+
+////////////////////////////////////////////////////////////////////////////////
+// LED output
+////////////////////////////////////////////////////////////////////////////////
+// state of the GPS light (on/off)
+static bool GPS_light;
+
+////////////////////////////////////////////////////////////////////////////////
+// GPS variables
+////////////////////////////////////////////////////////////////////////////////
+// This is used to scale GPS values for EEPROM storage
+// 10^7 times Decimal GPS means 1 == 1cm
+// This approximation makes calculations integer and it's easy to read
+static const float t7 = 10000000.0;
+// We use atan2 and other trig techniques to calaculate angles
+// We need to scale the longitude up to make these calcs work
+// to account for decreasing distance between lines of longitude away from the equator
+static float scaleLongUp = 1;
+// Sometimes we need to remove the scaling for distance calcs
+static float scaleLongDown = 1;
+// A counter used to count down valid gps fixes to allow the gps estimate to settle
+// before recording our home position (and executing a ground start if we booted with an air start)
+static byte ground_start_count = 5;
+// Used to compute a speed estimate from the first valid gps fixes to decide if we are
+// on the ground or in the air. Used to decide if a ground start is appropriate if we
+// booted with an air start.
+static int ground_start_avg;
+// Tracks if GPS is enabled based on statup routine
+// If we do not detect GPS at startup, we stop trying and assume GPS is not connected
+static bool GPS_enabled = false;
+static int32_t gps_base_alt;
+
+////////////////////////////////////////////////////////////////////////////////
+// Location & Navigation
+////////////////////////////////////////////////////////////////////////////////
+// Constants
+const float radius_of_earth = 6378100; // meters
+const float gravity = 9.81; // meters/ sec^2
+// This is the currently calculated direction to fly.
+// deg * 100 : 0 to 360
+static long nav_bearing;
+// This is the direction to the next waypoint or loiter center
+// deg * 100 : 0 to 360
+static long target_bearing;
+//This is the direction from the last waypoint to the next waypoint
+// deg * 100 : 0 to 360
+static long crosstrack_bearing;
+// A gain scaler to account for ground speed/headwind/tailwind
+static float nav_gain_scaler = 1;
+// Direction held during phases of takeoff and landing
+// deg * 100 dir of plane, A value of -1 indicates the course has not been set/is not in use
+static long hold_course = -1; // deg * 100 dir of plane
+
+// There may be two active commands in Auto mode.
+// This indicates the active navigation command by index number
+static byte nav_command_index;
+// This indicates the active non-navigation command by index number
+static byte non_nav_command_index;
+// This is the command type (eg navigate to waypoint) of the active navigation command
+static byte nav_command_ID = NO_COMMAND;
+static byte non_nav_command_ID = NO_COMMAND;
+
+////////////////////////////////////////////////////////////////////////////////
+// Airspeed
+////////////////////////////////////////////////////////////////////////////////
+// The current airspeed estimate/measurement in centimeters per second
+static int airspeed;
+// The calculated airspeed to use in FBW-B. Also used in higher modes for insuring min ground speed is met.
+// Also used for flap deployment criteria. Centimeters per second.static long target_airspeed;
+static long target_airspeed;
+// The difference between current and desired airspeed. Used in the pitch controller. Centimeters per second.
+static float airspeed_error;
+static float groundspeed_error;
+// The calculated total energy error (kinetic (altitude) plus potential (airspeed)).
+// Used by the throttle controller
+static long energy_error;
+// kinetic portion of energy error (m^2/s^2)
+static long airspeed_energy_error;
+// An amount that the airspeed should be increased in auto modes based on the user positioning the
+// throttle stick in the top half of the range. Centimeters per second.
+static int airspeed_nudge;
+// Similar to airspeed_nudge, but used when no airspeed sensor.
+// 0-(throttle_max - throttle_cruise) : throttle nudge in Auto mode using top 1/2 of throttle stick travel
+static int throttle_nudge = 0;
+
+////////////////////////////////////////////////////////////////////////////////
+// Ground speed
+////////////////////////////////////////////////////////////////////////////////
+// The amount current ground speed is below min ground speed. Centimeters per second
+static long groundspeed_undershoot = 0;
+static long ground_speed = 0;
+
+////////////////////////////////////////////////////////////////////////////////
+// Location Errors
+////////////////////////////////////////////////////////////////////////////////
+// Difference between current bearing and desired bearing. Hundredths of a degree
+static long bearing_error;
+// Difference between current altitude and desired altitude. Centimeters
+static long altitude_error;
+// Distance perpandicular to the course line that we are off trackline. Meters
+static float crosstrack_error;
+
+////////////////////////////////////////////////////////////////////////////////
+// CH7 control
+////////////////////////////////////////////////////////////////////////////////
+
+// Used to track the CH7 toggle state.
+// When CH7 goes LOW PWM from HIGH PWM, this value will have been set true
+// This allows advanced functionality to know when to execute
+static boolean trim_flag;
+// This register tracks the current Mission Command index when writing
+// a mission using CH7 in flight
+static int8_t CH7_wp_index;
+float tuning_value;
+
+////////////////////////////////////////////////////////////////////////////////
+// Battery Sensors
+////////////////////////////////////////////////////////////////////////////////
+// Battery pack 1 voltage. Initialized above the low voltage threshold to pre-load the filter and prevent low voltage events at startup.
+static float battery_voltage1 = LOW_VOLTAGE * 1.05;
+// Battery pack 1 instantaneous currrent draw. Amperes
+static float current_amps1;
+// Totalized current (Amp-hours) from battery 1
+static float current_total1;
+
+// To Do - Add support for second battery pack
+//static float battery_voltage2 = LOW_VOLTAGE * 1.05; // Battery 2 Voltage, initialized above threshold for filter
+//static float current_amps2; // Current (Amperes) draw from battery 2
+//static float current_total2; // Totalized current (Amp-hours) from battery 2
+
+////////////////////////////////////////////////////////////////////////////////
+// Airspeed Sensors
+////////////////////////////////////////////////////////////////////////////////
+// Raw differential pressure measurement (filtered). ADC units
+static float airspeed_raw;
+// Raw differential pressure less the zero pressure offset. ADC units
+static float airspeed_pressure;
+// The pressure at home location - calibrated at arming
+static int32_t ground_pressure;
+// The ground temperature at home location - calibrated at arming
+static int16_t ground_temperature;
+////////////////////////////////////////////////////////////////////////////////
+// Altitude Sensor variables
+////////////////////////////////////////////////////////////////////////////////
+// Raw absolute pressure measurement (filtered). ADC units
+static unsigned long abs_pressure;
+// Altitude from the sonar sensor. Meters. Not yet implemented.
+static int sonar_alt;
+// The altitude as reported by Baro in cm – Values can be quite high
+static int32_t baro_alt;
+
+////////////////////////////////////////////////////////////////////////////////
+// flight mode specific
+////////////////////////////////////////////////////////////////////////////////
+// Flag for using gps ground course instead of IMU yaw. Set false when takeoff command in process.
+static bool takeoff_complete = true;
+// Flag to indicate if we have landed.
+//Set land_complete if we are within 2 seconds distance or within 3 meters altitude of touchdown
+static bool land_complete;
+// Altitude threshold to complete a takeoff command in autonomous modes. Centimeters
+static long takeoff_altitude;
+// Pitch to hold during landing command in the no airspeed sensor case. Hundredths of a degree
+static int landing_pitch;
+// Minimum pitch to hold during takeoff command execution. Hundredths of a degree
+static int takeoff_pitch;
+static bool final = false;
+
+// JLN Update
+unsigned long timesw = 0;
+static long ground_course = 0; // deg * 100 dir of plane
+
+////////////////////////////////////////////////////////////////////////////////
+// Loiter management
+////////////////////////////////////////////////////////////////////////////////
+// Previous target bearing. Used to calculate loiter rotations. Hundredths of a degree
+static long old_target_bearing;
+// Total desired rotation in a loiter. Used for Loiter Turns commands. Degrees
+static int loiter_total;
+// The amount in degrees we have turned since recording old_target_bearing
+static int loiter_delta;
+// Total rotation in a loiter. Used for Loiter Turns commands and to check for missed waypoints. Degrees
+static int loiter_sum;
+// The amount of time we have been in a Loiter. Used for the Loiter Time command. Milliseconds.
+static long loiter_time;
+// The amount of time we should stay in a loiter for the Loiter Time command. Milliseconds.
+static int loiter_time_max;
+
+////////////////////////////////////////////////////////////////////////////////
+// Navigation control variables
+////////////////////////////////////////////////////////////////////////////////
+// The instantaneous desired bank angle. Hundredths of a degree
+static long nav_roll;
+// The instantaneous desired pitch angle. Hundredths of a degree
+static long nav_pitch;
+// Calculated radius for the wp turn based on ground speed and max turn angle
+static long wp_radius;
+static long toff_yaw; // deg * 100 : yaw angle for takeoff
+static long nav_yaw = 1; // deg * 100 : target yaw angle - used only for thermal hunt
+static long altitude_estimate = 0; // for smoothing GPS output
+
+////////////////////////////////////////////////////////////////////////////////
+// Waypoint distances
+////////////////////////////////////////////////////////////////////////////////
+// Distance between plane and next waypoint. Meters
+static long wp_distance;
+// Distance between previous and next waypoint. Meters
+static long wp_totalDistance;
+
+static long max_dist_set; // used for HEADALT (LEO)
+
+////////////////////////////////////////////////////////////////////////////////
+// repeating event control
+////////////////////////////////////////////////////////////////////////////////
+// Flag indicating current event type
+static byte event_id;
+// when the event was started in ms
+static long event_timer;
+// how long to delay the next firing of event in millis
+static uint16_t event_delay;
+// how many times to cycle : -1 (or -2) = forever, 2 = do one cycle, 4 = do two cycles
+static int event_repeat = 0;
+// per command value, such as PWM for servos
+static int event_value;
+// the value used to cycle events (alternate value to event_value)
+static int event_undo_value;
+
+////////////////////////////////////////////////////////////////////////////////
+// Conditional command
+////////////////////////////////////////////////////////////////////////////////
+// A value used in condition commands (eg delay, change alt, etc.)
+// For example in a change altitude command, it is the altitude to change to.
+static long condition_value;
+// A starting value used to check the status of a conditional command.
+// For example in a delay command the condition_start records that start time for the delay
+static long condition_start;
+// A value used in condition commands. For example the rate at which to change altitude.
+static int condition_rate;
+
+////////////////////////////////////////////////////////////////////////////////
+// 3D Location vectors
+// Location structure defined in AP_Common
+////////////////////////////////////////////////////////////////////////////////
+// The home location used for RTL. The location is set when we first get stable GPS lock
+static struct Location home;
+// Flag for if we have g_gps lock and have set the home location
+static bool home_is_set;
+// The location of the previous waypoint. Used for track following and altitude ramp calculations
+static struct Location prev_WP;
+// The plane's current location
+static struct Location current_loc;
+// The location of the current/active waypoint. Used for altitude ramp, track following and loiter calculations.
+static struct Location next_WP;
+// The location of the active waypoint in Guided mode.
+static struct Location guided_WP;
+static struct Location soarwp1_WP = {0,0,0}; // temp waypoint for Ridge Soaring
+static struct Location soarwp2_WP = {0,0,0}; // temp waypoint for Ridge Soaring
+// The location structure information from the Nav command being processed
+static struct Location next_nav_command;
+// The location structure information from the Non-Nav command being processed
+static struct Location next_nonnav_command;
+
+////////////////////////////////////////////////////////////////////////////////
+// Altitude / Climb rate control
+////////////////////////////////////////////////////////////////////////////////
+// The current desired altitude. Altitude is linearly ramped between waypoints. Centimeters
+static long target_altitude;
+// Altitude difference between previous and current waypoint. Centimeters
+static long offset_altitude;
+
+////////////////////////////////////////////////////////////////////////////////
+// IMU variables
+////////////////////////////////////////////////////////////////////////////////
+// The main loop execution time. Seconds
+//This is the time between calls to the DCM algorithm and is the Integration time for the gyros.
+static float G_Dt = 0.02;
+
+////////////////////////////////////////////////////////////////////////////////
+// Performance monitoring
+////////////////////////////////////////////////////////////////////////////////
+// Timer used to accrue data and trigger recording of the performanc monitoring log message
+static long perf_mon_timer;
+// The maximum main loop execution time recorded in the current performance monitoring interval
+static int G_Dt_max = 0;
+// The number of gps fixes recorded in the current performance monitoring interval
+static int gps_fix_count = 0;
+// A variable used by developers to track performanc metrics.
+// Currently used to record the number of GCS heartbeat messages received
+static int pmTest1 = 0;
+
+
+////////////////////////////////////////////////////////////////////////////////
+// System Timers
+////////////////////////////////////////////////////////////////////////////////
+// Time in miliseconds of start of main control loop. Milliseconds
+static unsigned long fast_loopTimer;
+// Time Stamp when fast loop was complete. Milliseconds
+static unsigned long fast_loopTimeStamp;
+// Number of milliseconds used in last main loop cycle
+static uint8_t delta_ms_fast_loop;
+// Counter of main loop executions. Used for performance monitoring and failsafe processing
+static uint16_t mainLoop_count;
+
+// Time in miliseconds of start of medium control loop. Milliseconds
+static unsigned long medium_loopTimer;
+// Counters for branching from main control loop to slower loops
+static byte medium_loopCounter;
+// Number of milliseconds used in last medium loop cycle
+static uint8_t delta_ms_medium_loop;
+
+// Counters for branching from medium control loop to slower loops
+static byte slow_loopCounter;
+// Counter to trigger execution of very low rate processes
+static byte superslow_loopCounter;
+// Counter to trigger execution of 1 Hz processes
+static byte counter_one_herz;
+
+// used to track the elapsed time for navigation PID integral terms
+static unsigned long nav_loopTimer;
+// Elapsed time since last call to navigation pid functions
+static unsigned long dTnav;
+// % MCU cycles used
+static float load;
+
+////////////////////////////////////////////////////////////////////////////////
+// Top-level logic
+////////////////////////////////////////////////////////////////////////////////
+
+void setup() {
+ memcheck_init();
+ init_ardupilot();
+}
+
+void loop()
+{
+ // We want this to execute at 50Hz if possible
+ // -------------------------------------------
+ if (millis()-fast_loopTimer > 19) {
+ delta_ms_fast_loop = millis() - fast_loopTimer;
+ load = (float)(fast_loopTimeStamp - fast_loopTimer)/delta_ms_fast_loop;
+ G_Dt = (float)delta_ms_fast_loop / 1000.f;
+ fast_loopTimer = millis();
+
+ mainLoop_count++;
+
+ // Execute the fast loop
+ // ---------------------
+ fast_loop();
+
+ // Execute the medium loop
+ // -----------------------
+ medium_loop();
+
+ counter_one_herz++;
+ if(counter_one_herz == 50){
+ one_second_loop();
+ counter_one_herz = 0;
+ }
+
+ if (millis() - perf_mon_timer > 20000) {
+ if (mainLoop_count != 0) {
+ if (g.log_bitmask & MASK_LOG_PM)
+ #if HIL_MODE != HIL_MODE_ATTITUDE
+ Log_Write_Performance();
+ #endif
+
+ resetPerfData();
+ }
+ }
+
+ fast_loopTimeStamp = millis();
+ }
+}
+
+// Main loop 50Hz
+static void fast_loop()
+{
+ // This is the fast loop - we want it to execute at 50Hz if possible
+ // -----------------------------------------------------------------
+ if (delta_ms_fast_loop > G_Dt_max)
+ G_Dt_max = delta_ms_fast_loop;
+
+ // Read radio
+ // ----------
+ read_radio();
+
+ // try to send any deferred messages if the serial port now has
+ // some space available
+ gcs_send_message(MSG_RETRY_DEFERRED);
+
+ // check for loss of control signal failsafe condition
+ // ------------------------------------
+ check_short_failsafe();
+
+ #if HIL_MODE == HIL_MODE_SENSORS
+ // update hil before dcm update
+ gcs_update();
+ #endif
+
+ ahrs.update();
+
+ // uses the yaw from the DCM to give more accurate turns
+ calc_bearing_error();
+
+ # if HIL_MODE == HIL_MODE_DISABLED
+ if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST)
+ Log_Write_Attitude((int)ahrs.roll_sensor, (int)ahrs.pitch_sensor, (uint16_t)ahrs.yaw_sensor);
+
+ if (g.log_bitmask & MASK_LOG_RAW)
+ Log_Write_Raw();
+ #endif
+
+ // inertial navigation
+ // ------------------
+ #if INERTIAL_NAVIGATION == ENABLED
+ // TODO: implement inertial nav function
+ inertialNavigation();
+ #endif
+
+ // custom code/exceptions for flight modes
+ // ---------------------------------------
+ update_current_flight_mode();
+
+ // apply desired roll, pitch and yaw to the plane
+ // ----------------------------------------------
+ if (control_mode > MANUAL)
+ stabilize();
+
+ // write out the servo PWM values
+ // ------------------------------
+ set_servos();
+
+
+ // XXX is it appropriate to be doing the comms below on the fast loop?
+
+ gcs_update();
+ gcs_data_stream_send(45,1000);
+}
+
+static void medium_loop()
+{
+#if MOUNT == ENABLED
+ camera_mount.update_mount_position();
+#endif
+
+ // This is the start of the medium (10 Hz) loop pieces
+ // -----------------------------------------
+ switch(medium_loopCounter) {
+
+ // This case deals with the GPS
+ //-------------------------------
+ case 0:
+ medium_loopCounter++;
+ if(GPS_enabled){
+ update_GPS();
+ calc_gndspeed_undershoot();
+ }
+
+ #if HIL_MODE != HIL_MODE_ATTITUDE
+ if (g.compass_enabled && compass.read()) {
+ ahrs.set_compass(&compass);
+ // Calculate heading
+ Matrix3f m = ahrs.get_dcm_matrix();
+ compass.calculate(m);
+ compass.null_offsets();
+ } else {
+ ahrs.set_compass(NULL);
+ }
+ #endif
+/*{
+Serial.print(ahrs.roll_sensor, DEC); Serial.printf_P(PSTR("\t"));
+Serial.print(ahrs.pitch_sensor, DEC); Serial.printf_P(PSTR("\t"));
+Serial.print(ahrs.yaw_sensor, DEC); Serial.printf_P(PSTR("\t"));
+Vector3f tempaccel = imu.get_accel();
+Serial.print(tempaccel.x, DEC); Serial.printf_P(PSTR("\t"));
+Serial.print(tempaccel.y, DEC); Serial.printf_P(PSTR("\t"));
+Serial.println(tempaccel.z, DEC);
+}*/
+
+ break;
+
+ // This case performs some navigation computations
+ //------------------------------------------------
+ case 1:
+ medium_loopCounter++;
+
+
+ if(g_gps->new_data){
+ g_gps->new_data = false;
+ dTnav = millis() - nav_loopTimer;
+ nav_loopTimer = millis();
+
+ // calculate the plane's desired bearing
+ // -------------------------------------
+ navigate();
+ }
+
+ break;
+
+ // command processing
+ //------------------------------
+ case 2:
+ medium_loopCounter++;
+
+ // perform next command
+ // --------------------
+ update_commands();
+ break;
+
+ // This case deals with sending high rate telemetry
+ //-------------------------------------------------
+ case 3:
+ medium_loopCounter++;
+
+ #if HIL_MODE != HIL_MODE_ATTITUDE
+ if ((g.log_bitmask & MASK_LOG_ATTITUDE_MED) && !(g.log_bitmask & MASK_LOG_ATTITUDE_FAST))
+ Log_Write_Attitude((int)ahrs.roll_sensor, (int)ahrs.pitch_sensor, (uint16_t)ahrs.yaw_sensor);
+
+ if (g.log_bitmask & MASK_LOG_CTUN)
+ Log_Write_Control_Tuning();
+ #endif
+
+ if (g.log_bitmask & MASK_LOG_NTUN)
+ Log_Write_Nav_Tuning();
+
+ if (g.log_bitmask & MASK_LOG_GPS)
+ Log_Write_GPS(g_gps->time, current_loc.lat, current_loc.lng, g_gps->altitude, current_loc.alt, (long) g_gps->ground_speed, g_gps->ground_course, g_gps->fix, g_gps->num_sats);
+
+ // send all requested output streams with rates requested
+ // between 5 and 45 Hz
+ gcs_data_stream_send(5,45);
+ break;
+
+ // This case controls the slow loop
+ //---------------------------------
+ case 4:
+ medium_loopCounter = 0;
+ delta_ms_medium_loop = millis() - medium_loopTimer;
+ medium_loopTimer = millis();
+
+ if (g.battery_monitoring != 0){
+ read_battery();
+ }
+
+ read_trim_switch();
+
+ slow_loop();
+ break;
+ }
+}
+
+static void slow_loop()
+{
+ // This is the slow (3 1/3 Hz) loop pieces
+ //----------------------------------------
+ switch (slow_loopCounter){
+ case 0:
+ slow_loopCounter++;
+ check_long_failsafe();
+ superslow_loopCounter++;
+ if(superslow_loopCounter >=200) { // 200 = Execute every minute
+ #if HIL_MODE != HIL_MODE_ATTITUDE
+ if(g.compass_enabled) {
+ compass.save_offsets();
+ }
+ #endif
+
+ superslow_loopCounter = 0;
+ }
+ break;
+
+ case 1:
+ slow_loopCounter++;
+
+ // Read 3-position switch on radio
+ // -------------------------------
+ read_control_switch();
+
+ // Read Control Surfaces/Mix switches
+ // ----------------------------------
+ update_servo_switches();
+
+ update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8);
+
+#if MOUNT == ENABLED
+ camera_mount.update_mount_type();
+#endif
+ break;
+
+ case 2:
+ slow_loopCounter = 0;
+
+ update_events();
+
+ mavlink_system.sysid = g.sysid_this_mav; // This is just an ugly hack to keep mavlink_system.sysid sync'd with our parameter
+ gcs_data_stream_send(3,5);
+
+#if USB_MUX_PIN > 0
+ check_usb_mux();
+#endif
+ break;
+ }
+}
+
+static void one_second_loop()
+{
+ if (g.log_bitmask & MASK_LOG_CUR)
+ Log_Write_Current();
+
+ // send a heartbeat
+ gcs_send_message(MSG_HEARTBEAT);
+ gcs_data_stream_send(1,3);
+}
+
+static void update_GPS(void)
+{
+ g_gps->update();
+ update_GPS_light();
+
+ if (g_gps->new_data && g_gps->fix) {
+ // for performance
+ // ---------------
+ gps_fix_count++;
+
+ if(ground_start_count > 1){
+ ground_start_count--;
+ ground_start_avg += g_gps->ground_speed;
+
+ } 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 {
+ if(ENABLE_AIR_START == 1 && (ground_start_avg / 5) < SPEEDFILT){
+ startup_ground();
+
+ if (g.log_bitmask & MASK_LOG_CMD)
+ Log_Write_Startup(TYPE_GROUNDSTART_MSG);
+
+ init_home();
+ } else if (ENABLE_AIR_START == 0) {
+ init_home();
+ }
+
+ if (g.compass_enabled) {
+ // Set compass declination automatically
+ compass.set_initial_location(g_gps->latitude, g_gps->longitude);
+ }
+ ground_start_count = 0;
+ }
+ }
+
+
+ current_loc.lng = g_gps->longitude; // Lon * 10**7
+ current_loc.lat = g_gps->latitude; // Lat * 10**7
+ current_loc.alt = max((g_gps->altitude - home.alt),0);
+ ground_course = g_gps->ground_course;
+ ground_speed = g_gps->ground_speed;
+
+ // see if we've breached the geo-fence
+ geofence_check(false);
+ }
+}
+
+static void update_current_flight_mode(void)
+{ int AOAstart;
+ if(control_mode == AUTO){
+
+ switch(nav_command_ID){
+ case MAV_CMD_NAV_TAKEOFF:
+ case MAV_CMD_NAV_LAND:
+ break;
+ default:
+ hold_course = -1;
+ calc_nav_roll();
+ calc_throttle();
+ break;
+ }
+ }else{
+ switch(control_mode){
+ case RTL:
+ case LOITER:
+ case GUIDED:
+ hold_course = -1;
+ calc_nav_roll();
+ calc_throttle();
+ break;
+
+ case FLY_BY_WIRE_A:
+ case FLY_BY_WIRE_B:
+ break;
+
+ case STABILIZE:
+ nav_roll = 0;
+ nav_pitch = 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
+ // ----------------------------------------------------
+ nav_roll = g.roll_limit / 3;
+ nav_pitch = 0;
+
+ if (failsafe != FAILSAFE_NONE){
+ g.channel_throttle.servo_out = g.throttle_cruise;
+ }
+ break;
+
+ case MANUAL:
+ // servo_out is for Sim control only
+ // ---------------------------------
+ g.channel_roll.servo_out = g.channel_roll.pwm_to_angle();
+ g.channel_pitch.servo_out = g.channel_pitch.pwm_to_angle();
+ g.channel_rudder.servo_out = g.channel_rudder.pwm_to_angle();
+ break;
+ //roll: -13788.000, pitch: -13698.000, thr: 0.000, rud: -13742.000
+
+ }
+ }
+}
+
+static void update_navigation()
+{
+ // wp_distance is in ACTUAL meters, not the *100 meters we get from the GPS
+ // ------------------------------------------------------------------------
+
+ // distance and bearing calcs only
+ if(control_mode == AUTO){
+ verify_commands();
+ }else{
+
+ switch(control_mode){
+ case LOITER:
+ case RTL:
+ case GUIDED:
+ update_loiter();
+ calc_bearing_error();
+ break;
+
+ }
+ }
+}
diff --git a/APMrover2/Attitude.pde b/APMrover2/Attitude.pde
new file mode 100644
index 000000000..5a31fd23c
--- /dev/null
+++ b/APMrover2/Attitude.pde
@@ -0,0 +1,208 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+//****************************************************************
+// Function that controls aileron/rudder, elevator, rudder (if 4 channel control) and throttle to produce desired attitude and airspeed.
+//****************************************************************
+
+static void stabilize()
+{
+ float ch1_inf = 1.0;
+
+ // Calculate desired servo output for the turn // Wheels Direction
+ // ---------------------------------------------
+ g.channel_roll.servo_out = nav_roll;
+
+ // Mix Stick input to allow users to override control surfaces
+ // -----------------------------------------------------------
+ if ((control_mode < FLY_BY_WIRE_A) ||
+ (ENABLE_STICK_MIXING == 1 &&
+ geofence_stickmixing() &&
+ control_mode > FLY_BY_WIRE_B &&
+ failsafe == FAILSAFE_NONE)) {
+
+ // TODO: use RC_Channel control_mix function?
+ ch1_inf = (float)g.channel_roll.radio_in - (float)g.channel_roll.radio_trim;
+ ch1_inf = fabs(ch1_inf);
+ ch1_inf = min(ch1_inf, 400.0);
+ ch1_inf = ((400.0 - ch1_inf) /400.0);
+
+ // scale the sensor input based on the stick input
+ // -----------------------------------------------
+ g.channel_roll.servo_out *= ch1_inf;
+
+ // Mix in stick inputs
+ // -------------------
+ g.channel_roll.servo_out += g.channel_roll.pwm_to_angle();
+
+ //Serial.printf_P(PSTR(" servo_out[CH_ROLL] "));
+ //Serial.println(servo_out[CH_ROLL],DEC);
+
+ }
+
+ g.channel_roll.servo_out = g.channel_roll.servo_out * g.turn_gain;
+ g.channel_rudder.servo_out = g.channel_roll.servo_out;
+}
+
+
+static void crash_checker()
+{
+ if(ahrs.pitch_sensor < -4500){
+ crash_timer = 255;
+ }
+ if(crash_timer > 0)
+ crash_timer--;
+}
+
+static void calc_throttle()
+{
+ int throttle_target = g.throttle_cruise + throttle_nudge + 50;
+
+ // Normal airspeed target
+ target_airspeed = g.airspeed_cruise;
+ groundspeed_error = target_airspeed - (float)ground_speed;
+ g.channel_throttle.servo_out = throttle_target + g.pidTeThrottle.get_pid(groundspeed_error, dTnav);
+ g.channel_throttle.servo_out = constrain(g.channel_throttle.servo_out, g.throttle_min.get(), g.throttle_max.get());
+}
+
+/*****************************************
+ * Calculate desired turn angles (in medium freq loop)
+ *****************************************/
+
+static void calc_nav_roll()
+{
+
+ // Adjust gain based on ground speed
+ nav_gain_scaler = (float)ground_speed / (g.airspeed_cruise * 100.0);
+ nav_gain_scaler = constrain(nav_gain_scaler, 0.2, 1.4);
+
+ // Calculate the required turn of the wheels rover
+ // ----------------------------------------
+
+ // negative error = left turn
+ // positive error = right turn
+
+ nav_roll = g.pidNavRoll.get_pid(bearing_error, dTnav, nav_gain_scaler); //returns desired bank angle in degrees*100
+ nav_roll = constrain(nav_roll, -g.roll_limit.get(), g.roll_limit.get());
+
+}
+
+/*****************************************
+ * Roll servo slew limit
+ *****************************************/
+/*
+float roll_slew_limit(float servo)
+{
+ static float last;
+ float temp = constrain(servo, last-ROLL_SLEW_LIMIT * delta_ms_fast_loop/1000.f, last + ROLL_SLEW_LIMIT * delta_ms_fast_loop/1000.f);
+ last = servo;
+ return temp;
+}*/
+
+/*****************************************
+ * Throttle slew limit
+ *****************************************/
+static void throttle_slew_limit()
+{
+ static int last = 1000;
+ if(g.throttle_slewrate) { // if slew limit rate is set to zero then do not slew limit
+
+ float temp = g.throttle_slewrate * G_Dt * 10.f; // * 10 to scale % to pwm range of 1000 to 2000
+ g.channel_throttle.radio_out = constrain(g.channel_throttle.radio_out, last - (int)temp, last + (int)temp);
+ last = g.channel_throttle.radio_out;
+ }
+}
+
+
+// Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc.
+// Keeps outdated data out of our calculations
+static void reset_I(void)
+{
+ g.pidNavRoll.reset_I();
+ g.pidTeThrottle.reset_I();
+// g.pidAltitudeThrottle.reset_I();
+}
+
+/*****************************************
+* Set the flight control servos based on the current calculated values
+*****************************************/
+static void set_servos(void)
+{
+ int flapSpeedSource = 0;
+
+ // vectorize the rc channels
+ RC_Channel_aux* rc_array[NUM_CHANNELS];
+ rc_array[CH_1] = NULL;
+ rc_array[CH_2] = NULL;
+ rc_array[CH_3] = NULL;
+ rc_array[CH_4] = NULL;
+ rc_array[CH_5] = &g.rc_5;
+ rc_array[CH_6] = &g.rc_6;
+ rc_array[CH_7] = &g.rc_7;
+ rc_array[CH_8] = &g.rc_8;
+
+ if((control_mode == MANUAL) || (control_mode == STABILIZE)){
+ // do a direct pass through of radio values
+ g.channel_roll.radio_out = g.channel_roll.radio_in;
+ g.channel_pitch.radio_out = g.channel_pitch.radio_in;
+
+ g.channel_throttle.radio_out = g.channel_throttle.radio_in;
+ g.channel_rudder.radio_out = g.channel_roll.radio_in;
+ } else {
+
+ g.channel_roll.calc_pwm();
+ g.channel_pitch.calc_pwm();
+ g.channel_rudder.calc_pwm();
+
+ g.channel_throttle.radio_out = g.channel_throttle.radio_in;
+
+ // convert 0 to 100% into PWM
+ g.channel_throttle.servo_out = constrain(g.channel_throttle.servo_out, g.throttle_min.get(), g.throttle_max.get());
+
+ // g.channel_throttle.calc_pwm();
+
+ /* TO DO - fix this for RC_Channel library
+ #if THROTTLE_REVERSE == 1
+ radio_out[CH_THROTTLE] = radio_max(CH_THROTTLE) + radio_min(CH_THROTTLE) - radio_out[CH_THROTTLE];
+ #endif
+ */
+ }
+ if (control_mode >= FLY_BY_WIRE_B) {
+ g.channel_throttle.calc_pwm();
+ /* only do throttle slew limiting in modes where throttle
+ control is automatic */
+ throttle_slew_limit();
+ }
+
+
+#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS
+ // send values to the PWM timers for output
+ // ----------------------------------------
+ APM_RC.OutputCh(CH_1, g.channel_roll.radio_out); // send to Servos
+ APM_RC.OutputCh(CH_2, g.channel_pitch.radio_out); // send to Servos
+ APM_RC.OutputCh(CH_3, g.channel_throttle.radio_out); // send to Servos
+ APM_RC.OutputCh(CH_4, g.channel_rudder.radio_out); // send to Servos
+ // Route configurable aux. functions to their respective servos
+
+ g.rc_5.output_ch(CH_5);
+ g.rc_6.output_ch(CH_6);
+ g.rc_7.output_ch(CH_7);
+ g.rc_8.output_ch(CH_8);
+
+#endif
+}
+
+static void demo_servos(byte i) {
+
+ while(i > 0){
+ gcs_send_text_P(SEVERITY_LOW,PSTR("Demo Servos!"));
+#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS
+ APM_RC.OutputCh(1, 1400);
+ mavlink_delay(400);
+ APM_RC.OutputCh(1, 1600);
+ mavlink_delay(200);
+ APM_RC.OutputCh(1, 1500);
+#endif
+ mavlink_delay(400);
+ i--;
+ }
+}
diff --git a/APMrover2/GCS.h b/APMrover2/GCS.h
new file mode 100644
index 000000000..7047beaf0
--- /dev/null
+++ b/APMrover2/GCS.h
@@ -0,0 +1,178 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/// @file GCS.h
+/// @brief Interface definition for the various Ground Control System protocols.
+
+#ifndef __GCS_H
+#define __GCS_H
+
+#include <FastSerial.h>
+#include <AP_Common.h>
+#include <GCS_MAVLink.h>
+#include <GPS.h>
+#include <Stream.h>
+#include <stdint.h>
+
+///
+/// @class GCS
+/// @brief Class describing the interface between the APM code
+/// proper and the GCS implementation.
+///
+/// GCS' are currently implemented inside the sketch and as such have
+/// access to all global state. The sketch should not, however, call GCS
+/// internal functions - all calls to the GCS should be routed through
+/// this interface (or functions explicitly exposed by a subclass).
+///
+class GCS_Class
+{
+public:
+
+ /// Startup initialisation.
+ ///
+ /// This routine performs any one-off initialisation required before
+ /// GCS messages are exchanged.
+ ///
+ /// @note The stream is expected to be set up and configured for the
+ /// correct bitrate before ::init is called.
+ ///
+ /// @note The stream is currently BetterStream so that we can use the _P
+ /// methods; this may change if Arduino adds them to Print.
+ ///
+ /// @param port The stream over which messages are exchanged.
+ ///
+ void init(FastSerial *port) {
+ _port = port;
+ initialised = true;
+ }
+
+ /// Update GCS state.
+ ///
+ /// This may involve checking for received bytes on the stream,
+ /// or sending additional periodic messages.
+ void update(void) {}
+
+ /// Send a message with a single numeric parameter.
+ ///
+ /// This may be a standalone message, or the GCS driver may
+ /// have its own way of locating additional parameters to send.
+ ///
+ /// @param id ID of the message to send.
+ /// @param param Explicit message parameter.
+ ///
+ void send_message(enum ap_message id) {}
+
+ /// Send a text message.
+ ///
+ /// @param severity A value describing the importance of the message.
+ /// @param str The text to be sent.
+ ///
+ void send_text(gcs_severity severity, const char *str) {}
+
+ /// Send a text message with a PSTR()
+ ///
+ /// @param severity A value describing the importance of the message.
+ /// @param str The text to be sent.
+ ///
+ void send_text(gcs_severity severity, const prog_char_t *str) {}
+
+ // test if frequency within range requested for loop
+ // used by data_stream_send
+ static bool freqLoopMatch(uint16_t freq, uint16_t freqMin, uint16_t freqMax)
+ {
+ if (freq != 0 && freq >= freqMin && freq < freqMax) return true;
+ else return false;
+ }
+
+ // send streams which match frequency range
+ void data_stream_send(uint16_t freqMin, uint16_t freqMax);
+
+ // set to true if this GCS link is active
+ bool initialised;
+
+protected:
+ /// The stream we are communicating over
+ FastSerial *_port;
+};
+
+//
+// GCS class definitions.
+//
+// These are here so that we can declare the GCS object early in the sketch
+// and then reference it statically rather than via a pointer.
+//
+
+///
+/// @class GCS_MAVLINK
+/// @brief The mavlink protocol for qgroundcontrol
+///
+class GCS_MAVLINK : public GCS_Class
+{
+public:
+ GCS_MAVLINK();
+ void update(void);
+ void init(FastSerial *port);
+ void send_message(enum ap_message id);
+ void send_text(gcs_severity severity, const char *str);
+ void send_text(gcs_severity severity, const prog_char_t *str);
+ void data_stream_send(uint16_t freqMin, uint16_t freqMax);
+ void queued_param_send();
+ void queued_waypoint_send();
+
+ static const struct AP_Param::GroupInfo var_info[];
+
+private:
+ void handleMessage(mavlink_message_t * msg);
+
+ /// Perform queued sending operations
+ ///
+ AP_Param *_queued_parameter; ///< next parameter to be sent in queue
+ enum ap_var_type _queued_parameter_type; ///< type of the next parameter
+ AP_Param::ParamToken _queued_parameter_token;///AP_Param token for next() call
+ uint16_t _queued_parameter_index; ///< next queued parameter's index
+ uint16_t _queued_parameter_count; ///< saved count of parameters for queued send
+
+ /// Count the number of reportable parameters.
+ ///
+ /// Not all parameters can be reported via MAVlink. We count the number that are
+ /// so that we can report to a GCS the number of parameters it should expect when it
+ /// requests the full set.
+ ///
+ /// @return The number of reportable parameters.
+ ///
+ uint16_t _count_parameters(); ///< count reportable parameters
+
+ uint16_t _parameter_count; ///< cache of reportable parameters
+
+ mavlink_channel_t chan;
+ uint16_t packet_drops;
+
+#if CLI_ENABLED == ENABLED
+ // this allows us to detect the user wanting the CLI to start
+ uint8_t crlf_count;
+#endif
+
+ // waypoints
+ uint16_t waypoint_request_i; // request index
+ uint16_t waypoint_dest_sysid; // where to send requests
+ uint16_t waypoint_dest_compid; // "
+ bool waypoint_sending; // currently in send process
+ bool waypoint_receiving; // currently receiving
+ uint16_t waypoint_count;
+ uint32_t waypoint_timelast_send; // milliseconds
+ uint32_t waypoint_timelast_receive; // milliseconds
+ uint32_t waypoint_timelast_request; // milliseconds
+ uint16_t waypoint_send_timeout; // milliseconds
+ uint16_t waypoint_receive_timeout; // milliseconds
+
+ // data stream rates
+ AP_Int16 streamRateRawSensors;
+ AP_Int16 streamRateExtendedStatus;
+ AP_Int16 streamRateRCChannels;
+ AP_Int16 streamRateRawController;
+ AP_Int16 streamRatePosition;
+ AP_Int16 streamRateExtra1;
+ AP_Int16 streamRateExtra2;
+ AP_Int16 streamRateExtra3;
+};
+
+#endif // __GCS_H
diff --git a/APMrover2/GCS_Mavlink.pde b/APMrover2/GCS_Mavlink.pde
new file mode 100644
index 000000000..53fd2a7a4
--- /dev/null
+++ b/APMrover2/GCS_Mavlink.pde
@@ -0,0 +1,2181 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#include "Mavlink_compat.h"
+
+// use this to prevent recursion during sensor init
+static bool in_mavlink_delay;
+
+// this costs us 51 bytes, but means that low priority
+// messages don't block the CPU
+static mavlink_statustext_t pending_status;
+
+// true when we have received at least 1 MAVLink packet
+static bool mavlink_active;
+
+// check if a message will fit in the payload space available
+#define CHECK_PAYLOAD_SIZE(id) if (payload_space < MAVLINK_MSG_ID_## id ##_LEN) return false
+
+/*
+ !!NOTE!!
+
+ the use of NOINLINE separate functions for each message type avoids
+ a compiler bug in gcc that would cause it to use far more stack
+ space than is needed. Without the NOINLINE we use the sum of the
+ stack needed for each message type. Please be careful to follow the
+ pattern below when adding any new messages
+ */
+
+static NOINLINE void send_heartbeat(mavlink_channel_t chan)
+{
+#ifdef MAVLINK10
+ uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
+ uint8_t system_status = MAV_STATE_ACTIVE;
+ uint32_t custom_mode = control_mode;
+
+ // work out the base_mode. This value is not very useful
+ // for APM, but we calculate it as best we can so a generic
+ // MAVLink enabled ground station can work out something about
+ // what the MAV is up to. The actual bit values are highly
+ // ambiguous for most of the APM flight modes. In practice, you
+ // only get useful information from the custom_mode, which maps to
+ // the APM flight mode and has a well defined meaning in the
+ // ArduPlane documentation
+ switch (control_mode) {
+ case MANUAL:
+ base_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
+ break;
+ case STABILIZE:
+ case FLY_BY_WIRE_A:
+ case FLY_BY_WIRE_B:
+ case FLY_BY_WIRE_C:
+ base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED;
+ break;
+ case AUTO:
+ case RTL:
+ case LOITER:
+ case GUIDED:
+ case CIRCLE:
+ base_mode = MAV_MODE_FLAG_GUIDED_ENABLED |
+ MAV_MODE_FLAG_STABILIZE_ENABLED;
+ // note that MAV_MODE_FLAG_AUTO_ENABLED does not match what
+ // APM does in any mode, as that is defined as "system finds its own goal
+ // positions", which APM does not currently do
+ break;
+ case INITIALISING:
+ system_status = MAV_STATE_CALIBRATING;
+ break;
+ }
+
+ if (control_mode != MANUAL && control_mode != INITIALISING) {
+ // stabiliser of some form is enabled
+ base_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
+ }
+
+#if ENABLE_STICK_MIXING==ENABLED
+ if (control_mode != INITIALISING) {
+ // all modes except INITIALISING have some form of manual
+ // override if stick mixing is enabled
+ base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
+ }
+#endif
+
+#if HIL_MODE != HIL_MODE_DISABLED
+ base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
+#endif
+
+ // we are armed if we are not initialising
+ if (control_mode != INITIALISING) {
+ base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
+ }
+
+ // indicate we have set a custom mode
+ base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
+
+ mavlink_msg_heartbeat_send(
+ chan,
+ MAV_TYPE_FIXED_WING,
+ MAV_AUTOPILOT_ARDUPILOTMEGA,
+ base_mode,
+ custom_mode,
+ system_status);
+#else // MAVLINK10
+ mavlink_msg_heartbeat_send(
+ chan,
+ mavlink_system.type,
+ MAV_AUTOPILOT_ARDUPILOTMEGA);
+#endif // MAVLINK10
+}
+
+static NOINLINE void send_attitude(mavlink_channel_t chan)
+{
+ Vector3f omega = ahrs.get_gyro();
+ mavlink_msg_attitude_send(
+ chan,
+ micros(),
+ ahrs.roll,
+ ahrs.pitch - radians(g.pitch_trim*0.01),
+ ahrs.yaw,
+ omega.x,
+ omega.y,
+ omega.z);
+}
+
+#if GEOFENCE_ENABLED == ENABLED
+static NOINLINE void send_fence_status(mavlink_channel_t chan)
+{
+ geofence_send_status(chan);
+}
+#endif
+
+
+static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t packet_drops)
+{
+#ifdef MAVLINK10
+ uint32_t control_sensors_present = 0;
+ uint32_t control_sensors_enabled;
+ uint32_t control_sensors_health;
+
+ // first what sensors/controllers we have
+ control_sensors_present |= (1<<0); // 3D gyro present
+ control_sensors_present |= (1<<1); // 3D accelerometer present
+ if (g.compass_enabled) {
+ control_sensors_present |= (1<<2); // compass present
+ }
+ control_sensors_present |= (1<<3); // absolute pressure sensor present
+ if (g_gps->fix) {
+ control_sensors_present |= (1<<5); // GPS present
+ }
+ control_sensors_present |= (1<<10); // 3D angular rate control
+ control_sensors_present |= (1<<11); // attitude stabilisation
+ control_sensors_present |= (1<<12); // yaw position
+ control_sensors_present |= (1<<13); // altitude control
+ control_sensors_present |= (1<<14); // X/Y position control
+ control_sensors_present |= (1<<15); // motor control
+
+ // now what sensors/controllers are enabled
+
+ // first the sensors
+ control_sensors_enabled = control_sensors_present & 0x1FF;
+
+ // now the controllers
+ control_sensors_enabled = control_sensors_present & 0x1FF;
+
+ switch (control_mode) {
+ case MANUAL:
+ break;
+
+ case STABILIZE:
+ case FLY_BY_WIRE_A:
+ control_sensors_enabled |= (1<<10); // 3D angular rate control
+ control_sensors_enabled |= (1<<11); // attitude stabilisation
+ break;
+
+ case FLY_BY_WIRE_B:
+ control_sensors_enabled |= (1<<10); // 3D angular rate control
+ control_sensors_enabled |= (1<<11); // attitude stabilisation
+ control_sensors_enabled |= (1<<15); // motor control
+ break;
+
+ case FLY_BY_WIRE_C:
+ control_sensors_enabled |= (1<<10); // 3D angular rate control
+ control_sensors_enabled |= (1<<11); // attitude stabilisation
+ control_sensors_enabled |= (1<<13); // altitude control
+ control_sensors_enabled |= (1<<15); // motor control
+ break;
+
+ case AUTO:
+ case RTL:
+ case LOITER:
+ case GUIDED:
+ case CIRCLE:
+ control_sensors_enabled |= (1<<10); // 3D angular rate control
+ control_sensors_enabled |= (1<<11); // attitude stabilisation
+ control_sensors_enabled |= (1<<12); // yaw position
+ control_sensors_enabled |= (1<<13); // altitude control
+ control_sensors_enabled |= (1<<14); // X/Y position control
+ control_sensors_enabled |= (1<<15); // motor control
+ break;
+
+ case INITIALISING:
+ break;
+ }
+
+ // at the moment all sensors/controllers are assumed healthy
+ control_sensors_health = control_sensors_present;
+
+ uint16_t battery_current = -1;
+ uint8_t battery_remaining = -1;
+
+ if (current_total1 != 0 && g.pack_capacity != 0) {
+ battery_remaining = (100.0 * (g.pack_capacity - current_total1) / g.pack_capacity);
+ }
+ if (current_total1 != 0) {
+ battery_current = current_amps1 * 100;
+ }
+
+ if (g.battery_monitoring == 3) {
+ /*setting a out-of-range value.
+ It informs to external devices that
+ it cannot be calculated properly just by voltage*/
+ battery_remaining = 150;
+ }
+
+ mavlink_msg_sys_status_send(
+ chan,
+ control_sensors_present,
+ control_sensors_enabled,
+ control_sensors_health,
+ (uint16_t)(load * 1000),
+ battery_voltage1 * 1000, // mV
+ battery_current, // in 10mA units
+ battery_remaining, // in %
+ 0, // comm drops %,
+ 0, // comm drops in pkts,
+ 0, 0, 0, 0);
+
+#else // MAVLINK10
+ uint8_t mode = MAV_MODE_UNINIT;
+ uint8_t nav_mode = MAV_NAV_VECTOR;
+
+ switch(control_mode) {
+ case MANUAL:
+ mode = MAV_MODE_MANUAL;
+ break;
+ case STABILIZE:
+ mode = MAV_MODE_TEST1;
+ break;
+ case FLY_BY_WIRE_A:
+ mode = MAV_MODE_TEST2;
+ nav_mode = 1; //FBW nav_mode mapping; 1=A, 2=B, 3=C, etc.
+ break;
+ case FLY_BY_WIRE_B:
+ mode = MAV_MODE_TEST2;
+ nav_mode = 2; //FBW nav_mode mapping; 1=A, 2=B, 3=C, etc.
+ break;
+ case GUIDED:
+ mode = MAV_MODE_GUIDED;
+ break;
+ case AUTO:
+ mode = MAV_MODE_AUTO;
+ nav_mode = MAV_NAV_WAYPOINT;
+ break;
+ case RTL:
+ mode = MAV_MODE_AUTO;
+ nav_mode = MAV_NAV_RETURNING;
+ break;
+ case LOITER:
+ mode = MAV_MODE_AUTO;
+ nav_mode = MAV_NAV_LOITER;
+ break;
+ case INITIALISING:
+ mode = MAV_MODE_UNINIT;
+ nav_mode = MAV_NAV_GROUNDED;
+ break;
+ case CIRCLE:
+ mode = MAV_MODE_TEST3;
+ break;
+ }
+
+ uint8_t status = MAV_STATE_ACTIVE;
+ uint16_t battery_remaining = 1000.0 * (float)(g.pack_capacity - current_total1)/(float)g.pack_capacity; //Mavlink scaling 100% = 1000
+
+ if (g.battery_monitoring == 3) {
+ /*setting a out-of-range value.
+ It informs to external devices that
+ it cannot be calculated properly just by voltage*/
+ battery_remaining = 1500;
+ }
+
+ mavlink_msg_sys_status_send(
+ chan,
+ mode,
+ nav_mode,
+ status,
+ load * 1000,
+ battery_voltage1 * 1000,
+ battery_remaining,
+ packet_drops);
+#endif // MAVLINK10
+}
+
+static void NOINLINE send_meminfo(mavlink_channel_t chan)
+{
+ extern unsigned __brkval;
+ mavlink_msg_meminfo_send(chan, __brkval, memcheck_available_memory());
+}
+
+static void NOINLINE send_location(mavlink_channel_t chan)
+{
+ Matrix3f rot = ahrs.get_dcm_matrix(); // neglecting angle of attack for now
+ mavlink_msg_global_position_int_send(
+ chan,
+ millis(),
+ current_loc.lat, // in 1E7 degrees
+ current_loc.lng, // in 1E7 degrees
+ g_gps->altitude*10, // millimeters above sea level
+ current_loc.alt * 10, // millimeters above ground
+ g_gps->ground_speed * rot.a.x, // X speed cm/s
+ g_gps->ground_speed * rot.b.x, // Y speed cm/s
+ g_gps->ground_speed * rot.c.x,
+ g_gps->ground_course); // course in 1/100 degree
+}
+
+static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
+{
+ int16_t bearing = (hold_course==-1?nav_bearing:hold_course) / 100;
+ mavlink_msg_nav_controller_output_send(
+ chan,
+ nav_roll / 1.0e2,
+ nav_pitch / 1.0e2,
+ bearing,
+ target_bearing / 100,
+ wp_distance,
+ altitude_error / 1.0e2,
+ airspeed_error,
+ crosstrack_error);
+}
+
+static void NOINLINE send_gps_raw(mavlink_channel_t chan)
+{
+#ifdef MAVLINK10
+ uint8_t fix;
+ if (g_gps->status() == 2) {
+ fix = 3;
+ } else {
+ fix = 0;
+ }
+
+ mavlink_msg_gps_raw_int_send(
+ chan,
+ micros(),
+ fix,
+ g_gps->latitude, // in 1E7 degrees
+ g_gps->longitude, // in 1E7 degrees
+ g_gps->altitude * 10, // in mm
+ g_gps->hdop,
+ 65535,
+ g_gps->ground_speed, // cm/s
+ g_gps->ground_course, // 1/100 degrees,
+ g_gps->num_sats);
+
+#else // MAVLINK10
+ mavlink_msg_gps_raw_send(
+ chan,
+ micros(),
+ g_gps->status(),
+ g_gps->latitude / 1.0e7,
+ g_gps->longitude / 1.0e7,
+ g_gps->altitude / 100.0,
+ g_gps->hdop,
+ 0.0,
+ g_gps->ground_speed / 100.0,
+ g_gps->ground_course / 100.0);
+#endif // MAVLINK10
+}
+
+static void NOINLINE send_servo_out(mavlink_channel_t chan)
+{
+ const uint8_t rssi = 1;
+ // normalized values scaled to -10000 to 10000
+ // This is used for HIL. Do not change without discussing with
+ // HIL maintainers
+#if X_PLANE == ENABLED
+ /* update by JLN for X-Plane or AeroSIM HIL */
+
+ int thr_out = constrain((g.channel_throttle.servo_out *2) - 100, -100, 100); // throttle set from -100 to 100
+ mavlink_msg_rc_channels_scaled_send(
+ chan,
+ g.channel_roll.servo_out,
+ g.channel_pitch.servo_out,
+ 100 * thr_out,
+ g.channel_rudder.servo_out,
+ 10000 * g.channel_roll.norm_output(),
+ 10000 * g.channel_pitch.norm_output(),
+ 10000 * g.channel_throttle.norm_output(),
+ 10000 * g.channel_rudder.norm_output(),
+ rssi);
+#else
+ mavlink_msg_rc_channels_scaled_send(
+ chan,
+ millis(),
+ 0, // port 0
+ 10000 * g.channel_roll.norm_output(),
+ 10000 * g.channel_pitch.norm_output(),
+ 10000 * g.channel_throttle.norm_output(),
+ 10000 * g.channel_rudder.norm_output(),
+ 0,
+ 0,
+ 0,
+ 0,
+ rssi);
+#endif
+}
+
+static void NOINLINE send_radio_in(mavlink_channel_t chan)
+{
+ uint8_t rssi = 1;
+ mavlink_msg_rc_channels_raw_send(
+ chan,
+ millis(),
+ 0, // port
+ g.channel_roll.radio_in,
+ g.channel_pitch.radio_in,
+ g.channel_throttle.radio_in,
+ g.channel_rudder.radio_in,
+ g.rc_5.radio_in, // XXX currently only 4 RC channels defined
+ g.rc_6.radio_in,
+ g.rc_7.radio_in,
+ g.rc_8.radio_in,
+ rssi);
+}
+
+static void NOINLINE send_radio_out(mavlink_channel_t chan)
+{
+ mavlink_msg_servo_output_raw_send(
+ chan,
+ micros(),
+ 0, // port
+ g.channel_roll.radio_out,
+ g.channel_pitch.radio_out,
+ g.channel_throttle.radio_out,
+ g.channel_rudder.radio_out,
+ g.rc_5.radio_out, // XXX currently only 4 RC channels defined
+ g.rc_6.radio_out,
+ g.rc_7.radio_out,
+ g.rc_8.radio_out);
+}
+
+static void NOINLINE send_vfr_hud(mavlink_channel_t chan)
+{
+ mavlink_msg_vfr_hud_send(
+ chan,
+ (float)airspeed / 100.0,
+ (float)g_gps->ground_speed / 100.0,
+ (ahrs.yaw_sensor / 100) % 360,
+ (uint16_t)(100 * (g.channel_throttle.norm_output() / 2.0 + 0.5)), // scale -1,1 to 0-100
+ current_loc.alt / 100.0,
+ 0);
+}
+
+#if HIL_MODE != HIL_MODE_ATTITUDE
+static void NOINLINE send_raw_imu1(mavlink_channel_t chan)
+{
+ Vector3f accel = imu.get_accel();
+ Vector3f gyro = imu.get_gyro();
+
+ mavlink_msg_raw_imu_send(
+ chan,
+ micros(),
+ accel.x * 1000.0 / gravity,
+ accel.y * 1000.0 / gravity,
+ accel.z * 1000.0 / gravity,
+ gyro.x * 1000.0,
+ gyro.y * 1000.0,
+ gyro.z * 1000.0,
+ compass.mag_x,
+ compass.mag_y,
+ compass.mag_z);
+}
+
+static void NOINLINE send_raw_imu2(mavlink_channel_t chan)
+{
+ int32_t pressure = barometer.get_pressure();
+ mavlink_msg_scaled_pressure_send(
+ chan,
+ micros(),
+ pressure/100.0,
+ (pressure - g.ground_pressure)/100.0,
+ barometer.get_temperature());
+}
+
+static void NOINLINE send_raw_imu3(mavlink_channel_t chan)
+{
+ Vector3f mag_offsets = compass.get_offsets();
+
+ mavlink_msg_sensor_offsets_send(chan,
+ mag_offsets.x,
+ mag_offsets.y,
+ mag_offsets.z,
+ compass.get_declination(),
+ barometer.get_raw_pressure(),
+ barometer.get_raw_temp(),
+ imu.gx(), imu.gy(), imu.gz(),
+ imu.ax(), imu.ay(), imu.az());
+}
+#endif // HIL_MODE != HIL_MODE_ATTITUDE
+
+static void NOINLINE send_gps_status(mavlink_channel_t chan)
+{
+ mavlink_msg_gps_status_send(
+ chan,
+ g_gps->num_sats,
+ NULL,
+ NULL,
+ NULL,
+ NULL,
+ NULL);
+}
+
+static void NOINLINE send_current_waypoint(mavlink_channel_t chan)
+{
+ mavlink_msg_waypoint_current_send(
+ chan,
+ g.command_index);
+}
+
+static void NOINLINE send_statustext(mavlink_channel_t chan)
+{
+ mavlink_msg_statustext_send(
+ chan,
+ pending_status.severity,
+ pending_status.text);
+}
+
+
+// try to send a message, return false if it won't fit in the serial tx buffer
+static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, uint16_t packet_drops)
+{
+ int payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES;
+
+ if (chan == MAVLINK_COMM_1 && millis() < MAVLINK_TELEMETRY_PORT_DELAY) {
+ // defer any messages on the telemetry port for 1 second after
+ // bootup, to try to prevent bricking of Xbees
+ return false;
+ }
+
+ switch (id) {
+ case MSG_HEARTBEAT:
+ CHECK_PAYLOAD_SIZE(HEARTBEAT);
+ send_heartbeat(chan);
+ return true;
+
+ case MSG_EXTENDED_STATUS1:
+ CHECK_PAYLOAD_SIZE(SYS_STATUS);
+ send_extended_status1(chan, packet_drops);
+ break;
+
+ case MSG_EXTENDED_STATUS2:
+ CHECK_PAYLOAD_SIZE(MEMINFO);
+ send_meminfo(chan);
+ break;
+
+ case MSG_ATTITUDE:
+ CHECK_PAYLOAD_SIZE(ATTITUDE);
+ send_attitude(chan);
+ break;
+
+ case MSG_LOCATION:
+ CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT);
+ send_location(chan);
+ break;
+
+ case MSG_NAV_CONTROLLER_OUTPUT:
+ if (control_mode != MANUAL) {
+ CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
+ send_nav_controller_output(chan);
+ }
+ break;
+
+ case MSG_GPS_RAW:
+#ifdef MAVLINK10
+ CHECK_PAYLOAD_SIZE(GPS_RAW_INT);
+#else
+ CHECK_PAYLOAD_SIZE(GPS_RAW);
+#endif
+ send_gps_raw(chan);
+ break;
+
+ case MSG_SERVO_OUT:
+ CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED);
+ send_servo_out(chan);
+ break;
+
+ case MSG_RADIO_IN:
+ CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW);
+ send_radio_in(chan);
+ break;
+
+ case MSG_RADIO_OUT:
+ CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW);
+ send_radio_out(chan);
+ break;
+
+ case MSG_VFR_HUD:
+ CHECK_PAYLOAD_SIZE(VFR_HUD);
+ send_vfr_hud(chan);
+ break;
+
+#if HIL_MODE != HIL_MODE_ATTITUDE
+ case MSG_RAW_IMU1:
+ CHECK_PAYLOAD_SIZE(RAW_IMU);
+ send_raw_imu1(chan);
+ break;
+
+ case MSG_RAW_IMU2:
+ CHECK_PAYLOAD_SIZE(SCALED_PRESSURE);
+ send_raw_imu2(chan);
+ break;
+
+ case MSG_RAW_IMU3:
+ CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS);
+ send_raw_imu3(chan);
+ break;
+#endif // HIL_MODE != HIL_MODE_ATTITUDE
+
+ case MSG_GPS_STATUS:
+ CHECK_PAYLOAD_SIZE(GPS_STATUS);
+ send_gps_status(chan);
+ break;
+
+ case MSG_CURRENT_WAYPOINT:
+ CHECK_PAYLOAD_SIZE(WAYPOINT_CURRENT);
+ send_current_waypoint(chan);
+ break;
+
+ case MSG_NEXT_PARAM:
+ CHECK_PAYLOAD_SIZE(PARAM_VALUE);
+ if (chan == MAVLINK_COMM_0) {
+ gcs0.queued_param_send();
+ } else if (gcs3.initialised) {
+ gcs3.queued_param_send();
+ }
+ break;
+
+ case MSG_NEXT_WAYPOINT:
+ CHECK_PAYLOAD_SIZE(WAYPOINT_REQUEST);
+ if (chan == MAVLINK_COMM_0) {
+ gcs0.queued_waypoint_send();
+ } else if (gcs3.initialised) {
+ gcs3.queued_waypoint_send();
+ }
+ break;
+
+ case MSG_STATUSTEXT:
+ CHECK_PAYLOAD_SIZE(STATUSTEXT);
+ send_statustext(chan);
+ break;
+
+#if GEOFENCE_ENABLED == ENABLED
+ case MSG_FENCE_STATUS:
+ CHECK_PAYLOAD_SIZE(FENCE_STATUS);
+ send_fence_status(chan);
+ break;
+#endif
+
+ case MSG_RETRY_DEFERRED:
+ break; // just here to prevent a warning
+ }
+ return true;
+}
+
+
+#define MAX_DEFERRED_MESSAGES MSG_RETRY_DEFERRED
+static struct mavlink_queue {
+ enum ap_message deferred_messages[MAX_DEFERRED_MESSAGES];
+ uint8_t next_deferred_message;
+ uint8_t num_deferred_messages;
+} mavlink_queue[2];
+
+// send a message using mavlink
+static void mavlink_send_message(mavlink_channel_t chan, enum ap_message id, uint16_t packet_drops)
+{
+ uint8_t i, nextid;
+ struct mavlink_queue *q = &mavlink_queue[(uint8_t)chan];
+
+ // see if we can send the deferred messages, if any
+ while (q->num_deferred_messages != 0) {
+ if (!mavlink_try_send_message(chan,
+ q->deferred_messages[q->next_deferred_message],
+ packet_drops)) {
+ break;
+ }
+ q->next_deferred_message++;
+ if (q->next_deferred_message == MAX_DEFERRED_MESSAGES) {
+ q->next_deferred_message = 0;
+ }
+ q->num_deferred_messages--;
+ }
+
+ if (id == MSG_RETRY_DEFERRED) {
+ return;
+ }
+
+ // this message id might already be deferred
+ for (i=0, nextid = q->next_deferred_message; i < q->num_deferred_messages; i++) {
+ if (q->deferred_messages[nextid] == id) {
+ // its already deferred, discard
+ return;
+ }
+ nextid++;
+ if (nextid == MAX_DEFERRED_MESSAGES) {
+ nextid = 0;
+ }
+ }
+
+ if (q->num_deferred_messages != 0 ||
+ !mavlink_try_send_message(chan, id, packet_drops)) {
+ // can't send it now, so defer it
+ if (q->num_deferred_messages == MAX_DEFERRED_MESSAGES) {
+ // the defer buffer is full, discard
+ return;
+ }
+ nextid = q->next_deferred_message + q->num_deferred_messages;
+ if (nextid >= MAX_DEFERRED_MESSAGES) {
+ nextid -= MAX_DEFERRED_MESSAGES;
+ }
+ q->deferred_messages[nextid] = id;
+ q->num_deferred_messages++;
+ }
+}
+
+void mavlink_send_text(mavlink_channel_t chan, gcs_severity severity, const char *str)
+{
+ if (chan == MAVLINK_COMM_1 && millis() < MAVLINK_TELEMETRY_PORT_DELAY) {
+ // don't send status MAVLink messages for 2 seconds after
+ // bootup, to try to prevent Xbee bricking
+ return;
+ }
+
+ if (severity == SEVERITY_LOW) {
+ // send via the deferred queuing system
+ pending_status.severity = (uint8_t)severity;
+ strncpy((char *)pending_status.text, str, sizeof(pending_status.text));
+ mavlink_send_message(chan, MSG_STATUSTEXT, 0);
+ } else {
+ // send immediately
+ mavlink_msg_statustext_send(chan, severity, str);
+ }
+}
+
+const AP_Param::GroupInfo GCS_MAVLINK::var_info[] PROGMEM = {
+ AP_GROUPINFO("RAW_SENS", 0, GCS_MAVLINK, streamRateRawSensors),
+ AP_GROUPINFO("EXT_STAT", 1, GCS_MAVLINK, streamRateExtendedStatus),
+ AP_GROUPINFO("RC_CHAN", 2, GCS_MAVLINK, streamRateRCChannels),
+ AP_GROUPINFO("RAW_CTRL", 3, GCS_MAVLINK, streamRateRawController),
+ AP_GROUPINFO("POSITION", 4, GCS_MAVLINK, streamRatePosition),
+ AP_GROUPINFO("EXTRA1", 5, GCS_MAVLINK, streamRateExtra1),
+ AP_GROUPINFO("EXTRA2", 6, GCS_MAVLINK, streamRateExtra2),
+ AP_GROUPINFO("EXTRA3", 7, GCS_MAVLINK, streamRateExtra3),
+ AP_GROUPEND
+};
+
+
+GCS_MAVLINK::GCS_MAVLINK() :
+ packet_drops(0),
+ waypoint_send_timeout(1000), // 1 second
+ waypoint_receive_timeout(1000) // 1 second
+{
+}
+
+void
+GCS_MAVLINK::init(FastSerial * port)
+{
+ GCS_Class::init(port);
+ if (port == &Serial) {
+ mavlink_comm_0_port = port;
+ chan = MAVLINK_COMM_0;
+ }else{
+ mavlink_comm_1_port = port;
+ chan = MAVLINK_COMM_1;
+ }
+ _queued_parameter = NULL;
+}
+
+void
+GCS_MAVLINK::update(void)
+{
+ // receive new packets
+ mavlink_message_t msg;
+ mavlink_status_t status;
+ status.packet_rx_drop_count = 0;
+
+ // process received bytes
+ while (comm_get_available(chan))
+ {
+ uint8_t c = comm_receive_ch(chan);
+
+#if CLI_ENABLED == ENABLED
+ /* allow CLI to be started by hitting enter 3 times, if no
+ heartbeat packets have been received */
+ if (mavlink_active == 0 && millis() < 20000) {
+ if (c == '\n' || c == '\r') {
+ crlf_count++;
+ } else {
+ crlf_count = 0;
+ }
+ if (crlf_count == 3) {
+ run_cli();
+ }
+ }
+#endif
+
+ // Try to get a new message
+ if (mavlink_parse_char(chan, c, &msg, &status)) {
+ mavlink_active = 1;
+ handleMessage(&msg);
+ }
+ }
+
+ // Update packet drops counter
+ packet_drops += status.packet_rx_drop_count;
+
+ // send out queued params/ waypoints
+ if (NULL != _queued_parameter) {
+ send_message(MSG_NEXT_PARAM);
+ }
+
+ if (!waypoint_receiving && !waypoint_sending) {
+ return;
+ }
+
+ uint32_t tnow = millis();
+
+ if (waypoint_receiving &&
+ waypoint_request_i <= (unsigned)g.command_total &&
+ tnow > waypoint_timelast_request + 500) {
+ waypoint_timelast_request = tnow;
+ send_message(MSG_NEXT_WAYPOINT);
+ }
+
+ // stop waypoint sending if timeout
+ if (waypoint_sending && (millis() - waypoint_timelast_send) > waypoint_send_timeout){
+ waypoint_sending = false;
+ }
+
+ // stop waypoint receiving if timeout
+ if (waypoint_receiving && (millis() - waypoint_timelast_receive) > waypoint_receive_timeout){
+ waypoint_receiving = false;
+ }
+}
+
+void
+GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax)
+{
+ if (waypoint_sending == false && waypoint_receiving == false && _queued_parameter == NULL) {
+
+ if (freqLoopMatch(streamRateRawSensors, freqMin, freqMax)){
+ send_message(MSG_RAW_IMU1);
+ send_message(MSG_RAW_IMU2);
+ send_message(MSG_RAW_IMU3);
+ }
+
+ if (freqLoopMatch(streamRateExtendedStatus, freqMin, freqMax)) {
+ send_message(MSG_EXTENDED_STATUS1);
+ send_message(MSG_EXTENDED_STATUS2);
+ send_message(MSG_GPS_STATUS);
+ send_message(MSG_CURRENT_WAYPOINT);
+ send_message(MSG_GPS_RAW); // TODO - remove this message after location message is working
+ send_message(MSG_NAV_CONTROLLER_OUTPUT);
+ send_message(MSG_FENCE_STATUS);
+ }
+
+ if (freqLoopMatch(streamRatePosition, freqMin, freqMax)) {
+ // sent with GPS read
+ send_message(MSG_LOCATION);
+ }
+
+ if (freqLoopMatch(streamRateRawController, freqMin, freqMax)) {
+ // This is used for HIL. Do not change without discussing with HIL maintainers
+ send_message(MSG_SERVO_OUT);
+ }
+
+ if (freqLoopMatch(streamRateRCChannels, freqMin, freqMax)) {
+ send_message(MSG_RADIO_OUT);
+ send_message(MSG_RADIO_IN);
+ }
+
+ if (freqLoopMatch(streamRateExtra1, freqMin, freqMax)){ // Use Extra 1 for AHRS info
+ send_message(MSG_ATTITUDE);
+ }
+
+ if (freqLoopMatch(streamRateExtra2, freqMin, freqMax)){ // Use Extra 2 for additional HIL info
+ send_message(MSG_VFR_HUD);
+ }
+
+ if (freqLoopMatch(streamRateExtra3, freqMin, freqMax)){
+ // Available datastream
+ }
+ }
+}
+
+
+
+void
+GCS_MAVLINK::send_message(enum ap_message id)
+{
+ mavlink_send_message(chan,id, packet_drops);
+}
+
+void
+GCS_MAVLINK::send_text(gcs_severity severity, const char *str)
+{
+ mavlink_send_text(chan,severity,str);
+}
+
+void
+GCS_MAVLINK::send_text(gcs_severity severity, const prog_char_t *str)
+{
+ mavlink_statustext_t m;
+ uint8_t i;
+ for (i=0; i<sizeof(m.text); i++) {
+ m.text[i] = pgm_read_byte((const prog_char *)(str++));
+ }
+ if (i < sizeof(m.text)) m.text[i] = 0;
+ mavlink_send_text(chan, severity, (const char *)m.text);
+}
+
+void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
+{
+ struct Location tell_command = {}; // command for telemetry
+ static uint8_t mav_nav=255; // For setting mode (some require receipt of 2 messages...)
+
+ switch (msg->msgid) {
+
+ case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
+ {
+ // decode
+ mavlink_request_data_stream_t packet;
+ mavlink_msg_request_data_stream_decode(msg, &packet);
+
+ if (mavlink_check_target(packet.target_system, packet.target_component))
+ break;
+
+ int freq = 0; // packet frequency
+
+ if (packet.start_stop == 0)
+ freq = 0; // stop sending
+ else if (packet.start_stop == 1)
+ freq = packet.req_message_rate; // start sending
+ else
+ break;
+
+ switch(packet.req_stream_id){
+
+ case MAV_DATA_STREAM_ALL:
+ streamRateRawSensors.set_and_save_ifchanged(freq);
+ streamRateExtendedStatus.set_and_save_ifchanged(freq);
+ streamRateRCChannels.set_and_save_ifchanged(freq);
+ streamRateRawController.set_and_save_ifchanged(freq);
+ streamRatePosition.set_and_save_ifchanged(freq);
+ streamRateExtra1.set_and_save_ifchanged(freq);
+ streamRateExtra2.set_and_save_ifchanged(freq);
+ streamRateExtra3.set_and_save_ifchanged(freq);
+ break;
+
+ case MAV_DATA_STREAM_RAW_SENSORS:
+ streamRateRawSensors = freq; // We do not set and save this one so that if HIL is shut down incorrectly
+ // we will not continue to broadcast raw sensor data at 50Hz.
+ break;
+ case MAV_DATA_STREAM_EXTENDED_STATUS:
+ streamRateExtendedStatus.set_and_save_ifchanged(freq);
+ break;
+
+ case MAV_DATA_STREAM_RC_CHANNELS:
+ streamRateRCChannels.set_and_save_ifchanged(freq);
+ break;
+
+ case MAV_DATA_STREAM_RAW_CONTROLLER:
+ streamRateRawController.set_and_save_ifchanged(freq);
+ break;
+
+ //case MAV_DATA_STREAM_RAW_SENSOR_FUSION:
+ // streamRateRawSensorFusion.set_and_save(freq);
+ // break;
+
+ case MAV_DATA_STREAM_POSITION:
+ streamRatePosition.set_and_save_ifchanged(freq);
+ break;
+
+ case MAV_DATA_STREAM_EXTRA1:
+ streamRateExtra1.set_and_save_ifchanged(freq);
+ break;
+
+ case MAV_DATA_STREAM_EXTRA2:
+ streamRateExtra2.set_and_save_ifchanged(freq);
+ break;
+
+ case MAV_DATA_STREAM_EXTRA3:
+ streamRateExtra3.set_and_save_ifchanged(freq);
+ break;
+
+ default:
+ break;
+ }
+ break;
+ }
+
+#ifdef MAVLINK10
+ case MAVLINK_MSG_ID_COMMAND_LONG:
+ {
+ // decode
+ mavlink_command_long_t packet;
+ mavlink_msg_command_long_decode(msg, &packet);
+ if (mavlink_check_target(packet.target_system, packet.target_component)) break;
+
+ uint8_t result;
+
+ // do command
+ send_text(SEVERITY_LOW,PSTR("command received: "));
+
+ switch(packet.command) {
+
+ case MAV_CMD_NAV_LOITER_UNLIM:
+ set_mode(LOITER);
+ result = MAV_RESULT_ACCEPTED;
+ break;
+
+ case MAV_CMD_NAV_RETURN_TO_LAUNCH:
+ set_mode(RTL);
+ result = MAV_RESULT_ACCEPTED;
+ break;
+
+#if 0
+ // not implemented yet, but could implement some of them
+ case MAV_CMD_NAV_LAND:
+ case MAV_CMD_NAV_TAKEOFF:
+ case MAV_CMD_NAV_ROI:
+ case MAV_CMD_NAV_PATHPLANNING:
+ break;
+#endif
+
+
+ case MAV_CMD_PREFLIGHT_CALIBRATION:
+ if (packet.param1 == 1 ||
+ packet.param2 == 1 ||
+ packet.param3 == 1) {
+ startup_IMU_ground(false);
+ }
+ if (packet.param4 == 1) {
+ trim_radio();
+ }
+ result = MAV_RESULT_ACCEPTED;
+ break;
+
+
+ default:
+ result = MAV_RESULT_UNSUPPORTED;
+ break;
+ }
+
+ mavlink_msg_command_ack_send(
+ chan,
+ packet.command,
+ result);
+
+ break;
+ }
+
+#else // MAVLINK10
+ case MAVLINK_MSG_ID_ACTION:
+ {
+ // decode
+ mavlink_action_t packet;
+ mavlink_msg_action_decode(msg, &packet);
+ if (mavlink_check_target(packet.target,packet.target_component)) break;
+
+ uint8_t result = 0;
+
+ // do action
+ send_text(SEVERITY_LOW,PSTR("action received: "));
+//Serial.println(packet.action);
+ switch(packet.action){
+
+ case MAV_ACTION_LAUNCH:
+ //set_mode(TAKEOFF);
+ break;
+
+ case MAV_ACTION_RETURN:
+ set_mode(RTL);
+ result=1;
+ break;
+
+ case MAV_ACTION_EMCY_LAND:
+ //set_mode(LAND);
+ break;
+
+ case MAV_ACTION_HALT:
+ do_loiter_at_location();
+ result=1;
+ break;
+
+ /* No mappable implementation in APM 2.0
+ case MAV_ACTION_MOTORS_START:
+ case MAV_ACTION_CONFIRM_KILL:
+ case MAV_ACTION_EMCY_KILL:
+ case MAV_ACTION_MOTORS_STOP:
+ case MAV_ACTION_SHUTDOWN:
+ break;
+ */
+
+ case MAV_ACTION_CONTINUE:
+ process_next_command();
+ result=1;
+ break;
+
+ case MAV_ACTION_SET_MANUAL:
+ set_mode(MANUAL);
+ result=1;
+ break;
+
+ case MAV_ACTION_SET_AUTO:
+ set_mode(AUTO);
+ result=1;
+ // clearing failsafe should not be needed
+ // here. Added based on some puzzling results in
+ // the simulator (tridge)
+ failsafe = FAILSAFE_NONE;
+ break;
+
+ case MAV_ACTION_STORAGE_READ:
+ // we load all variables at startup, and
+ // save on each mavlink set
+ result=1;
+ break;
+
+ case MAV_ACTION_STORAGE_WRITE:
+ // this doesn't make any sense, as we save
+ // all settings as they come in
+ result=1;
+ break;
+
+ case MAV_ACTION_CALIBRATE_RC: //break;
+ trim_radio();
+ result=1;
+ break;
+
+ case MAV_ACTION_CALIBRATE_GYRO:
+ case MAV_ACTION_CALIBRATE_MAG:
+ case MAV_ACTION_CALIBRATE_ACC:
+ case MAV_ACTION_CALIBRATE_PRESSURE:
+ case MAV_ACTION_REBOOT: // this is a rough interpretation
+ startup_IMU_ground(true);
+ result=1;
+ break;
+
+ /* For future implemtation
+ case MAV_ACTION_REC_START: break;
+ case MAV_ACTION_REC_PAUSE: break;
+ case MAV_ACTION_REC_STOP: break;
+ */
+
+ /* Takeoff is not an implemented flight mode in APM 2.0
+ case MAV_ACTION_TAKEOFF:
+ set_mode(TAKEOFF);
+ break;
+ */
+
+ case MAV_ACTION_NAVIGATE:
+ set_mode(AUTO);
+ result=1;
+ break;
+
+ /* Land is not an implemented flight mode in APM 2.0
+ case MAV_ACTION_LAND:
+ set_mode(LAND);
+ break;
+ */
+
+ case MAV_ACTION_LOITER:
+ set_mode(LOITER);
+ result=1;
+ break;
+
+ default: break;
+ }
+
+ mavlink_msg_action_ack_send(
+ chan,
+ packet.action,
+ result
+ );
+
+ break;
+ }
+#endif
+
+ case MAVLINK_MSG_ID_SET_MODE:
+ {
+ // decode
+ mavlink_set_mode_t packet;
+ mavlink_msg_set_mode_decode(msg, &packet);
+
+#ifdef MAVLINK10
+ if (!(packet.base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED)) {
+ // we ignore base_mode as there is no sane way to map
+ // from that bitmap to a APM flight mode. We rely on
+ // custom_mode instead.
+ break;
+ }
+ switch (packet.custom_mode) {
+ case MANUAL:
+ case CIRCLE:
+ case STABILIZE:
+ case FLY_BY_WIRE_A:
+ case FLY_BY_WIRE_B:
+ case FLY_BY_WIRE_C:
+ case AUTO:
+ case RTL:
+ case LOITER:
+ set_mode(packet.custom_mode);
+ break;
+ }
+
+#else // MAVLINK10
+
+ switch(packet.mode){
+
+ case MAV_MODE_MANUAL:
+ set_mode(MANUAL);
+ break;
+
+ case MAV_MODE_GUIDED:
+ set_mode(GUIDED);
+ break;
+
+ case MAV_MODE_AUTO:
+ if(mav_nav == 255 || mav_nav == MAV_NAV_WAYPOINT) set_mode(AUTO);
+ if(mav_nav == MAV_NAV_RETURNING) set_mode(RTL);
+ if(mav_nav == MAV_NAV_LOITER) set_mode(LOITER);
+ mav_nav = 255;
+ break;
+
+ case MAV_MODE_TEST1:
+ set_mode(STABILIZE);
+ break;
+
+ case MAV_MODE_TEST2:
+ if(mav_nav == 255 || mav_nav == 1) set_mode(FLY_BY_WIRE_A);
+ if(mav_nav == 2) set_mode(FLY_BY_WIRE_B);
+ //if(mav_nav == 3) set_mode(FLY_BY_WIRE_C);
+ mav_nav = 255;
+ break;
+
+ }
+#endif
+ break;
+ }
+
+#ifndef MAVLINK10
+ case MAVLINK_MSG_ID_SET_NAV_MODE:
+ {
+ // decode
+ mavlink_set_nav_mode_t packet;
+ mavlink_msg_set_nav_mode_decode(msg, &packet);
+ // To set some flight modes we must first receive a "set nav mode" message and then a "set mode" message
+ mav_nav = packet.nav_mode;
+ break;
+ }
+#endif // MAVLINK10
+
+
+ case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST:
+ {
+ // decode
+ mavlink_waypoint_request_list_t packet;
+ mavlink_msg_waypoint_request_list_decode(msg, &packet);
+ if (mavlink_check_target(packet.target_system, packet.target_component))
+ break;
+
+ // Start sending waypoints
+ mavlink_msg_waypoint_count_send(
+ chan,msg->sysid,
+ msg->compid,
+ g.command_total + 1); // + home
+
+ waypoint_timelast_send = millis();
+ waypoint_sending = true;
+ waypoint_receiving = false;
+ waypoint_dest_sysid = msg->sysid;
+ waypoint_dest_compid = msg->compid;
+ break;
+ }
+
+
+ // XXX read a WP from EEPROM and send it to the GCS
+ case MAVLINK_MSG_ID_WAYPOINT_REQUEST:
+ {
+ // Check if sending waypiont
+ //if (!waypoint_sending) break;
+ // 5/10/11 - We are trying out relaxing the requirement that we be in waypoint sending mode to respond to a waypoint request. DEW
+
+ // decode
+ mavlink_waypoint_request_t packet;
+ mavlink_msg_waypoint_request_decode(msg, &packet);
+
+ if (mavlink_check_target(packet.target_system, packet.target_component))
+ break;
+
+ // send waypoint
+ tell_command = get_cmd_with_index(packet.seq);
+
+ // set frame of waypoint
+ uint8_t frame;
+
+ if (tell_command.options & MASK_OPTIONS_RELATIVE_ALT) {
+ frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; // reference frame
+ } else {
+ frame = MAV_FRAME_GLOBAL; // reference frame
+ }
+
+ float param1 = 0, param2 = 0 , param3 = 0, param4 = 0;
+
+ // time that the mav should loiter in milliseconds
+ uint8_t current = 0; // 1 (true), 0 (false)
+
+ if (packet.seq == (uint16_t)g.command_index)
+ current = 1;
+
+ uint8_t autocontinue = 1; // 1 (true), 0 (false)
+
+ float x = 0, y = 0, z = 0;
+
+ if (tell_command.id < MAV_CMD_NAV_LAST || tell_command.id == MAV_CMD_CONDITION_CHANGE_ALT) {
+ // command needs scaling
+ x = tell_command.lat/1.0e7; // local (x), global (latitude)
+ y = tell_command.lng/1.0e7; // local (y), global (longitude)
+ if (tell_command.options & MASK_OPTIONS_RELATIVE_ALT) {
+ z = (tell_command.alt - home.alt) / 1.0e2; // because tell_command.alt already includes a += home.alt
+ } else {
+ z = tell_command.alt/1.0e2; // local (z), global/relative (altitude)
+ }
+ }
+
+ switch (tell_command.id) { // Switch to map APM command fields inot MAVLink command fields
+
+ case MAV_CMD_NAV_LOITER_TURNS:
+ case MAV_CMD_NAV_TAKEOFF:
+ case MAV_CMD_DO_SET_HOME:
+ param1 = tell_command.p1;
+ break;
+
+ case MAV_CMD_NAV_LOITER_TIME:
+ param1 = tell_command.p1*10; // APM loiter time is in ten second increments
+ break;
+
+ case MAV_CMD_CONDITION_CHANGE_ALT:
+ x=0; // Clear fields loaded above that we don't want sent for this command
+ y=0;
+ case MAV_CMD_CONDITION_DELAY:
+ case MAV_CMD_CONDITION_DISTANCE:
+ param1 = tell_command.lat;
+ break;
+
+ case MAV_CMD_DO_JUMP:
+ param2 = tell_command.lat;
+ param1 = tell_command.p1;
+ break;
+
+ case MAV_CMD_DO_REPEAT_SERVO:
+ param4 = tell_command.lng;
+ case MAV_CMD_DO_REPEAT_RELAY:
+ case MAV_CMD_DO_CHANGE_SPEED:
+ param3 = tell_command.lat;
+ param2 = tell_command.alt;
+ param1 = tell_command.p1;
+ break;
+
+ case MAV_CMD_DO_SET_PARAMETER:
+ case MAV_CMD_DO_SET_RELAY:
+ case MAV_CMD_DO_SET_SERVO:
+ param2 = tell_command.alt;
+ param1 = tell_command.p1;
+ break;
+ }
+
+ mavlink_msg_waypoint_send(chan,msg->sysid,
+ msg->compid,
+ packet.seq,
+ frame,
+ tell_command.id,
+ current,
+ autocontinue,
+ param1,
+ param2,
+ param3,
+ param4,
+ x,
+ y,
+ z);
+
+ // update last waypoint comm stamp
+ waypoint_timelast_send = millis();
+ break;
+ }
+
+
+ case MAVLINK_MSG_ID_WAYPOINT_ACK:
+ {
+ // decode
+ mavlink_waypoint_ack_t packet;
+ mavlink_msg_waypoint_ack_decode(msg, &packet);
+ if (mavlink_check_target(packet.target_system,packet.target_component)) break;
+
+ // turn off waypoint send
+ waypoint_sending = false;
+ break;
+ }
+
+ case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
+ {
+ // decode
+ mavlink_param_request_list_t packet;
+ mavlink_msg_param_request_list_decode(msg, &packet);
+ if (mavlink_check_target(packet.target_system,packet.target_component)) break;
+
+ // Start sending parameters - next call to ::update will kick the first one out
+
+ _queued_parameter = AP_Param::first(&_queued_parameter_token, &_queued_parameter_type);
+ _queued_parameter_index = 0;
+ _queued_parameter_count = _count_parameters();
+ break;
+ }
+
+ case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL:
+ {
+ // decode
+ mavlink_waypoint_clear_all_t packet;
+ mavlink_msg_waypoint_clear_all_decode(msg, &packet);
+ if (mavlink_check_target(packet.target_system, packet.target_component)) break;
+
+ // clear all commands
+ g.command_total.set_and_save(0);
+
+ // note that we don't send multiple acks, as otherwise a
+ // GCS that is doing a clear followed by a set may see
+ // the additional ACKs as ACKs of the set operations
+ mavlink_msg_waypoint_ack_send(chan, msg->sysid, msg->compid, MAV_MISSION_ACCEPTED);
+ break;
+ }
+
+ case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT:
+ {
+ // decode
+ mavlink_waypoint_set_current_t packet;
+ mavlink_msg_waypoint_set_current_decode(msg, &packet);
+ if (mavlink_check_target(packet.target_system,packet.target_component)) break;
+
+ // set current command
+ change_command(packet.seq);
+
+ mavlink_msg_waypoint_current_send(chan, g.command_index);
+ break;
+ }
+
+ case MAVLINK_MSG_ID_WAYPOINT_COUNT:
+ {
+ // decode
+ mavlink_waypoint_count_t packet;
+ mavlink_msg_waypoint_count_decode(msg, &packet);
+ if (mavlink_check_target(packet.target_system,packet.target_component)) break;
+
+ // start waypoint receiving
+ if (packet.count > MAX_WAYPOINTS) {
+ packet.count = MAX_WAYPOINTS;
+ }
+ g.command_total.set_and_save(packet.count - 1);
+
+ waypoint_timelast_receive = millis();
+ waypoint_timelast_request = 0;
+ waypoint_receiving = true;
+ waypoint_sending = false;
+ waypoint_request_i = 0;
+ break;
+ }
+
+#ifdef MAVLINK_MSG_ID_SET_MAG_OFFSETS
+ case MAVLINK_MSG_ID_SET_MAG_OFFSETS:
+ {
+ mavlink_set_mag_offsets_t packet;
+ mavlink_msg_set_mag_offsets_decode(msg, &packet);
+ if (mavlink_check_target(packet.target_system,packet.target_component)) break;
+ compass.set_offsets(Vector3f(packet.mag_ofs_x, packet.mag_ofs_y, packet.mag_ofs_z));
+ break;
+ }
+#endif
+
+ // XXX receive a WP from GCS and store in EEPROM
+ case MAVLINK_MSG_ID_WAYPOINT:
+ {
+ // decode
+ mavlink_waypoint_t packet;
+ uint8_t result = MAV_MISSION_ACCEPTED;
+
+ mavlink_msg_waypoint_decode(msg, &packet);
+ if (mavlink_check_target(packet.target_system,packet.target_component)) break;
+
+ // defaults
+ tell_command.id = packet.command;
+
+ switch (packet.frame)
+ {
+ case MAV_FRAME_MISSION:
+ case MAV_FRAME_GLOBAL:
+ {
+ tell_command.lat = 1.0e7*packet.x; // in as DD converted to * t7
+ tell_command.lng = 1.0e7*packet.y; // in as DD converted to * t7
+ tell_command.alt = packet.z*1.0e2; // in as m converted to cm
+ tell_command.options = 0; // absolute altitude
+ break;
+ }
+
+#ifdef MAV_FRAME_LOCAL_NED
+ case MAV_FRAME_LOCAL_NED: // local (relative to home position)
+ {
+ tell_command.lat = 1.0e7*ToDeg(packet.x/
+ (radius_of_earth*cos(ToRad(home.lat/1.0e7)))) + home.lat;
+ tell_command.lng = 1.0e7*ToDeg(packet.y/radius_of_earth) + home.lng;
+ tell_command.alt = -packet.z*1.0e2;
+ tell_command.options = MASK_OPTIONS_RELATIVE_ALT;
+ break;
+ }
+#endif
+
+#ifdef MAV_FRAME_LOCAL
+ case MAV_FRAME_LOCAL: // local (relative to home position)
+ {
+ tell_command.lat = 1.0e7*ToDeg(packet.x/
+ (radius_of_earth*cos(ToRad(home.lat/1.0e7)))) + home.lat;
+ tell_command.lng = 1.0e7*ToDeg(packet.y/radius_of_earth) + home.lng;
+ tell_command.alt = packet.z*1.0e2;
+ tell_command.options = MASK_OPTIONS_RELATIVE_ALT;
+ break;
+ }
+#endif
+
+ case MAV_FRAME_GLOBAL_RELATIVE_ALT: // absolute lat/lng, relative altitude
+ {
+ tell_command.lat = 1.0e7 * packet.x; // in as DD converted to * t7
+ tell_command.lng = 1.0e7 * packet.y; // in as DD converted to * t7
+ tell_command.alt = packet.z * 1.0e2;
+ tell_command.options = MASK_OPTIONS_RELATIVE_ALT; // store altitude relative!! Always!!
+ break;
+ }
+
+ default:
+ result = MAV_MISSION_UNSUPPORTED_FRAME;
+ break;
+ }
+
+
+ if (result != MAV_MISSION_ACCEPTED) goto mission_failed;
+
+ switch (tell_command.id) { // Switch to map APM command fields inot MAVLink command fields
+ case MAV_CMD_NAV_WAYPOINT:
+ case MAV_CMD_NAV_LOITER_UNLIM:
+ case MAV_CMD_NAV_RETURN_TO_LAUNCH:
+ case MAV_CMD_NAV_LAND:
+ break;
+
+ case MAV_CMD_NAV_LOITER_TURNS:
+ case MAV_CMD_NAV_TAKEOFF:
+ case MAV_CMD_DO_SET_HOME:
+ tell_command.p1 = packet.param1;
+ break;
+
+ case MAV_CMD_CONDITION_CHANGE_ALT:
+ tell_command.lat = packet.param1;
+ break;
+
+ case MAV_CMD_NAV_LOITER_TIME:
+ tell_command.p1 = packet.param1 / 10; // APM loiter time is in ten second increments
+ break;
+
+ case MAV_CMD_CONDITION_DELAY:
+ case MAV_CMD_CONDITION_DISTANCE:
+ tell_command.lat = packet.param1;
+ break;
+
+ case MAV_CMD_DO_JUMP:
+ tell_command.lat = packet.param2;
+ tell_command.p1 = packet.param1;
+ break;
+
+ case MAV_CMD_DO_REPEAT_SERVO:
+ tell_command.lng = packet.param4;
+ case MAV_CMD_DO_REPEAT_RELAY:
+ case MAV_CMD_DO_CHANGE_SPEED:
+ tell_command.lat = packet.param3;
+ tell_command.alt = packet.param2;
+ tell_command.p1 = packet.param1;
+ break;
+
+ case MAV_CMD_DO_SET_PARAMETER:
+ case MAV_CMD_DO_SET_RELAY:
+ case MAV_CMD_DO_SET_SERVO:
+ tell_command.alt = packet.param2;
+ tell_command.p1 = packet.param1;
+ break;
+
+ default:
+#ifdef MAVLINK10
+ result = MAV_MISSION_UNSUPPORTED;
+#endif
+ break;
+ }
+
+ if (result != MAV_MISSION_ACCEPTED) goto mission_failed;
+
+ if(packet.current == 2){ //current = 2 is a flag to tell us this is a "guided mode" waypoint and not for the mission
+ guided_WP = tell_command;
+
+ // add home alt if needed
+ if (guided_WP.options & MASK_OPTIONS_RELATIVE_ALT){
+ guided_WP.alt += home.alt;
+ }
+
+ set_mode(GUIDED);
+
+ // make any new wp uploaded instant (in case we are already in Guided mode)
+ set_guided_WP();
+
+ // verify we recevied the command
+ mavlink_msg_waypoint_ack_send(
+ chan,
+ msg->sysid,
+ msg->compid,
+ 0);
+
+ } else {
+ // Check if receiving waypoints (mission upload expected)
+ if (!waypoint_receiving) {
+ result = MAV_MISSION_ERROR;
+ goto mission_failed;
+ }
+
+ // check if this is the requested waypoint
+ if (packet.seq != waypoint_request_i) {
+ result = MAV_MISSION_INVALID_SEQUENCE;
+ goto mission_failed;
+ }
+
+ set_cmd_with_index(tell_command, packet.seq);
+
+ // update waypoint receiving state machine
+ waypoint_timelast_receive = millis();
+ waypoint_timelast_request = 0;
+ waypoint_request_i++;
+
+ if (waypoint_request_i > (uint16_t)g.command_total){
+ mavlink_msg_waypoint_ack_send(
+ chan,
+ msg->sysid,
+ msg->compid,
+ result);
+
+ send_text(SEVERITY_LOW,PSTR("flight plan received"));
+ waypoint_receiving = false;
+ // XXX ignores waypoint radius for individual waypoints, can
+ // only set WP_RADIUS parameter
+ }
+ }
+ break;
+
+ mission_failed:
+ // we are rejecting the mission/waypoint
+ mavlink_msg_waypoint_ack_send(
+ chan,
+ msg->sysid,
+ msg->compid,
+ result);
+ break;
+ }
+
+#if GEOFENCE_ENABLED == ENABLED
+ // receive a fence point from GCS and store in EEPROM
+ case MAVLINK_MSG_ID_FENCE_POINT: {
+ mavlink_fence_point_t packet;
+ mavlink_msg_fence_point_decode(msg, &packet);
+ if (mavlink_check_target(packet.target_system, packet.target_component))
+ break;
+ if (g.fence_action != FENCE_ACTION_NONE) {
+ send_text(SEVERITY_LOW,PSTR("fencing must be disabled"));
+ } else if (packet.count != g.fence_total) {
+ send_text(SEVERITY_LOW,PSTR("bad fence point"));
+ } else {
+ Vector2l point;
+ point.x = packet.lat*1.0e7;
+ point.y = packet.lng*1.0e7;
+ set_fence_point_with_index(point, packet.idx);
+ }
+ break;
+ }
+
+ // send a fence point to GCS
+ case MAVLINK_MSG_ID_FENCE_FETCH_POINT: {
+ mavlink_fence_fetch_point_t packet;
+ mavlink_msg_fence_fetch_point_decode(msg, &packet);
+ if (mavlink_check_target(packet.target_system, packet.target_component))
+ break;
+ if (packet.idx >= g.fence_total) {
+ send_text(SEVERITY_LOW,PSTR("bad fence point"));
+ } else {
+ Vector2l point = get_fence_point_with_index(packet.idx);
+ mavlink_msg_fence_point_send(chan, 0, 0, packet.idx, g.fence_total,
+ point.x*1.0e-7, point.y*1.0e-7);
+ }
+ break;
+ }
+#endif // GEOFENCE_ENABLED
+
+ case MAVLINK_MSG_ID_PARAM_SET:
+ {
+ AP_Param *vp;
+ enum ap_var_type var_type;
+
+ // decode
+ mavlink_param_set_t packet;
+ mavlink_msg_param_set_decode(msg, &packet);
+
+ if (mavlink_check_target(packet.target_system, packet.target_component))
+ break;
+
+ // set parameter
+
+ char key[ONBOARD_PARAM_NAME_LENGTH+1];
+ strncpy(key, (char *)packet.param_id, ONBOARD_PARAM_NAME_LENGTH);
+ key[ONBOARD_PARAM_NAME_LENGTH] = 0;
+
+ // find the requested parameter
+ vp = AP_Param::find(key, &var_type);
+ if ((NULL != vp) && // exists
+ !isnan(packet.param_value) && // not nan
+ !isinf(packet.param_value)) { // not inf
+
+ // add a small amount before casting parameter values
+ // from float to integer to avoid truncating to the
+ // next lower integer value.
+ float rounding_addition = 0.01;
+
+ // handle variables with standard type IDs
+ if (var_type == AP_PARAM_FLOAT) {
+ ((AP_Float *)vp)->set_and_save(packet.param_value);
+ } else if (var_type == AP_PARAM_INT32) {
+ if (packet.param_value < 0) rounding_addition = -rounding_addition;
+ ((AP_Int32 *)vp)->set_and_save(packet.param_value+rounding_addition);
+ } else if (var_type == AP_PARAM_INT16) {
+ if (packet.param_value < 0) rounding_addition = -rounding_addition;
+ ((AP_Int16 *)vp)->set_and_save(packet.param_value+rounding_addition);
+ } else if (var_type == AP_PARAM_INT8) {
+ if (packet.param_value < 0) rounding_addition = -rounding_addition;
+ ((AP_Int8 *)vp)->set_and_save(packet.param_value+rounding_addition);
+ } else {
+ // we don't support mavlink set on this parameter
+ break;
+ }
+
+ // Report back the new value if we accepted the change
+ // we send the value we actually set, which could be
+ // different from the value sent, in case someone sent
+ // a fractional value to an integer type
+ mavlink_msg_param_value_send(
+ chan,
+ key,
+ vp->cast_to_float(var_type),
+ mav_var_type(var_type),
+ _count_parameters(),
+ -1); // XXX we don't actually know what its index is...
+ }
+
+ break;
+ } // end case
+
+ case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE:
+ {
+ // allow override of RC channel values for HIL
+ // or for complete GCS control of switch position
+ // and RC PWM values.
+ if(msg->sysid != g.sysid_my_gcs) break; // Only accept control from our gcs
+ mavlink_rc_channels_override_t packet;
+ int16_t v[8];
+ mavlink_msg_rc_channels_override_decode(msg, &packet);
+
+ if (mavlink_check_target(packet.target_system,packet.target_component))
+ break;
+
+ v[0] = packet.chan1_raw;
+ v[1] = packet.chan2_raw;
+ v[2] = packet.chan3_raw;
+ v[3] = packet.chan4_raw;
+ v[4] = packet.chan5_raw;
+ v[5] = packet.chan6_raw;
+ v[6] = packet.chan7_raw;
+ v[7] = packet.chan8_raw;
+ rc_override_active = APM_RC.setHIL(v);
+ rc_override_fs_timer = millis();
+ break;
+ }
+
+ case MAVLINK_MSG_ID_HEARTBEAT:
+ {
+ // We keep track of the last time we received a heartbeat from our GCS for failsafe purposes
+ if(msg->sysid != g.sysid_my_gcs) break;
+ rc_override_fs_timer = millis();
+ pmTest1++;
+ break;
+ }
+
+ #if HIL_MODE != HIL_MODE_DISABLED
+ // This is used both as a sensor and to pass the location
+ // in HIL_ATTITUDE mode.
+#ifdef MAVLINK10
+ case MAVLINK_MSG_ID_GPS_RAW_INT:
+ {
+ // decode
+ mavlink_gps_raw_int_t packet;
+ mavlink_msg_gps_raw_int_decode(msg, &packet);
+
+ // set gps hil sensor
+ g_gps->setHIL(packet.time_usec/1000.0,
+ packet.lat*1.0e-7, packet.lon*1.0e-7, packet.alt*1.0e-3,
+ packet.vel*1.0e-2, packet.cog*1.0e-2, 0, 0);
+ break;
+ }
+#else // MAVLINK10
+ case MAVLINK_MSG_ID_GPS_RAW:
+ {
+ // decode
+ mavlink_gps_raw_t packet;
+ mavlink_msg_gps_raw_decode(msg, &packet);
+
+ // set gps hil sensor
+ g_gps->setHIL(packet.usec/1000.0,packet.lat,packet.lon,packet.alt,
+ packet.v,packet.hdg,0,0);
+ //if ((gps_base_alt == 0) && (airspeed ==0 )) { // we are on the ground so, get the altitude offset
+ // gps_base_alt = packet.alt*100;
+ //}
+
+ current_loc.lng = packet.lon * T7;
+ current_loc.lat = packet.lat * T7;
+ //current_loc.alt = g_gps->altitude - gps_base_alt;
+ //if (!home_is_set) {
+ // init_home();
+ //}
+ break;
+ }
+#endif // MAVLINK10
+
+ // Is this resolved? - MAVLink protocol change.....
+ case MAVLINK_MSG_ID_VFR_HUD:
+ {
+ // decode
+ mavlink_vfr_hud_t packet;
+ mavlink_msg_vfr_hud_decode(msg, &packet);
+
+ // set airspeed
+ airspeed = 100*packet.airspeed;
+ break;
+ }
+#ifdef MAVLINK10
+ case MAVLINK_MSG_ID_HIL_STATE:
+ {
+ mavlink_hil_state_t packet;
+ mavlink_msg_hil_state_decode(msg, &packet);
+
+ float vel = sqrt((packet.vx * packet.vx) + (packet.vy * packet.vy));
+ float cog = wrap_360(ToDeg(atan2(packet.vx, packet.vy)) * 100);
+
+ // set gps hil sensor
+ g_gps->setHIL(packet.time_usec/1000.0,
+ packet.lat*1.0e-7, packet.lon*1.0e-7, packet.alt*1.0e-3,
+ vel*1.0e-2, cog*1.0e-2, 0, 0);
+
+ #if HIL_MODE == HIL_MODE_SENSORS
+
+ // rad/sec
+ Vector3f gyros;
+ gyros.x = (float)packet.xgyro / 1000.0;
+ gyros.y = (float)packet.ygyro / 1000.0;
+ gyros.z = (float)packet.zgyro / 1000.0;
+ // m/s/s
+ Vector3f accels;
+ accels.x = (float)packet.xacc / 1000.0;
+ accels.y = (float)packet.yacc / 1000.0;
+ accels.z = (float)packet.zacc / 1000.0;
+
+ imu.set_gyro(gyros);
+
+ imu.set_accel(accels);
+
+ #else
+
+ // set dcm hil sensor
+ ahrs.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed,
+ packet.pitchspeed,packet.yawspeed);
+
+ #endif
+
+ break;
+ }
+#endif // MAVLINK10
+#endif
+#if HIL_MODE == HIL_MODE_ATTITUDE
+ case MAVLINK_MSG_ID_ATTITUDE:
+ {
+ // decode
+ mavlink_attitude_t packet;
+ mavlink_msg_attitude_decode(msg, &packet);
+
+ // set dcm hil sensor
+ ahrs.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed,
+ packet.pitchspeed,packet.yawspeed);
+ // rad/sec // JLN update - FOR HIL SIMULATION WITH AEROSIM
+ Vector3f gyros;
+ gyros.x = (float)packet.rollspeed / 1000.0;
+ gyros.y = (float)packet.pitchspeed / 1000.0;
+ gyros.z = (float)packet.yawspeed / 1000.0;
+
+ imu.set_gyro(gyros);
+
+ // m/s/s
+ Vector3f accels;
+ accels.x = (float)packet.roll * gravity / 1000.0;
+ accels.y = (float)packet.pitch * gravity / 1000.0;
+ accels.z = (float)packet.yaw * gravity / 1000.0;
+
+ imu.set_accel(accels);
+ break;
+ }
+#endif
+#if HIL_MODE == HIL_MODE_SENSORS
+
+ case MAVLINK_MSG_ID_RAW_IMU:
+ {
+ // decode
+ mavlink_raw_imu_t packet;
+ mavlink_msg_raw_imu_decode(msg, &packet);
+
+ // set imu hil sensors
+ // TODO: check scaling for temp/absPress
+ float temp = 70;
+ float absPress = 1;
+ //Serial.printf_P(PSTR("accel: %d %d %d\n"), packet.xacc, packet.yacc, packet.zacc);
+ //Serial.printf_P(PSTR("gyro: %d %d %d\n"), packet.xgyro, packet.ygyro, packet.zgyro);
+
+ // rad/sec
+ Vector3f gyros;
+ gyros.x = (float)packet.xgyro / 1000.0;
+ gyros.y = (float)packet.ygyro / 1000.0;
+ gyros.z = (float)packet.zgyro / 1000.0;
+ // m/s/s
+ Vector3f accels;
+ accels.x = (float)packet.xacc / 1000.0;
+ accels.y = (float)packet.yacc / 1000.0;
+ accels.z = (float)packet.zacc / 1000.0;
+
+ imu.set_gyro(gyros);
+
+ imu.set_accel(accels);
+
+ compass.setHIL(packet.xmag,packet.ymag,packet.zmag);
+ break;
+ }
+
+ case MAVLINK_MSG_ID_RAW_PRESSURE:
+ {
+ // decode
+ mavlink_raw_pressure_t packet;
+ mavlink_msg_raw_pressure_decode(msg, &packet);
+
+ // set pressure hil sensor
+ // TODO: check scaling
+ float temp = 70;
+ barometer.setHIL(temp,packet.press_diff1 + 101325);
+ break;
+ }
+#endif // HIL_MODE
+
+#if MOUNT == ENABLED
+ case MAVLINK_MSG_ID_MOUNT_CONFIGURE:
+ {
+ camera_mount.configure_msg(msg);
+ break;
+ }
+
+ case MAVLINK_MSG_ID_MOUNT_CONTROL:
+ {
+ camera_mount.control_msg(msg);
+ break;
+ }
+
+ case MAVLINK_MSG_ID_MOUNT_STATUS:
+ {
+ camera_mount.status_msg(msg);
+ break;
+ }
+#endif // MOUNT == ENABLED
+ } // end switch
+} // end handle mavlink
+
+uint16_t
+GCS_MAVLINK::_count_parameters()
+{
+ // if we haven't cached the parameter count yet...
+ if (0 == _parameter_count) {
+ AP_Param *vp;
+ AP_Param::ParamToken token;
+
+ vp = AP_Param::first(&token, NULL);
+ do {
+ _parameter_count++;
+ } while (NULL != (vp = AP_Param::next_scalar(&token, NULL)));
+ }
+ return _parameter_count;
+}
+
+/**
+* @brief Send the next pending parameter, called from deferred message
+* handling code
+*/
+void
+GCS_MAVLINK::queued_param_send()
+{
+ // Check to see if we are sending parameters
+ if (NULL == _queued_parameter) return;
+
+ AP_Param *vp;
+ float value;
+
+ // copy the current parameter and prepare to move to the next
+ vp = _queued_parameter;
+
+ // if the parameter can be cast to float, report it here and break out of the loop
+ value = vp->cast_to_float(_queued_parameter_type);
+
+ char param_name[ONBOARD_PARAM_NAME_LENGTH];
+ vp->copy_name(param_name, sizeof(param_name));
+
+ mavlink_msg_param_value_send(
+ chan,
+ param_name,
+ value,
+ mav_var_type(_queued_parameter_type),
+ _queued_parameter_count,
+ _queued_parameter_index);
+
+ _queued_parameter = AP_Param::next_scalar(&_queued_parameter_token, &_queued_parameter_type);
+ _queued_parameter_index++;
+}
+
+/**
+* @brief Send the next pending waypoint, called from deferred message
+* handling code
+*/
+void
+GCS_MAVLINK::queued_waypoint_send()
+{
+ if (waypoint_receiving &&
+ waypoint_request_i <= (unsigned)g.command_total) {
+ mavlink_msg_waypoint_request_send(
+ chan,
+ waypoint_dest_sysid,
+ waypoint_dest_compid,
+ waypoint_request_i);
+ }
+}
+
+/*
+ a delay() callback that processes MAVLink packets. We set this as the
+ callback in long running library initialisation routines to allow
+ MAVLink to process packets while waiting for the initialisation to
+ complete
+*/
+static void mavlink_delay(unsigned long t)
+{
+ unsigned long tstart;
+ static unsigned long last_1hz, last_50hz;
+
+ if (in_mavlink_delay) {
+ // this should never happen, but let's not tempt fate by
+ // letting the stack grow too much
+ delay(t);
+ return;
+ }
+
+ in_mavlink_delay = true;
+
+ tstart = millis();
+ do {
+ unsigned long tnow = millis();
+ if (tnow - last_1hz > 1000) {
+ last_1hz = tnow;
+ gcs_send_message(MSG_HEARTBEAT);
+ gcs_send_message(MSG_EXTENDED_STATUS1);
+ }
+ if (tnow - last_50hz > 20) {
+ last_50hz = tnow;
+ gcs_update();
+ }
+ delay(1);
+#if USB_MUX_PIN > 0
+ check_usb_mux();
+#endif
+ } while (millis() - tstart < t);
+
+ in_mavlink_delay = false;
+}
+
+/*
+ send a message on both GCS links
+ */
+static void gcs_send_message(enum ap_message id)
+{
+ gcs0.send_message(id);
+ if (gcs3.initialised) {
+ gcs3.send_message(id);
+ }
+}
+
+/*
+ send data streams in the given rate range on both links
+ */
+static void gcs_data_stream_send(uint16_t freqMin, uint16_t freqMax)
+{
+ gcs0.data_stream_send(freqMin, freqMax);
+ if (gcs3.initialised) {
+ gcs3.data_stream_send(freqMin, freqMax);
+ }
+}
+
+/*
+ look for incoming commands on the GCS links
+ */
+static void gcs_update(void)
+{
+ gcs0.update();
+ if (gcs3.initialised) {
+ gcs3.update();
+ }
+}
+
+static void gcs_send_text_P(gcs_severity severity, const prog_char_t *str)
+{
+ gcs0.send_text(severity, str);
+ if (gcs3.initialised) {
+ gcs3.send_text(severity, str);
+ }
+}
+
+/*
+ send a low priority formatted message to the GCS
+ only one fits in the queue, so if you send more than one before the
+ last one gets into the serial buffer then the old one will be lost
+ */
+static void gcs_send_text_fmt(const prog_char_t *fmt, ...)
+{
+ char fmtstr[40];
+ va_list ap;
+ uint8_t i;
+ for (i=0; i<sizeof(fmtstr)-1; i++) {
+ fmtstr[i] = pgm_read_byte((const prog_char *)(fmt++));
+ if (fmtstr[i] == 0) break;
+ }
+ fmtstr[i] = 0;
+ pending_status.severity = (uint8_t)SEVERITY_LOW;
+ va_start(ap, fmt);
+ vsnprintf((char *)pending_status.text, sizeof(pending_status.text), fmtstr, ap);
+ va_end(ap);
+ mavlink_send_message(MAVLINK_COMM_0, MSG_STATUSTEXT, 0);
+ if (gcs3.initialised) {
+ mavlink_send_message(MAVLINK_COMM_1, MSG_STATUSTEXT, 0);
+ }
+}
diff --git a/APMrover2/Log.pde b/APMrover2/Log.pde
new file mode 100644
index 000000000..1ce9537e9
--- /dev/null
+++ b/APMrover2/Log.pde
@@ -0,0 +1,696 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#if LOGGING_ENABLED == ENABLED
+
+// Code to Write and Read packets from DataFlash log memory
+// Code to interact with the user to dump or erase logs
+
+#define HEAD_BYTE1 0xA3 // Decimal 163
+#define HEAD_BYTE2 0x95 // Decimal 149
+#define END_BYTE 0xBA // Decimal 186
+
+
+// These are function definitions so the Menu can be constructed before the functions
+// are defined below. Order matters to the compiler.
+static int8_t dump_log(uint8_t argc, const Menu::arg *argv);
+static int8_t erase_logs(uint8_t argc, const Menu::arg *argv);
+static int8_t select_logs(uint8_t argc, const Menu::arg *argv);
+
+static int16_t cur_throttle =0;
+
+// This is the help function
+// PSTR is an AVR macro to read strings from flash memory
+// printf_P is a version of print_f that reads from flash memory
+//static int8_t help_log(uint8_t argc, const Menu::arg *argv)
+/*{
+ Serial.printf_P(PSTR("\n"
+ "Commands:\n"
+ " dump <n>"
+ " erase (all logs)\n"
+ " enable <name> | all\n"
+ " disable <name> | all\n"
+ "\n"));
+ return 0;
+}*/
+
+// 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[] PROGMEM = {
+ {"dump", dump_log},
+ {"erase", erase_logs},
+ {"enable", select_logs},
+ {"disable", select_logs}
+};
+
+// A Macro to create the Menu
+MENU2(log_menu, "Log", log_menu_commands, print_log_menu);
+
+static bool
+print_log_menu(void)
+{
+ int log_start;
+ int log_end;
+ int temp;
+ int last_log_num = DataFlash.find_last_log();
+
+ uint16_t num_logs = DataFlash.get_num_logs();
+
+ Serial.printf_P(PSTR("logs enabled: "));
+
+ if (0 == g.log_bitmask) {
+ Serial.printf_P(PSTR("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) Serial.printf_P(PSTR(" %S"), PSTR(#_s))
+ PLOG(ATTITUDE_FAST);
+ PLOG(ATTITUDE_MED);
+ PLOG(GPS);
+ PLOG(PM);
+ PLOG(CTUN);
+ PLOG(NTUN);
+ PLOG(MODE);
+ PLOG(RAW);
+ PLOG(CMD);
+ PLOG(CUR);
+ #undef PLOG
+ }
+
+ Serial.println();
+
+ if (num_logs == 0) {
+ Serial.printf_P(PSTR("\nNo logs\n\n"));
+ }else{
+ Serial.printf_P(PSTR("\n%d logs\n"), num_logs);
+
+ for(int i=num_logs;i>=1;i--) {
+ int last_log_start = log_start, last_log_end = log_end;
+ temp = last_log_num-i+1;
+ DataFlash.get_log_boundaries(temp, log_start, log_end);
+ Serial.printf_P(PSTR("Log %d, start %d, end %d\n"), temp, log_start, log_end);
+ if (last_log_start == log_start && last_log_end == log_end) {
+ // we are printing bogus logs
+ break;
+ }
+ }
+ Serial.println();
+ }
+ return(true);
+}
+
+static int8_t
+dump_log(uint8_t argc, const Menu::arg *argv)
+{
+ int dump_log;
+ int dump_log_start;
+ int dump_log_end;
+ byte last_log_num;
+
+ // check that the requested log number can be read
+ dump_log = argv[1].i;
+ last_log_num = DataFlash.find_last_log();
+
+ if (dump_log == -2) {
+ for(uint16_t count=1; count<=DataFlash.df_NumPages; count++) {
+ DataFlash.StartRead(count);
+ Serial.printf_P(PSTR("DF page, log file #, log page: %d,\t"), count);
+ Serial.printf_P(PSTR("%d,\t"), DataFlash.GetFileNumber());
+ Serial.printf_P(PSTR("%d\n"), DataFlash.GetFilePage());
+ }
+ return(-1);
+ } else if (dump_log <= 0) {
+ Serial.printf_P(PSTR("dumping all\n"));
+ Log_Read(1, DataFlash.df_NumPages);
+ return(-1);
+ } else if ((argc != 2) || (dump_log <= (last_log_num - DataFlash.get_num_logs())) || (dump_log > last_log_num)) {
+ Serial.printf_P(PSTR("bad log number\n"));
+ return(-1);
+ }
+
+ DataFlash.get_log_boundaries(dump_log, dump_log_start, dump_log_end);
+ Serial.printf_P(PSTR("Dumping Log %d, start pg %d, end pg %d\n"),
+ dump_log,
+ dump_log_start,
+ dump_log_end);
+
+ Log_Read(dump_log_start, dump_log_end);
+ Serial.printf_P(PSTR("Done\n"));
+ return 0;
+}
+
+
+void erase_callback(unsigned long t) {
+ mavlink_delay(t);
+ if (DataFlash.GetWritePage() % 128 == 0) {
+ Serial.printf_P(PSTR("+"));
+ }
+}
+
+static void do_erase_logs(void)
+{
+ Serial.printf_P(PSTR("\nErasing log...\n"));
+ DataFlash.EraseAll(erase_callback);
+ Serial.printf_P(PSTR("\nLog erased.\n"));
+}
+
+static int8_t
+erase_logs(uint8_t argc, const Menu::arg *argv)
+{
+ in_mavlink_delay = true;
+ do_erase_logs();
+ in_mavlink_delay = false;
+ return 0;
+}
+
+static int8_t
+select_logs(uint8_t argc, const Menu::arg *argv)
+{
+ uint16_t bits;
+
+ if (argc != 2) {
+ Serial.printf_P(PSTR("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_P(argv[1].str, PSTR("all"))) {
+ bits = ~0;
+ } else {
+ #define TARG(_s) if (!strcasecmp_P(argv[1].str, PSTR(#_s))) bits |= MASK_LOG_ ## _s
+ TARG(ATTITUDE_FAST);
+ TARG(ATTITUDE_MED);
+ TARG(GPS);
+ TARG(PM);
+ TARG(CTUN);
+ TARG(NTUN);
+ TARG(MODE);
+ TARG(RAW);
+ TARG(CMD);
+ TARG(CUR);
+ #undef TARG
+ }
+
+ if (!strcasecmp_P(argv[0].str, PSTR("enable"))) {
+ g.log_bitmask.set_and_save(g.log_bitmask | bits);
+ }else{
+ g.log_bitmask.set_and_save(g.log_bitmask & ~bits);
+ }
+ return(0);
+}
+
+static int8_t
+process_logs(uint8_t argc, const Menu::arg *argv)
+{
+ log_menu.run();
+ return 0;
+}
+
+
+
+
+// Write an attitude packet. Total length : 10 bytes
+static void Log_Write_Attitude(int16_t log_roll, int16_t log_pitch, uint16_t log_yaw)
+{
+ DataFlash.WriteByte(HEAD_BYTE1);
+ DataFlash.WriteByte(HEAD_BYTE2);
+ DataFlash.WriteByte(LOG_ATTITUDE_MSG);
+ DataFlash.WriteInt(log_roll);
+ DataFlash.WriteInt(log_pitch);
+ DataFlash.WriteInt(log_yaw);
+ DataFlash.WriteByte(END_BYTE);
+}
+
+// Write a performance monitoring packet. Total length : 19 bytes
+#if HIL_MODE != HIL_MODE_ATTITUDE
+static void Log_Write_Performance()
+{
+ DataFlash.WriteByte(HEAD_BYTE1);
+ DataFlash.WriteByte(HEAD_BYTE2);
+ DataFlash.WriteByte(LOG_PERFORMANCE_MSG);
+ DataFlash.WriteLong(millis()- perf_mon_timer);
+ DataFlash.WriteInt((int16_t)mainLoop_count);
+ DataFlash.WriteInt(G_Dt_max);
+ DataFlash.WriteByte(0);
+ DataFlash.WriteByte(imu.adc_constraints);
+ DataFlash.WriteByte(ahrs.renorm_range_count);
+ DataFlash.WriteByte(ahrs.renorm_blowup_count);
+ DataFlash.WriteByte(gps_fix_count);
+ DataFlash.WriteInt(1);
+ DataFlash.WriteInt((int)(ahrs.get_gyro_drift().x * 1000));
+ DataFlash.WriteInt((int)(ahrs.get_gyro_drift().y * 1000));
+ DataFlash.WriteInt((int)(ahrs.get_gyro_drift().z * 1000));
+ DataFlash.WriteInt(pmTest1);
+ DataFlash.WriteByte(END_BYTE);
+}
+#endif
+
+// Write a command processing packet. Total length : 19 bytes
+//void Log_Write_Cmd(byte num, byte id, byte p1, int32_t alt, int32_t lat, int32_t lng)
+static void Log_Write_Cmd(byte num, struct Location *wp)
+{
+ DataFlash.WriteByte(HEAD_BYTE1);
+ DataFlash.WriteByte(HEAD_BYTE2);
+ DataFlash.WriteByte(LOG_CMD_MSG);
+ DataFlash.WriteByte(num);
+ DataFlash.WriteByte(wp->id);
+ DataFlash.WriteByte(wp->p1);
+ DataFlash.WriteLong(wp->alt);
+ DataFlash.WriteLong(wp->lat);
+ DataFlash.WriteLong(wp->lng);
+ DataFlash.WriteByte(END_BYTE);
+}
+
+static void Log_Write_Startup(byte type)
+{
+ DataFlash.WriteByte(HEAD_BYTE1);
+ DataFlash.WriteByte(HEAD_BYTE2);
+ DataFlash.WriteByte(LOG_STARTUP_MSG);
+ DataFlash.WriteByte(type);
+ DataFlash.WriteByte(g.command_total);
+ DataFlash.WriteByte(END_BYTE);
+
+ // create a location struct to hold the temp Waypoints for printing
+ struct Location cmd = get_cmd_with_index(0);
+ Log_Write_Cmd(0, &cmd);
+
+ for (int i = 1; i <= g.command_total; i++){
+ cmd = get_cmd_with_index(i);
+ Log_Write_Cmd(i, &cmd);
+ }
+}
+
+
+// Write a control tuning packet. Total length : 22 bytes
+#if HIL_MODE != HIL_MODE_ATTITUDE
+static void Log_Write_Control_Tuning()
+{
+ Vector3f accel = imu.get_accel();
+
+ DataFlash.WriteByte(HEAD_BYTE1);
+ DataFlash.WriteByte(HEAD_BYTE2);
+ DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG);
+ DataFlash.WriteInt((int)(g.channel_roll.servo_out));
+ DataFlash.WriteInt((int)nav_roll);
+ DataFlash.WriteInt((int)ahrs.roll_sensor);
+ DataFlash.WriteInt((int)(g.channel_pitch.servo_out));
+ DataFlash.WriteInt((int)nav_pitch);
+ DataFlash.WriteInt((int)ahrs.pitch_sensor);
+ DataFlash.WriteInt((int)(g.channel_throttle.servo_out));
+ DataFlash.WriteInt((int)(g.channel_rudder.servo_out));
+ DataFlash.WriteInt((int)(accel.y * 10000));
+ DataFlash.WriteByte(END_BYTE);
+}
+#endif
+
+// Write a navigation tuning packet. Total length : 18 bytes
+static void Log_Write_Nav_Tuning()
+{
+ DataFlash.WriteByte(HEAD_BYTE1);
+ DataFlash.WriteByte(HEAD_BYTE2);
+ DataFlash.WriteByte(LOG_NAV_TUNING_MSG);
+ DataFlash.WriteInt((uint16_t)ahrs.yaw_sensor);
+ DataFlash.WriteInt((int)wp_distance);
+ DataFlash.WriteInt((uint16_t)target_bearing);
+ DataFlash.WriteInt((uint16_t)nav_bearing);
+ DataFlash.WriteInt(altitude_error);
+ DataFlash.WriteInt((int)airspeed);
+ DataFlash.WriteInt((int)(nav_gain_scaler*1000));
+ DataFlash.WriteByte(END_BYTE);
+}
+
+// Write a mode packet. Total length : 5 bytes
+static void Log_Write_Mode(byte mode)
+{
+ DataFlash.WriteByte(HEAD_BYTE1);
+ DataFlash.WriteByte(HEAD_BYTE2);
+ DataFlash.WriteByte(LOG_MODE_MSG);
+ DataFlash.WriteByte(mode);
+ DataFlash.WriteByte(END_BYTE);
+}
+
+// Write an GPS packet. Total length : 30 bytes
+static void Log_Write_GPS( int32_t log_Time, int32_t log_Lattitude, int32_t log_Longitude, int32_t log_gps_alt, int32_t log_mix_alt,
+ int32_t log_Ground_Speed, int32_t log_Ground_Course, byte log_Fix, byte log_NumSats)
+{
+ DataFlash.WriteByte(HEAD_BYTE1);
+ DataFlash.WriteByte(HEAD_BYTE2);
+ DataFlash.WriteByte(LOG_GPS_MSG);
+ DataFlash.WriteLong(log_Time);
+ DataFlash.WriteByte(log_Fix);
+ DataFlash.WriteByte(log_NumSats);
+ DataFlash.WriteLong(log_Lattitude);
+ DataFlash.WriteLong(log_Longitude);
+ DataFlash.WriteInt(sonar_alt); // This one is just temporary for testing out sonar in fixed wing
+ DataFlash.WriteLong(log_mix_alt);
+ DataFlash.WriteLong(log_gps_alt);
+ DataFlash.WriteLong(log_Ground_Speed);
+ DataFlash.WriteLong(log_Ground_Course);
+ DataFlash.WriteInt((int16_t)Vz);
+ DataFlash.WriteInt((int16_t)delta_Vz);
+ DataFlash.WriteInt((int)airspeed);
+ DataFlash.WriteByte(END_BYTE);
+}
+
+// Write an raw accel/gyro data packet. Total length : 28 bytes
+#if HIL_MODE != HIL_MODE_ATTITUDE
+static void Log_Write_Raw()
+{
+ Vector3f gyro = imu.get_gyro();
+ Vector3f accel = imu.get_accel();
+ gyro *= t7; // Scale up for storage as long integers
+ accel *= t7;
+ DataFlash.WriteByte(HEAD_BYTE1);
+ DataFlash.WriteByte(HEAD_BYTE2);
+ DataFlash.WriteByte(LOG_RAW_MSG);
+
+ DataFlash.WriteLong((long)gyro.x);
+ DataFlash.WriteLong((long)gyro.y);
+ DataFlash.WriteLong((long)gyro.z);
+ DataFlash.WriteLong((long)accel.x);
+ DataFlash.WriteLong((long)accel.y);
+ DataFlash.WriteLong((long)accel.z);
+
+ DataFlash.WriteByte(END_BYTE);
+}
+#endif
+
+static void Log_Write_Current()
+{
+ DataFlash.WriteByte(HEAD_BYTE1);
+ DataFlash.WriteByte(HEAD_BYTE2);
+ DataFlash.WriteByte(LOG_CURRENT_MSG);
+ DataFlash.WriteInt(g.channel_throttle.control_in);
+ DataFlash.WriteInt((int)(battery_voltage1 * 100.0));
+ DataFlash.WriteInt((int)(current_amps1 * 100.0));
+ DataFlash.WriteInt((int)current_total1);
+ DataFlash.WriteByte(END_BYTE);
+}
+
+// Read a Current packet
+static void Log_Read_Current()
+{
+ Serial.printf_P(PSTR("CURR: %d, %4.4f, %4.4f, %d\n"),
+ DataFlash.ReadInt(),
+ ((float)DataFlash.ReadInt() / 100.f),
+ ((float)DataFlash.ReadInt() / 100.f),
+ DataFlash.ReadInt());
+}
+
+// Read an control tuning packet
+static void Log_Read_Control_Tuning()
+{
+ float logvar;
+
+ Serial.printf_P(PSTR("CTUN: "));
+ for (int y = 1; y < 10; y++) {
+ logvar = DataFlash.ReadInt();
+ if(y == 7) cur_throttle = logvar;
+ if(y < 8) logvar = logvar/100.f;
+ if(y == 9) logvar = logvar/10000.f;
+ Serial.print(logvar);
+ Serial.print(comma);
+ Serial.print(" ");
+ }
+ Serial.println("");
+}
+
+// Read a nav tuning packet
+static void Log_Read_Nav_Tuning()
+{
+ int16_t d[7];
+ for (int8_t i=0; i<7; i++) {
+ d[i] = DataFlash.ReadInt();
+ }
+ Serial.printf_P(PSTR("NTUN: %4.4f, %d, %4.4f, %4.4f, %4.4f, %4.4f, %4.4f,\n"), // \n
+ d[0]/100.0,
+ d[1],
+ ((uint16_t)d[2])/100.0,
+ ((uint16_t)d[3])/100.0,
+ d[4]/100.0,
+ d[5]/100.0,
+ d[5]/1000.0);
+}
+
+// Read a performance packet
+static void Log_Read_Performance()
+{
+ int32_t pm_time;
+ int logvar;
+
+ Serial.printf_P(PSTR("PM: "));
+ pm_time = DataFlash.ReadLong();
+ Serial.print(pm_time);
+ Serial.print(comma);
+ for (int y = 1; y <= 12; y++) {
+ if(y < 3 || y > 7){
+ logvar = DataFlash.ReadInt();
+ }else{
+ logvar = DataFlash.ReadByte();
+ }
+ Serial.print(logvar);
+ Serial.print(comma);
+ Serial.print(" ");
+ }
+ Serial.println("");
+}
+
+// Read a command processing packet
+static void Log_Read_Cmd()
+{
+ byte logvarb;
+ int32_t logvarl;
+
+ Serial.printf_P(PSTR("CMD: "));
+ for(int i = 1; i < 4; i++) {
+ logvarb = DataFlash.ReadByte();
+ Serial.print(logvarb, DEC);
+ Serial.print(comma);
+ Serial.print(" ");
+ }
+ for(int i = 1; i < 4; i++) {
+ logvarl = DataFlash.ReadLong();
+ Serial.print(logvarl, DEC);
+ Serial.print(comma);
+ Serial.print(" ");
+ }
+ Serial.println("");
+}
+
+static void Log_Read_Startup()
+{
+ byte logbyte = DataFlash.ReadByte();
+
+ if (logbyte == TYPE_AIRSTART_MSG)
+ Serial.printf_P(PSTR("AIR START - "));
+ else if (logbyte == TYPE_GROUNDSTART_MSG)
+ Serial.printf_P(PSTR("GROUND START - "));
+ else
+ Serial.printf_P(PSTR("UNKNOWN STARTUP - "));
+
+ Serial.printf_P(PSTR(" %d commands in memory\n"),(int)DataFlash.ReadByte());
+}
+
+// Read an attitude packet
+static void Log_Read_Attitude()
+{
+ int16_t d[3];
+ d[0] = DataFlash.ReadInt();
+ d[1] = DataFlash.ReadInt();
+ d[2] = DataFlash.ReadInt();
+ Serial.printf_P(PSTR("ATT: %d, %d, %u\n"),
+ d[0], d[1],
+ (uint16_t)d[2]);
+}
+
+// Read a mode packet
+static void Log_Read_Mode()
+{
+ Serial.printf_P(PSTR("MOD: "));
+ Serial.println(flight_mode_strings[DataFlash.ReadByte()]);
+}
+
+// Read a GPS packet
+static void Log_Read_GPS()
+{
+ int32_t l[7];
+ byte b[2];
+ int16_t i,j,k,m;
+ l[0] = DataFlash.ReadLong();
+ b[0] = DataFlash.ReadByte();
+ b[1] = DataFlash.ReadByte();
+ l[1] = DataFlash.ReadLong();
+ l[2] = DataFlash.ReadLong();
+ i = DataFlash.ReadInt();
+ l[3] = DataFlash.ReadLong();
+ l[4] = DataFlash.ReadLong();
+ l[5] = DataFlash.ReadLong();
+ l[6] = DataFlash.ReadLong();
+ j = DataFlash.ReadInt();
+ k = DataFlash.ReadInt();
+ m = DataFlash.ReadInt();
+ /*
+ Serial.printf_P(PSTR("GPS: %ld, %d, %d, %4.7f, %4.7f, %d, %4.4f, %4.4f, %4.4f, %4.4f\n"),
+ (long)l[0], (int)b[0], (int)b[1],
+ l[1]/t7, l[2]/t7,
+ (int)i,
+ l[3]/100.0, l[4]/100.0, l[5]/100.0, l[6]/100.0); */
+ Serial.printf_P(PSTR("GPS: %ld, %d, %d, %4.7f, %4.7f, %4.4f, %4.4f, %4.4f, %4.4f\n"),
+ (long)l[0], (int)b[0], (int)b[1],
+ l[1]/t7, l[2]/t7,
+ l[4]/100.0, l[3]/100.0, l[5]/100.0, l[6]/100.0);
+
+ Serial.printf_P(PSTR("THP: %4.7f, %4.7f, %4.4f, %2.1f, %2.1f, %2.1f, %d\n"),
+ l[1]/t7, l[2]/t7, l[3]/100.0,
+ (float)j/100.0, (float)k/100.0, (float)m/100.0, cur_throttle);
+}
+
+// Read a raw accel/gyro packet
+static void Log_Read_Raw()
+{
+ float logvar;
+ Serial.printf_P(PSTR("RAW: "));
+ for (int y = 0; y < 6; y++) {
+ logvar = (float)DataFlash.ReadLong() / t7;
+ Serial.print(logvar);
+ Serial.print(comma);
+ Serial.print(" ");
+ }
+ Serial.println("");
+}
+
+// Read the DataFlash log memory : Packet Parser
+static void Log_Read(int16_t start_page, int16_t end_page)
+{
+ int packet_count = 0;
+
+ #ifdef AIRFRAME_NAME
+ Serial.printf_P(PSTR((AIRFRAME_NAME)
+ #endif
+ Serial.printf_P(PSTR("\n" THISFIRMWARE
+ "\nFree RAM: %u\n"),
+ memcheck_available_memory());
+
+ if(start_page > end_page)
+ {
+ packet_count = Log_Read_Process(start_page, DataFlash.df_NumPages);
+ packet_count += Log_Read_Process(1, end_page);
+ } else {
+ packet_count = Log_Read_Process(start_page, end_page);
+ }
+
+ Serial.printf_P(PSTR("Number of packets read: %d\n"), packet_count);
+}
+
+// Read the DataFlash log memory : Packet Parser
+static int Log_Read_Process(int16_t start_page, int16_t end_page)
+{
+ byte data;
+ byte log_step = 0;
+ int page = start_page;
+ int packet_count = 0;
+
+ DataFlash.StartRead(start_page);
+ while (page < end_page && page != -1){
+ data = DataFlash.ReadByte();
+
+ switch(log_step) // This is a state machine to read the packets
+ {
+ case 0:
+ if(data == HEAD_BYTE1) // Head byte 1
+ log_step++;
+ break;
+ case 1:
+ if(data == HEAD_BYTE2) // Head byte 2
+ log_step++;
+ else
+ log_step = 0;
+ break;
+ case 2:
+ if(data == LOG_ATTITUDE_MSG){
+ Log_Read_Attitude();
+ log_step++;
+
+ }else if(data == LOG_MODE_MSG){
+ Log_Read_Mode();
+ log_step++;
+
+ }else if(data == LOG_CONTROL_TUNING_MSG){
+ Log_Read_Control_Tuning();
+ log_step++;
+
+ }else if(data == LOG_NAV_TUNING_MSG){
+ Log_Read_Nav_Tuning();
+ log_step++;
+
+ }else if(data == LOG_PERFORMANCE_MSG){
+ Log_Read_Performance();
+ log_step++;
+
+ }else if(data == LOG_RAW_MSG){
+ Log_Read_Raw();
+ log_step++;
+
+ }else if(data == LOG_CMD_MSG){
+ Log_Read_Cmd();
+ log_step++;
+
+ }else if(data == LOG_CURRENT_MSG){
+ Log_Read_Current();
+ log_step++;
+
+ }else if(data == LOG_STARTUP_MSG){
+ Log_Read_Startup();
+ log_step++;
+ }else {
+ if(data == LOG_GPS_MSG){
+ Log_Read_GPS();
+ log_step++;
+ }else{
+ Serial.printf_P(PSTR("Error Reading Packet: %d\n"),packet_count);
+ log_step = 0; // Restart, we have a problem...
+ }
+ }
+ break;
+ case 3:
+ if(data == END_BYTE){
+ packet_count++;
+ }else{
+ Serial.printf_P(PSTR("Error Reading END_BYTE: %d\n"),data);
+ }
+ log_step = 0; // Restart sequence: new packet...
+ break;
+ }
+ page = DataFlash.GetPage();
+ }
+ return packet_count;
+}
+
+#else // LOGGING_ENABLED
+
+// dummy functions
+static void Log_Write_Mode(byte mode) {}
+static void Log_Write_Startup(byte type) {}
+static void Log_Write_Cmd(byte num, struct Location *wp) {}
+static void Log_Write_Current() {}
+static void Log_Write_Nav_Tuning() {}
+static void Log_Write_GPS( int32_t log_Time, int32_t log_Lattitude, int32_t log_Longitude, int32_t log_gps_alt, int32_t log_mix_alt,
+ int32_t log_Ground_Speed, int32_t log_Ground_Course, byte log_Fix, byte log_NumSats) {}
+static void Log_Write_Performance() {}
+static int8_t process_logs(uint8_t argc, const Menu::arg *argv) { return 0; }
+static void Log_Write_Attitude(int16_t log_roll, int16_t log_pitch, uint16_t log_yaw) {}
+static void Log_Write_Control_Tuning() {}
+static void Log_Write_Raw() {}
+
+
+#endif // LOGGING_ENABLED
diff --git a/APMrover2/Makefile b/APMrover2/Makefile
new file mode 100644
index 000000000..84b05b1e0
--- /dev/null
+++ b/APMrover2/Makefile
@@ -0,0 +1,35 @@
+include ../libraries/AP_Common/Arduino.mk
+
+nologging:
+ make -f Makefile EXTRAFLAGS="-DLOGGING_ENABLED=DISABLED"
+
+nogps:
+ make -f Makefile EXTRAFLAGS="-DGPS_PROTOCOL=GPS_PROTOCOL_NONE -DLOGGING_ENABLED=DISABLED"
+
+hil:
+ make -f Makefile EXTRAFLAGS="-DHIL_MODE=HIL_MODE_ATTITUDE -DCLI_SLIDER_ENABLED=DISABLED -DLOGGING_ENABLED=DISABLED"
+
+hilsensors:
+ make -f Makefile EXTRAFLAGS="-DHIL_MODE=HIL_MODE_SENSORS -DCLI_SLIDER_ENABLED=DISABLED -DLOGGING_ENABLED=DISABLED"
+
+hilnocli:
+ make -f Makefile EXTRAFLAGS="-DHIL_MODE=HIL_MODE_ATTITUDE -DCLI_ENABLED=DISABLED -DLOGGING_ENABLED=DISABLED"
+
+heli:
+ make -f Makefile EXTRAFLAGS="-DFRAME_CONFIG=HELI_FRAME"
+
+apm2:
+ make -f Makefile EXTRAFLAGS="-DCONFIG_APM_HARDWARE=APM_HARDWARE_APM2"
+
+apm2beta:
+ make -f Makefile EXTRAFLAGS="-DCONFIG_APM_HARDWARE=APM_HARDWARE_APM2 -DAPM2_BETA_HARDWARE"
+
+mavlink10:
+ make -f Makefile EXTRAFLAGS="-DMAVLINK10"
+
+sitl:
+ make -f ../libraries/Desktop/Makefile.desktop
+
+etags:
+ cd .. && etags -f ArduPlane/TAGS --langmap=C++:.pde.cpp.h $$(git ls-files ArduPlane libraries)
+
diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h
new file mode 100644
index 000000000..8060df868
--- /dev/null
+++ b/APMrover2/Parameters.h
@@ -0,0 +1,510 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#ifndef PARAMETERS_H
+#define PARAMETERS_H
+
+#include <AP_Common.h>
+
+// Global parameter class.
+//
+class Parameters {
+public:
+ // The version of the layout as described by the parameter enum.
+ //
+ // When changing the parameter enum in an incompatible fashion, this
+ // value should be incremented by one.
+ //
+ // The increment will prevent old parameters from being used incorrectly
+ // by newer code.
+ //
+ static const uint16_t k_format_version = 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,
+
+ // Misc
+ //
+ k_param_auto_trim,
+ k_param_switch_enable,
+ k_param_log_bitmask,
+ k_param_pitch_trim,
+ 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_num_resets,
+ k_param_log_last_filenumber, // *** Deprecated - remove with next eeprom number change
+ k_param_reset_switch_chan,
+ k_param_manual_level,
+
+
+ // 110: Telemetry control
+ //
+ k_param_gcs0 = 110, // stream rates for port0
+ k_param_gcs3, // stream rates for port3
+ k_param_sysid_this_mav,
+ k_param_sysid_my_gcs,
+ k_param_serial3_baud,
+
+ // 120: Fly-by-wire control
+ //
+ k_param_flybywire_airspeed_min = 120,
+ k_param_flybywire_airspeed_max,
+ k_param_FBWB_min_altitude, // 0=disabled, minimum value for altitude in cm (for first time try 30 meters = 3000 cm)
+
+ //
+ // 130: Sensor parameters
+ //
+ k_param_imu = 130, // sensor calibration
+ k_param_altitude_mix,
+ k_param_airspeed_ratio,
+ k_param_ground_temperature,
+ k_param_ground_pressure,
+ k_param_compass_enabled,
+ k_param_compass,
+ k_param_battery_monitoring,
+ k_param_volt_div_ratio,
+ k_param_curr_amp_per_volt,
+ k_param_input_voltage,
+ k_param_pack_capacity,
+ k_param_airspeed_offset,
+
+// k_param_sonar_enabled,
+ k_param_airspeed_enabled,
+
+ //
+ // 150: Navigation parameters
+ //
+ k_param_crosstrack_gain = 150,
+ k_param_crosstrack_entry_angle,
+ k_param_roll_limit,
+ k_param_pitch_limit_max,
+ k_param_pitch_limit_min,
+ k_param_airspeed_cruise,
+ k_param_RTL_altitude,
+ k_param_inverted_flight_ch,
+ k_param_min_gndspeed,
+ k_param_ch7_option,
+ //
+ // 160: Radio settings
+ //
+ k_param_channel_roll = 160,
+ k_param_channel_pitch,
+ k_param_channel_throttle,
+ k_param_channel_rudder,
+ k_param_rc_5,
+ k_param_rc_6,
+ k_param_rc_7,
+ k_param_rc_8,
+
+ k_param_throttle_min,
+ k_param_throttle_max,
+ k_param_throttle_fs_enabled, // 170
+ 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,
+
+ // ************************************************************
+ // 180: APMrover parameters - JLN update
+
+ k_param_closed_loop_nav,
+ k_param_auto_wp_radius,
+ k_param_nudgeoffset,
+ k_param_turn_gain,
+
+// ************************************************************
+//
+ // 200: Feed-forward gains
+ //
+ k_param_kff_pitch_compensation = 200,
+ k_param_kff_rudder_mix,
+ k_param_kff_pitch_to_throttle,
+ k_param_kff_throttle_to_pitch,
+
+ //
+ // 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,
+
+ //
+ // 220: Waypoint data
+ //
+ k_param_waypoint_mode = 220,
+ k_param_command_total,
+ k_param_command_index,
+ 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,
+
+ //
+ // 240: PID Controllers
+ //
+ // Heading-to-roll PID:
+ // heading error from command to roll command deviation from trim
+ // (bank to turn strategy)
+ //
+ k_param_pidNavRoll = 240,
+
+ // Roll-to-servo PID:
+ // roll error from command to roll servo deviation from trim
+ // (tracks commanded bank angle)
+ //
+ k_param_pidServoRoll,
+
+ //
+ // Pitch control
+ //
+ // Pitch-to-servo PID:
+ // pitch error from command to pitch servo deviation from trim
+ // (front-side strategy)
+ //
+ k_param_pidServoPitch,
+
+ // Airspeed-to-pitch PID:
+ // airspeed error from command to pitch servo deviation from trim
+ // (back-side strategy)
+ //
+ k_param_pidNavPitchAirspeed,
+
+ //
+ // Yaw control
+ //
+ // Yaw-to-servo PID:
+ // yaw rate error from command to yaw servo deviation from trim
+ // (stabilizes dutch roll)
+ //
+ k_param_pidServoRudder,
+
+ //
+ // Throttle control
+ //
+ // Energy-to-throttle PID:
+ // total energy error from command to throttle servo deviation from trim
+ // (throttle back-side strategy alternative)
+ //
+ k_param_pidTeThrottle,
+
+ // Altitude-to-pitch PID:
+ // altitude error from command to pitch servo deviation from trim
+ // (throttle front-side strategy alternative)
+ //
+ k_param_pidNavPitchAltitude,
+
+ // 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 serial3_baud;
+
+ // Feed-forward gains
+ //
+ AP_Float kff_pitch_compensation;
+ AP_Float kff_rudder_mix;
+ AP_Float kff_pitch_to_throttle;
+ AP_Float kff_throttle_to_pitch;
+
+ // Crosstrack navigation
+ //
+ AP_Float crosstrack_gain;
+ AP_Int16 crosstrack_entry_angle;
+
+ // Estimation
+ //
+ AP_Float altitude_mix;
+ AP_Float airspeed_ratio;
+ AP_Int16 airspeed_offset;
+
+ // Waypoints
+ //
+ AP_Int8 waypoint_mode;
+ AP_Int8 command_total;
+ AP_Int8 command_index;
+ AP_Int8 waypoint_radius;
+ AP_Int8 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
+ #endif
+
+ // Fly-by-wire
+ //
+ AP_Int8 flybywire_airspeed_min;
+ AP_Int8 flybywire_airspeed_max;
+ AP_Int16 FBWB_min_altitude;
+
+ // Throttle
+ //
+ AP_Int8 throttle_min;
+ AP_Int8 throttle_max;
+ AP_Int8 throttle_slewrate;
+ AP_Int8 throttle_fs_enabled;
+ AP_Int16 throttle_fs_value;
+ AP_Int8 throttle_cruise;
+
+ // Failsafe
+ AP_Int8 short_fs_action;
+ AP_Int8 long_fs_action;
+ AP_Int8 gcs_heartbeat_fs_enabled;
+
+ // 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;
+
+ // Navigational maneuvering limits
+ //
+ AP_Int16 roll_limit;
+ AP_Int16 pitch_limit_max;
+ AP_Int16 pitch_limit_min;
+
+ // Misc
+ //
+ AP_Int8 auto_trim;
+ AP_Int8 switch_enable;
+ AP_Int8 mix_mode;
+ AP_Int8 reverse_elevons;
+ AP_Int8 reverse_ch1_elevon;
+ AP_Int8 reverse_ch2_elevon;
+ AP_Int16 num_resets;
+ AP_Int16 log_bitmask;
+ AP_Int16 log_last_filenumber; // *** Deprecated - remove with next eeprom number change
+ AP_Int8 reset_switch_chan;
+ AP_Int8 manual_level;
+ AP_Int16 airspeed_cruise;
+ AP_Int16 min_gndspeed;
+ AP_Int8 ch7_option;
+
+ AP_Int16 pitch_trim;
+ AP_Int16 RTL_altitude;
+ AP_Int16 ground_temperature;
+ AP_Int32 ground_pressure;
+ AP_Int8 compass_enabled;
+ AP_Int16 angle_of_attack;
+ AP_Int8 battery_monitoring; // 0=disabled, 3=voltage only, 4=voltage and current
+ AP_Float volt_div_ratio;
+ AP_Float curr_amp_per_volt;
+ AP_Float input_voltage;
+ AP_Int16 pack_capacity; // Battery pack capacity less reserve
+ AP_Int8 inverted_flight_ch; // 0=disabled, 1-8 is channel for inverted flight trigger
+
+ // AP_Int8 sonar_enabled;
+ AP_Int8 airspeed_enabled;
+ AP_Int8 flap_1_percent;
+ AP_Int8 flap_1_speed;
+ AP_Int8 flap_2_percent;
+ AP_Int8 flap_2_speed;
+
+// ************ ThermoPilot parameters ************************
+// - JLN update
+
+ AP_Int8 closed_loop_nav;
+ AP_Int8 auto_wp_radius;
+ AP_Int16 nudgeoffset;
+ AP_Int16 turn_gain;
+
+// ************************************************************
+
+ // RC channels
+ RC_Channel channel_roll;
+ RC_Channel channel_pitch;
+ RC_Channel channel_throttle;
+ RC_Channel channel_rudder;
+ RC_Channel_aux rc_5;
+ RC_Channel_aux rc_6;
+ RC_Channel_aux rc_7;
+ RC_Channel_aux rc_8;
+
+ // PID controllers
+ //
+ PID pidNavRoll;
+ PID pidServoRoll;
+ PID pidServoPitch;
+ PID pidNavPitchAirspeed;
+ PID pidServoRudder;
+ PID pidTeThrottle;
+ PID pidNavPitchAltitude;
+
+ Parameters() :
+ format_version (k_format_version),
+ software_type (k_software_type),
+
+ sysid_this_mav (MAV_SYSTEM_ID),
+ sysid_my_gcs (255),
+ serial3_baud (SERIAL3_BAUD/1000),
+
+ kff_pitch_compensation (PITCH_COMP),
+ kff_rudder_mix (RUDDER_MIX),
+ kff_pitch_to_throttle (P_TO_T),
+ kff_throttle_to_pitch (T_TO_P),
+
+ crosstrack_gain (XTRACK_GAIN_SCALED),
+ crosstrack_entry_angle (XTRACK_ENTRY_ANGLE_CENTIDEGREE),
+
+ altitude_mix (ALTITUDE_MIX),
+ airspeed_ratio (AIRSPEED_RATIO),
+ airspeed_offset (0),
+
+ /* XXX waypoint_mode missing here */
+ command_total (0),
+ command_index (0),
+ waypoint_radius (WP_RADIUS_DEFAULT),
+ loiter_radius (LOITER_RADIUS_DEFAULT),
+
+#if GEOFENCE_ENABLED == ENABLED
+ fence_action (0),
+ fence_total (0),
+ fence_channel (0),
+ fence_minalt (0),
+ fence_maxalt (0),
+#endif
+
+ flybywire_airspeed_min (AIRSPEED_FBW_MIN),
+ flybywire_airspeed_max (AIRSPEED_FBW_MAX),
+
+ throttle_min (THROTTLE_MIN),
+ throttle_max (THROTTLE_MAX),
+ throttle_slewrate (THROTTLE_SLEW_LIMIT),
+ throttle_fs_enabled (THROTTLE_FAILSAFE),
+ throttle_fs_value (THROTTLE_FS_VALUE),
+ throttle_cruise (THROTTLE_CRUISE),
+
+ short_fs_action (SHORT_FAILSAFE_ACTION),
+ long_fs_action (LONG_FAILSAFE_ACTION),
+ gcs_heartbeat_fs_enabled(GCS_HEARTBEAT_FAILSAFE),
+
+ flight_mode_channel (FLIGHT_MODE_CHANNEL),
+ flight_mode1 (FLIGHT_MODE_1),
+ flight_mode2 (FLIGHT_MODE_2),
+ flight_mode3 (FLIGHT_MODE_3),
+ flight_mode4 (FLIGHT_MODE_4),
+ flight_mode5 (FLIGHT_MODE_5),
+ flight_mode6 (FLIGHT_MODE_6),
+
+ roll_limit (HEAD_MAX_CENTIDEGREE),
+ pitch_limit_max (PITCH_MAX_CENTIDEGREE),
+ pitch_limit_min (PITCH_MIN_CENTIDEGREE),
+
+ auto_trim (AUTO_TRIM),
+ manual_level (MANUAL_LEVEL),
+
+ switch_enable (REVERSE_SWITCH),
+ mix_mode (ELEVON_MIXING),
+ reverse_elevons (ELEVON_REVERSE),
+ reverse_ch1_elevon (ELEVON_CH1_REVERSE),
+ reverse_ch2_elevon (ELEVON_CH2_REVERSE),
+ num_resets (0),
+ log_bitmask (DEFAULT_LOG_BITMASK),
+ log_last_filenumber (0),
+ reset_switch_chan (0),
+ airspeed_cruise (AIRSPEED_CRUISE_CM),
+ min_gndspeed (MIN_GNDSPEED_CM),
+ ch7_option (CH7_OPTION),
+ pitch_trim (0),
+ RTL_altitude (ALT_HOLD_HOME_CM),
+ FBWB_min_altitude (ALT_HOLD_FBW_CM),
+
+ ground_temperature (0),
+ ground_pressure (0),
+ compass_enabled (MAGNETOMETER),
+ flap_1_percent (FLAP_1_PERCENT),
+ flap_1_speed (FLAP_1_SPEED),
+ flap_2_percent (FLAP_2_PERCENT),
+ flap_2_speed (FLAP_2_SPEED),
+
+
+ battery_monitoring (DISABLED),
+ volt_div_ratio (VOLT_DIV_RATIO),
+ curr_amp_per_volt (CURR_AMP_PER_VOLT),
+ input_voltage (INPUT_VOLTAGE),
+ pack_capacity (HIGH_DISCHARGE),
+ inverted_flight_ch (0),
+
+// sonar_enabled (SONAR_ENABLED),
+ airspeed_enabled (AIRSPEED_SENSOR),
+
+// ************ APMrover parameters ************************
+// - JLN update
+
+ closed_loop_nav (CLOSED_LOOP_NAV),
+ auto_wp_radius (AUTO_WP_RADIUS),
+ nudgeoffset (NUDGE_OFFSET),
+ turn_gain (TURN_GAIN),
+
+// ************************************************************
+
+
+
+ // PID controller initial P initial I initial D initial imax
+ //-----------------------------------------------------------------------------------
+ pidNavRoll (NAV_ROLL_P, NAV_ROLL_I, NAV_ROLL_D, NAV_ROLL_INT_MAX_CENTIDEGREE),
+ pidServoRoll (SERVO_ROLL_P, SERVO_ROLL_I, SERVO_ROLL_D, SERVO_ROLL_INT_MAX_CENTIDEGREE),
+ pidServoPitch (SERVO_PITCH_P, SERVO_PITCH_I, SERVO_PITCH_D, SERVO_PITCH_INT_MAX_CENTIDEGREE),
+ pidNavPitchAirspeed (NAV_PITCH_ASP_P, NAV_PITCH_ASP_I, NAV_PITCH_ASP_D, NAV_PITCH_ASP_INT_MAX_CMSEC),
+ pidServoRudder (SERVO_YAW_P, SERVO_YAW_I, SERVO_YAW_D, SERVO_YAW_INT_MAX),
+ pidTeThrottle (THROTTLE_TE_P, THROTTLE_TE_I, THROTTLE_TE_D, THROTTLE_TE_INT_MAX),
+ pidNavPitchAltitude (NAV_PITCH_ALT_P, NAV_PITCH_ALT_I, NAV_PITCH_ALT_D, NAV_PITCH_ALT_INT_MAX_CM)
+ {}
+};
+
+#endif // PARAMETERS_H
+
+/* ************ ThermoPilot parameters (old parameters setup ) ************************
+
+ low_rate_turn (LOW_RATE_TURN, k_param_low_rate_turn, PSTR("TP_LOWR_TURN")),
+ medium_rate_turn (MEDIUM_RATE_TURN, k_param_medium_rate_turn, PSTR("TP_MEDR_TURN")),
+ high_rate_turn (HIGH_RATE_TURN, k_param_high_rate_turn, PSTR("TP_HIGR_TURN")),
+ search_mode_turn (SEARCH_MODE_TURN, k_param_search_mode_turn, PSTR("TP_SRCM_TURN")),
+ slope_thermal (SLOPE_THERMAL, k_param_slope_thermal, PSTR("TP_SLOPE_THER")),
+ auto_thermal (AUTO_THERMAL, k_param_auto_thermal, PSTR("TP_AUTO_THER")),
+ stab_thermal (STAB_THERMAL, k_param_auto_thermal, PSTR("TP_STAB_THER")),
+ closed_loop_nav (CLOSED_LOOP_NAV, k_param_closed_loop_nav, PSTR("TP_CL_NAV")),
+ auto_wp_radius (AUTO_WP_RADIUS, k_param_closed_loop_nav, PSTR("TP_AWPR_NAV")),
+ min_alt (MIN_ALT, k_param_min_alt, PSTR("TP_MIN_ALT")),
+ max_alt (MAX_ALT, k_param_max_alt, PSTR("TP_MAX_ALT")),
+ max_dist (MAX_DIST, k_param_max_dist, PSTR("TP_MAX_DIST")),
+ sarsec_branch (SARSEC_BRANCH, k_param_sarsec_branch, PSTR("TP_SARSEC")),
+
+ ************************************************************/
+
diff --git a/APMrover2/Parameters.pde b/APMrover2/Parameters.pde
new file mode 100644
index 000000000..c52d3832a
--- /dev/null
+++ b/APMrover2/Parameters.pde
@@ -0,0 +1,174 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
+
+/*
+ ArduPlane parameter definitions
+
+ This firmware is free software; you can redistribute it and/or
+ modify it under the terms of the GNU Lesser General Public
+ License as published by the Free Software Foundation; either
+ version 2.1 of the License, or (at your option) any later version.
+*/
+
+#define GSCALAR(v, name) { g.v.vtype, name, Parameters::k_param_ ## v, &g.v }
+#define GGROUP(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &g.v, class::var_info }
+#define GOBJECT(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &v, class::var_info }
+
+static const AP_Param::Info var_info[] PROGMEM = {
+ GSCALAR(format_version, "FORMAT_VERSION"),
+ GSCALAR(software_type, "SYSID_SW_TYPE"),
+ GSCALAR(sysid_this_mav, "SYSID_THISMAV"),
+ GSCALAR(sysid_my_gcs, "SYSID_MYGCS"),
+ GSCALAR(serial3_baud, "SERIAL3_BAUD"),
+ GSCALAR(kff_pitch_compensation, "KFF_PTCHCOMP"),
+ GSCALAR(kff_rudder_mix, "KFF_RDDRMIX"),
+ GSCALAR(kff_pitch_to_throttle, "KFF_PTCH2THR"),
+ GSCALAR(kff_throttle_to_pitch, "KFF_THR2PTCH"),
+ GSCALAR(manual_level, "MANUAL_LEVEL"),
+
+ GSCALAR(crosstrack_gain, "XTRK_GAIN_SC"),
+ GSCALAR(crosstrack_entry_angle, "XTRK_ANGLE_CD"),
+
+ GSCALAR(altitude_mix, "ALT_MIX"),
+ GSCALAR(airspeed_ratio, "ARSPD_RATIO"),
+ GSCALAR(airspeed_offset, "ARSPD_OFFSET"),
+
+ GSCALAR(command_total, "CMD_TOTAL"),
+ GSCALAR(command_index, "CMD_INDEX"),
+ GSCALAR(waypoint_radius, "WP_RADIUS"),
+ GSCALAR(loiter_radius, "WP_LOITER_RAD"),
+
+#if GEOFENCE_ENABLED == ENABLED
+ GSCALAR(fence_action, "FENCE_ACTION"),
+ GSCALAR(fence_total, "FENCE_TOTAL"),
+ GSCALAR(fence_channel, "FENCE_CHANNEL"),
+ GSCALAR(fence_minalt, "FENCE_MINALT"),
+ GSCALAR(fence_maxalt, "FENCE_MAXALT"),
+#endif
+
+ GSCALAR(flybywire_airspeed_min, "ARSPD_FBW_MIN"),
+ GSCALAR(flybywire_airspeed_max, "ARSPD_FBW_MAX"),
+
+ GSCALAR(throttle_min, "THR_MIN"),
+ GSCALAR(throttle_max, "THR_MAX"),
+ GSCALAR(throttle_slewrate, "THR_SLEWRATE"),
+ GSCALAR(throttle_fs_enabled, "THR_FAILSAFE"),
+ GSCALAR(throttle_fs_value, "THR_FS_VALUE"),
+ GSCALAR(throttle_cruise, "TRIM_THROTTLE"),
+
+ GSCALAR(short_fs_action, "FS_SHORT_ACTN"),
+ GSCALAR(long_fs_action, "FS_LONG_ACTN"),
+ GSCALAR(gcs_heartbeat_fs_enabled, "FS_GCS_ENABL"),
+
+ GSCALAR(flight_mode_channel, "FLTMODE_CH"),
+ GSCALAR(flight_mode1, "FLTMODE1"),
+ GSCALAR(flight_mode2, "FLTMODE2"),
+ GSCALAR(flight_mode3, "FLTMODE3"),
+ GSCALAR(flight_mode4, "FLTMODE4"),
+ GSCALAR(flight_mode5, "FLTMODE5"),
+ GSCALAR(flight_mode6, "FLTMODE6"),
+
+ GSCALAR(roll_limit, "LIM_ROLL_CD"),
+ GSCALAR(pitch_limit_max, "LIM_PITCH_MAX"),
+ GSCALAR(pitch_limit_min, "LIM_PITCH_MIN"),
+
+ GSCALAR(auto_trim, "TRIM_AUTO"),
+ GSCALAR(switch_enable, "SWITCH_ENABLE"),
+ GSCALAR(mix_mode, "ELEVON_MIXING"),
+ GSCALAR(reverse_elevons, "ELEVON_REVERSE"),
+ GSCALAR(reverse_ch1_elevon, "ELEVON_CH1_REV"),
+ GSCALAR(reverse_ch2_elevon, "ELEVON_CH2_REV"),
+ GSCALAR(num_resets, "SYS_NUM_RESETS"),
+ GSCALAR(log_bitmask, "LOG_BITMASK"),
+ GSCALAR(log_last_filenumber, "LOG_LASTFILE"),
+ GSCALAR(reset_switch_chan, "RST_SWITCH_CH"),
+ GSCALAR(airspeed_cruise, "TRIM_ARSPD_CM"),
+ GSCALAR(min_gndspeed, "MIN_GNDSPD_CM"),
+ GSCALAR(ch7_option, "CH7_OPT"),
+
+ GSCALAR(pitch_trim, "TRIM_PITCH_CD"),
+ GSCALAR(RTL_altitude, "ALT_HOLD_RTL"),
+ GSCALAR(FBWB_min_altitude, "ALT_HOLD_FBWCM"),
+
+ GSCALAR(ground_temperature, "GND_TEMP"),
+ GSCALAR(ground_pressure, "GND_ABS_PRESS"),
+ GSCALAR(compass_enabled, "MAG_ENABLE"),
+ GSCALAR(flap_1_percent, "FLAP_1_PERCNT"),
+ GSCALAR(flap_1_speed, "FLAP_1_SPEED"),
+ GSCALAR(flap_2_percent, "FLAP_2_PERCNT"),
+ GSCALAR(flap_2_speed, "FLAP_2_SPEED"),
+
+
+ GSCALAR(battery_monitoring, "BATT_MONITOR"),
+ GSCALAR(volt_div_ratio, "VOLT_DIVIDER"),
+ GSCALAR(curr_amp_per_volt, "AMP_PER_VOLT"),
+ GSCALAR(input_voltage, "INPUT_VOLTS"),
+ GSCALAR(pack_capacity, "BATT_CAPACITY"),
+ GSCALAR(inverted_flight_ch, "INVERTEDFLT_CH"),
+
+ //GSCALAR(sonar_enabled, "SONAR_ENABLE"),
+ GSCALAR(airspeed_enabled, "ARSPD_ENABLE"),
+
+ // ************************************************************
+ // APMrover parameters - JLN update
+
+ GSCALAR(closed_loop_nav, "ROV_CL_NAV"),
+ GSCALAR(auto_wp_radius, "ROV_AWPR_NAV"),
+ GSCALAR(nudgeoffset, "ROV_NUDGE"),
+ GSCALAR(turn_gain, "ROV_GAIN"),
+// ************************************************************
+
+ GGROUP(channel_roll, "RC1_", RC_Channel),
+ GGROUP(channel_pitch, "RC2_", RC_Channel),
+ GGROUP(channel_throttle, "RC3_", RC_Channel),
+ GGROUP(channel_rudder, "RC4_", RC_Channel),
+ GGROUP(rc_5, "RC5_", RC_Channel_aux),
+ GGROUP(rc_6, "RC6_", RC_Channel_aux),
+ GGROUP(rc_7, "RC7_", RC_Channel_aux),
+ GGROUP(rc_8, "RC8_", RC_Channel_aux),
+
+ GGROUP(pidNavRoll, "HDNG2RLL_", PID),
+ GGROUP(pidServoRoll, "RLL2SRV_", PID),
+ GGROUP(pidServoPitch, "PTCH2SRV_", PID),
+ GGROUP(pidNavPitchAirspeed, "ARSP2PTCH_", PID),
+ GGROUP(pidServoRudder, "YW2SRV_", PID),
+ GGROUP(pidTeThrottle, "ENRGY2THR_", PID),
+ GGROUP(pidNavPitchAltitude, "ALT2PTCH_", PID),
+
+ // variables not in the g class which contain EEPROM saved variables
+ GOBJECT(compass, "COMPASS_", Compass),
+ GOBJECT(gcs0, "SR0_", GCS_MAVLINK),
+ GOBJECT(gcs3, "SR3_", GCS_MAVLINK),
+ GOBJECT(imu, "IMU_", IMU)
+};
+
+
+static void load_parameters(void)
+{
+ // setup the AP_Var subsystem for storage to EEPROM
+ if (!AP_Param::setup(var_info, sizeof(var_info)/sizeof(var_info[0]), WP_START_BYTE)) {
+ // this can only happen on startup, and its a definate coding
+ // error. Best not to continue so the programmer catches it
+ while (1) {
+ Serial.println_P(PSTR("ERROR: Failed to setup AP_Param"));
+ delay(1000);
+ }
+ }
+
+ if (!g.format_version.load() ||
+ g.format_version != Parameters::k_format_version) {
+
+ // erase all parameters
+ Serial.printf_P(PSTR("Firmware change: erasing EEPROM...\n"));
+ AP_Param::erase_all();
+
+ // save the current format version
+ g.format_version.set_and_save(Parameters::k_format_version);
+ Serial.println_P(PSTR("done."));
+ } else {
+ unsigned long before = micros();
+ // Load all auto-loaded EEPROM variables
+ AP_Param::load_all();
+
+ Serial.printf_P(PSTR("load_all took %luus\n"), micros() - before);
+ }
+}
diff --git a/APMrover2/command_description.txt b/APMrover2/command_description.txt
new file mode 100644
index 000000000..2d3102c9d
--- /dev/null
+++ b/APMrover2/command_description.txt
@@ -0,0 +1,85 @@
+ArduPilotMega 2.0 Commands
+
+Command Structure in bytes
+0 0x00 byte Command ID
+1 0x01 byte Parameter 1
+2 0x02 long Parameter 2
+3 0x03 ..
+4 0x04 ..
+5 0x05 ..
+6 0x06 long Parameter 3
+7 0x07 ..
+8 0x08 ..
+9 0x09 ..
+10 0x0A long Parameter 4
+11 0x0B ..
+11 0x0C ..
+11 0x0D ..
+
+
+
+***********************************
+Commands below MAV_CMD_NAV_LAST are commands that have a end criteria, eg "reached waypoint" or "reached altitude"
+***********************************
+Command ID Name Parameter 1 Altitude Latitude Longitude
+0x10 / 16 MAV_CMD_NAV_WAYPOINT - altitude lat lon
+
+0x11 / 17 MAV_CMD_NAV_LOITER_UNLIM (indefinitely) altitude lat lon
+
+0x12 / 18 MAV_CMD_NAV_LOITER_TURNS turns altitude lat lon
+
+0x13 / 19 MAV_CMD_NAV_LOITER_TIME time (seconds*10) altitude lat lon
+
+0x14 / 20 MAV_CMD_NAV_RETURN_TO_LAUNCH - altitude lat lon
+
+0x15 / 21 MAV_CMD_NAV_LAND - altitude lat lon
+
+0x16 / 22 MAV_CMD_NAV_TAKEOFF takeoff pitch altitude - -
+ NOTE: for command 0x16 the value takeoff pitch specifies the minimum pitch for the case with airspeed sensor and the target pitch for the case without.
+
+0x17 / 23 MAV_CMD_NAV_TARGET - altitude lat lon
+
+
+***********************************
+May Commands - these commands are optional to finish
+Command ID Name Parameter 1 Parameter 2 Parameter 3 Parameter 4
+0x70 / 112 MAV_CMD_CONDITION_DELAY - - time (seconds) -
+
+0x71 / 113 MAV_CMD_CONDITION_CHANGE_ALT - alt (finish) rate (cm/sec) -
+ Note: rate must be > 10 cm/sec due to integer math
+
+MAV_CMD_NAV_LAND_OPTIONS (NOT CURRENTLY IN MAVLINK PROTOCOL OR IMPLEMENTED IN APM)
+
+0x72 / 114 MAV_CMD_CONDITION_DISTANCE - - distance (meters) -
+
+0x71 / 115 MAV_CMD_CONDITION_YAW angle speed direction (-1,1) rel (1), abs (0)
+
+***********************************
+Unexecuted commands > MAV_CMD_NAV_LAST are dropped when ready for the next command < MAV_CMD_NAV_LAST so plan/queue commands accordingly!
+For example if you had a string of CMD_MAV_CONDITION commands following a 0x10 command that had not finished when the waypoint was
+reached, the unexecuted CMD_MAV_CONDITION and CMD_MAV_DO commands would be skipped and the next command < MAV_CMD_NAV_LAST would be loaded
+***********************************
+Now Commands - these commands are executed once until no more new now commands are available
+
+Command ID Name Parameter 1 Parameter 2 Parameter 3 Parameter 4
+0xB1 / 177 MAV_CMD_DO_JUMP index - repeat count -
+ Note: The repeat count must be greater than 1 for the command to execute. Use a repeat count of 1 if you intend a single use.
+
+0XB2 / 178 MAV_CMD_DO_CHANGE_SPEED Speed type Speed (m/s) Throttle (Percent) -
+ (0=Airspeed, 1=Ground Speed) (-1 indicates no change)(-1 indicates no change)
+
+0xB3 / 179 MAV_CMD_DO_SET_HOME Use current altitude lat lon
+ (1=use current location, 0=use specified location)
+
+0xB4 / 180 MAV_CMD_DO_SET_PARAMETER Param number Param value (NOT CURRENTLY IMPLEMENTED IN APM)
+
+0xB5 / 181 MAV_CMD_DO_SET_RELAY Relay number On/off (1/0) - -
+
+0xB6 / 182 MAV_CMD_DO_REPEAT_RELAY Relay number Cycle count Cycle time (sec) -
+ Note: Max cycle time = 60 sec, A repeat relay or repeat servo command will cancel any current repeating event
+
+0xB7 / 183 MAV_CMD_DO_SET_SERVO Servo number (5-8) On/off (1/0) - -
+
+0xB6 / 184 MAV_CMD_DO_REPEAT_SERVO Servo number (5-8) Cycle count Cycle time (sec) -
+ Note: Max cycle time = 60 sec, A repeat relay or repeat servo command will cancel any current repeating event
+
diff --git a/APMrover2/commands.pde b/APMrover2/commands.pde
new file mode 100644
index 000000000..0d853e1e7
--- /dev/null
+++ b/APMrover2/commands.pde
@@ -0,0 +1,284 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/* Functions in this file:
+ void init_commands()
+ void update_auto()
+ void reload_commands()
+ struct Location get_cmd_with_index(int i)
+ void set_cmd_with_index(struct Location temp, int i)
+ void increment_cmd_index()
+ void decrement_cmd_index()
+ long read_alt_to_hold()
+ void set_next_WP(struct Location *wp)
+ void set_guided_WP(void)
+ void init_home()
+************************************************************
+*/
+
+static void init_commands()
+{
+ g.command_index.set_and_save(0);
+ nav_command_ID = NO_COMMAND;
+ non_nav_command_ID = NO_COMMAND;
+ next_nav_command.id = CMD_BLANK;
+}
+
+static void update_auto()
+{
+ if (g.command_index >= g.command_total) {
+ handle_no_commands();
+ if(g.command_total == 0) {
+ next_WP.lat = home.lat + 1000; // so we don't have bad calcs
+ next_WP.lng = home.lng + 1000; // so we don't have bad calcs
+ }
+ } else {
+ if(g.command_index != 0) {
+ g.command_index = nav_command_index;
+ nav_command_index--;
+ }
+ nav_command_ID = NO_COMMAND;
+ non_nav_command_ID = NO_COMMAND;
+ next_nav_command.id = CMD_BLANK;
+ process_next_command();
+ }
+}
+
+// Reload all the wp
+static void reload_commands()
+{
+ init_commands();
+ g.command_index.load(); // XXX can we assume it's been loaded already by ::load_all?
+ decrement_cmd_index();
+}
+
+// Getters
+// -------
+static struct Location get_cmd_with_index(int i)
+{
+ struct Location temp;
+ long mem;
+
+ // Find out proper location in memory by using the start_byte position + the index
+ // --------------------------------------------------------------------------------
+ if (i > g.command_total) {
+ memset(&temp, 0, sizeof(temp));
+ temp.id = CMD_BLANK;
+ }else{
+ // read WP position
+ mem = (WP_START_BYTE) + (i * WP_SIZE);
+ temp.id = eeprom_read_byte((uint8_t*)mem);
+
+ mem++;
+ temp.options = eeprom_read_byte((uint8_t*)mem);
+
+ mem++;
+ temp.p1 = eeprom_read_byte((uint8_t*)mem);
+
+ mem++;
+ temp.alt = (long)eeprom_read_dword((uint32_t*)mem);
+
+ mem += 4;
+ temp.lat = (long)eeprom_read_dword((uint32_t*)mem);
+
+ mem += 4;
+ temp.lng = (long)eeprom_read_dword((uint32_t*)mem);
+ }
+
+ // Add on home altitude if we are a nav command (or other command with altitude) and stored alt is relative
+ if((temp.id < MAV_CMD_NAV_LAST || temp.id == MAV_CMD_CONDITION_CHANGE_ALT) && temp.options & MASK_OPTIONS_RELATIVE_ALT){
+ temp.alt += home.alt;
+ }
+
+ return temp;
+}
+
+// Setters
+// -------
+static void set_cmd_with_index(struct Location temp, int i)
+{
+ i = constrain(i, 0, g.command_total.get());
+ uint32_t mem = WP_START_BYTE + (i * WP_SIZE);
+
+ // Set altitude options bitmask
+ // XXX What is this trying to do?
+ if (temp.options & MASK_OPTIONS_RELATIVE_ALT && i != 0){
+ temp.options = MASK_OPTIONS_RELATIVE_ALT;
+ } else {
+ temp.options = 0;
+ }
+
+ eeprom_write_byte((uint8_t *) mem, temp.id);
+
+ mem++;
+ eeprom_write_byte((uint8_t *) mem, temp.options);
+
+ mem++;
+ eeprom_write_byte((uint8_t *) mem, temp.p1);
+
+ mem++;
+ eeprom_write_dword((uint32_t *) mem, temp.alt);
+
+ mem += 4;
+ eeprom_write_dword((uint32_t *) mem, temp.lat);
+
+ mem += 4;
+ eeprom_write_dword((uint32_t *) mem, temp.lng);
+}
+
+static void increment_cmd_index()
+{
+ if (g.command_index <= g.command_total) {
+ g.command_index.set_and_save(g.command_index + 1);
+ }
+}
+
+static void decrement_cmd_index()
+{
+ if (g.command_index > 0) {
+ g.command_index.set_and_save(g.command_index - 1);
+ }
+}
+
+static long read_alt_to_hold()
+{
+ if(g.RTL_altitude < 0)
+ return current_loc.alt;
+ else
+ return g.RTL_altitude + home.alt;
+}
+
+
+/*
+This function stores waypoint commands
+It looks to see what the next command type is and finds the last command.
+*/
+static void set_next_WP(struct Location *wp)
+{
+ // copy the current WP into the OldWP slot
+ // ---------------------------------------
+ prev_WP = next_WP;
+
+ // Load the next_WP slot
+ // ---------------------
+ next_WP = *wp;
+
+ // used to control FBW and limit the rate of climb
+ // -----------------------------------------------
+ target_altitude = current_loc.alt;
+
+ if(prev_WP.id != MAV_CMD_NAV_TAKEOFF && prev_WP.alt != home.alt && (next_WP.id == MAV_CMD_NAV_WAYPOINT || next_WP.id == MAV_CMD_NAV_LAND))
+ offset_altitude = next_WP.alt - prev_WP.alt;
+ else
+ offset_altitude = 0;
+
+ // zero out our loiter vals to watch for missed waypoints
+ loiter_delta = 0;
+ loiter_sum = 0;
+ loiter_total = 0;
+
+ // this is used to offset the shrinking longitude as we go towards the poles
+ float rads = (fabs((float)next_WP.lat)/t7) * 0.0174532925;
+ scaleLongDown = cos(rads);
+ scaleLongUp = 1.0f/cos(rads);
+ // this is handy for the groundstation
+ wp_totalDistance = get_distance(¤t_loc, &next_WP);
+ wp_distance = wp_totalDistance;
+ target_bearing = get_bearing(¤t_loc, &next_WP);
+ nav_bearing = target_bearing;
+
+ // to check if we have missed the WP
+ // ----------------------------
+ old_target_bearing = target_bearing;
+
+ // set a new crosstrack bearing
+ // ----------------------------
+ reset_crosstrack();
+}
+
+static void set_guided_WP(void)
+{
+ // copy the current location into the OldWP slot
+ // ---------------------------------------
+ prev_WP = current_loc;
+
+ // Load the next_WP slot
+ // ---------------------
+ next_WP = guided_WP;
+
+ // used to control FBW and limit the rate of climb
+ // -----------------------------------------------
+ target_altitude = current_loc.alt;
+ offset_altitude = next_WP.alt - prev_WP.alt;
+
+ // this is used to offset the shrinking longitude as we go towards the poles
+ float rads = (abs(next_WP.lat)/t7) * 0.0174532925;
+ scaleLongDown = cos(rads);
+ scaleLongUp = 1.0f/cos(rads);
+
+ // this is handy for the groundstation
+ wp_totalDistance = get_distance(¤t_loc, &next_WP);
+ wp_distance = wp_totalDistance;
+ target_bearing = get_bearing(¤t_loc, &next_WP);
+
+ // to check if we have missed the WP
+ // ----------------------------
+ old_target_bearing = target_bearing;
+
+ // set a new crosstrack bearing
+ // ----------------------------
+ reset_crosstrack();
+}
+
+// run this at setup on the ground
+// -------------------------------
+void init_home()
+{
+ gcs_send_text_P(SEVERITY_LOW, PSTR("init home"));
+
+ // block until we get a good fix
+ // -----------------------------
+ while (!g_gps->new_data || !g_gps->fix) {
+ g_gps->update();
+ }
+
+ home.id = MAV_CMD_NAV_WAYPOINT;
+#if HIL_MODE != HIL_MODE_ATTITUDE
+
+ home.lng = g_gps->longitude; // Lon * 10**7
+ home.lat = g_gps->latitude; // Lat * 10**7
+ gps_base_alt = max(g_gps->altitude, 0);
+ home.alt = g_gps->altitude;;
+ // Home is always 0
+#else
+ struct Location temp = get_cmd_with_index(0); // JLN update - for HIL test only get the home param stored in the FPL
+ if (temp.alt > 0) {
+ home.lng = temp.lng; // Lon * 10**7
+ home.lat = temp.lat; // Lat * 10**7
+ } else {
+ home.lng = g_gps->longitude; // Lon * 10**7
+ home.lat = g_gps->latitude; // Lat * 10**7
+ }
+
+ gps_base_alt = g_gps->altitude;; // get the stored home altitude as the base ref for AGL calculation.
+ home.alt = g_gps->altitude;;
+
+#endif
+ home_is_set = true;
+
+ //gcs_send_text_fmt(PSTR("gps alt: %lu"), (unsigned long)home.alt);
+
+ // Save Home to EEPROM - Command 0
+ // -------------------
+ set_cmd_with_index(home, 0);
+
+ // Save prev loc
+ // -------------
+ next_WP = prev_WP = home;
+
+ // Load home for a default guided_WP
+ // -------------
+ guided_WP = home;
+ guided_WP.alt += g.RTL_altitude;
+
+}
+
diff --git a/APMrover2/commands_logic.pde b/APMrover2/commands_logic.pde
new file mode 100644
index 000000000..c4241143e
--- /dev/null
+++ b/APMrover2/commands_logic.pde
@@ -0,0 +1,519 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/********************************************************************************/
+// Command Event Handlers
+/********************************************************************************/
+static void
+handle_process_nav_cmd()
+{
+ // reset navigation integrators
+ // -------------------------
+ reset_I();
+
+ gcs_send_text_fmt(PSTR("Executing command ID #%i"),next_nav_command.id);
+ switch(next_nav_command.id){
+
+ case MAV_CMD_NAV_TAKEOFF:
+ do_takeoff();
+ break;
+
+ case MAV_CMD_NAV_WAYPOINT: // Navigate to Waypoint
+ do_nav_wp();
+ break;
+
+ case MAV_CMD_NAV_LAND: // LAND to Waypoint
+ do_land();
+ break;
+
+ case MAV_CMD_NAV_LOITER_UNLIM: // Loiter indefinitely
+ do_loiter_unlimited();
+ break;
+
+ case MAV_CMD_NAV_LOITER_TURNS: // Loiter N Times
+ do_loiter_turns();
+ break;
+
+ case MAV_CMD_NAV_LOITER_TIME:
+ do_loiter_time();
+ break;
+
+ case MAV_CMD_NAV_RETURN_TO_LAUNCH:
+ do_RTL();
+ break;
+
+ default:
+ break;
+ }
+}
+
+static void
+handle_process_condition_command()
+{
+ gcs_send_text_fmt(PSTR("Executing command ID #%i"),next_nonnav_command.id);
+ switch(next_nonnav_command.id){
+
+ case MAV_CMD_CONDITION_DELAY:
+ do_wait_delay();
+ break;
+
+ case MAV_CMD_CONDITION_DISTANCE:
+ do_within_distance();
+ break;
+
+ case MAV_CMD_CONDITION_CHANGE_ALT:
+ do_change_alt();
+ break;
+
+ default:
+ break;
+ }
+}
+
+static void handle_process_do_command()
+{
+ gcs_send_text_fmt(PSTR("Executing command ID #%i"),next_nonnav_command.id);
+ switch(next_nonnav_command.id){
+
+ case MAV_CMD_DO_JUMP:
+ do_jump();
+ break;
+
+ case MAV_CMD_DO_CHANGE_SPEED:
+ do_change_speed();
+ break;
+
+ case MAV_CMD_DO_SET_HOME:
+ do_set_home();
+ break;
+
+ case MAV_CMD_DO_SET_SERVO:
+ do_set_servo();
+ break;
+
+ case MAV_CMD_DO_SET_RELAY:
+ do_set_relay();
+ break;
+
+ case MAV_CMD_DO_REPEAT_SERVO:
+ do_repeat_servo();
+ break;
+
+ case MAV_CMD_DO_REPEAT_RELAY:
+ do_repeat_relay();
+ break;
+
+#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:
+ camera_mount.set_roi_cmd();
+ break;
+
+ case MAV_CMD_DO_MOUNT_CONFIGURE: // Mission command to configure a camera mount |Mount operation mode (see MAV_CONFIGURE_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty|
+ camera_mount.configure_cmd();
+ break;
+
+ case MAV_CMD_DO_MOUNT_CONTROL: // Mission command to control a camera mount |pitch(deg*100) or lat, depending on mount mode.| roll(deg*100) or lon depending on mount mode| yaw(deg*100) or alt (in cm) depending on mount mode| Empty| Empty| Empty| Empty|
+ camera_mount.control_cmd();
+ break;
+#endif
+ }
+}
+
+static void handle_no_commands()
+{
+ gcs_send_text_fmt(PSTR("Returning to Home"));
+ next_nav_command = home;
+ next_nav_command.alt = read_alt_to_hold();
+ next_nav_command.id = MAV_CMD_NAV_LOITER_UNLIM;
+ nav_command_ID = MAV_CMD_NAV_LOITER_UNLIM;
+ non_nav_command_ID = WAIT_COMMAND;
+ handle_process_nav_cmd();
+}
+
+/********************************************************************************/
+// Verify command Handlers
+/********************************************************************************/
+
+static bool verify_nav_command() // Returns true if command complete
+{
+ switch(nav_command_ID) {
+
+ case MAV_CMD_NAV_TAKEOFF:
+ return verify_takeoff();
+ break;
+
+ case MAV_CMD_NAV_LAND:
+ return verify_land();
+ break;
+
+ case MAV_CMD_NAV_WAYPOINT:
+ return verify_nav_wp();
+ break;
+
+ case MAV_CMD_NAV_LOITER_UNLIM:
+ return verify_loiter_unlim();
+ break;
+
+ case MAV_CMD_NAV_LOITER_TURNS:
+ return verify_loiter_turns();
+ break;
+
+ case MAV_CMD_NAV_LOITER_TIME:
+ return verify_loiter_time();
+ break;
+
+ case MAV_CMD_NAV_RETURN_TO_LAUNCH:
+ return verify_RTL();
+ break;
+
+ default:
+ gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_nav: Invalid or no current Nav cmd"));
+ return false;
+ break;
+ }
+}
+
+static bool verify_condition_command() // Returns true if command complete
+{
+ switch(non_nav_command_ID) {
+ case NO_COMMAND:
+ break;
+
+ case MAV_CMD_CONDITION_DELAY:
+ return verify_wait_delay();
+ break;
+
+ case MAV_CMD_CONDITION_DISTANCE:
+ return verify_within_distance();
+ break;
+
+ case MAV_CMD_CONDITION_CHANGE_ALT:
+ return verify_change_alt();
+ break;
+
+ case WAIT_COMMAND:
+ return 0;
+ break;
+
+
+ default:
+ gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_conditon: Invalid or no current Condition cmd"));
+ break;
+ }
+ return false;
+}
+
+/********************************************************************************/
+// Nav (Must) commands
+/********************************************************************************/
+
+static void do_RTL(void)
+{
+ prev_WP = current_loc;
+ control_mode = RTL;
+ crash_timer = 0;
+ next_WP = home;
+
+ // Altitude to hold over home
+ // Set by configuration tool
+ // -------------------------
+ next_WP.alt = read_alt_to_hold();
+}
+
+static void do_takeoff()
+{
+ set_next_WP(&next_nav_command);
+}
+
+static void do_nav_wp()
+{
+ set_next_WP(&next_nav_command);
+}
+
+static void do_land()
+{
+ set_next_WP(&next_nav_command);
+}
+
+static void do_loiter_unlimited()
+{
+ set_next_WP(&next_nav_command);
+}
+
+static void do_loiter_turns()
+{
+ set_next_WP(&next_nav_command);
+ loiter_total = next_nav_command.p1 * 360;
+}
+
+static void do_loiter_time()
+{
+ set_next_WP(&next_nav_command);
+ loiter_time = millis();
+ loiter_time_max = next_nav_command.p1; // units are (seconds * 10)
+}
+
+/********************************************************************************/
+// Verify Nav (Must) commands
+/********************************************************************************/
+static bool verify_takeoff()
+{ return true;
+}
+
+static bool verify_land()
+{
+}
+
+static bool verify_nav_wp()
+{
+ hold_course = -1;
+ update_crosstrack();
+
+ if(g.auto_wp_radius)
+ { calc_turn_radius(); // JLN update - auto-adap the wp_radius Vs the gspeed and max roll angle
+
+ if ((wp_distance > 0) && (wp_distance <= wp_radius)) {
+ gcs_send_text_fmt(PSTR("Reached Waypoint #%i"),nav_command_index);
+ return true;
+ }
+ } else {
+ if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) {
+ gcs_send_text_fmt(PSTR("Reached Waypoint #%i"),nav_command_index);
+ return true;
+ }
+ }
+
+ // add in a more complex case
+ // Doug to do
+ if(loiter_sum > 300){
+ gcs_send_text_P(SEVERITY_MEDIUM,PSTR("Missed WP"));
+ return true;
+ }
+ return false;
+}
+
+static bool verify_loiter_unlim()
+{
+ update_loiter();
+ calc_bearing_error();
+ return false;
+}
+
+static bool verify_loiter_time()
+{
+ update_loiter();
+ calc_bearing_error();
+ if ((millis() - loiter_time) > (unsigned long)loiter_time_max * 10000l) { // scale loiter_time_max from (sec*10) to milliseconds
+ gcs_send_text_P(SEVERITY_LOW,PSTR("verify_nav: LOITER time complete"));
+ return true;
+ }
+ return false;
+}
+
+static bool verify_loiter_turns()
+{
+ update_loiter();
+ calc_bearing_error();
+ if(loiter_sum > loiter_total) {
+ loiter_total = 0;
+ gcs_send_text_P(SEVERITY_LOW,PSTR("verify_nav: LOITER orbits complete"));
+ // clear the command queue;
+ return true;
+ }
+ return false;
+}
+
+static bool verify_RTL()
+{
+ if (wp_distance <= g.waypoint_radius) {
+ gcs_send_text_P(SEVERITY_LOW,PSTR("Reached home"));
+ return true;
+ }else{
+ return false;
+ }
+}
+
+/********************************************************************************/
+// Condition (May) commands
+/********************************************************************************/
+
+static void do_wait_delay()
+{
+ condition_start = millis();
+ condition_value = next_nonnav_command.lat * 1000; // convert to milliseconds
+}
+
+static void do_change_alt()
+{
+ condition_rate = abs((int)next_nonnav_command.lat);
+ condition_value = next_nonnav_command.alt;
+ if(condition_value < current_loc.alt) condition_rate = -condition_rate;
+ target_altitude = current_loc.alt + (condition_rate / 10); // Divide by ten for 10Hz update
+ next_WP.alt = condition_value; // For future nav calculations
+ offset_altitude = 0; // For future nav calculations
+}
+
+static void do_within_distance()
+{
+ condition_value = next_nonnav_command.lat;
+}
+
+/********************************************************************************/
+// Verify Condition (May) commands
+/********************************************************************************/
+
+static bool verify_wait_delay()
+{
+ if ((unsigned)(millis() - condition_start) > condition_value){
+ condition_value = 0;
+ return true;
+ }
+ return false;
+}
+
+static bool verify_change_alt()
+{
+ if( (condition_rate>=0 && current_loc.alt >= condition_value) || (condition_rate<=0 && current_loc.alt <= condition_value)) {
+ condition_value = 0;
+ return true;
+ }
+ target_altitude += condition_rate / 10;
+ return false;
+}
+
+static bool verify_within_distance()
+{
+ if (wp_distance < condition_value){
+ condition_value = 0;
+ return true;
+ }
+ return false;
+}
+
+/********************************************************************************/
+// Do (Now) commands
+/********************************************************************************/
+
+static void do_loiter_at_location()
+{
+ next_WP = current_loc;
+}
+
+static void do_jump()
+{
+ struct Location temp;
+ gcs_send_text_fmt(PSTR("In jump. Jumps left: %i"),next_nonnav_command.lat);
+ if(next_nonnav_command.lat > 0) {
+
+ nav_command_ID = NO_COMMAND;
+ next_nav_command.id = NO_COMMAND;
+ non_nav_command_ID = NO_COMMAND;
+
+ temp = get_cmd_with_index(g.command_index);
+ temp.lat = next_nonnav_command.lat - 1; // Decrement repeat counter
+
+ set_cmd_with_index(temp, g.command_index);
+ gcs_send_text_fmt(PSTR("setting command index: %i"),next_nonnav_command.p1 - 1);
+ g.command_index.set_and_save(next_nonnav_command.p1 - 1);
+ nav_command_index = next_nonnav_command.p1 - 1;
+ next_WP = prev_WP; // Need to back "next_WP" up as it was set to the next waypoint following the jump
+ process_next_command();
+ } else if (next_nonnav_command.lat == -1) { // A repeat count of -1 = repeat forever
+ nav_command_ID = NO_COMMAND;
+ non_nav_command_ID = NO_COMMAND;
+ gcs_send_text_fmt(PSTR("setting command index: %i"),next_nonnav_command.p1 - 1);
+ g.command_index.set_and_save(next_nonnav_command.p1 - 1);
+ nav_command_index = next_nonnav_command.p1 - 1;
+ next_WP = prev_WP; // Need to back "next_WP" up as it was set to the next waypoint following the jump
+ process_next_command();
+ }
+}
+
+static void do_change_speed()
+{
+ switch (next_nonnav_command.p1)
+ {
+ case 0: // Airspeed
+ if(next_nonnav_command.alt > 0)
+ g.airspeed_cruise.set(next_nonnav_command.alt * 100);
+ break;
+ case 1: // Ground speed
+ g.min_gndspeed.set(next_nonnav_command.alt * 100);
+ break;
+ }
+
+ if(next_nonnav_command.lat > 0)
+ g.throttle_cruise.set(next_nonnav_command.lat);
+}
+
+static void do_set_home()
+{
+ if(next_nonnav_command.p1 == 1 && GPS_enabled) {
+ init_home();
+ } else {
+ home.id = MAV_CMD_NAV_WAYPOINT;
+ home.lng = next_nonnav_command.lng; // Lon * 10**7
+ home.lat = next_nonnav_command.lat; // Lat * 10**7
+ home.alt = max(next_nonnav_command.alt, 0);
+ home_is_set = true;
+ }
+}
+
+static void do_set_servo()
+{
+ APM_RC.OutputCh(next_nonnav_command.p1 - 1, next_nonnav_command.alt);
+}
+
+static void do_set_relay()
+{
+ if (next_nonnav_command.p1 == 1) {
+ relay.on();
+ } else if (next_nonnav_command.p1 == 0) {
+ relay.off();
+ }else{
+ relay.toggle();
+ }
+}
+
+static void do_repeat_servo()
+{
+ event_id = next_nonnav_command.p1 - 1;
+
+ if(next_nonnav_command.p1 >= CH_5 + 1 && next_nonnav_command.p1 <= CH_8 + 1) {
+
+ event_timer = 0;
+ event_delay = next_nonnav_command.lng * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds)
+ event_repeat = next_nonnav_command.lat * 2;
+ event_value = next_nonnav_command.alt;
+
+ switch(next_nonnav_command.p1) {
+ case CH_5:
+ event_undo_value = g.rc_5.radio_trim;
+ break;
+ case CH_6:
+ event_undo_value = g.rc_6.radio_trim;
+ break;
+ case CH_7:
+ event_undo_value = g.rc_7.radio_trim;
+ break;
+ case CH_8:
+ event_undo_value = g.rc_8.radio_trim;
+ break;
+ }
+ update_events();
+ }
+}
+
+static void do_repeat_relay()
+{
+ event_id = RELAY_TOGGLE;
+ event_timer = 0;
+ event_delay = next_nonnav_command.lat * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds)
+ event_repeat = next_nonnav_command.alt * 2;
+ update_events();
+}
+
diff --git a/APMrover2/commands_process.pde b/APMrover2/commands_process.pde
new file mode 100644
index 000000000..b6e98f307
--- /dev/null
+++ b/APMrover2/commands_process.pde
@@ -0,0 +1,146 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+// For changing active command mid-mission
+//----------------------------------------
+static void change_command(uint8_t cmd_index)
+{
+ struct Location temp = get_cmd_with_index(cmd_index);
+
+ if (temp.id > MAV_CMD_NAV_LAST ){
+ gcs_send_text_P(SEVERITY_LOW,PSTR("Bad Request - cannot change to non-Nav cmd"));
+ } else {
+ gcs_send_text_fmt(PSTR("Received Request - jump to command #%i"),cmd_index);
+
+ nav_command_ID = NO_COMMAND;
+ next_nav_command.id = NO_COMMAND;
+ non_nav_command_ID = NO_COMMAND;
+
+ nav_command_index = cmd_index - 1;
+ g.command_index.set_and_save(cmd_index);
+ update_commands();
+ }
+}
+
+// called by 10 Hz loop
+// --------------------
+static void update_commands(void)
+{
+ if(control_mode == AUTO){
+ if(home_is_set == true && g.command_total > 1){
+ process_next_command();
+ }
+ } // Other (eg GCS_Auto) modes may be implemented here
+}
+
+static void verify_commands(void)
+{
+ if(verify_nav_command()){
+ nav_command_ID = NO_COMMAND;
+ }
+
+ if(verify_condition_command()){
+ non_nav_command_ID = NO_COMMAND;
+ }
+}
+
+
+static void process_next_command()
+{
+ // This function makes sure that we always have a current navigation command
+ // and loads conditional or immediate commands if applicable
+
+ struct Location temp;
+ byte old_index = 0;
+
+ // these are Navigation/Must commands
+ // ---------------------------------
+ if (nav_command_ID == NO_COMMAND){ // no current navigation command loaded
+ old_index = nav_command_index;
+ temp.id = MAV_CMD_NAV_LAST;
+ while(temp.id >= MAV_CMD_NAV_LAST && nav_command_index <= g.command_total) {
+ nav_command_index++;
+ temp = get_cmd_with_index(nav_command_index);
+ }
+
+ gcs_send_text_fmt(PSTR("Nav command index updated to #%i"),nav_command_index);
+
+ if(nav_command_index > g.command_total){
+ // we are out of commands!
+ gcs_send_text_P(SEVERITY_LOW,PSTR("out of commands!"));
+
+ if (g.closed_loop_nav) { // JLN update - replay the FPL (CLOSED_LOOP_NAV)
+ change_command(1);
+ return;
+ } else {
+ handle_no_commands();
+ }
+ } else {
+ next_nav_command = temp;
+ nav_command_ID = next_nav_command.id;
+ non_nav_command_index = NO_COMMAND; // This will cause the next intervening non-nav command (if any) to be loaded
+ non_nav_command_ID = NO_COMMAND;
+
+ process_nav_cmd();
+ }
+ }
+
+ // these are Condition/May and Do/Now commands
+ // -------------------------------------------
+ if (non_nav_command_index == NO_COMMAND) { // If the index is NO_COMMAND then we have just loaded a nav command
+ non_nav_command_index = old_index + 1;
+ //gcs_send_text_fmt(PSTR("Non-Nav command index #%i"),non_nav_command_index);
+ } else if (non_nav_command_ID == NO_COMMAND) { // If the ID is NO_COMMAND then we have just completed a non-nav command
+ non_nav_command_index++;
+ }
+
+ //gcs_send_text_fmt(PSTR("Nav command index #%i"),nav_command_index);
+ //gcs_send_text_fmt(PSTR("Non-Nav command index #%i"),non_nav_command_index);
+ //gcs_send_text_fmt(PSTR("Non-Nav command ID #%i"),non_nav_command_ID);
+ if(nav_command_index <= (int)g.command_total && non_nav_command_ID == NO_COMMAND) {
+ temp = get_cmd_with_index(non_nav_command_index);
+ if(temp.id <= MAV_CMD_NAV_LAST) { // The next command is a nav command. No non-nav commands to do
+ g.command_index.set_and_save(nav_command_index);
+ non_nav_command_index = nav_command_index;
+ non_nav_command_ID = WAIT_COMMAND;
+ gcs_send_text_fmt(PSTR("Non-Nav command ID updated to #%i"),non_nav_command_ID);
+
+ } else { // The next command is a non-nav command. Prepare to execute it.
+ g.command_index.set_and_save(non_nav_command_index);
+ next_nonnav_command = temp;
+ non_nav_command_ID = next_nonnav_command.id;
+ gcs_send_text_fmt(PSTR("Non-Nav command ID updated to #%i"),non_nav_command_ID);
+
+ process_non_nav_command();
+ }
+
+ }
+}
+
+/**************************************************/
+// These functions implement the commands.
+/**************************************************/
+static void process_nav_cmd()
+{
+ //gcs_send_text_P(SEVERITY_LOW,PSTR("New nav command loaded"));
+
+ // clear non-nav command ID and index
+ non_nav_command_index = NO_COMMAND; // Redundant - remove?
+ non_nav_command_ID = NO_COMMAND; // Redundant - remove?
+
+ handle_process_nav_cmd();
+
+}
+
+static void process_non_nav_command()
+{
+ //gcs_send_text_P(SEVERITY_LOW,PSTR("new non-nav command loaded"));
+
+ if(non_nav_command_ID < MAV_CMD_CONDITION_LAST) {
+ handle_process_condition_command();
+ } else {
+ handle_process_do_command();
+ // flag command ID so a new one is loaded
+ // -----------------------------------------
+ non_nav_command_ID = NO_COMMAND;
+ }
+}
diff --git a/APMrover2/config.h b/APMrover2/config.h
new file mode 100644
index 000000000..85514f325
--- /dev/null
+++ b/APMrover2/config.h
@@ -0,0 +1,873 @@
+// -*- 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.
+///
+#include "APM_Config.h" // <== THIS INCLUDE, DO NOT EDIT IT. EVER.
+///
+/// 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
+
+//////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////
+// HARDWARE CONFIGURATION AND CONNECTIONS
+//////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////
+
+//////////////////////////////////////////////////////////////////////////////
+// APM HARDWARE
+//
+
+#ifndef CONFIG_APM_HARDWARE
+# define CONFIG_APM_HARDWARE APM_HARDWARE_APM1
+#endif
+
+#if defined( __AVR_ATmega1280__ )
+#define CLI_ENABLED DISABLED
+#define LOGGING_ENABLED DISABLED
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// APM2 HARDWARE DEFAULTS
+//
+
+#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
+# define CONFIG_IMU_TYPE CONFIG_IMU_MPU6000
+# define CONFIG_PUSHBUTTON DISABLED
+# define CONFIG_RELAY DISABLED
+# define MAG_ORIENTATION AP_COMPASS_APM2_SHIELD
+# define CONFIG_SONAR_SOURCE SONAR_SOURCE_ANALOG_PIN
+# define CONFIG_PITOT_SOURCE PITOT_SOURCE_ANALOG_PIN
+# define MAGNETOMETER ENABLED
+# ifdef APM2_BETA_HARDWARE
+# define CONFIG_BARO AP_BARO_BMP085
+# else // APM2 Production Hardware (default)
+# define CONFIG_BARO AP_BARO_MS5611
+# endif
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// LED and IO Pins
+//
+#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM1
+# define A_LED_PIN 37
+# define B_LED_PIN 36
+# define C_LED_PIN 35
+# define LED_ON HIGH
+# define LED_OFF LOW
+# define SLIDE_SWITCH_PIN 40
+# define PUSHBUTTON_PIN 41
+# define USB_MUX_PIN -1
+# define CONFIG_RELAY ENABLED
+# define BATTERY_PIN_1 0
+# define CURRENT_PIN_1 1
+#elif CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
+# define A_LED_PIN 27
+# define B_LED_PIN 26
+# define C_LED_PIN 25
+# define LED_ON LOW
+# define LED_OFF HIGH
+# define SLIDE_SWITCH_PIN (-1)
+# define PUSHBUTTON_PIN (-1)
+# define CLI_SLIDER_ENABLED DISABLED
+# define USB_MUX_PIN 23
+# define BATTERY_PIN_1 1
+# define CURRENT_PIN_1 2
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// AIRSPEED_SENSOR
+// AIRSPEED_RATIO
+//
+#ifndef AIRSPEED_SENSOR
+# define AIRSPEED_SENSOR DISABLED
+#endif
+
+#ifndef AIRSPEED_RATIO
+# define AIRSPEED_RATIO 1.9936 // Note - this varies from the value in ArduPilot due to the difference in ADC resolution
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// IMU Selection
+//
+#ifndef CONFIG_IMU_TYPE
+# define CONFIG_IMU_TYPE CONFIG_IMU_OILPAN
+#endif
+
+#if CONFIG_IMU_TYPE == CONFIG_IMU_MPU6000
+# ifndef CONFIG_MPU6000_CHIP_SELECT_PIN
+# define CONFIG_MPU6000_CHIP_SELECT_PIN 53
+# endif
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// ADC Enable - used to eliminate for systems which don't have ADC.
+//
+#ifndef CONFIG_ADC
+# if CONFIG_IMU_TYPE == CONFIG_IMU_OILPAN
+# define CONFIG_ADC ENABLED
+# else
+# define CONFIG_ADC DISABLED
+# endif
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// Barometer
+//
+
+#ifndef CONFIG_BARO
+# define CONFIG_BARO AP_BARO_BMP085
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// Pitot
+//
+
+#ifndef PITOT_ENABLED
+# define PITOT_ENABLED DISABLED
+#endif
+
+#ifndef CONFIG_PITOT_SOURCE
+# define CONFIG_PITOT_SOURCE PITOT_SOURCE_ADC
+#endif
+
+#if CONFIG_PITOT_SOURCE == PITOT_SOURCE_ADC
+# ifndef CONFIG_PITOT_SOURCE_ADC_CHANNEL
+# define CONFIG_PITOT_SOURCE_ADC_CHANNEL 7
+# endif
+#elif CONFIG_PITOT_SOURCE == PITOT_SOURCE_ANALOG_PIN
+# ifndef CONFIG_PITOT_SOURCE_ANALOG_PIN
+# define CONFIG_PITOT_SOURCE_ANALOG_PIN 0
+# endif
+#else
+# warning Invalid value for CONFIG_PITOT_SOURCE, disabling airspeed
+# undef PITOT_ENABLED
+# define PITOT_ENABLED DISABLED
+#endif
+
+#ifndef SONAR_TYPE
+# define SONAR_TYPE MAX_SONAR_LV // MAX_SONAR_XL,
+#endif
+
+#ifndef SONAR_ENABLED
+#define SONAR_ENABLED DISABLED
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// HIL_MODE OPTIONAL
+
+#ifndef HIL_MODE
+#define HIL_MODE HIL_MODE_DISABLED
+#endif
+
+#if HIL_MODE != HIL_MODE_DISABLED // we are in HIL mode
+ # undef GPS_PROTOCOL
+ # define GPS_PROTOCOL GPS_PROTOCOL_NONE
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// GPS_PROTOCOL
+//
+// Note that this test must follow the HIL_PROTOCOL block as the HIL
+// setup may override the GPS configuration.
+//
+#ifndef GPS_PROTOCOL
+# define GPS_PROTOCOL GPS_PROTOCOL_AUTO
+#endif
+
+#ifndef MAV_SYSTEM_ID
+# define MAV_SYSTEM_ID 1
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// Serial port speeds.
+//
+#ifndef SERIAL0_BAUD
+# define SERIAL0_BAUD 115200
+#endif
+#ifndef SERIAL3_BAUD
+# define SERIAL3_BAUD 57600
+#endif
+
+#ifndef CH7_OPTION
+# define CH7_OPTION CH7_DO_NOTHING
+#endif
+
+#ifndef TUNING_OPTION
+# define TUNING_OPTION TUN_NONE
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// Battery monitoring
+//
+#ifndef BATTERY_EVENT
+# define BATTERY_EVENT DISABLED
+#endif
+#ifndef LOW_VOLTAGE
+# define LOW_VOLTAGE 9.6
+#endif
+#ifndef VOLT_DIV_RATIO
+# define VOLT_DIV_RATIO 3.56 // This is the proper value for an on-board APM1 voltage divider with a 3.9kOhm resistor
+//# define VOLT_DIV_RATIO 15.70 // This is the proper value for the AttoPilot 50V/90A sensor
+//# define VOLT_DIV_RATIO 4.127 // This is the proper value for the AttoPilot 13.6V/45A sensor
+
+#endif
+
+#ifndef CURR_AMP_PER_VOLT
+# define CURR_AMP_PER_VOLT 27.32 // This is the proper value for the AttoPilot 50V/90A sensor
+//# define CURR_AMP_PER_VOLT 13.66 // This is the proper value for the AttoPilot 13.6V/45A sensor
+#endif
+
+#ifndef CURR_AMPS_OFFSET
+# define CURR_AMPS_OFFSET 0.0
+#endif
+#ifndef HIGH_DISCHARGE
+# define HIGH_DISCHARGE 1760
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// INPUT_VOLTAGE
+//
+#ifndef INPUT_VOLTAGE
+# 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
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// MAGNETOMETER
+#ifndef MAGNETOMETER
+# define MAGNETOMETER DISABLED
+#endif
+#ifndef MAG_ORIENTATION
+# define MAG_ORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD
+#endif
+
+
+//////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////
+// 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 255
+#endif
+#ifndef FLAP_2_PERCENT
+# define FLAP_2_PERCENT 0
+#endif
+#ifndef FLAP_2_SPEED
+# define FLAP_2_SPEED 255
+#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 STABILIZE
+#endif
+#if !defined(FLIGHT_MODE_2)
+# define FLIGHT_MODE_2 STABILIZE
+#endif
+#if !defined(FLIGHT_MODE_3)
+# define FLIGHT_MODE_3 STABILIZE
+#endif
+#if !defined(FLIGHT_MODE_4)
+# define FLIGHT_MODE_4 STABILIZE
+#endif
+#if !defined(FLIGHT_MODE_5)
+# define FLIGHT_MODE_5 STABILIZE
+#endif
+#if !defined(FLIGHT_MODE_6)
+# define FLIGHT_MODE_6 MANUAL
+#endif
+
+
+//////////////////////////////////////////////////////////////////////////////
+// THROTTLE_FAILSAFE
+// THROTTLE_FS_VALUE
+// SHORT_FAILSAFE_ACTION
+// LONG_FAILSAFE_ACTION
+// GCS_HEARTBEAT_FAILSAFE
+//
+#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
+#ifndef GCS_HEARTBEAT_FAILSAFE
+# define GCS_HEARTBEAT_FAILSAFE DISABLED
+#endif
+
+
+//////////////////////////////////////////////////////////////////////////////
+// AUTO_TRIM
+//
+#ifndef AUTO_TRIM
+# define AUTO_TRIM DISABLED
+#endif
+
+
+//////////////////////////////////////////////////////////////////////////////
+// MANUAL_LEVEL
+//
+#ifndef MANUAL_LEVEL
+# define MANUAL_LEVEL DISABLED
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// THROTTLE_REVERSE
+//
+#ifndef THROTTLE_REVERSE
+# define THROTTLE_REVERSE DISABLED
+#endif
+
+
+//////////////////////////////////////////////////////////////////////////////
+// ENABLE_STICK_MIXING
+//
+#ifndef ENABLE_STICK_MIXING
+# define ENABLE_STICK_MIXING ENABLED
+#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_AIR_START
+//
+#ifndef ENABLE_AIR_START
+# define ENABLE_AIR_START DISABLED
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// ENABLE REVERSE_SWITCH
+//
+#ifndef REVERSE_SWITCH
+# define REVERSE_SWITCH ENABLED
+#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
+#ifndef FLAPERON
+# define FLAPERON DISABLED
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// MOUNT (ANTENNA OR CAMERA)
+//
+#ifndef MOUNT
+# define MOUNT DISABLED
+#endif
+
+#if defined( __AVR_ATmega1280__ ) && CAMERA == ENABLED
+// The small ATmega1280 chip does not have enough memory for camera support
+// so disable CLI, this will allow camera support and other improvements to fit.
+// This should almost have no side effects, because the APM planner can now do a complete board setup.
+#define CLI_ENABLED DISABLED
+#endif
+
+
+//////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////
+// FLIGHT AND NAVIGATION CONTROL
+//////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////
+
+//////////////////////////////////////////////////////////////////////////////
+// Altitude measurement and control.
+//
+#ifndef ALT_EST_GAIN
+# define ALT_EST_GAIN 0.01
+#endif
+#ifndef ALTITUDE_MIX
+# define ALTITUDE_MIX 1
+#endif
+
+
+//////////////////////////////////////////////////////////////////////////////
+// AIRSPEED_CRUISE
+//
+#ifndef AIRSPEED_CRUISE
+# define AIRSPEED_CRUISE 3 // 12 m/s
+#endif
+#define AIRSPEED_CRUISE_CM AIRSPEED_CRUISE*100
+
+#ifndef GSBOOST
+# define GSBOOST 0
+#endif
+#ifndef GSBOOST
+# define GSBOOST 0
+#endif
+#ifndef NUDGE_OFFSET
+# define NUDGE_OFFSET 0
+#endif
+
+#ifndef E_GLIDER
+# define E_GLIDER ENABLED
+#endif
+
+#ifndef TURN_GAIN
+# define TURN_GAIN 5
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// 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 6
+#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
+
+
+
+/* The following parmaeters have no corresponding control implementation
+#ifndef THROTTLE_ALT_P
+# define THROTTLE_ALT_P 0.32
+#endif
+#ifndef THROTTLE_ALT_I
+# define THROTTLE_ALT_I 0.0
+#endif
+#ifndef THROTTLE_ALT_D
+# define THROTTLE_ALT_D 0.0
+#endif
+#ifndef THROTTLE_ALT_INT_MAX
+# define THROTTLE_ALT_INT_MAX 20
+#endif
+#define THROTTLE_ALT_INT_MAX_CM THROTTLE_ALT_INT_MAX*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 15
+#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
+
+//////////////////////////////////////////////////////////////////////////////
+// Attitude control gains
+//
+#ifndef SERVO_ROLL_P
+# define SERVO_ROLL_P 0.4
+#endif
+#ifndef SERVO_ROLL_I
+# define SERVO_ROLL_I 0.0
+#endif
+#ifndef SERVO_ROLL_D
+# define SERVO_ROLL_D 0.0
+#endif
+#ifndef SERVO_ROLL_INT_MAX
+# define SERVO_ROLL_INT_MAX 5
+#endif
+#define SERVO_ROLL_INT_MAX_CENTIDEGREE SERVO_ROLL_INT_MAX*100
+#ifndef ROLL_SLEW_LIMIT
+# define ROLL_SLEW_LIMIT 0
+#endif
+#ifndef SERVO_PITCH_P
+# define SERVO_PITCH_P 0.6
+#endif
+#ifndef SERVO_PITCH_I
+# define SERVO_PITCH_I 0.0
+#endif
+#ifndef SERVO_PITCH_D
+# define SERVO_PITCH_D 0.0
+#endif
+#ifndef SERVO_PITCH_INT_MAX
+# define SERVO_PITCH_INT_MAX 5
+#endif
+#define SERVO_PITCH_INT_MAX_CENTIDEGREE SERVO_PITCH_INT_MAX*100
+#ifndef PITCH_COMP
+# define PITCH_COMP 0.2
+#endif
+#ifndef SERVO_YAW_P
+# define SERVO_YAW_P 0.0
+#endif
+#ifndef SERVO_YAW_I
+# define SERVO_YAW_I 0.0
+#endif
+#ifndef SERVO_YAW_D
+# define SERVO_YAW_D 0.0
+#endif
+#ifndef SERVO_YAW_INT_MAX
+# define SERVO_YAW_INT_MAX 0
+#endif
+#ifndef RUDDER_MIX
+# define RUDDER_MIX 0.5
+#endif
+
+
+//////////////////////////////////////////////////////////////////////////////
+// Navigation control gains
+//
+#ifndef NAV_ROLL_P
+# define NAV_ROLL_P 0.7
+#endif
+#ifndef NAV_ROLL_I
+# define NAV_ROLL_I 0.0
+#endif
+#ifndef NAV_ROLL_D
+# define NAV_ROLL_D 0.02
+#endif
+#ifndef NAV_ROLL_INT_MAX
+# define NAV_ROLL_INT_MAX 5
+#endif
+#define NAV_ROLL_INT_MAX_CENTIDEGREE NAV_ROLL_INT_MAX*100
+#ifndef NAV_PITCH_ASP_P
+# define NAV_PITCH_ASP_P 0.65
+#endif
+#ifndef NAV_PITCH_ASP_I
+# define NAV_PITCH_ASP_I 0.0
+#endif
+#ifndef NAV_PITCH_ASP_D
+# define NAV_PITCH_ASP_D 0.0
+#endif
+#ifndef NAV_PITCH_ASP_INT_MAX
+# define NAV_PITCH_ASP_INT_MAX 5
+#endif
+#define NAV_PITCH_ASP_INT_MAX_CMSEC NAV_PITCH_ASP_INT_MAX*100
+#ifndef NAV_PITCH_ALT_P
+# define NAV_PITCH_ALT_P 0.65
+#endif
+#ifndef NAV_PITCH_ALT_I
+# define NAV_PITCH_ALT_I 0.0
+#endif
+#ifndef NAV_PITCH_ALT_D
+# define NAV_PITCH_ALT_D 0.0
+#endif
+#ifndef NAV_PITCH_ALT_INT_MAX
+# define NAV_PITCH_ALT_INT_MAX 5
+#endif
+#define NAV_PITCH_ALT_INT_MAX_CM NAV_PITCH_ALT_INT_MAX*100
+
+
+//////////////////////////////////////////////////////////////////////////////
+// Energy/Altitude control gains
+//
+#ifndef THROTTLE_TE_P
+# define THROTTLE_TE_P 0.50
+#endif
+#ifndef THROTTLE_TE_I
+# define THROTTLE_TE_I 0.0
+#endif
+#ifndef THROTTLE_TE_D
+# define THROTTLE_TE_D 0.0
+#endif
+#ifndef THROTTLE_TE_INT_MAX
+# define THROTTLE_TE_INT_MAX 20
+#endif
+#ifndef THROTTLE_SLEW_LIMIT
+# define THROTTLE_SLEW_LIMIT 0
+#endif
+#ifndef P_TO_T
+# define P_TO_T 0
+#endif
+#ifndef T_TO_P
+# define T_TO_P 0
+#endif
+#ifndef PITCH_TARGET
+# define PITCH_TARGET 0
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// Crosstrack compensation
+//
+#ifndef XTRACK_GAIN
+# define XTRACK_GAIN 1 // deg/m
+#endif
+#ifndef XTRACK_ENTRY_ANGLE
+# define XTRACK_ENTRY_ANGLE 20 // deg
+#endif
+# define XTRACK_GAIN_SCALED XTRACK_GAIN*100
+# define XTRACK_ENTRY_ANGLE_CENTIDEGREE XTRACK_ENTRY_ANGLE*100
+
+//////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////
+// DEBUGGING
+//////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////
+
+//////////////////////////////////////////////////////////////////////////////
+// Dataflash logging control
+//
+
+#ifndef LOGGING_ENABLED
+# define LOGGING_ENABLED ENABLED
+#endif
+
+
+#ifndef LOG_ATTITUDE_FAST
+# define LOG_ATTITUDE_FAST DISABLED
+#endif
+#ifndef LOG_ATTITUDE_MED
+# define LOG_ATTITUDE_MED ENABLED
+#endif
+#ifndef LOG_GPS
+# define LOG_GPS ENABLED
+#endif
+#ifndef LOG_PM
+# define LOG_PM DISABLED
+#endif
+#ifndef LOG_CTUN
+# define LOG_CTUN ENABLED
+#endif
+#ifndef LOG_NTUN
+# define LOG_NTUN DISABLED
+#endif
+#ifndef LOG_MODE
+# define LOG_MODE ENABLED
+#endif
+#ifndef LOG_RAW
+# define LOG_RAW DISABLED
+#endif
+#ifndef LOG_CMD
+# define LOG_CMD ENABLED
+#endif
+#ifndef LOG_CUR
+# define LOG_CUR DISABLED
+#endif
+
+// calculate the default log_bitmask
+#define LOGBIT(_s) (LOG_##_s ? MASK_LOG_##_s : 0)
+
+#define DEFAULT_LOG_BITMASK \
+ LOGBIT(ATTITUDE_FAST) | \
+ LOGBIT(ATTITUDE_MED) | \
+ LOGBIT(GPS) | \
+ LOGBIT(PM) | \
+ LOGBIT(CTUN) | \
+ LOGBIT(NTUN) | \
+ LOGBIT(MODE) | \
+ LOGBIT(RAW) | \
+ LOGBIT(CMD) | \
+ LOGBIT(CUR)
+
+
+//////////////////////////////////////////////////////////////////////////////
+// Navigation defaults
+//
+#ifndef WP_RADIUS_DEFAULT
+# define WP_RADIUS_DEFAULT 30
+#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
+
+//////////////////////////////////////////////////////////////////////////////
+// Developer Items
+//
+
+#ifndef STANDARD_SPEED
+# define STANDARD_SPEED 3.0
+#define STANDARD_SPEED_SQUARED (STANDARD_SPEED * STANDARD_SPEED)
+#endif
+#define STANDARD_THROTTLE_SQUARED (THROTTLE_CRUISE * THROTTLE_CRUISE)
+
+// use this to enable servos in HIL mode
+#ifndef HIL_SERVOS
+# define HIL_SERVOS DISABLED
+#endif
+
+// use this to completely disable the CLI
+#ifndef CLI_ENABLED
+# define CLI_ENABLED ENABLED
+#endif
+
+// use this to disable the CLI slider switch
+#ifndef CLI_SLIDER_ENABLED
+# define CLI_SLIDER_ENABLED ENABLED
+#endif
+
+// delay to prevent Xbee bricking, in milliseconds
+#ifndef MAVLINK_TELEMETRY_PORT_DELAY
+# define MAVLINK_TELEMETRY_PORT_DELAY 2000
+#endif
+
+// use this to disable gen-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
+
+// experimental quaternion code
+#ifndef QUATERNION_ENABLE
+# define QUATERNION_ENABLE DISABLED
+#endif
diff --git a/APMrover2/control_modes.pde b/APMrover2/control_modes.pde
new file mode 100644
index 000000000..68a3e899f
--- /dev/null
+++ b/APMrover2/control_modes.pde
@@ -0,0 +1,147 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+static void read_control_switch()
+{
+
+ byte 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;
+
+ // 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 &&
+ APM_RC.InputCh(g.reset_switch_chan-1) > RESET_SWITCH_CHAN_PWM)) {
+
+ set_mode(flight_modes[switchPosition]);
+
+ oldSwitchPosition = switchPosition;
+ prev_WP = current_loc;
+
+ // reset navigation integrators
+ // -------------------------
+ reset_I();
+
+ }
+
+}
+
+static byte readSwitch(void){
+ uint16_t pulsewidth = APM_RC.InputCh(g.flight_mode_channel - 1);
+ if (pulsewidth <= 910 || pulsewidth >= 2090) 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;
+}
+
+static void reset_control_switch()
+{
+ oldSwitchPosition = 0;
+ read_control_switch();
+}
+
+#define CH_7_PWM_TRIGGER 1800
+
+// read at 10 hz
+// set this to your trainer switch
+static void read_trim_switch()
+{
+if (g.ch7_option == CH7_SAVE_WP){ // set to 1
+ if (g.rc_7.radio_in > CH_7_PWM_TRIGGER){ // switch is engaged
+ trim_flag = true;
+
+ }else{ // switch is disengaged
+ if(trim_flag){
+ trim_flag = false;
+
+ if(control_mode == MANUAL){ // if SW7 is ON in MANUAL = Erase the Flight Plan
+ // reset the mission
+ CH7_wp_index = 0;
+ g.command_total.set_and_save(CH7_wp_index);
+ #if X_PLANE == ENABLED
+ Serial.printf_P(PSTR("*** RESET the FPL\n"));
+ #endif
+ CH7_wp_index = 1;
+ return;
+ } else if (control_mode == STABILIZE) { // if SW7 is ON in STABILIZE = record the Wp
+ // set the next_WP (home is stored at 0)
+ // max out at 100 since I think we need to stay under the EEPROM limit
+ CH7_wp_index = constrain(CH7_wp_index, 1, 100);
+
+ current_loc.id = MAV_CMD_NAV_WAYPOINT;
+
+ // store the index
+ g.command_total.set_and_save(CH7_wp_index);
+
+ // save command
+ set_cmd_with_index(current_loc, CH7_wp_index);
+
+ #if X_PLANE == ENABLED
+ Serial.printf_P(PSTR("*** Store WP #%d\n"),CH7_wp_index);
+ #endif
+
+ // increment index
+ CH7_wp_index++;
+
+ } else if (control_mode == AUTO) { // if SW7 is ON in AUTO = set to RTL
+ set_mode(RTL);
+ }
+ }
+ }
+ }
+ else if (g.ch7_option == CH7_RTL){ // set to 6
+ if (g.rc_7.radio_in > CH_7_PWM_TRIGGER){ // switch is engaged
+ trim_flag = true;
+
+ }else{ // switch is disengaged
+ if(trim_flag){
+ trim_flag = false;
+ if (control_mode == STABILIZE) {
+ set_mode(RTL);
+ }
+ }
+ }
+ }
+}
+
+static void update_servo_switches()
+{
+#if CONFIG_APM_HARDWARE != APM_HARDWARE_APM2
+
+#if HIL_MODE != HIL_MODE_ATTITUDE // JLN update
+ if (!g.switch_enable) {
+ // switches are disabled in EEPROM (see SWITCH_ENABLE option)
+ // this means the EEPROM control of all channel reversal is enabled
+ return;
+ }
+ // up is reverse
+ // up is elevon
+ g.mix_mode = (PINL & 128) ? 1 : 0; // 1 for elevon mode
+ if (g.mix_mode == 0) {
+ g.channel_roll.set_reverse((PINE & 128) ? true : false);
+ g.channel_pitch.set_reverse((PINE & 64) ? true : false);
+ g.channel_rudder.set_reverse((PINL & 64) ? true : false);
+ } else {
+ g.reverse_elevons = (PINE & 128) ? true : false;
+ g.reverse_ch1_elevon = (PINE & 64) ? true : false;
+ g.reverse_ch2_elevon = (PINL & 64) ? true : false;
+ }
+#else
+ g.mix_mode = 0; // 1 for elevon mode // setup for the the Predator MQ9 of AeroSim - JLN update
+ g.channel_roll.set_reverse(false); // roll reversed
+ g.channel_pitch.set_reverse(false); // pitch normal
+ g.channel_rudder.set_reverse(false); // yaw normal
+#endif
+#endif
+}
+
+
diff --git a/APMrover2/createTags b/APMrover2/createTags
new file mode 100644
index 000000000..1b9c3aff7
--- /dev/null
+++ b/APMrover2/createTags
@@ -0,0 +1,68 @@
+#!/bin/bash
+#" Autocompletion enabled vim for arduino pde's
+
+ctags -RV --language-force=C++ --c++-kinds=+p --fields=+iaS --extra=+q \
+ . \
+ ../libraries/* \
+ /usr/lib/avr/include
+
+# 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
diff --git a/APMrover2/defines.h b/APMrover2/defines.h
new file mode 100644
index 000000000..fe0f4e93d
--- /dev/null
+++ b/APMrover2/defines.h
@@ -0,0 +1,264 @@
+// -*- 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) (x*0.01745329252) // *pi/180
+#define ToDeg(x) (x*57.2957795131) // *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
+// ----------------------
+#define FAILSAFE_NONE 0
+#define FAILSAFE_SHORT 1
+#define FAILSAFE_LONG 2
+#define FAILSAFE_GCS 3
+#define FAILSAFE_SHORT_TIME 1500 // Miliiseconds
+#define FAILSAFE_LONG_TIME 20000 // Miliiseconds
+
+
+// active altitude sensor
+// ----------------------
+#define SONAR 0
+#define BARO 1
+
+// CH 7 control
+#define CH7_DO_NOTHING 0
+#define CH7_SAVE_WP 1
+#define CH7_LEO 2
+#define CH7_THERMAL 3
+#define CH7_SARSEC 4
+#define CH7_SARGRID 5
+#define CH7_RTL 6
+#define CH7_TUNING 7
+
+// CH_7 Tuning
+// -----------
+#define TUN_NONE 0
+// Attitude
+#define TUN_STABILIZE_KP 1
+#define TUN_STABILIZE_KI 2
+#define TUN_STABILIZE_KD 3
+#define TUN_YAW_KP 4
+#define TUN_YAW_KI 5
+#define TUN_YAW_KD 6
+#define TUN_STABROLL_KP 7
+#define TUN_STABROLL_KI 8
+#define TUN_STABROLL_KD 9
+#define TUN_STABPITCH_KP 10
+#define TUN_STABPITCH_KI 11
+#define TUN_STABPITCH_KD 12
+
+#define PITOT_SOURCE_ADC 1
+#define PITOT_SOURCE_ANALOG_PIN 2
+
+#define T6 1000000
+#define T7 10000000
+
+// GPS type codes - use the names, not the numbers
+#define GPS_PROTOCOL_NONE -1
+#define GPS_PROTOCOL_NMEA 0
+#define GPS_PROTOCOL_SIRF 1
+#define GPS_PROTOCOL_UBLOX 2
+#define GPS_PROTOCOL_IMU 3
+#define GPS_PROTOCOL_MTK 4
+#define GPS_PROTOCOL_HIL 5
+#define GPS_PROTOCOL_MTK16 6
+#define GPS_PROTOCOL_AUTO 7
+
+#define CH_ROLL CH_1
+#define CH_PITCH CH_2
+#define CH_THROTTLE CH_3
+#define CH_RUDDER CH_4
+#define CH_YAW CH_4
+#define CH_AIL1 CH_5
+#define CH_AIL2 CH_6
+
+// HIL enumerations
+#define HIL_MODE_DISABLED 0
+#define HIL_MODE_ATTITUDE 1
+#define HIL_MODE_SENSORS 2
+
+// Auto Pilot modes
+// ----------------
+#define MANUAL 0
+#define CIRCLE 1 // When flying sans GPS, and we loose the radio, just circle
+#define STABILIZE 2
+
+#define FLY_BY_WIRE_A 5 // Fly By Wire A has left stick horizontal => desired roll angle, left stick vertical => desired pitch angle, right stick vertical = manual throttle
+#define FLY_BY_WIRE_B 6 // Fly By Wire B has left stick horizontal => desired roll angle, left stick vertical => desired pitch angle, right stick vertical => desired airspeed
+#define FLY_BY_WIRE_C 7 // Fly By Wire C has left stick horizontal => desired roll angle, left stick vertical => desired climb rate, right stick vertical => desired airspeed
+ // Fly By Wire B and Fly By Wire C require airspeed sensor
+#define AUTO 10
+#define RTL 11
+#define LOITER 12
+//#define TAKEOFF 13 // This is not used by APM. It appears here for consistency with ACM
+//#define LAND 14 // This is not used by APM. It appears here for consistency with ACM
+#define GUIDED 15
+#define INITIALISING 16 // in startup routines
+#define HEADALT 17 // Lock the current heading and altitude
+#define SARSEC 18 // Run a SAR type Sector Pattern
+#define SARGRID 19 // Run a SAR type Grid Pattern
+#define THERMAL 20 // Thermal hunter mode
+#define LAND 21 // Landing mode
+
+// Commands - Note that APM now uses a subset of the MAVLink protocol commands. See enum MAV_CMD in the GCS_Mavlink library
+#define CMD_BLANK 0 // there is no command stored in the mem location requested
+#define NO_COMMAND 0
+#define WAIT_COMMAND 255
+
+// Command/Waypoint/Location Options Bitmask
+//--------------------
+#define MASK_OPTIONS_RELATIVE_ALT (1<<0) // 1 = Relative altitude
+
+//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
+
+#define MAV_CMD_CONDITION_YAW 23
+
+// GCS Message ID's
+/// NOTE: to ensure we never block on sending MAVLink messages
+/// please keep each MSG_ to a single MAVLink message. If need be
+/// create new MSG_ IDs for additional messages on the same
+/// stream
+enum ap_message {
+ MSG_HEARTBEAT,
+ MSG_ATTITUDE,
+ MSG_LOCATION,
+ MSG_EXTENDED_STATUS1,
+ MSG_EXTENDED_STATUS2,
+ MSG_NAV_CONTROLLER_OUTPUT,
+ MSG_CURRENT_WAYPOINT,
+ MSG_VFR_HUD,
+ MSG_RADIO_OUT,
+ MSG_RADIO_IN,
+ MSG_RAW_IMU1,
+ MSG_RAW_IMU2,
+ MSG_RAW_IMU3,
+ MSG_GPS_STATUS,
+ MSG_GPS_RAW,
+ MSG_SERVO_OUT,
+ MSG_NEXT_WAYPOINT,
+ MSG_NEXT_PARAM,
+ MSG_STATUSTEXT,
+ MSG_FENCE_STATUS,
+ MSG_RETRY_DEFERRED // this must be last
+};
+
+enum gcs_severity {
+ SEVERITY_LOW=1,
+ SEVERITY_MEDIUM,
+ SEVERITY_HIGH,
+ SEVERITY_CRITICAL
+};
+
+// Logging parameters
+#define LOG_INDEX_MSG 0xF0
+#define LOG_ATTITUDE_MSG 0x01
+#define LOG_GPS_MSG 0x02
+#define LOG_MODE_MSG 0X03
+#define LOG_CONTROL_TUNING_MSG 0X04
+#define LOG_NAV_TUNING_MSG 0X05
+#define LOG_PERFORMANCE_MSG 0X06
+#define LOG_RAW_MSG 0x07
+#define LOG_CMD_MSG 0x08
+#define LOG_CURRENT_MSG 0x09
+#define LOG_STARTUP_MSG 0x0A
+#define TYPE_AIRSTART_MSG 0x00
+#define TYPE_GROUNDSTART_MSG 0x01
+#define MAX_NUM_LOGS 100
+
+#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_RAW (1<<7)
+#define MASK_LOG_CMD (1<<8)
+#define MASK_LOG_CUR (1<<9)
+
+// 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 BATTERY_VOLTAGE(x) (x*(g.input_voltage/1024.0))*g.volt_div_ratio
+#define CURRENT_AMPS(x) ((x*(g.input_voltage/1024.0))-CURR_AMPS_OFFSET)*g.curr_amp_per_volt
+
+#define RELAY_PIN 47
+
+
+// sonar
+#define MAX_SONAR_XL 0
+#define MAX_SONAR_LV 1
+#define SonarToCm(x) (x*1.26) // Sonar raw value to centimeters
+#define AN4 4
+#define AN5 5
+
+#define SPEEDFILT 400 // centimeters/second; the speed below which a groundstart will be triggered
+
+
+// EEPROM addresses
+#define EEPROM_MAX_ADDR 4096
+// parameters get the first 1KiB of EEPROM, remainder is for waypoints
+#define WP_START_BYTE 0x400 // where in memory home WP is stored + all other WP
+#define WP_SIZE 15
+
+// fence points are stored at the end of the EEPROM
+#define MAX_FENCEPOINTS 20
+#define FENCE_WP_SIZE sizeof(Vector2l)
+#define FENCE_START_BYTE (EEPROM_MAX_ADDR-(MAX_FENCEPOINTS*FENCE_WP_SIZE))
+
+#define MAX_WAYPOINTS ((FENCE_START_BYTE - WP_START_BYTE) / WP_SIZE) - 1 // - 1 to be safe
+
+#define ONBOARD_PARAM_NAME_LENGTH 15
+
+// 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)
+
+// mark a function as not to be inlined
+#define NOINLINE __attribute__((noinline))
+
+#define CONFIG_IMU_OILPAN 1
+#define CONFIG_IMU_MPU6000 2
+
+#define APM_HARDWARE_APM1 1
+#define APM_HARDWARE_APM2 2
+
+#define AP_BARO_BMP085 1
+#define AP_BARO_MS5611 2
+
+#endif // _DEFINES_H
diff --git a/APMrover2/events.pde b/APMrover2/events.pde
new file mode 100644
index 000000000..de38c848c
--- /dev/null
+++ b/APMrover2/events.pde
@@ -0,0 +1,114 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+
+static void failsafe_short_on_event(int fstype)
+{
+ // This is how to handle a short loss of control signal failsafe.
+ failsafe = fstype;
+ ch3_failsafe_timer = millis();
+ gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Short event on, "));
+ switch(control_mode)
+ {
+ case MANUAL:
+ case STABILIZE:
+ case FLY_BY_WIRE_A: // middle position
+ case FLY_BY_WIRE_B: // middle position
+ set_mode(CIRCLE);
+ break;
+
+ case AUTO:
+ case GUIDED:
+ case LOITER:
+ if(g.short_fs_action == 1) {
+ set_mode(RTL);
+ }
+ break;
+
+ case CIRCLE:
+ case RTL:
+ default:
+ break;
+ }
+ gcs_send_text_fmt(PSTR("flight mode = %u"), (unsigned)control_mode);
+}
+
+static void failsafe_long_on_event(int fstype)
+{
+ // This is how to handle a long loss of control signal failsafe.
+ gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Long event on, "));
+ APM_RC.clearOverride(); // If the GCS is locked up we allow control to revert to RC
+ failsafe = fstype;
+ switch(control_mode)
+ {
+ case MANUAL:
+ case STABILIZE:
+ case FLY_BY_WIRE_A: // middle position
+ case FLY_BY_WIRE_B: // middle position
+ case CIRCLE:
+ set_mode(RTL);
+ break;
+
+ case AUTO:
+ case GUIDED:
+ case LOITER:
+ if(g.long_fs_action == 1) {
+ set_mode(RTL);
+ }
+ break;
+
+ case RTL:
+ default:
+ break;
+ }
+ gcs_send_text_fmt(PSTR("flight mode = %u"), (unsigned)control_mode);
+}
+
+static void failsafe_short_off_event()
+{
+ // We're back in radio contact
+ gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Short event off"));
+ failsafe = FAILSAFE_NONE;
+
+ // re-read the switch so we can return to our preferred mode
+ // --------------------------------------------------------
+ reset_control_switch();
+
+ // Reset control integrators
+ // ---------------------
+ reset_I();
+}
+
+#if BATTERY_EVENT == ENABLED
+static void low_battery_event(void)
+{
+ gcs_send_text_P(SEVERITY_HIGH,PSTR("Low Battery!"));
+ set_mode(RTL);
+ g.throttle_cruise = THROTTLE_CRUISE;
+}
+#endif
+
+static void update_events(void) // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_RELAY
+{
+ if(event_repeat == 0 || (millis() - event_timer) < event_delay)
+ return;
+
+ if (event_repeat > 0){
+ event_repeat --;
+ }
+
+ if(event_repeat != 0) { // event_repeat = -1 means repeat forever
+ event_timer = millis();
+
+ if (event_id >= CH_5 && event_id <= CH_8) {
+ if(event_repeat%2) {
+ APM_RC.OutputCh(event_id, event_value); // send to Servos
+ } else {
+ APM_RC.OutputCh(event_id, event_undo_value);
+ }
+ }
+
+ if (event_id == RELAY_TOGGLE) {
+ relay.toggle();
+ }
+ }
+}
diff --git a/APMrover2/failsafe.pde b/APMrover2/failsafe.pde
new file mode 100644
index 000000000..a1d2b80c7
--- /dev/null
+++ b/APMrover2/failsafe.pde
@@ -0,0 +1,46 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+ 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 failsafe_check(uint32_t tnow)
+{
+ static uint16_t last_mainLoop_count;
+ static uint32_t last_timestamp;
+ static bool in_failsafe;
+
+ 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) {
+ // pass RC inputs to outputs every 20ms
+ last_timestamp = tnow;
+ APM_RC.clearOverride();
+ for (uint8_t ch=0; ch<8; ch++) {
+ APM_RC.OutputCh(ch, APM_RC.InputCh(ch));
+ }
+ }
+}
diff --git a/APMrover2/geofence.pde b/APMrover2/geofence.pde
new file mode 100644
index 000000000..41223ae1e
--- /dev/null
+++ b/APMrover2/geofence.pde
@@ -0,0 +1,325 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+ geo-fencing support
+ Andrew Tridgell, December 2011
+ */
+
+#if GEOFENCE_ENABLED == ENABLED
+
+/*
+ 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 geofence_state {
+ uint8_t num_points;
+ bool boundary_uptodate;
+ bool fence_triggered;
+ uint16_t breach_count;
+ uint8_t breach_type;
+ uint32_t breach_time;
+ byte old_switch_position;
+ /* point 0 is the return point */
+ Vector2l boundary[MAX_FENCEPOINTS];
+} *geofence_state;
+
+
+/*
+ fence boundaries fetch/store
+ */
+static Vector2l get_fence_point_with_index(unsigned i)
+{
+ uint32_t mem;
+ Vector2l ret;
+
+ if (i > (unsigned)g.fence_total) {
+ return Vector2l(0,0);
+ }
+
+ // read fence point
+ mem = FENCE_START_BYTE + (i * FENCE_WP_SIZE);
+ ret.x = eeprom_read_dword((uint32_t *)mem);
+ mem += sizeof(uint32_t);
+ ret.y = eeprom_read_dword((uint32_t *)mem);
+
+ return ret;
+}
+
+// save a fence point
+static void set_fence_point_with_index(Vector2l &point, unsigned i)
+{
+ uint32_t mem;
+
+ if (i >= (unsigned)g.fence_total.get()) {
+ // not allowed
+ return;
+ }
+
+ mem = FENCE_START_BYTE + (i * FENCE_WP_SIZE);
+
+ eeprom_write_dword((uint32_t *)mem, point.x);
+ mem += sizeof(uint32_t);
+ eeprom_write_dword((uint32_t *)mem, point.y);
+
+ if (geofence_state != NULL) {
+ geofence_state->boundary_uptodate = false;
+ }
+}
+
+/*
+ allocate and fill the geofence state structure
+ */
+static void geofence_load(void)
+{
+ uint8_t i;
+
+ if (geofence_state == NULL) {
+ if (memcheck_available_memory() < 512 + sizeof(struct geofence_state)) {
+ // too risky to enable as we could run out of stack
+ goto failed;
+ }
+ geofence_state = (struct geofence_state *)calloc(1, sizeof(struct geofence_state));
+ if (geofence_state == NULL) {
+ // not much we can do here except disable it
+ goto failed;
+ }
+ }
+
+ 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_P(SEVERITY_LOW,PSTR("geo-fence loaded"));
+ gcs_send_message(MSG_FENCE_STATUS);
+ return;
+
+failed:
+ g.fence_action.set(FENCE_ACTION_NONE);
+ gcs_send_text_P(SEVERITY_HIGH,PSTR("geo-fence setup error"));
+}
+
+/*
+ return true if geo-fencing is enabled
+ */
+static bool geofence_enabled(void)
+{
+ if (g.fence_action == FENCE_ACTION_NONE ||
+ g.fence_channel == 0 ||
+ APM_RC.InputCh(g.fence_channel-1) < FENCE_ENABLE_PWM) {
+ // geo-fencing is disabled
+ if (geofence_state != NULL) {
+ // re-arm for when the channel trigger is switched on
+ geofence_state->fence_triggered = false;
+ }
+ return false;
+ }
+
+ if (!g_gps->fix) {
+ // we can't do much without a GPS fix
+ return false;
+ }
+
+ return true;
+}
+
+
+/*
+ return true if we have breached the geo-fence minimum altiude
+ */
+static bool geofence_check_minalt(void)
+{
+ if (g.fence_maxalt <= g.fence_minalt) {
+ return false;
+ }
+ if (g.fence_minalt == 0) {
+ return false;
+ }
+ return (current_loc.alt < (g.fence_minalt*100) + home.alt);
+}
+
+/*
+ return true if we have breached the geo-fence maximum altiude
+ */
+static bool geofence_check_maxalt(void)
+{
+ if (g.fence_maxalt <= g.fence_minalt) {
+ return false;
+ }
+ if (g.fence_maxalt == 0) {
+ return false;
+ }
+ return (current_loc.alt > (g.fence_maxalt*100) + home.alt);
+}
+
+
+/*
+ check if we have breached the geo-fence
+ */
+static void 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_channel != 0 &&
+ control_mode == GUIDED &&
+ g.fence_total >= 5 &&
+ geofence_state->boundary_uptodate &&
+ geofence_state->old_switch_position == oldSwitchPosition &&
+ guided_WP.lat == geofence_state->boundary[0].x &&
+ guided_WP.lng == geofence_state->boundary[0].y) {
+ geofence_state->old_switch_position = 0;
+ reset_control_switch();
+ }
+ 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;
+
+ if (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) {
+ Vector2l location;
+ location.x = current_loc.lat;
+ location.y = current_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_P(SEVERITY_LOW,PSTR("geo-fence OK"));
+#if FENCE_TRIGGERED_PIN > 0
+ digitalWrite(FENCE_TRIGGERED_PIN, LOW);
+#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) {
+ // 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
+ digitalWrite(FENCE_TRIGGERED_PIN, HIGH);
+#endif
+
+ gcs_send_text_P(SEVERITY_LOW,PSTR("geo-fence triggered"));
+ gcs_send_message(MSG_FENCE_STATUS);
+
+ // see what action the user wants
+ switch (g.fence_action) {
+ case FENCE_ACTION_GUIDED:
+ // fly to the return point, with an altitude half way between
+ // min and max
+ if (g.fence_minalt >= g.fence_maxalt) {
+ // invalid min/max, use RTL_altitude
+ guided_WP.alt = home.alt + (g.RTL_altitude * 100);
+ } else {
+ guided_WP.alt = home.alt + 100*(g.fence_minalt + g.fence_maxalt)/2;
+ }
+ guided_WP.id = 0;
+ guided_WP.p1 = 0;
+ guided_WP.options = 0;
+ guided_WP.lat = geofence_state->boundary[0].x;
+ guided_WP.lng = geofence_state->boundary[0].y;
+
+ geofence_state->old_switch_position = oldSwitchPosition;
+
+ if (control_mode == MANUAL && g.auto_trim) {
+ // make sure we don't auto trim the surfaces on this change
+ control_mode = STABILIZE;
+ }
+
+ set_mode(GUIDED);
+ 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
+ */
+static bool 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;
+}
+
+/*
+
+ */
+static void 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);
+ }
+}
+
+#else // GEOFENCE_ENABLED
+
+static void geofence_check(bool altitude_check_only) { }
+static bool geofence_stickmixing(void) { return true; }
+static bool geofence_enabled(void) { return false; }
+
+#endif // GEOFENCE_ENABLED
diff --git a/APMrover2/navigation.pde b/APMrover2/navigation.pde
new file mode 100644
index 000000000..f74396b31
--- /dev/null
+++ b/APMrover2/navigation.pde
@@ -0,0 +1,195 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+//****************************************************************
+// Function that will calculate the desired direction to fly and distance
+//****************************************************************
+static void navigate()
+{
+ // do not navigate with corrupt data
+ // ---------------------------------
+ if (g_gps->fix == 0)
+ {
+ g_gps->new_data = false;
+ return;
+ }
+
+#if HIL_MODE != HIL_MODE_ATTITUDE
+ if((next_WP.lat == 0)||(home_is_set==false)){
+#else
+ if(next_WP.lat == 0){
+#endif
+ return;
+ }
+
+ if(control_mode < INITIALISING) {
+
+ // waypoint distance from plane
+ // ----------------------------
+ wp_distance = get_distance(¤t_loc, &next_WP);
+
+ if (wp_distance < 0){
+ gcs_send_text_P(SEVERITY_HIGH,PSTR("<navigate> WP error - distance < 0"));
+ //Serial.println(wp_distance,DEC);
+ return;
+ }
+
+ // target_bearing is where we should be heading
+ // --------------------------------------------
+ target_bearing = get_bearing(¤t_loc, &next_WP);
+
+ // nav_bearing will includes xtrac correction
+ // ------------------------------------------
+ nav_bearing = target_bearing;
+
+ // check if we have missed the WP
+ loiter_delta = (target_bearing - old_target_bearing)/100;
+
+ // reset the old value
+ old_target_bearing = target_bearing;
+
+ // wrap values
+ if (loiter_delta > 180) loiter_delta -= 360;
+ if (loiter_delta < -180) loiter_delta += 360;
+ loiter_sum += abs(loiter_delta);
+ }
+
+ // control mode specific updates to nav_bearing
+ // --------------------------------------------
+ update_navigation();
+}
+
+
+#if 0
+// Disabled for now
+void calc_distance_error()
+{
+ distance_estimate += (float)g_gps->ground_speed * .0002 * cos(radians(bearing_error * .01));
+ distance_estimate -= DST_EST_GAIN * (float)(distance_estimate - GPS_wp_distance);
+ wp_distance = max(distance_estimate,10);
+}
+#endif
+
+static void calc_gndspeed_undershoot()
+{
+ // Function is overkill, but here in case we want to add filtering later
+ groundspeed_undershoot = (g.min_gndspeed > 0) ? (g.min_gndspeed - ground_speed) : 0;
+}
+
+static void calc_bearing_error()
+{
+ if(takeoff_complete == true || g.compass_enabled == true) {
+ /*
+ most of the time we use the yaw sensor for heading, even if
+ we don't have a compass. The yaw sensor is drift corrected
+ in the DCM library. We only use the gps ground course
+ directly if we haven't completed takeoff, as the yaw drift
+ correction won't have had a chance to kick in. Drift
+ correction using the GPS typically takes 10 seconds or so
+ for a 180 degree correction.
+ */
+ bearing_error = nav_bearing - ahrs.yaw_sensor;
+ } else {
+
+ // TODO: we need to use the Yaw gyro for in between GPS reads,
+ // maybe as an offset from a saved gryo value.
+ bearing_error = nav_bearing - ground_course;
+ }
+
+ bearing_error = wrap_180(bearing_error);
+}
+
+static void calc_altitude_error()
+{
+}
+
+static long wrap_360(long error)
+{
+ if (error > 36000) error -= 36000;
+ if (error < 0) error += 36000;
+ return error;
+}
+
+static long wrap_180(long error)
+{
+ if (error > 18000) error -= 36000;
+ if (error < -18000) error += 36000;
+ return error;
+}
+
+static void calc_turn_radius() // JLN update - adjut automaticaly the wp_radius Vs the speed and the turn angle
+{
+ wp_radius = g_gps->ground_speed * 150 / g.roll_limit.get();
+ //Serial.println(wp_radius, DEC);
+}
+
+static void update_loiter()
+{
+ float power;
+
+ if(wp_distance <= g.loiter_radius){
+ power = float(wp_distance) / float(g.loiter_radius);
+ power = constrain(power, 0.5, 1);
+ nav_bearing += (int)(9000.0 * (2.0 + power));
+ }else if(wp_distance < (g.loiter_radius + LOITER_RANGE)){
+ power = -((float)(wp_distance - g.loiter_radius - LOITER_RANGE) / LOITER_RANGE);
+ power = constrain(power, 0.5, 1); //power = constrain(power, 0, 1);
+ nav_bearing -= power * 9000;
+
+ }else{
+ update_crosstrack();
+ loiter_time = millis(); // keep start time for loiter updating till we get within LOITER_RANGE of orbit
+
+ }
+/*
+ if (wp_distance < g.loiter_radius){
+ nav_bearing += 9000;
+ }else{
+ nav_bearing -= 100 * M_PI / 180 * asin(g.loiter_radius / wp_distance);
+ }
+
+ update_crosstrack();
+*/
+ nav_bearing = wrap_360(nav_bearing);
+}
+
+static void update_crosstrack(void)
+{
+ // Crosstrack Error
+ // ----------------
+ if (abs(wrap_180(target_bearing - crosstrack_bearing)) < 4500) { // If we are too far off or too close we don't do track following
+ crosstrack_error = sin(radians((target_bearing - crosstrack_bearing) / (float)100)) * (float)wp_distance; // Meters we are off track line
+ nav_bearing += constrain(crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get());
+ nav_bearing = wrap_360(nav_bearing);
+ }
+}
+
+static void reset_crosstrack()
+{
+ crosstrack_bearing = get_bearing(&prev_WP, &next_WP); // Used for track following
+}
+
+static long get_distance(struct Location *loc1, struct Location *loc2)
+{
+ if(loc1->lat == 0 || loc1->lng == 0)
+ return -1;
+ if(loc2->lat == 0 || loc2->lng == 0)
+ return -1;
+ float dlat = (float)(loc2->lat - loc1->lat);
+ float dlong = ((float)(loc2->lng - loc1->lng)) * scaleLongDown;
+ return sqrt(sq(dlat) + sq(dlong)) * .01113195;
+}
+
+static long get_bearing(struct Location *loc1, struct Location *loc2)
+{
+ long off_x = loc2->lng - loc1->lng;
+ long off_y = (loc2->lat - loc1->lat) * scaleLongUp;
+ long bearing = 9000 + atan2(-off_y, off_x) * 5729.57795;
+ if (bearing < 0) bearing += 36000;
+ return bearing;
+}
+
+void reached_waypoint()
+{
+
+}
+
diff --git a/APMrover2/planner.pde b/APMrover2/planner.pde
new file mode 100644
index 000000000..94ef061e4
--- /dev/null
+++ b/APMrover2/planner.pde
@@ -0,0 +1,56 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+// These are function definitions so the Menu can be constructed before the functions
+// are defined below. Order matters to the compiler.
+static int8_t planner_gcs(uint8_t argc, const Menu::arg *argv);
+
+// 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 planner_menu_commands[] PROGMEM = {
+ {"gcs", planner_gcs},
+};
+
+// A Macro to create the Menu
+MENU(planner_menu, "planner", planner_menu_commands);
+
+static int8_t
+planner_mode(uint8_t argc, const Menu::arg *argv)
+{
+ Serial.printf_P(PSTR("Planner Mode\n\nThis mode is not intended for manual use\n\n"));
+ planner_menu.run();
+ return 0;
+}
+static int8_t
+planner_gcs(uint8_t argc, const Menu::arg *argv)
+{
+ gcs0.init(&Serial);
+
+#if USB_MUX_PIN > 0
+ // we don't have gcs3 if we have the USB mux setup
+ gcs3.init(&Serial3);
+#endif
+
+ int loopcount = 0;
+ while (1) {
+ if (millis()-fast_loopTimer > 19) {
+ fast_loopTimer = millis();
+
+ gcs_update();
+
+ read_radio();
+
+ gcs_data_stream_send(45,1000);
+ if ((loopcount % 5) == 0) // 10 hz
+ gcs_data_stream_send(5,45);
+ if ((loopcount % 16) == 0) { // 3 hz
+ gcs_data_stream_send(1,5);
+ gcs_send_message(MSG_HEARTBEAT);
+ }
+ loopcount++;
+ }
+ }
+ return 0;
+}
+
diff --git a/APMrover2/radio.pde b/APMrover2/radio.pde
new file mode 100644
index 000000000..23c972de7
--- /dev/null
+++ b/APMrover2/radio.pde
@@ -0,0 +1,263 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+//Function that will read the radio data, limit servos and trigger a failsafe
+// ----------------------------------------------------------------------------
+static byte failsafeCounter = 0; // we wait a second to take over the throttle and send the plane circling
+
+
+static void init_rc_in()
+{
+ // set rc reversing
+ update_servo_switches();
+
+ // set rc channel ranges
+ g.channel_roll.set_angle(SERVO_MAX);
+ g.channel_pitch.set_angle(SERVO_MAX);
+ g.channel_rudder.set_angle(SERVO_MAX);
+ g.channel_throttle.set_range(0, 100);
+
+ // set rc dead zones
+ g.channel_roll.set_dead_zone(60);
+ g.channel_pitch.set_dead_zone(60);
+ g.channel_rudder.set_dead_zone(60);
+ g.channel_throttle.set_dead_zone(6);
+
+ //g.channel_roll.dead_zone = 60;
+ //g.channel_pitch.dead_zone = 60;
+ //g.channel_rudder.dead_zone = 60;
+ //g.channel_throttle.dead_zone = 6;
+
+ //set auxiliary ranges
+ update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8);
+}
+
+static void init_rc_out()
+{
+ APM_RC.Init( &isr_registry ); // APM Radio initialization
+
+ APM_RC.enable_out(CH_1);
+ APM_RC.enable_out(CH_2);
+ APM_RC.enable_out(CH_3);
+ APM_RC.enable_out(CH_4);
+ APM_RC.enable_out(CH_5);
+ APM_RC.enable_out(CH_6);
+ APM_RC.enable_out(CH_7);
+ APM_RC.enable_out(CH_8);
+
+#if HIL_MODE != HIL_MODE_ATTITUDE
+ APM_RC.OutputCh(CH_1, g.channel_roll.radio_trim); // Initialization of servo outputs
+ APM_RC.OutputCh(CH_2, g.channel_pitch.radio_trim);
+ APM_RC.OutputCh(CH_3, g.channel_throttle.radio_min);
+ APM_RC.OutputCh(CH_4, g.channel_rudder.radio_trim);
+
+ APM_RC.OutputCh(CH_5, g.rc_5.radio_trim);
+ APM_RC.OutputCh(CH_6, g.rc_6.radio_trim);
+ APM_RC.OutputCh(CH_7, g.rc_7.radio_trim);
+ APM_RC.OutputCh(CH_8, g.rc_8.radio_trim);
+#else
+ APM_RC.OutputCh(CH_1, 1500); // Initialization of servo outputs
+ APM_RC.OutputCh(CH_2, 1500);
+ APM_RC.OutputCh(CH_3, 1000);
+ APM_RC.OutputCh(CH_4, 1500);
+
+ APM_RC.OutputCh(CH_5, 1500);
+ APM_RC.OutputCh(CH_6, 1500);
+ APM_RC.OutputCh(CH_7, 1500);
+ APM_RC.OutputCh(CH_8, 2000);
+#endif
+
+}
+
+static void read_radio()
+{
+ static uint16_t aileron1;
+ static uint16_t aileron2;
+
+ ch1_temp = APM_RC.InputCh(CH_ROLL);
+ ch2_temp = APM_RC.InputCh(CH_PITCH);
+
+ if(g.mix_mode == 0){
+ g.channel_roll.set_pwm(ch1_temp);
+ g.channel_pitch.set_pwm(ch2_temp);
+ }else{
+ g.channel_roll.set_pwm(BOOL_TO_SIGN(g.reverse_elevons) * (BOOL_TO_SIGN(g.reverse_ch2_elevon) * int(ch2_temp - elevon2_trim) - BOOL_TO_SIGN(g.reverse_ch1_elevon) * int(ch1_temp - elevon1_trim)) / 2 + 1500);
+ g.channel_pitch.set_pwm((BOOL_TO_SIGN(g.reverse_ch2_elevon) * int(ch2_temp - elevon2_trim) + BOOL_TO_SIGN(g.reverse_ch1_elevon) * int(ch1_temp - elevon1_trim)) / 2 + 1500);
+ }
+
+ g.channel_throttle.set_pwm(APM_RC.InputCh(CH_3));
+ g.channel_rudder.set_pwm(APM_RC.InputCh(CH_4));
+
+ #if FLAPERON == ENABLED
+ // JLN update for true flaperons
+ if (control_mode == MANUAL) {
+ g.rc_5.set_pwm(APM_RC.InputCh(CH_5));
+ g.rc_6.set_pwm(APM_RC.InputCh(CH_6));
+ } else {
+ aileron1 = g.rc_5.radio_trim + (BOOL_TO_SIGN(-g.rc_5.get_reverse()) *g.channel_roll.angle_to_pwm());
+ aileron2 = g.rc_6.radio_trim + (BOOL_TO_SIGN(-g.rc_6.get_reverse()) *g.channel_roll.angle_to_pwm());
+
+ aileron1 = constrain(aileron1,(uint16_t)g.rc_5.radio_min,(uint16_t)g.rc_5.radio_max);
+ aileron2 = constrain(aileron2,(uint16_t)g.rc_6.radio_min,(uint16_t)g.rc_6.radio_max);
+
+ g.rc_5.set_pwm(aileron1);
+ g.rc_6.set_pwm(aileron2);
+ }
+ #else
+ g.rc_5.set_pwm(APM_RC.InputCh(CH_5));
+ g.rc_6.set_pwm(APM_RC.InputCh(CH_6));
+ #endif
+
+ g.rc_7.set_pwm(APM_RC.InputCh(CH_7));
+ g.rc_8.set_pwm(APM_RC.InputCh(CH_8));
+
+ // TO DO - go through and patch throttle reverse for RC_Channel library compatibility
+ #if THROTTLE_REVERSE == 1
+ g.channel_throttle.radio_in = g.channel_throttle.radio_max + g.channel_throttle.radio_min - g.channel_throttle.radio_in;
+ #endif
+
+ control_failsafe(g.channel_throttle.radio_in);
+
+ g.channel_throttle.servo_out = g.channel_throttle.control_in;
+
+ if (g.channel_throttle.servo_out > 50) {
+ if(g.airspeed_enabled == true) {
+ airspeed_nudge = (g.flybywire_airspeed_max * 100 - g.airspeed_cruise) * ((g.channel_throttle.norm_input()-0.5) / 0.5);
+ airspeed_nudge = g.nudgeoffset + airspeed_nudge;
+ } else {
+ throttle_nudge = (g.throttle_max - g.throttle_cruise) * ((g.channel_throttle.norm_input()-0.5) / 0.5);
+ }
+ } else {
+ airspeed_nudge = g.nudgeoffset;
+ throttle_nudge = 0;
+ }
+
+ /*
+ Serial.printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d \n"),
+ g.rc_1.control_in,
+ g.rc_2.control_in,
+ g.rc_3.control_in,
+ g.rc_4.control_in);
+ */
+}
+
+static void control_failsafe(uint16_t pwm)
+{
+ if(g.throttle_fs_enabled == 0)
+ return;
+
+ // Check for failsafe condition based on loss of GCS control
+ if (rc_override_active) {
+ if(millis() - rc_override_fs_timer > FAILSAFE_SHORT_TIME) {
+ ch3_failsafe = true;
+ } else {
+ ch3_failsafe = false;
+ }
+
+ //Check for failsafe and debounce funky reads
+ } else if (g.throttle_fs_enabled) {
+ if (pwm < (unsigned)g.throttle_fs_value){
+ // we detect a failsafe from radio
+ // throttle has dropped below the mark
+ failsafeCounter++;
+ if (failsafeCounter == 9){
+ gcs_send_text_fmt(PSTR("MSG FS ON %u"), (unsigned)pwm);
+ }else if(failsafeCounter == 10) {
+ ch3_failsafe = true;
+ }else if (failsafeCounter > 10){
+ failsafeCounter = 11;
+ }
+
+ }else if(failsafeCounter > 0){
+ // we are no longer in failsafe condition
+ // but we need to recover quickly
+ failsafeCounter--;
+ if (failsafeCounter > 3){
+ failsafeCounter = 3;
+ }
+ if (failsafeCounter == 1){
+ gcs_send_text_fmt(PSTR("MSG FS OFF %u"), (unsigned)pwm);
+ }else if(failsafeCounter == 0) {
+ ch3_failsafe = false;
+ }else if (failsafeCounter <0){
+ failsafeCounter = -1;
+ }
+ }
+ }
+}
+
+static void trim_control_surfaces()
+{
+ read_radio();
+ // Store control surface trim values
+ // ---------------------------------
+ if(g.mix_mode == 0){
+ if ((g.channel_roll.radio_in > 1400) && (g.channel_pitch.radio_trim > 1400)) {
+ g.channel_roll.radio_trim = g.channel_roll.radio_max + g.channel_roll.radio_min - g.channel_roll.radio_in;
+ g.channel_pitch.radio_trim = g.channel_pitch.radio_max + g.channel_pitch.radio_min - g.channel_pitch.radio_in;
+ g.channel_rudder.radio_trim = g.channel_rudder.radio_max + g.channel_rudder.radio_min - g.channel_rudder.radio_in;
+ G_RC_AUX(k_aileron)->radio_trim = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; // Second aileron channel
+ } else {
+ g.channel_roll.radio_trim = 1500; // case of HIL test without receiver active
+ g.channel_pitch.radio_trim = 1500;
+ g.channel_rudder.radio_trim = 1500;
+ g.channel_throttle.radio_trim = 1000;
+ G_RC_AUX(k_aileron)->radio_trim = 1500;
+ }
+
+ }else{
+ elevon1_trim = ch1_temp;
+ elevon2_trim = 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;
+ g.channel_roll.radio_trim = center;
+ g.channel_pitch.radio_trim = center;
+ }
+
+ // save to eeprom
+ g.channel_roll.save_eeprom();
+ g.channel_pitch.save_eeprom();
+ //g.channel_throttle.save_eeprom();
+ g.channel_rudder.save_eeprom();
+ G_RC_AUX(k_aileron)->save_eeprom();
+}
+
+static void trim_radio()
+{
+ for (int y = 0; y < 30; y++) {
+ read_radio();
+ }
+
+ // Store the trim values
+ // ---------------------
+ if(g.mix_mode == 0){
+ if ((g.channel_roll.radio_in > 1400) && (g.channel_pitch.radio_trim > 1400)) {
+ g.channel_roll.radio_trim = g.channel_roll.radio_in;
+ g.channel_pitch.radio_trim = g.channel_pitch.radio_in;
+ //g.channel_throttle.radio_trim = g.channel_throttle.radio_in;
+ g.channel_rudder.radio_trim = g.channel_rudder.radio_in;
+ G_RC_AUX(k_aileron)->radio_trim = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; // Second aileron channel
+ } else {
+ g.channel_roll.radio_trim = 1500; // case of HIL test without receiver active
+ g.channel_pitch.radio_trim = 1500;
+ g.channel_rudder.radio_trim = 1500;
+ g.channel_throttle.radio_trim = 1000;
+ G_RC_AUX(k_aileron)->radio_trim = 1500;
+ }
+
+ } else {
+ elevon1_trim = ch1_temp;
+ elevon2_trim = ch2_temp;
+ uint16_t center = 1500;
+ g.channel_roll.radio_trim = center;
+ g.channel_pitch.radio_trim = center;
+ g.channel_rudder.radio_trim = g.channel_rudder.radio_in;
+ }
+
+ // save to eeprom
+ g.channel_roll.save_eeprom();
+ g.channel_pitch.save_eeprom();
+ //g.channel_throttle.save_eeprom();
+ g.channel_rudder.save_eeprom();
+ G_RC_AUX(k_aileron)->save_eeprom();
+}
diff --git a/APMrover2/sensors.pde b/APMrover2/sensors.pde
new file mode 100644
index 000000000..2df661650
--- /dev/null
+++ b/APMrover2/sensors.pde
@@ -0,0 +1,106 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+// Sensors are not available in HIL_MODE_ATTITUDE
+#if HIL_MODE != HIL_MODE_ATTITUDE
+
+void ReadSCP1000(void) {}
+
+static void init_barometer(void)
+{
+ int flashcount = 0;
+ long ground_pressure = 0;
+ int ground_temperature;
+
+ while (ground_pressure == 0 || !barometer.healthy) {
+ barometer.read(); // Get initial data from absolute pressure sensor
+ ground_pressure = baro_filter.apply(barometer.get_pressure());
+ //ground_pressure = barometer.get_pressure();
+ ground_temperature = barometer.get_temperature();
+ mavlink_delay(20);
+ //Serial.printf("barometer.Press %ld\n", barometer.get_pressure());
+ }
+
+ for(int i = 0; i < 30; i++){ // We take some readings...
+
+ #if HIL_MODE == HIL_MODE_SENSORS
+ gcs_update(); // look for inbound hil packets
+ #endif
+
+ do {
+ barometer.read(); // Get pressure sensor
+ } while (!barometer.healthy);
+ ground_pressure = baro_filter.apply(barometer.get_pressure());
+ //ground_pressure = (ground_pressure * 9l + barometer.get_pressure()) / 10l;
+ ground_temperature = (ground_temperature * 9 + barometer.get_temperature()) / 10;
+
+ mavlink_delay(20);
+ if(flashcount == 5) {
+ digitalWrite(C_LED_PIN, LED_OFF);
+ digitalWrite(A_LED_PIN, LED_ON);
+ digitalWrite(B_LED_PIN, LED_OFF);
+ }
+
+ if(flashcount >= 10) {
+ flashcount = 0;
+ digitalWrite(C_LED_PIN, LED_ON);
+ digitalWrite(A_LED_PIN, LED_OFF);
+ digitalWrite(B_LED_PIN, LED_ON);
+ }
+ flashcount++;
+ }
+
+ g.ground_pressure.set_and_save(ground_pressure);
+ g.ground_temperature.set_and_save(ground_temperature / 10.0f);
+ abs_pressure = ground_pressure;
+
+ Serial.printf_P(PSTR("abs_pressure %ld\n"), abs_pressure);
+ gcs_send_text_P(SEVERITY_MEDIUM, PSTR("barometer calibration complete."));
+}
+
+static int32_t read_barometer(void)
+{
+ float x, scaling, temp;
+
+ barometer.read(); // Get new data from absolute pressure sensor
+ float abs_pressure = baro_filter.apply(barometer.get_pressure());
+
+ //abs_pressure = (abs_pressure + barometer.get_pressure()) >> 1; // Small filtering
+ //abs_pressure = ((float)abs_pressure * .7) + ((float)barometer.get_pressure() * .3); // large filtering
+ scaling = (float)g.ground_pressure / (float)abs_pressure;
+ temp = ((float)g.ground_temperature) + 273.15f;
+ x = log(scaling) * temp * 29271.267f;
+ return (x / 10);
+}
+
+
+// in M/S * 100
+static void read_airspeed(void)
+{
+}
+
+static void zero_airspeed(void)
+{
+}
+
+#endif // HIL_MODE != HIL_MODE_ATTITUDE
+
+static void read_battery(void)
+{
+ if(g.battery_monitoring == 0) {
+ battery_voltage1 = 0;
+ return;
+ }
+
+ if(g.battery_monitoring == 3 || g.battery_monitoring == 4)
+ battery_voltage1 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN_1)) * .1 + battery_voltage1 * .9;
+ if(g.battery_monitoring == 4) {
+ current_amps1 = CURRENT_AMPS(analogRead(CURRENT_PIN_1)) * .1 + current_amps1 * .9; //reads power sensor current pin
+ current_total1 += current_amps1 * (float)delta_ms_medium_loop * 0.0002778; // .0002778 is 1/3600 (conversion to hours)
+ }
+
+ #if BATTERY_EVENT == ENABLED
+ if(battery_voltage1 < LOW_VOLTAGE) low_battery_event();
+ if(g.battery_monitoring == 4 && current_total1 > g.pack_capacity) low_battery_event();
+ #endif
+}
+
diff --git a/APMrover2/setup.pde b/APMrover2/setup.pde
new file mode 100644
index 000000000..6bd709991
--- /dev/null
+++ b/APMrover2/setup.pde
@@ -0,0 +1,604 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#if CLI_ENABLED == ENABLED
+
+// Functions called from the setup menu
+static int8_t setup_radio (uint8_t argc, const Menu::arg *argv);
+static int8_t setup_show (uint8_t argc, const Menu::arg *argv);
+static int8_t setup_factory (uint8_t argc, const Menu::arg *argv);
+static int8_t setup_flightmodes (uint8_t argc, const Menu::arg *argv);
+static int8_t setup_erase (uint8_t argc, const Menu::arg *argv);
+static int8_t setup_compass (uint8_t argc, const Menu::arg *argv);
+static int8_t setup_declination (uint8_t argc, const Menu::arg *argv);
+static int8_t setup_batt_monitor (uint8_t argc, const Menu::arg *argv);
+
+// Command/function table for the setup menu
+static const struct Menu::command setup_menu_commands[] PROGMEM = {
+ // command function called
+ // ======= ===============
+ {"reset", setup_factory},
+ {"radio", setup_radio},
+ {"modes", setup_flightmodes},
+ {"compass", setup_compass},
+ {"declination", setup_declination},
+ {"battery", setup_batt_monitor},
+ {"show", setup_show},
+ {"erase", 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.
+static int8_t
+setup_mode(uint8_t argc, const Menu::arg *argv)
+{
+ // Give the user some guidance
+ Serial.printf_P(PSTR("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;
+}
+
+// Print the current configuration.
+// Called by the setup menu 'show' command.
+static int8_t
+setup_show(uint8_t argc, const Menu::arg *argv)
+{
+ // clear the area
+ print_blanks(8);
+
+ report_radio();
+ report_batt_monitor();
+ report_gains();
+ report_xtrack();
+ report_throttle();
+ report_flight_modes();
+ report_imu();
+ report_compass();
+
+ Serial.printf_P(PSTR("Raw Values\n"));
+ print_divider();
+
+ AP_Param::show_all();
+
+ return(0);
+}
+
+// Initialise the EEPROM to 'factory' settings (mostly defined in APM_Config.h or via defaults).
+// Called by the setup menu 'factoryreset' command.
+static int8_t
+setup_factory(uint8_t argc, const Menu::arg *argv)
+{
+ int c;
+
+ Serial.printf_P(PSTR("\nType 'Y' and hit Enter to perform factory reset, any other key to abort: "));
+
+ do {
+ c = Serial.read();
+ } while (-1 == c);
+
+ if (('y' != c) && ('Y' != c))
+ return(-1);
+ AP_Param::erase_all();
+ Serial.printf_P(PSTR("\nFACTORY RESET complete - please reset APM to continue"));
+
+ //default_flight_modes(); // This will not work here. Replacement code located in init_ardupilot()
+
+ for (;;) {
+ }
+ // note, cannot actually return here
+ return(0);
+}
+
+
+// Perform radio setup.
+// Called by the setup menu 'radio' command.
+static int8_t
+setup_radio(uint8_t argc, const Menu::arg *argv)
+{
+ Serial.printf_P(PSTR("\n\nRadio Setup:\n"));
+ uint8_t i;
+
+ for(i = 0; i < 100;i++){
+ delay(20);
+ read_radio();
+ }
+
+
+ if(g.channel_roll.radio_in < 500){
+ while(1){
+ Serial.printf_P(PSTR("\nNo radio; Check connectors."));
+ delay(1000);
+ // stop here
+ }
+ }
+
+ g.channel_roll.radio_min = g.channel_roll.radio_in;
+ g.channel_pitch.radio_min = g.channel_pitch.radio_in;
+ g.channel_throttle.radio_min = g.channel_throttle.radio_in;
+ g.channel_rudder.radio_min = g.channel_rudder.radio_in;
+ g.rc_5.radio_min = g.rc_5.radio_in;
+ g.rc_6.radio_min = g.rc_6.radio_in;
+ g.rc_7.radio_min = g.rc_7.radio_in;
+ g.rc_8.radio_min = g.rc_8.radio_in;
+
+ g.channel_roll.radio_max = g.channel_roll.radio_in;
+ g.channel_pitch.radio_max = g.channel_pitch.radio_in;
+ g.channel_throttle.radio_max = g.channel_throttle.radio_in;
+ g.channel_rudder.radio_max = g.channel_rudder.radio_in;
+ g.rc_5.radio_max = g.rc_5.radio_in;
+ g.rc_6.radio_max = g.rc_6.radio_in;
+ g.rc_7.radio_max = g.rc_7.radio_in;
+ g.rc_8.radio_max = g.rc_8.radio_in;
+
+ g.channel_roll.radio_trim = g.channel_roll.radio_in;
+ g.channel_pitch.radio_trim = g.channel_pitch.radio_in;
+ g.channel_rudder.radio_trim = g.channel_rudder.radio_in;
+ g.rc_5.radio_trim = 1500;
+ g.rc_6.radio_trim = 1500;
+ g.rc_7.radio_trim = 1500;
+ g.rc_8.radio_trim = 1500;
+
+ Serial.printf_P(PSTR("\nMove all controls to each extreme. Hit Enter to save: \n"));
+ while(1){
+
+ delay(20);
+ // Filters radio input - adjust filters in the radio.pde file
+ // ----------------------------------------------------------
+ read_radio();
+
+ g.channel_roll.update_min_max();
+ g.channel_pitch.update_min_max();
+ g.channel_throttle.update_min_max();
+ g.channel_rudder.update_min_max();
+ g.rc_5.update_min_max();
+ g.rc_6.update_min_max();
+ g.rc_7.update_min_max();
+ g.rc_8.update_min_max();
+
+ if(Serial.available() > 0){
+ Serial.flush();
+ g.channel_roll.save_eeprom();
+ g.channel_pitch.save_eeprom();
+ g.channel_throttle.save_eeprom();
+ g.channel_rudder.save_eeprom();
+ g.rc_5.save_eeprom();
+ g.rc_6.save_eeprom();
+ g.rc_7.save_eeprom();
+ g.rc_8.save_eeprom();
+ print_done();
+ break;
+ }
+ }
+ trim_radio();
+ report_radio();
+ return(0);
+}
+
+
+static int8_t
+setup_flightmodes(uint8_t argc, const Menu::arg *argv)
+{
+ byte switchPosition, mode = 0;
+
+ Serial.printf_P(PSTR("\nMove RC toggle switch to each position to edit, move aileron stick to select modes."));
+ print_hit_enter();
+ trim_radio();
+
+ while(1){
+ delay(20);
+ read_radio();
+ switchPosition = readSwitch();
+
+
+ // look for control switch change
+ if (oldSwitchPosition != switchPosition){
+ // force position 5 to MANUAL
+ if (switchPosition > 4) {
+ flight_modes[switchPosition] = MANUAL;
+ }
+ // update our current mode
+ mode = flight_modes[switchPosition];
+
+ // update the user
+ print_switch(switchPosition, mode);
+
+ // Remember switch position
+ oldSwitchPosition = switchPosition;
+ }
+
+ // look for stick input
+ int radioInputSwitch = radio_input_switch();
+
+ if (radioInputSwitch != 0){
+
+ mode += radioInputSwitch;
+
+ while (
+ mode != MANUAL &&
+ mode != CIRCLE &&
+ mode != STABILIZE &&
+ mode != FLY_BY_WIRE_A &&
+ mode != FLY_BY_WIRE_B &&
+ mode != AUTO &&
+ mode != RTL &&
+ mode != LOITER)
+ {
+ if (mode < MANUAL)
+ mode = LOITER;
+ else if (mode >LOITER)
+ mode = MANUAL;
+ else
+ mode += radioInputSwitch;
+ }
+
+ // Override position 5
+ if(switchPosition > 4)
+ mode = MANUAL;
+
+ // save new mode
+ flight_modes[switchPosition] = mode;
+
+ // print new mode
+ print_switch(switchPosition, mode);
+ }
+
+ // escape hatch
+ if(Serial.available() > 0){
+ // save changes
+ for (mode=0; mode<6; mode++)
+ flight_modes[mode].save();
+ report_flight_modes();
+ print_done();
+ return (0);
+ }
+ }
+}
+
+static int8_t
+setup_declination(uint8_t argc, const Menu::arg *argv)
+{
+ compass.set_declination(radians(argv[1].f));
+ report_compass();
+ return 0;
+}
+
+
+static int8_t
+setup_erase(uint8_t argc, const Menu::arg *argv)
+{
+ int c;
+
+ Serial.printf_P(PSTR("\nType 'Y' and hit Enter to erase all waypoint and parameter data, any other key to abort: "));
+
+ do {
+ c = Serial.read();
+ } while (-1 == c);
+
+ if (('y' != c) && ('Y' != c))
+ return(-1);
+ zero_eeprom();
+ return 0;
+}
+
+static int8_t
+setup_compass(uint8_t argc, const Menu::arg *argv)
+{
+ if (!strcmp_P(argv[1].str, PSTR("on"))) {
+ compass.set_orientation(MAG_ORIENTATION); // set compass's orientation on aircraft
+ if (!compass.init()) {
+ Serial.println_P(PSTR("Compass initialisation failed!"));
+ g.compass_enabled = false;
+ } else {
+ g.compass_enabled = true;
+ }
+ } else if (!strcmp_P(argv[1].str, PSTR("off"))) {
+ g.compass_enabled = false;
+
+ } else if (!strcmp_P(argv[1].str, PSTR("reset"))) {
+ compass.set_offsets(0,0,0);
+
+ } else {
+ Serial.printf_P(PSTR("\nOptions:[on,off,reset]\n"));
+ report_compass();
+ return 0;
+ }
+
+ g.compass_enabled.save();
+ report_compass();
+ return 0;
+}
+
+static int8_t
+setup_batt_monitor(uint8_t argc, const Menu::arg *argv)
+{
+ if(argv[1].i >= 0 && argv[1].i <= 4){
+ g.battery_monitoring.set_and_save(argv[1].i);
+
+ } else {
+ Serial.printf_P(PSTR("\nOptions: 3-4"));
+ }
+
+ report_batt_monitor();
+ return 0;
+}
+
+/***************************************************************************/
+// CLI reports
+/***************************************************************************/
+
+static void report_batt_monitor()
+{
+ //print_blanks(2);
+ Serial.printf_P(PSTR("Batt Mointor\n"));
+ print_divider();
+ if(g.battery_monitoring == 0) Serial.printf_P(PSTR("Batt monitoring disabled"));
+ if(g.battery_monitoring == 3) Serial.printf_P(PSTR("Monitoring batt volts"));
+ if(g.battery_monitoring == 4) Serial.printf_P(PSTR("Monitoring volts and current"));
+ print_blanks(2);
+}
+static void report_radio()
+{
+ //print_blanks(2);
+ Serial.printf_P(PSTR("Radio\n"));
+ print_divider();
+ // radio
+ print_radio_values();
+ print_blanks(2);
+}
+
+static void report_gains()
+{
+ //print_blanks(2);
+ Serial.printf_P(PSTR("Gains\n"));
+ print_divider();
+
+ Serial.printf_P(PSTR("servo roll:\n"));
+ print_PID(&g.pidServoRoll);
+
+ Serial.printf_P(PSTR("servo pitch:\n"));
+ print_PID(&g.pidServoPitch);
+
+ Serial.printf_P(PSTR("servo rudder:\n"));
+ print_PID(&g.pidServoRudder);
+
+ Serial.printf_P(PSTR("nav roll:\n"));
+ print_PID(&g.pidNavRoll);
+
+ Serial.printf_P(PSTR("nav pitch airspeed:\n"));
+ print_PID(&g.pidNavPitchAirspeed);
+
+ Serial.printf_P(PSTR("energry throttle:\n"));
+ print_PID(&g.pidTeThrottle);
+
+ Serial.printf_P(PSTR("nav pitch alt:\n"));
+ print_PID(&g.pidNavPitchAltitude);
+
+ print_blanks(2);
+}
+
+static void report_xtrack()
+{
+ //print_blanks(2);
+ Serial.printf_P(PSTR("Crosstrack\n"));
+ print_divider();
+ // radio
+ Serial.printf_P(PSTR("XTRACK: %4.2f\n"
+ "XTRACK angle: %d\n"),
+ (float)g.crosstrack_gain,
+ (int)g.crosstrack_entry_angle);
+ print_blanks(2);
+}
+
+static void report_throttle()
+{
+ //print_blanks(2);
+ Serial.printf_P(PSTR("Throttle\n"));
+ print_divider();
+
+ Serial.printf_P(PSTR("min: %d\n"
+ "max: %d\n"
+ "cruise: %d\n"
+ "failsafe_enabled: %d\n"
+ "failsafe_value: %d\n"),
+ (int)g.throttle_min,
+ (int)g.throttle_max,
+ (int)g.throttle_cruise,
+ (int)g.throttle_fs_enabled,
+ (int)g.throttle_fs_value);
+ print_blanks(2);
+}
+
+static void report_imu()
+{
+ //print_blanks(2);
+ Serial.printf_P(PSTR("IMU\n"));
+ print_divider();
+
+ print_gyro_offsets();
+ print_accel_offsets();
+ print_blanks(2);
+}
+
+static void report_compass()
+{
+ //print_blanks(2);
+ Serial.printf_P(PSTR("Compass: "));
+
+ switch (compass.product_id) {
+ case AP_COMPASS_TYPE_HMC5883L:
+ Serial.println_P(PSTR("HMC5883L"));
+ break;
+ case AP_COMPASS_TYPE_HMC5843:
+ Serial.println_P(PSTR("HMC5843"));
+ break;
+ case AP_COMPASS_TYPE_HIL:
+ Serial.println_P(PSTR("HIL"));
+ break;
+ default:
+ Serial.println_P(PSTR("??"));
+ break;
+ }
+
+ print_divider();
+
+ print_enabled(g.compass_enabled);
+
+ // mag declination
+ Serial.printf_P(PSTR("Mag Declination: %4.4f\n"),
+ degrees(compass.get_declination()));
+
+ Vector3f offsets = compass.get_offsets();
+
+ // mag offsets
+ Serial.printf_P(PSTR("Mag offsets: %4.4f, %4.4f, %4.4f\n"),
+ offsets.x,
+ offsets.y,
+ offsets.z);
+ print_blanks(2);
+}
+
+static void report_flight_modes()
+{
+ //print_blanks(2);
+ Serial.printf_P(PSTR("Flight modes\n"));
+ print_divider();
+
+ for(int i = 0; i < 6; i++ ){
+ print_switch(i, flight_modes[i]);
+ }
+ print_blanks(2);
+}
+
+/***************************************************************************/
+// CLI utilities
+/***************************************************************************/
+
+static void
+print_PID(PID * pid)
+{
+ Serial.printf_P(PSTR("P: %4.3f, I:%4.3f, D:%4.3f, IMAX:%ld\n"),
+ pid->kP(),
+ pid->kI(),
+ pid->kD(),
+ (long)pid->imax());
+}
+
+static void
+print_radio_values()
+{
+ Serial.printf_P(PSTR("CH1: %d | %d | %d\n"), (int)g.channel_roll.radio_min, (int)g.channel_roll.radio_trim, (int)g.channel_roll.radio_max);
+ Serial.printf_P(PSTR("CH2: %d | %d | %d\n"), (int)g.channel_pitch.radio_min, (int)g.channel_pitch.radio_trim, (int)g.channel_pitch.radio_max);
+ Serial.printf_P(PSTR("CH3: %d | %d | %d\n"), (int)g.channel_throttle.radio_min, (int)g.channel_throttle.radio_trim, (int)g.channel_throttle.radio_max);
+ Serial.printf_P(PSTR("CH4: %d | %d | %d\n"), (int)g.channel_rudder.radio_min, (int)g.channel_rudder.radio_trim, (int)g.channel_rudder.radio_max);
+ Serial.printf_P(PSTR("CH5: %d | %d | %d\n"), (int)g.rc_5.radio_min, (int)g.rc_5.radio_trim, (int)g.rc_5.radio_max);
+ Serial.printf_P(PSTR("CH6: %d | %d | %d\n"), (int)g.rc_6.radio_min, (int)g.rc_6.radio_trim, (int)g.rc_6.radio_max);
+ Serial.printf_P(PSTR("CH7: %d | %d | %d\n"), (int)g.rc_7.radio_min, (int)g.rc_7.radio_trim, (int)g.rc_7.radio_max);
+ Serial.printf_P(PSTR("CH8: %d | %d | %d\n"), (int)g.rc_8.radio_min, (int)g.rc_8.radio_trim, (int)g.rc_8.radio_max);
+
+}
+
+static void
+print_switch(byte p, byte m)
+{
+ Serial.printf_P(PSTR("Pos %d: "),p);
+ Serial.println(flight_mode_strings[m]);
+}
+
+static void
+print_done()
+{
+ Serial.printf_P(PSTR("\nSaved Settings\n\n"));
+}
+
+static void
+print_blanks(int num)
+{
+ while(num > 0){
+ num--;
+ Serial.println("");
+ }
+}
+
+static void
+print_divider(void)
+{
+ for (int i = 0; i < 40; i++) {
+ Serial.printf_P(PSTR("-"));
+ }
+ Serial.println("");
+}
+
+static int8_t
+radio_input_switch(void)
+{
+ static int8_t bouncer = 0;
+
+
+ if (int16_t(g.channel_roll.radio_in - g.channel_roll.radio_trim) > 100) {
+ bouncer = 10;
+ }
+ if (int16_t(g.channel_roll.radio_in - g.channel_roll.radio_trim) < -100) {
+ bouncer = -10;
+ }
+ if (bouncer >0) {
+ bouncer --;
+ }
+ if (bouncer <0) {
+ bouncer ++;
+ }
+
+ if (bouncer == 1 || bouncer == -1) {
+ return bouncer;
+ } else {
+ return 0;
+ }
+}
+
+
+static void zero_eeprom(void)
+{
+ byte b = 0;
+ Serial.printf_P(PSTR("\nErasing EEPROM\n"));
+ for (int i = 0; i < EEPROM_MAX_ADDR; i++) {
+ eeprom_write_byte((uint8_t *) i, b);
+ }
+ Serial.printf_P(PSTR("done\n"));
+}
+
+static void print_enabled(bool b)
+{
+ if(b)
+ Serial.printf_P(PSTR("en"));
+ else
+ Serial.printf_P(PSTR("dis"));
+ Serial.printf_P(PSTR("abled\n"));
+}
+
+static void
+print_accel_offsets(void)
+{
+ Serial.printf_P(PSTR("Accel offsets: %4.2f, %4.2f, %4.2f\n"),
+ (float)imu.ax(),
+ (float)imu.ay(),
+ (float)imu.az());
+}
+
+static void
+print_gyro_offsets(void)
+{
+ Serial.printf_P(PSTR("Gyro offsets: %4.2f, %4.2f, %4.2f\n"),
+ (float)imu.gx(),
+ (float)imu.gy(),
+ (float)imu.gz());
+}
+
+
+#endif // CLI_ENABLED
diff --git a/APMrover2/system.pde b/APMrover2/system.pde
new file mode 100644
index 000000000..e5898eb03
--- /dev/null
+++ b/APMrover2/system.pde
@@ -0,0 +1,583 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*****************************************************************************
+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
+
+// Functions called from the top-level menu
+static int8_t process_logs(uint8_t argc, const Menu::arg *argv); // in Log.pde
+static int8_t setup_mode(uint8_t argc, const Menu::arg *argv); // in setup.pde
+static int8_t test_mode(uint8_t argc, const Menu::arg *argv); // in test.cpp
+static int8_t planner_mode(uint8_t argc, const Menu::arg *argv); // in planner.pde
+
+// This is the help function
+// PSTR is an AVR macro to read strings from flash memory
+// printf_P is a version of print_f that reads from flash memory
+static int8_t main_menu_help(uint8_t argc, const Menu::arg *argv)
+{
+ Serial.printf_P(PSTR("Commands:\n"
+ " logs log readback/setup mode\n"
+ " setup setup mode\n"
+ " test test mode\n"
+ "\n"
+ "Move the slide switch and reset to FLY.\n"
+ "\n"));
+ return(0);
+}
+
+// Command/function table for the top-level menu.
+static const struct Menu::command main_menu_commands[] PROGMEM = {
+// command function called
+// ======= ===============
+ {"logs", process_logs},
+ {"setup", setup_mode},
+ {"test", test_mode},
+ {"help", main_menu_help},
+ {"planner", planner_mode}
+};
+
+// Create the top-level menu object.
+MENU(main_menu, THISFIRMWARE, main_menu_commands);
+
+// the user wants the CLI. It never exits
+static void run_cli(void)
+{
+ // disable the failsafe code in the CLI
+ timer_scheduler.set_failsafe(NULL);
+
+ while (1) {
+ main_menu.run();
+ }
+}
+
+#endif // CLI_ENABLED
+
+static void init_ardupilot()
+{
+#if USB_MUX_PIN > 0
+ // on the APM2 board we have a mux thet switches UART0 between
+ // USB and the board header. If the right ArduPPM firmware is
+ // installed we can detect if USB is connected using the
+ // USB_MUX_PIN
+ pinMode(USB_MUX_PIN, INPUT);
+
+ usb_connected = !digitalRead(USB_MUX_PIN);
+ if (!usb_connected) {
+ // USB is not connected, this means UART0 may be a Xbee, with
+ // its darned bricking problem. We can't write to it for at
+ // least one second after powering up. Simplest solution for
+ // now is to delay for 1 second. Something more elegant may be
+ // added later
+ delay(1000);
+ }
+#endif
+
+ // Console serial port
+ //
+ // The console port buffers are defined to be sufficiently large to support
+ // the console's use as a logging device, optionally as the GPS port when
+ // GPS_PROTOCOL_IMU is selected, and as the telemetry port.
+ //
+ // XXX This could be optimised to reduce the buffer sizes in the cases
+ // where they are not otherwise required.
+ //
+ Serial.begin(SERIAL0_BAUD, 128, 128);
+
+ // GPS serial port.
+ //
+ // XXX currently the EM406 (SiRF receiver) is nominally configured
+ // at 57600, however it's not been supported to date. We should
+ // probably standardise on 38400.
+ //
+ // XXX the 128 byte receive buffer may be too small for NMEA, depending
+ // on the message set configured.
+ //
+ // standard gps running
+ Serial1.begin(115200, 128, 16);
+
+ Serial.printf_P(PSTR("\n\nInit " THISFIRMWARE
+ "\n\nFree RAM: %u\n"),
+ memcheck_available_memory());
+
+#if QUATERNION_ENABLE == ENABLED
+ Serial.printf_P(PSTR("Quaternion test\n"));
+#endif
+ //
+ // Initialize Wire and SPI libraries
+ //
+#ifndef DESKTOP_BUILD
+ I2c.begin();
+ I2c.timeOut(5);
+ // initially set a fast I2c speed, and drop it on first failures
+ I2c.setSpeed(true);
+#endif
+ SPI.begin();
+ SPI.setClockDivider(SPI_CLOCK_DIV16); // 1MHZ SPI rate
+ //
+ // Initialize the ISR registry.
+ //
+ isr_registry.init();
+
+ //
+ // Initialize the timer scheduler to use the ISR registry.
+ //
+
+ timer_scheduler.init( & isr_registry );
+
+ //
+ // Check the EEPROM format version before loading any parameters from EEPROM.
+ //
+
+ load_parameters();
+
+ // 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 the GCS
+ gcs0.init(&Serial);
+
+#if USB_MUX_PIN > 0
+ if (!usb_connected) {
+ // we are not connected via USB, re-init UART0 with right
+ // baud rate
+ Serial.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128);
+ }
+#else
+ // we have a 2nd serial port for telemetry
+ Serial3.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128);
+ gcs3.init(&Serial3);
+#endif
+
+ mavlink_system.sysid = g.sysid_this_mav;
+
+#if LOGGING_ENABLED == ENABLED
+ DataFlash.Init(); // DataFlash log initialization
+ if (!DataFlash.CardInserted()) {
+ gcs_send_text_P(SEVERITY_LOW, PSTR("No dataflash card inserted"));
+ g.log_bitmask.set(0);
+ } else if (DataFlash.NeedErase()) {
+ gcs_send_text_P(SEVERITY_LOW, PSTR("ERASING LOGS"));
+ do_erase_logs();
+ }
+ if (g.log_bitmask != 0) {
+ DataFlash.start_new_log();
+ }
+#endif
+
+#if HIL_MODE != HIL_MODE_ATTITUDE
+
+#if CONFIG_ADC == ENABLED
+ adc.Init(&timer_scheduler); // APM ADC library initialization
+#endif
+
+ barometer.init(&timer_scheduler);
+
+ if (g.compass_enabled==true) {
+ compass.set_orientation(MAG_ORIENTATION); // set compass's orientation on aircraft
+ if (!compass.init()|| !compass.read()) {
+ Serial.println_P(PSTR("Compass initialisation failed!"));
+ g.compass_enabled = false;
+ } else {
+ ahrs.set_compass(&compass);
+ //compass.get_offsets(); // load offsets to account for airframe magnetic interference
+ compass.null_offsets_enable();
+ }
+ }
+#endif
+
+ // Do GPS init
+ g_gps = &g_gps_driver;
+ g_gps->init(); // GPS Initialization
+ g_gps->callback = mavlink_delay;
+
+ //mavlink_system.sysid = MAV_SYSTEM_ID; // Using g.sysid_this_mav
+ mavlink_system.compid = 1; //MAV_COMP_ID_IMU; // We do not check for comp id
+ mavlink_system.type = MAV_FIXED_WING;
+
+ rc_override_active = APM_RC.setHIL(rc_override); // Set initial values for no override
+
+ RC_Channel::set_apm_rc( &APM_RC ); // Provide reference to RC outputs.
+ init_rc_in(); // sets up rc channels from radio
+ init_rc_out(); // sets up the timer libs
+
+ pinMode(C_LED_PIN, OUTPUT); // GPS status LED
+ pinMode(A_LED_PIN, OUTPUT); // GPS status LED
+ pinMode(B_LED_PIN, OUTPUT); // GPS status LED
+#if SLIDE_SWITCH_PIN > 0
+ pinMode(SLIDE_SWITCH_PIN, INPUT); // To enter interactive mode
+#endif
+#if CONFIG_PUSHBUTTON == ENABLED
+ pinMode(PUSHBUTTON_PIN, INPUT); // unused
+#endif
+#if CONFIG_RELAY == ENABLED
+ DDRL |= B00000100; // Set Port L, pin 2 to output for the relay
+#endif
+
+#if FENCE_TRIGGERED_PIN > 0
+ pinMode(FENCE_TRIGGERED_PIN, OUTPUT);
+ digitalWrite(FENCE_TRIGGERED_PIN, LOW);
+#endif
+
+ /*
+ setup the 'main loop is dead' check. Note that this relies on
+ the RC library being initialised.
+ */
+ timer_scheduler.set_failsafe(failsafe_check);
+
+
+ // If the switch is in 'menu' mode, run the main menu.
+ //
+ // Since we can't be sure that the setup or test mode won't leave
+ // the system in an odd state, we don't let the user exit the top
+ // menu; they must reset in order to fly.
+ //
+#if CLI_ENABLED == ENABLED && CLI_SLIDER_ENABLED == ENABLED
+ if (digitalRead(SLIDE_SWITCH_PIN) == 0) {
+ digitalWrite(A_LED_PIN,LED_ON); // turn on setup-mode LED
+ Serial.printf_P(PSTR("\n"
+ "Entering interactive setup mode...\n"
+ "\n"
+ "If using the Arduino Serial Monitor, ensure Line Ending is set to Carriage Return.\n"
+ "Type 'help' to list commands, 'exit' to leave a submenu.\n"
+ "Visit the 'setup' menu for first-time configuration.\n"));
+ Serial.println_P(PSTR("\nMove the slide switch and reset to FLY.\n"));
+ run_cli();
+ }
+#else
+ Serial.printf_P(PSTR("\nPress ENTER 3 times to start interactive setup\n\n"));
+#endif // CLI_ENABLED
+
+ // read in the flight switches
+ update_servo_switches();
+
+ startup_ground();
+
+ if (g.log_bitmask & MASK_LOG_CMD)
+ Log_Write_Startup(TYPE_GROUNDSTART_MSG);
+
+ set_mode(MANUAL);
+
+ // set the correct flight mode
+ // ---------------------------
+ reset_control_switch();
+}
+
+//********************************************************************************
+//This function does all the calibrations, etc. that we need during a ground start
+//********************************************************************************
+static void startup_ground(void)
+{
+ set_mode(INITIALISING);
+
+ gcs_send_text_P(SEVERITY_LOW,PSTR("<startup_ground> GROUND START"));
+
+ #if(GROUND_START_DELAY > 0)
+ gcs_send_text_P(SEVERITY_LOW,PSTR("<startup_ground> With Delay"));
+ delay(GROUND_START_DELAY * 1000);
+ #endif
+
+ // Makes the servos wiggle
+ // step 1 = 1 wiggle
+ // -----------------------
+ demo_servos(1);
+
+ //IMU ground start
+ //------------------------
+ //
+
+ startup_IMU_ground(false);
+
+ // read the radio to set trims
+ // ---------------------------
+ trim_radio(); // This was commented out as a HACK. Why? I don't find a problem.
+
+ // Save the settings for in-air restart
+ // ------------------------------------
+ //save_EEPROM_groundstart();
+
+ // initialize commands
+ // -------------------
+ init_commands();
+
+ // Read in the GPS - see if one is connected
+ GPS_enabled = false;
+ for (byte counter = 0; ; counter++) {
+ g_gps->update();
+ if (g_gps->status() != 0 || HIL_MODE != HIL_MODE_DISABLED){
+ GPS_enabled = true;
+ break;
+ }
+
+ if (counter >= 2) {
+ GPS_enabled = false;
+ break;
+ }
+ }
+
+ // Makes the servos wiggle - 3 times signals ready to fly
+ // -----------------------
+ demo_servos(3);
+
+ gcs_send_text_P(SEVERITY_LOW,PSTR("\n\n Ready to FLY."));
+}
+
+static void set_mode(byte mode)
+{ struct Location temp;
+
+ 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();
+
+ control_mode = mode;
+ crash_timer = 0;
+
+ switch(control_mode)
+ {
+ case MANUAL:
+ case STABILIZE:
+ case CIRCLE:
+ case FLY_BY_WIRE_A:
+ case FLY_BY_WIRE_B:
+ break;
+
+ case AUTO:
+ reload_commands();
+ update_auto();
+ break;
+
+ case RTL:
+ do_RTL();
+ break;
+
+ case LOITER:
+ do_loiter_at_location();
+ break;
+
+ case GUIDED:
+ set_guided_WP();
+ break;
+
+ default:
+ //do_RTL();
+ break;
+ }
+
+ if (g.log_bitmask & MASK_LOG_MODE)
+ Log_Write_Mode(control_mode);
+}
+
+static void check_long_failsafe()
+{
+ // only act on changes
+ // -------------------
+ if(failsafe != FAILSAFE_LONG && failsafe != FAILSAFE_GCS){
+ if(rc_override_active && millis() - rc_override_fs_timer > FAILSAFE_LONG_TIME) {
+ failsafe_long_on_event(FAILSAFE_LONG);
+ }
+ if(! rc_override_active && failsafe == FAILSAFE_SHORT && millis() - ch3_failsafe_timer > FAILSAFE_LONG_TIME) {
+ failsafe_long_on_event(FAILSAFE_LONG);
+ }
+ if(g.gcs_heartbeat_fs_enabled && millis() - rc_override_fs_timer > FAILSAFE_LONG_TIME) {
+ failsafe_long_on_event(FAILSAFE_GCS);
+ }
+ } else {
+ // We do not change state but allow for user to change mode
+ if(failsafe == FAILSAFE_GCS && millis() - rc_override_fs_timer < FAILSAFE_SHORT_TIME) failsafe = FAILSAFE_NONE;
+ if(failsafe == FAILSAFE_LONG && rc_override_active && millis() - rc_override_fs_timer < FAILSAFE_SHORT_TIME) failsafe = FAILSAFE_NONE;
+ if(failsafe == FAILSAFE_LONG && !rc_override_active && !ch3_failsafe) failsafe = FAILSAFE_NONE;
+ }
+}
+
+static void check_short_failsafe()
+{
+ // only act on changes
+ // -------------------
+ if(failsafe == FAILSAFE_NONE){
+ if(ch3_failsafe) { // The condition is checked and the flag ch3_failsafe is set in radio.pde
+ failsafe_short_on_event(FAILSAFE_SHORT);
+ }
+ }
+
+ if(failsafe == FAILSAFE_SHORT){
+ if(!ch3_failsafe) {
+ failsafe_short_off_event();
+ }
+ }
+}
+
+
+static void startup_IMU_ground(bool force_accel_level)
+{
+#if HIL_MODE != HIL_MODE_ATTITUDE
+ gcs_send_text_P(SEVERITY_MEDIUM, PSTR("Warming up ADC..."));
+ mavlink_delay(500);
+
+ // Makes the servos wiggle twice - about to begin IMU calibration - HOLD LEVEL AND STILL!!
+ // -----------------------
+ demo_servos(2);
+ gcs_send_text_P(SEVERITY_MEDIUM, PSTR("Beginning IMU calibration; do not move plane"));
+ mavlink_delay(1000);
+
+ imu.init(IMU::COLD_START, mavlink_delay, flash_leds, &timer_scheduler);
+ if (force_accel_level || g.manual_level == 0) {
+ // when MANUAL_LEVEL is set to 1 we don't do accelerometer
+ // levelling on each boot, and instead rely on the user to do
+ // it once via the ground station
+ imu.init_accel(mavlink_delay, flash_leds);
+ }
+ ahrs.set_centripetal(1);
+ ahrs.reset();
+
+ // read Baro pressure at ground
+ //-----------------------------
+ init_barometer();
+
+ if (g.airspeed_enabled == true) {
+ // initialize airspeed sensor
+ // --------------------------
+ zero_airspeed();
+ gcs_send_text_P(SEVERITY_LOW,PSTR("<startup_ground> zero airspeed calibrated"));
+ } else {
+ gcs_send_text_P(SEVERITY_LOW,PSTR("<startup_ground> NO airspeed"));
+ }
+
+#endif // HIL_MODE_ATTITUDE
+
+ digitalWrite(B_LED_PIN, LED_ON); // Set LED B high to indicate IMU ready
+ digitalWrite(A_LED_PIN, LED_OFF);
+ digitalWrite(C_LED_PIN, LED_OFF);
+}
+
+
+static void update_GPS_light(void)
+{
+ // GPS LED on if we have a fix or Blink GPS LED if we are receiving data
+ // ---------------------------------------------------------------------
+ switch (g_gps->status()) {
+ case(2):
+ digitalWrite(C_LED_PIN, LED_ON); //Turn LED C on when gps has valid fix.
+ break;
+
+ case(1):
+ if (g_gps->valid_read == true){
+ GPS_light = !GPS_light; // Toggle light on and off to indicate gps messages being received, but no GPS fix lock
+ if (GPS_light){
+ digitalWrite(C_LED_PIN, LED_OFF);
+ } else {
+ digitalWrite(C_LED_PIN, LED_ON);
+ }
+ g_gps->valid_read = false;
+ }
+ break;
+
+ default:
+ digitalWrite(C_LED_PIN, LED_OFF);
+ break;
+ }
+}
+
+
+static void resetPerfData(void) {
+ mainLoop_count = 0;
+ G_Dt_max = 0;
+ imu.adc_constraints = 0;
+ ahrs.renorm_range_count = 0;
+ ahrs.renorm_blowup_count = 0;
+ gps_fix_count = 0;
+ pmTest1 = 0;
+ perf_mon_timer = millis();
+}
+
+
+/*
+ map from a 8 bit EEPROM baud rate to a real baud rate
+ */
+static uint32_t map_baudrate(int8_t rate, uint32_t default_baud)
+{
+ switch (rate) {
+ case 1: return 1200;
+ case 2: return 2400;
+ case 4: return 4800;
+ case 9: return 9600;
+ case 19: return 19200;
+ case 38: return 38400;
+ case 57: return 57600;
+ case 111: return 111100;
+ case 115: return 115200;
+ }
+ Serial.println_P(PSTR("Invalid SERIAL3_BAUD"));
+ return default_baud;
+}
+
+
+#if USB_MUX_PIN > 0
+static void check_usb_mux(void)
+{
+ bool usb_check = !digitalRead(USB_MUX_PIN);
+ if (usb_check == usb_connected) {
+ return;
+ }
+
+ // the user has switched to/from the telemetry port
+ usb_connected = usb_check;
+ if (usb_connected) {
+ Serial.begin(SERIAL0_BAUD, 128, 128);
+ } else {
+ Serial.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128);
+ }
+}
+#endif
+
+
+/*
+ called by gyro/accel init to flash LEDs so user
+ has some mesmerising lights to watch while waiting
+ */
+void flash_leds(bool on)
+{
+ digitalWrite(A_LED_PIN, on?LED_OFF:LED_ON);
+ digitalWrite(C_LED_PIN, on?LED_ON:LED_OFF);
+}
+
+#ifndef DESKTOP_BUILD
+/*
+ * Read Vcc vs 1.1v internal reference
+ *
+ * This call takes about 150us total. ADC conversion is 13 cycles of
+ * 125khz default changes the mux if it isn't set, and return last
+ * reading (allows necessary settle time) otherwise trigger the
+ * conversion
+ */
+uint16_t board_voltage(void)
+{
+ const uint8_t mux = (_BV(REFS0)|_BV(MUX4)|_BV(MUX3)|_BV(MUX2)|_BV(MUX1));
+
+ if (ADMUX == mux) {
+ ADCSRA |= _BV(ADSC); // Convert
+ uint16_t counter=4000; // normally takes about 1700 loops
+ while (bit_is_set(ADCSRA, ADSC) && counter) // Wait
+ counter--;
+ if (counter == 0) {
+ // we don't actually expect this timeout to happen,
+ // but we don't want any more code that could hang. We
+ // report 0V so it is clear in the logs that we don't know
+ // the value
+ return 0;
+ }
+ uint32_t result = ADCL | ADCH<<8;
+ return 1126400UL / result; // Read and back-calculate Vcc in mV
+ }
+ // switch mux, settle time is needed. We don't want to delay
+ // waiting for the settle, so report 0 as a "don't know" value
+ ADMUX = mux;
+ return 0; // we don't know the current voltage
+}
+#endif
diff --git a/APMrover2/test.pde b/APMrover2/test.pde
new file mode 100644
index 000000000..2c1c051d8
--- /dev/null
+++ b/APMrover2/test.pde
@@ -0,0 +1,695 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#if CLI_ENABLED == ENABLED
+
+// These are function definitions so the Menu can be constructed before the functions
+// are defined below. Order matters to the compiler.
+static int8_t test_radio_pwm(uint8_t argc, const Menu::arg *argv);
+static int8_t test_radio(uint8_t argc, const Menu::arg *argv);
+static int8_t test_passthru(uint8_t argc, const Menu::arg *argv);
+static int8_t test_failsafe(uint8_t argc, const Menu::arg *argv);
+static int8_t test_gps(uint8_t argc, const Menu::arg *argv);
+#if CONFIG_ADC == ENABLED
+static int8_t test_adc(uint8_t argc, const Menu::arg *argv);
+#endif
+static int8_t test_imu(uint8_t argc, const Menu::arg *argv);
+static int8_t test_battery(uint8_t argc, const Menu::arg *argv);
+static int8_t test_relay(uint8_t argc, const Menu::arg *argv);
+static int8_t test_wp(uint8_t argc, const Menu::arg *argv);
+static int8_t test_airspeed(uint8_t argc, const Menu::arg *argv);
+static int8_t test_pressure(uint8_t argc, const Menu::arg *argv);
+static int8_t test_vario(uint8_t argc, const Menu::arg *argv);
+static int8_t test_mag(uint8_t argc, const Menu::arg *argv);
+static int8_t test_xbee(uint8_t argc, const Menu::arg *argv);
+static int8_t test_eedump(uint8_t argc, const Menu::arg *argv);
+static int8_t test_rawgps(uint8_t argc, const Menu::arg *argv);
+static int8_t test_modeswitch(uint8_t argc, const Menu::arg *argv);
+#if CONFIG_APM_HARDWARE != APM_HARDWARE_APM2
+static int8_t test_dipswitches(uint8_t argc, const Menu::arg *argv);
+#endif
+static int8_t test_logging(uint8_t argc, const Menu::arg *argv);
+
+// 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[] PROGMEM = {
+ {"pwm", test_radio_pwm},
+ {"radio", test_radio},
+ {"passthru", test_passthru},
+ {"failsafe", test_failsafe},
+ {"battery", test_battery},
+ {"relay", test_relay},
+ {"waypoints", test_wp},
+ {"xbee", test_xbee},
+ {"eedump", test_eedump},
+ {"modeswitch", test_modeswitch},
+#if CONFIG_APM_HARDWARE != APM_HARDWARE_APM2
+ {"dipswitches", test_dipswitches},
+#endif
+
+ // Tests below here are for hardware sensors only present
+ // when real sensors are attached or they are emulated
+#if HIL_MODE == HIL_MODE_DISABLED
+#if CONFIG_ADC == ENABLED
+ {"adc", test_adc},
+#endif
+ {"gps", test_gps},
+ {"rawgps", test_rawgps},
+ {"imu", test_imu},
+ {"airspeed", test_airspeed},
+ {"airpressure", test_pressure},
+ {"vario", test_vario},
+ {"compass", test_mag},
+#elif HIL_MODE == HIL_MODE_SENSORS
+ {"adc", test_adc},
+ {"gps", test_gps},
+ {"imu", test_imu},
+ {"compass", test_mag},
+#elif HIL_MODE == HIL_MODE_ATTITUDE
+#endif
+ {"logging", test_logging},
+
+};
+
+// A Macro to create the Menu
+MENU(test_menu, "test", test_menu_commands);
+
+static int8_t
+test_mode(uint8_t argc, const Menu::arg *argv)
+{
+ Serial.printf_P(PSTR("Test Mode\n\n"));
+ test_menu.run();
+ return 0;
+}
+
+static void print_hit_enter()
+{
+ Serial.printf_P(PSTR("Hit Enter to exit.\n\n"));
+}
+
+static int8_t
+test_eedump(uint8_t argc, const Menu::arg *argv)
+{
+ int i, j;
+
+ // hexdump the EEPROM
+ for (i = 0; i < EEPROM_MAX_ADDR; i += 16) {
+ Serial.printf_P(PSTR("%04x:"), i);
+ for (j = 0; j < 16; j++)
+ Serial.printf_P(PSTR(" %02x"), eeprom_read_byte((const uint8_t *)(i + j)));
+ Serial.println();
+ }
+ return(0);
+}
+
+static int8_t
+test_radio_pwm(uint8_t argc, const Menu::arg *argv)
+{
+ print_hit_enter();
+ delay(1000);
+
+ while(1){
+ delay(20);
+
+ // Filters radio input - adjust filters in the radio.pde file
+ // ----------------------------------------------------------
+ read_radio();
+
+ Serial.printf_P(PSTR("IN:\t1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"),
+ g.channel_roll.radio_in,
+ g.channel_pitch.radio_in,
+ g.channel_throttle.radio_in,
+ g.channel_rudder.radio_in,
+ g.rc_5.radio_in,
+ g.rc_6.radio_in,
+ g.rc_7.radio_in,
+ g.rc_8.radio_in);
+
+ if(Serial.available() > 0){
+ return (0);
+ }
+ }
+}
+
+
+static int8_t
+test_passthru(uint8_t argc, const Menu::arg *argv)
+{
+ print_hit_enter();
+ delay(1000);
+
+ while(1){
+ delay(20);
+
+ // New radio frame? (we could use also if((millis()- timer) > 20)
+ if (APM_RC.GetState() == 1){
+ Serial.print("CH:");
+ for(int i = 0; i < 8; i++){
+ Serial.print(APM_RC.InputCh(i)); // Print channel values
+ Serial.print(",");
+ APM_RC.OutputCh(i, APM_RC.InputCh(i)); // Copy input to Servos
+ }
+ Serial.println();
+ }
+ if (Serial.available() > 0){
+ return (0);
+ }
+ }
+ return 0;
+}
+
+static int8_t
+test_radio(uint8_t argc, const Menu::arg *argv)
+{
+ print_hit_enter();
+ delay(1000);
+
+ #if THROTTLE_REVERSE == 1
+ Serial.printf_P(PSTR("Throttle is reversed in config: \n"));
+ delay(1000);
+ #endif
+
+ // read the radio to set trims
+ // ---------------------------
+ trim_radio();
+
+ while(1){
+ delay(20);
+ read_radio();
+ update_servo_switches();
+
+ g.channel_roll.calc_pwm();
+ g.channel_pitch.calc_pwm();
+ g.channel_throttle.calc_pwm();
+ g.channel_rudder.calc_pwm();
+
+ // write out the servo PWM values
+ // ------------------------------
+ set_servos();
+
+ tuning_value = constrain(((float)(g.rc_7.radio_in - g.rc_7.radio_min) / (float)(g.rc_7.radio_max - g.rc_7.radio_min)),0,1);
+
+ Serial.printf_P(PSTR("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d Tuning = %2.3f\n"),
+ g.channel_roll.control_in,
+ g.channel_pitch.control_in,
+ g.channel_throttle.control_in,
+ g.channel_rudder.control_in,
+ g.rc_5.control_in,
+ g.rc_6.control_in,
+ g.rc_7.control_in,
+ g.rc_8.control_in,
+ tuning_value);
+
+ if(Serial.available() > 0){
+ return (0);
+ }
+ }
+}
+
+static int8_t
+test_failsafe(uint8_t argc, const Menu::arg *argv)
+{
+ byte fail_test;
+ print_hit_enter();
+ for(int i = 0; i < 50; i++){
+ delay(20);
+ read_radio();
+ }
+
+ // read the radio to set trims
+ // ---------------------------
+ trim_radio();
+
+ oldSwitchPosition = readSwitch();
+
+ Serial.printf_P(PSTR("Unplug battery, throttle in neutral, turn off radio.\n"));
+ while(g.channel_throttle.control_in > 0){
+ delay(20);
+ read_radio();
+ }
+
+ while(1){
+ delay(20);
+ read_radio();
+
+ if(g.channel_throttle.control_in > 0){
+ Serial.printf_P(PSTR("THROTTLE CHANGED %d \n"), g.channel_throttle.control_in);
+ fail_test++;
+ }
+
+ if(oldSwitchPosition != readSwitch()){
+ Serial.printf_P(PSTR("CONTROL MODE CHANGED: "));
+ Serial.println(flight_mode_strings[readSwitch()]);
+ fail_test++;
+ }
+
+ if(g.throttle_fs_enabled && g.channel_throttle.get_failsafe()){
+ Serial.printf_P(PSTR("THROTTLE FAILSAFE ACTIVATED: %d, "), g.channel_throttle.radio_in);
+ Serial.println(flight_mode_strings[readSwitch()]);
+ fail_test++;
+ }
+
+ if(fail_test > 0){
+ return (0);
+ }
+ if(Serial.available() > 0){
+ Serial.printf_P(PSTR("LOS caused no change in APM.\n"));
+ return (0);
+ }
+ }
+}
+
+static int8_t
+test_battery(uint8_t argc, const Menu::arg *argv)
+{
+if (g.battery_monitoring == 3 || g.battery_monitoring == 4) {
+ print_hit_enter();
+ delta_ms_medium_loop = 100;
+
+ while(1){
+ delay(100);
+ read_radio();
+ read_battery();
+ if (g.battery_monitoring == 3){
+ Serial.printf_P(PSTR("V: %4.4f\n"),
+ battery_voltage1,
+ current_amps1,
+ current_total1);
+ } else {
+ Serial.printf_P(PSTR("V: %4.4f, A: %4.4f, mAh: %4.4f\n"),
+ battery_voltage1,
+ current_amps1,
+ current_total1);
+ }
+
+ // write out the servo PWM values
+ // ------------------------------
+ set_servos();
+
+ if(Serial.available() > 0){
+ return (0);
+ }
+ }
+} else {
+ Serial.printf_P(PSTR("Not enabled\n"));
+ return (0);
+}
+
+}
+
+static int8_t
+test_relay(uint8_t argc, const Menu::arg *argv)
+{
+ print_hit_enter();
+ delay(1000);
+
+ while(1){
+ Serial.printf_P(PSTR("Relay on\n"));
+ relay.on();
+ delay(3000);
+ if(Serial.available() > 0){
+ return (0);
+ }
+
+ Serial.printf_P(PSTR("Relay off\n"));
+ relay.off();
+ delay(3000);
+ if(Serial.available() > 0){
+ return (0);
+ }
+ }
+}
+
+static int8_t
+test_wp(uint8_t argc, const Menu::arg *argv)
+{
+ delay(1000);
+
+ // save the alitude above home option
+ if(g.RTL_altitude < 0){
+ Serial.printf_P(PSTR("Hold current altitude\n"));
+ }else{
+ Serial.printf_P(PSTR("Hold altitude of %dm\n"), (int)g.RTL_altitude/100);
+ }
+
+ Serial.printf_P(PSTR("%d waypoints\n"), (int)g.command_total);
+ Serial.printf_P(PSTR("Hit radius: %d\n"), (int)g.waypoint_radius);
+ Serial.printf_P(PSTR("Loiter radius: %d\n\n"), (int)g.loiter_radius);
+
+ for(byte i = 0; i <= g.command_total; i++){
+ struct Location temp = get_cmd_with_index(i);
+ test_wp_print(&temp, i);
+ }
+
+ return (0);
+}
+
+static void
+test_wp_print(struct Location *cmd, byte wp_index)
+{
+ Serial.printf_P(PSTR("command #: %d id:%d options:%d p1:%d p2:%ld p3:%ld p4:%ld \n"),
+ (int)wp_index,
+ (int)cmd->id,
+ (int)cmd->options,
+ (int)cmd->p1,
+ cmd->alt,
+ cmd->lat,
+ cmd->lng);
+}
+
+static int8_t
+test_xbee(uint8_t argc, const Menu::arg *argv)
+{
+ print_hit_enter();
+ delay(1000);
+ Serial.printf_P(PSTR("Begin XBee X-CTU Range and RSSI Test:\n"));
+
+ while(1){
+
+ if (Serial3.available())
+ Serial3.write(Serial3.read());
+
+ if(Serial.available() > 0){
+ return (0);
+ }
+ }
+}
+
+
+static int8_t
+test_modeswitch(uint8_t argc, const Menu::arg *argv)
+{
+ print_hit_enter();
+ delay(1000);
+
+ Serial.printf_P(PSTR("Control CH "));
+
+ Serial.println(FLIGHT_MODE_CHANNEL, DEC);
+
+ while(1){
+ delay(20);
+ byte switchPosition = readSwitch();
+ if (oldSwitchPosition != switchPosition){
+ Serial.printf_P(PSTR("Position %d\n"), switchPosition);
+ oldSwitchPosition = switchPosition;
+ }
+ if(Serial.available() > 0){
+ return (0);
+ }
+ }
+}
+
+/*
+ test the dataflash is working
+ */
+static int8_t
+test_logging(uint8_t argc, const Menu::arg *argv)
+{
+ Serial.println_P(PSTR("Testing dataflash logging"));
+ if (!DataFlash.CardInserted()) {
+ Serial.println_P(PSTR("ERR: No dataflash inserted"));
+ return 0;
+ }
+ DataFlash.ReadManufacturerID();
+ Serial.printf_P(PSTR("Manufacturer: 0x%02x Device: 0x%04x\n"),
+ (unsigned)DataFlash.df_manufacturer,
+ (unsigned)DataFlash.df_device);
+ Serial.printf_P(PSTR("NumPages: %u PageSize: %u\n"),
+ (unsigned)DataFlash.df_NumPages+1,
+ (unsigned)DataFlash.df_PageSize);
+ DataFlash.StartRead(DataFlash.df_NumPages+1);
+ Serial.printf_P(PSTR("Format version: %lx Expected format version: %lx\n"),
+ (unsigned long)DataFlash.ReadLong(), (unsigned long)DF_LOGGING_FORMAT);
+ return 0;
+}
+
+#if CONFIG_APM_HARDWARE != APM_HARDWARE_APM2
+static int8_t
+test_dipswitches(uint8_t argc, const Menu::arg *argv)
+{
+ print_hit_enter();
+ delay(1000);
+
+ if (!g.switch_enable) {
+ Serial.println_P(PSTR("dip switches disabled, using EEPROM"));
+ }
+
+ while(1){
+ delay(100);
+ update_servo_switches();
+
+ if (g.mix_mode == 0) {
+ Serial.printf_P(PSTR("Mix:standard \trev roll:%d, rev pitch:%d, rev rudder:%d\n"),
+ (int)g.channel_roll.get_reverse(),
+ (int)g.channel_pitch.get_reverse(),
+ (int)g.channel_rudder.get_reverse());
+ } else {
+ Serial.printf_P(PSTR("Mix:elevons \trev elev:%d, rev ch1:%d, rev ch2:%d\n"),
+ (int)g.reverse_elevons,
+ (int)g.reverse_ch1_elevon,
+ (int)g.reverse_ch2_elevon);
+ }
+ if(Serial.available() > 0){
+ return (0);
+ }
+ }
+}
+#endif // CONFIG_APM_HARDWARE != APM_HARDWARE_APM2
+
+
+//-------------------------------------------------------------------------------------------
+// tests in this section are for real sensors or sensors that have been simulated
+
+#if HIL_MODE == HIL_MODE_DISABLED || HIL_MODE == HIL_MODE_SENSORS
+
+#if CONFIG_ADC == ENABLED
+static int8_t
+test_adc(uint8_t argc, const Menu::arg *argv)
+{
+ print_hit_enter();
+ adc.Init(&timer_scheduler);
+ delay(1000);
+ Serial.printf_P(PSTR("ADC\n"));
+ delay(1000);
+
+ while(1){
+ for (int i=0;i<9;i++) Serial.printf_P(PSTR("%.1f\t"),adc.Ch(i));
+ Serial.println();
+ delay(100);
+ if(Serial.available() > 0){
+ return (0);
+ }
+ }
+}
+#endif // CONFIG_ADC
+
+static int8_t
+test_gps(uint8_t argc, const Menu::arg *argv)
+{
+ print_hit_enter();
+ delay(1000);
+
+ while(1){
+ delay(333);
+
+ // Blink GPS LED if we don't have a fix
+ // ------------------------------------
+ update_GPS_light();
+
+ g_gps->update();
+
+ if (g_gps->new_data){
+ Serial.printf_P(PSTR("Lat: %ld, Lon %ld, Alt: %ldm, #sats: %d\n"),
+ g_gps->latitude,
+ g_gps->longitude,
+ g_gps->altitude/100,
+ g_gps->num_sats);
+ }else{
+ Serial.printf_P(PSTR("."));
+ }
+ if(Serial.available() > 0){
+ return (0);
+ }
+ }
+}
+
+static int8_t
+test_imu(uint8_t argc, const Menu::arg *argv)
+{
+ //Serial.printf_P(PSTR("Calibrating."));
+ imu.init(IMU::COLD_START, delay, flash_leds, &timer_scheduler);
+ ahrs.reset();
+
+ print_hit_enter();
+ delay(1000);
+
+ while(1){
+ delay(20);
+ if (millis() - fast_loopTimer > 19) {
+ delta_ms_fast_loop = millis() - fast_loopTimer;
+ G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by DCM integrator
+ fast_loopTimer = millis();
+
+ // IMU
+ // ---
+ ahrs.update();
+
+ if(g.compass_enabled) {
+ medium_loopCounter++;
+ if(medium_loopCounter == 5){
+ if (compass.read()) {
+ compass.calculate(ahrs.get_dcm_matrix()); // Calculate heading
+ }
+ medium_loopCounter = 0;
+ }
+ }
+
+ // We are using the IMU
+ // ---------------------
+ Vector3f gyros = imu.get_gyro();
+ Vector3f accels = imu.get_accel();
+ Serial.printf_P(PSTR("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,
+ gyros.x, gyros.y, gyros.z,
+ accels.x, accels.y, accels.z);
+ }
+ if(Serial.available() > 0){
+ return (0);
+ }
+ }
+}
+
+
+static int8_t
+test_mag(uint8_t argc, const Menu::arg *argv)
+{
+ if (!g.compass_enabled) {
+ Serial.printf_P(PSTR("Compass: "));
+ print_enabled(false);
+ return (0);
+ }
+
+ compass.set_orientation(MAG_ORIENTATION);
+ if (!compass.init()) {
+ Serial.println_P(PSTR("Compass initialisation failed!"));
+ return 0;
+ }
+ compass.null_offsets_enable();
+ ahrs.set_compass(&compass);
+ report_compass();
+
+ // we need the AHRS initialised for this test
+ imu.init(IMU::COLD_START, delay, flash_leds, &timer_scheduler);
+ ahrs.reset();
+
+ int counter = 0;
+ //Serial.printf_P(PSTR("MAG_ORIENTATION: %d\n"), MAG_ORIENTATION);
+
+ print_hit_enter();
+
+ while(1) {
+ delay(20);
+ if (millis() - fast_loopTimer > 19) {
+ delta_ms_fast_loop = millis() - fast_loopTimer;
+ G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by DCM integrator
+ fast_loopTimer = millis();
+
+ // IMU
+ // ---
+ ahrs.update();
+
+ medium_loopCounter++;
+ if(medium_loopCounter == 5){
+ if (compass.read()) {
+ // Calculate heading
+ Matrix3f m = ahrs.get_dcm_matrix();
+ compass.calculate(m);
+ compass.null_offsets();
+ }
+ medium_loopCounter = 0;
+ }
+
+ counter++;
+ if (counter>20) {
+ if (compass.healthy) {
+ Vector3f maggy = compass.get_offsets();
+ Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d,\tXYZoff: %6.2f, %6.2f, %6.2f\n"),
+ (wrap_360(ToDeg(compass.heading) * 100)) /100,
+ (int)compass.mag_x,
+ (int)compass.mag_y,
+ (int)compass.mag_z,
+ maggy.x,
+ maggy.y,
+ maggy.z);
+ } else {
+ Serial.println_P(PSTR("compass not healthy"));
+ }
+ counter=0;
+ }
+ }
+ if (Serial.available() > 0) {
+ break;
+ }
+ }
+
+ // save offsets. This allows you to get sane offset values using
+ // the CLI before you go flying.
+ Serial.println_P(PSTR("saving offsets"));
+ compass.save_offsets();
+ return (0);
+}
+
+#endif // HIL_MODE == HIL_MODE_DISABLED || HIL_MODE == HIL_MODE_SENSORS
+
+//-------------------------------------------------------------------------------------------
+// real sensors that have not been simulated yet go here
+
+#if HIL_MODE == HIL_MODE_DISABLED
+
+static int8_t
+test_airspeed(uint8_t argc, const Menu::arg *argv)
+{
+
+}
+
+
+static int8_t
+test_pressure(uint8_t argc, const Menu::arg *argv)
+{
+
+}
+
+static int8_t
+test_vario(uint8_t argc, const Menu::arg *argv)
+{
+
+}
+
+static int8_t
+test_rawgps(uint8_t argc, const Menu::arg *argv)
+{
+ print_hit_enter();
+ delay(1000);
+
+ while(1){
+ if (Serial3.available()){
+ digitalWrite(B_LED_PIN, LED_ON); // Blink Yellow LED if we are sending data to GPS
+ Serial1.write(Serial3.read());
+ digitalWrite(B_LED_PIN, LED_OFF);
+ }
+ if (Serial1.available()){
+ digitalWrite(C_LED_PIN, LED_ON); // Blink Red LED if we are receiving data from GPS
+ Serial3.write(Serial1.read());
+ digitalWrite(C_LED_PIN, LED_OFF);
+ }
+ if(Serial.available() > 0){
+ return (0);
+ }
+ }
+}
+#endif // HIL_MODE == HIL_MODE_DISABLED
+
+#endif // CLI_ENABLED
--
GitLab