.editorconfig

+# This file provide reference settings for the APM code conventions
+# There are plug-ins available for nearly every text editor to automatically
+# respect the conventions contained within this file.
+#
+# Please see editorconfig.org for complete information.
+#
+# If you find errors in this file, please send a pull-request with a fix.
+#
+root = true
+
+[*]
+indent_style = space
+indent_size = 4
+end_of_line = lf
+charset = utf-8
+trim_trailing_whitespace = false # These are the correct rules for APM coding standards, but fixing up old files causes git spam
+insert_final_newline = false
+
+[*.mk]
+indent_style = tab
+indent_size = 8
+
+[{makefile,Makefile}]
+indent_style = tab
+indent_size = 8

.gitignore

+*~
+*.bin
+*.d
+*.dll
+*.elf
+*.hex
+*.dfu
+*.exe
+*.lst
+*.o
+*.obj
+*.px4
+*.vrx
+*.pyc
+*.tlog
+*.tlog.raw
+*.vbrain
+*.zip
+.*.swo
+.*.swp
+.built
+.context
+.cproject
+.depend
+.directory
+.DS_Store
+.metadata/
+.project
+.settings/
+.vagrant
+/tmp/*
+/Tools/ArdupilotMegaPlanner/3DRRadio/bin
+/Tools/ArdupilotMegaPlanner/3DRRadio/obj
+/Tools/ArdupilotMegaPlanner/APMPlannerXplanes/APMPlannerXplanes/Debug
+/Tools/ArdupilotMegaPlanner/bin
+/Tools/ArdupilotMegaPlanner/bin/APM Planner.app
+/Tools/ArdupilotMegaPlanner/bin/Debug
+/Tools/ArdupilotMegaPlanner/bin/Release/gmapcache
+/Tools/ArdupilotMegaPlanner/bin/Release/logs/
+/Tools/ArdupilotMegaPlanner/CustomImages
+/Tools/ArdupilotMegaPlanner/Msi/upload.bat
+/Tools/ArdupilotMegaPlanner/obj
+/Tools/ArdupilotMegaPlanner/resedit/bin
+/Tools/ArdupilotMegaPlanner/resedit/obj
+/Tools/ArdupilotMegaPlanner/Updater/bin
+/Tools/ArdupilotMegaPlanner/Updater/obj
+/Tools/ArdupilotMegaPlanner/wix/bin
+/Tools/ArdupilotMegaPlanner/wix/obj
+/Tools/autotest/ch7_mission.txt
+/Tools/autotest/aircraft/Rascal/reset.xml
+/Tools/autotest/jsbsim/fgout.xml
+/Tools/autotest/jsbsim/rascal_test.xml
+ArduCopter/Debug/
+ArduCopter/terrain/*.DAT
+ArduCopter/test.ArduCopter/
+ArduCopter/test/*
+ArduPlane/terrain/*.DAT
+ArduPlane/test.ArduPlane/
+APMrover2/test.APMrover2/
+autotest.lck
+build
+Build.ArduCopter/*
+Build.ArduPlane/*
+Build.APMrover2/*
+Build.AntennaTracker/*
+cmake_install.cmake
+CMakeCache.txt
+CMakeFiles
+config.mk
+dataflash.bin
+eeprom.bin
+index.html
+LASTLOG.TXT
+Make.dep
+mav.log
+mav.log.raw
+module.mk
+serialsent.raw
+status.txt
+tags
+test.ArduCopter/*
+Thumbs.db

.pydevproject

+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<?eclipse-pydev version="1.0"?><pydev_project>
+<pydev_property name="org.python.pydev.PYTHON_PROJECT_INTERPRETER">Default</pydev_property>
+<pydev_property name="org.python.pydev.PYTHON_PROJECT_VERSION">python 2.7</pydev_property>
+</pydev_project>

.travis.yml

+language: cpp
+
+before_install:
+  -  APMDIR=$(pwd) && pushd .. && $APMDIR/Tools/scripts/install-prereqs-ubuntu.sh -y && . ~/.profile && popd
+
+script: 
+  - Tools/scripts/build_all_travis.sh
+
+notifications:
+  webhooks:
+    urls:
+      - https://webhooks.gitter.im/e/e5e0b55e353e53945b4b
+    on_success: change  # options: [always|never|change] default: always
+    on_failure: always  # options: [always|never|change] default: always
+    on_start: false     # default: false

APMrover2/APM_Config.h

+// -*- 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.
+
+

APMrover2/APMrover2.pde

+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#define THISFIRMWARE "ArduRover v2.47"
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/* 
+   This is the APMrover2 firmware. It was originally derived from
+   ArduPlane by Jean-Louis Naudin (JLN), and then rewritten after the
+   AP_HAL merge by Andrew Tridgell
+
+   Maintainer: Andrew Tridgell
+
+   Authors:    Doug Weibel, Jose Julio, Jordi Munoz, Jason Short, Andrew Tridgell, Randy Mackay, Pat Hickey, John Arne Birkeland, Olivier Adler, Jean-Louis Naudin
+
+   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 
+
+   APMrover alpha version tester: Franco Borasio, Daniel Chapelat... 
+
+   Please contribute your ideas! See http://dev.ardupilot.com for details
+*/
+
+// Radio setup:
+// APM INPUT (Rec = receiver)
+// Rec ch1: Steering
+// Rec ch2: not used
+// Rec ch3: Throttle
+// Rec ch4: not used
+// Rec ch5: not used
+// Rec ch6: not used
+// Rec ch7: Option channel to 2 position switch
+// Rec ch8: Mode channel to 6 position switch
+// APM OUTPUT
+// Ch1: Wheel servo (direction)
+// Ch2: not used
+// Ch3: to the motor ESC
+// Ch4: not used
+
+////////////////////////////////////////////////////////////////////////////////
+// Header includes
+////////////////////////////////////////////////////////////////////////////////
+
+#include <math.h>
+#include <stdarg.h>
+#include <stdio.h>
+
+// Libraries
+#include <AP_Common.h>
+#include <AP_Progmem.h>
+#include <AP_HAL.h>
+#include <AP_Menu.h>
+#include <AP_Param.h>
+#include <StorageManager.h>
+#include <AP_GPS.h>         // ArduPilot GPS library
+#include <AP_ADC.h>         // ArduPilot Mega Analog to Digital Converter Library
+#include <AP_ADC_AnalogSource.h>
+#include <AP_Baro.h>
+#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_AHRS.h>         // ArduPilot Mega DCM Library
+#include <AP_NavEKF.h>
+#include <AP_Mission.h>     // Mission command library
+#include <AP_Rally.h>
+#include <AP_Terrain.h>
+#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 <Butter.h>			// Filter library - butterworth filter
+#include <AP_Buffer.h>      // FIFO buffer 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_ServoRelayEvents.h>
+#include <AP_Mount.h>		// Camera/Antenna mount
+#include <AP_Camera.h>		// Camera triggering
+#include <GCS_MAVLink.h>    // MAVLink GCS definitions
+#include <AP_Airspeed.h>    // needed for AHRS build
+#include <AP_Vehicle.h>     // needed for AHRS build
+#include <DataFlash.h>
+#include <AP_RCMapper.h>        // RC input mapping library
+#include <SITL.h>
+#include <AP_Scheduler.h>       // main loop scheduler
+#include <stdarg.h>
+#include <AP_Navigation.h>
+#include <APM_Control.h>
+#include <AP_L1_Control.h>
+#include <AP_BoardConfig.h>
+#include <AP_Frsky_Telem.h>
+
+#include <AP_HAL_AVR.h>
+#include <AP_HAL_AVR_SITL.h>
+#include <AP_HAL_PX4.h>
+#include <AP_HAL_VRBRAIN.h>
+#include <AP_HAL_FLYMAPLE.h>
+#include <AP_HAL_Linux.h>
+#include <AP_HAL_Empty.h>
+#include "compat.h"
+
+#include <AP_Notify.h>      // Notify library
+#include <AP_BattMonitor.h> // Battery monitor library
+#include <AP_OpticalFlow.h>     // Optical Flow library
+
+// Configuration
+#include "config.h"
+
+// Local modules
+#include "defines.h"
+#include "Parameters.h"
+#include "GCS.h"
+
+#include <AP_Declination.h> // ArduPilot Mega Declination Helper Library
+
+AP_HAL::BetterStream* cliSerial;
+
+const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
+
+// this sets up the parameter table, and sets the default values. This
+// must be the first AP_Param variable declared to ensure its
+// constructor runs before the constructors of the other AP_Param
+// variables
+AP_Param param_loader(var_info);
+
+////////////////////////////////////////////////////////////////////////////////
+// the rate we run the main loop at
+////////////////////////////////////////////////////////////////////////////////
+static const AP_InertialSensor::Sample_rate ins_sample_rate = AP_InertialSensor::RATE_50HZ;
+
+////////////////////////////////////////////////////////////////////////////////
+// Parameters
+////////////////////////////////////////////////////////////////////////////////
+//
+// Global parameters are all contained within the 'g' class.
+//
+static Parameters      g;
+
+// main loop scheduler
+static AP_Scheduler scheduler;
+
+// mapping between input channels
+static RCMapper rcmap;
+
+// board specific config
+static AP_BoardConfig BoardConfig;
+
+// primary control channels
+static RC_Channel *channel_steer;
+static RC_Channel *channel_throttle;
+static RC_Channel *channel_learn;
+
+////////////////////////////////////////////////////////////////////////////////
+// prototypes
+static void update_events(void);
+void gcs_send_text_fmt(const prog_char_t *fmt, ...);
+static void print_mode(AP_HAL::BetterStream *port, uint8_t mode);
+
+////////////////////////////////////////////////////////////////////////////////
+// DataFlash
+////////////////////////////////////////////////////////////////////////////////
+#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
+static DataFlash_APM1 DataFlash;
+#elif CONFIG_HAL_BOARD == HAL_BOARD_APM2
+static DataFlash_APM2 DataFlash;
+#elif defined(HAL_BOARD_LOG_DIRECTORY)
+static DataFlash_File DataFlash(HAL_BOARD_LOG_DIRECTORY);
+#else
+DataFlash_Empty DataFlash;
+#endif
+
+static bool in_log_download;
+
+////////////////////////////////////////////////////////////////////////////////
+// Sensors
+////////////////////////////////////////////////////////////////////////////////
+//
+// There are three basic options related to flight sensor selection.
+//
+// - Normal driving 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.
+//
+
+// GPS driver
+static AP_GPS gps;
+
+// flight modes convenience array
+static AP_Int8		*modes = &g.mode1;
+
+static AP_Baro barometer;
+
+#if CONFIG_COMPASS == HAL_COMPASS_PX4
+static AP_Compass_PX4 compass;
+#elif CONFIG_COMPASS == HAL_COMPASS_VRBRAIN
+static AP_Compass_VRBRAIN compass;
+#elif CONFIG_COMPASS == HAL_COMPASS_HMC5843
+static AP_Compass_HMC5843 compass;
+#elif CONFIG_COMPASS == HAL_COMPASS_HIL
+static AP_Compass_HIL compass;
+#elif CONFIG_COMPASS == HAL_COMPASS_AK8963
+static AP_Compass_AK8963_MPU9250 compass;
+#else
+ #error Unrecognized CONFIG_COMPASS setting
+#endif
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
+AP_ADC_ADS7844 apm1_adc;
+#endif
+
+AP_InertialSensor ins;
+
+// Inertial Navigation EKF
+#if AP_AHRS_NAVEKF_AVAILABLE
+AP_AHRS_NavEKF ahrs(ins, barometer, gps);
+#else
+AP_AHRS_DCM ahrs(ins, barometer, gps);
+#endif
+
+static AP_L1_Control L1_controller(ahrs);
+
+// selected navigation controller
+static AP_Navigation *nav_controller = &L1_controller;
+
+// steering controller
+static AP_SteerController steerController(ahrs);
+
+////////////////////////////////////////////////////////////////////////////////
+// Mission library
+// forward declaration to avoid compiler errors
+////////////////////////////////////////////////////////////////////////////////
+static bool start_command(const AP_Mission::Mission_Command& cmd);
+static bool verify_command(const AP_Mission::Mission_Command& cmd);
+static void exit_mission();
+AP_Mission mission(ahrs, &start_command, &verify_command, &exit_mission);
+
+static OpticalFlow optflow;
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
+SITL sitl;
+#endif
+
+////////////////////////////////////////////////////////////////////////////////
+// GCS selection
+////////////////////////////////////////////////////////////////////////////////
+//
+static const uint8_t num_gcs = MAVLINK_COMM_NUM_BUFFERS;
+static GCS_MAVLINK gcs[MAVLINK_COMM_NUM_BUFFERS];
+
+// a pin for reading the receiver RSSI voltage. The scaling by 0.25 
+// is to take the 0 to 1024 range down to an 8 bit range for MAVLink
+AP_HAL::AnalogSource *rssi_analog_source;
+
+////////////////////////////////////////////////////////////////////////////////
+// SONAR
+static RangeFinder sonar;
+
+// relay support
+AP_Relay relay;
+
+AP_ServoRelayEvents ServoRelayEvents(relay);
+
+// Camera
+#if CAMERA == ENABLED
+static AP_Camera camera(&relay);
+#endif
+
+// The rover's current location
+static struct 	Location current_loc;
+
+
+// Camera/Antenna mount tracking and stabilisation stuff
+// --------------------------------------
+#if MOUNT == ENABLED
+// current_loc uses the baro/gps soloution for altitude rather than gps only.
+AP_Mount camera_mount(&current_loc, ahrs, 0);
+#endif
+
+
+////////////////////////////////////////////////////////////////////////////////
+// Global variables
+////////////////////////////////////////////////////////////////////////////////
+
+// if USB is connected
+static bool usb_connected;
+
+/* Radio values
+		Channel assignments
+			1   Steering
+			2   ---
+			3   Throttle
+			4   ---
+			5   Aux5
+			6   Aux6
+			7   Aux7/learn
+			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
+enum mode   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
+uint8_t 	oldSwitchPosition;
+// 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. See 
+// FAILSAFE_EVENT_*
+static struct {
+    uint8_t bits;
+    uint32_t rc_override_timer;
+    uint32_t start_time;
+    uint8_t triggered;
+    uint32_t last_valid_rc_ms;
+} failsafe;
+
+// notification object for LEDs, buzzers etc (parameter set to false disables external leds)
+static AP_Notify notify;
+
+// 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 uint8_t 	ground_start_count	= 20;
+
+////////////////////////////////////////////////////////////////////////////////
+// Location & Navigation
+////////////////////////////////////////////////////////////////////////////////
+// Constants
+const	float radius_of_earth 	= 6378100;	// meters
+
+
+// true if we have a position estimate from AHRS
+static bool have_position;
+
+static bool rtl_complete = false;
+
+
+// angle of our next navigation waypoint
+static int32_t next_navigation_leg_cd;
+
+// ground speed error in m/s
+static float	groundspeed_error;	
+// 0-(throttle_max - throttle_cruise) : throttle nudge in Auto mode using top 1/2 of throttle stick travel
+static int16_t     throttle_nudge = 0;
+
+// receiver RSSI
+static uint8_t receiver_rssi;
+
+// the time when the last HEARTBEAT message arrived from a GCS
+static uint32_t last_heartbeat_ms;
+
+// obstacle detection information
+static struct {
+    // have we detected an obstacle?
+    uint8_t detected_count;
+    float turn_angle;
+    uint16_t sonar1_distance_cm;
+    uint16_t sonar2_distance_cm;
+
+    // time when we last detected an obstacle, in milliseconds
+    uint32_t detected_time_ms;
+} obstacle;
+
+// this is set to true when auto has been triggered to start
+static bool auto_triggered;
+
+////////////////////////////////////////////////////////////////////////////////
+// Ground speed
+////////////////////////////////////////////////////////////////////////////////
+// The amount current ground speed is below min ground speed.  meters per second
+static float 	ground_speed = 0;
+static int16_t throttle_last = 0, throttle = 500;
+
+////////////////////////////////////////////////////////////////////////////////
+// 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 bool ch7_flag;
+
+////////////////////////////////////////////////////////////////////////////////
+// Battery Sensors
+////////////////////////////////////////////////////////////////////////////////
+static AP_BattMonitor battery;
+
+////////////////////////////////////////////////////////////////////////////////
+// Battery Sensors
+////////////////////////////////////////////////////////////////////////////////
+#if FRSKY_TELEM_ENABLED == ENABLED
+static AP_Frsky_Telem frsky_telemetry(ahrs, battery);
+#endif
+
+////////////////////////////////////////////////////////////////////////////////
+// Navigation control variables
+////////////////////////////////////////////////////////////////////////////////
+// The instantaneous desired lateral acceleration in m/s/s
+static float lateral_acceleration;
+
+////////////////////////////////////////////////////////////////////////////////
+// Waypoint distances
+////////////////////////////////////////////////////////////////////////////////
+// Distance between rover and next waypoint.  Meters
+static float wp_distance;
+// Distance between previous and next waypoint.  Meters
+static int32_t wp_totalDistance;
+
+////////////////////////////////////////////////////////////////////////////////
+// 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 int32_t 	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 int32_t 	condition_start;
+
+////////////////////////////////////////////////////////////////////////////////
+// 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 const struct	Location &home = ahrs.get_home();
+// Flag for if we have 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 location of the current/active waypoint.  Used for track following
+static struct 	Location next_WP;
+// The location of the active waypoint in Guided mode.
+static struct  	Location guided_WP;
+
+////////////////////////////////////////////////////////////////////////////////
+// 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 int32_t 	perf_mon_timer;
+// The maximum main loop execution time recorded in the current performance monitoring interval
+static uint32_t 	G_Dt_max;
+
+////////////////////////////////////////////////////////////////////////////////
+// System Timers
+////////////////////////////////////////////////////////////////////////////////
+// Time in microseconds of start of main control loop. 
+static uint32_t 	fast_loopTimer_us;
+// Number of milliseconds used in last main loop cycle
+static uint32_t		delta_us_fast_loop;
+// Counter of main loop executions.  Used for performance monitoring and failsafe processing
+static uint16_t			mainLoop_count;
+
+// set if we are driving backwards
+static bool in_reverse;
+
+////////////////////////////////////////////////////////////////////////////////
+// Top-level logic
+////////////////////////////////////////////////////////////////////////////////
+
+/*
+  scheduler table - all regular tasks should be listed here, along
+  with how often they should be called (in 20ms units) and the maximum
+  time they are expected to take (in microseconds)
+ */
+static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
+	{ read_radio,             1,   1000 },
+    { ahrs_update,            1,   6400 },
+    { read_sonars,            1,   2000 },
+    { update_current_mode,    1,   1500 },
+    { set_servos,             1,   1500 },
+    { update_GPS_50Hz,        1,   2500 },
+    { update_GPS_10Hz,        5,   2500 },
+    { update_alt,             5,   3400 },
+    { navigate,               5,   1600 },
+    { update_compass,         5,   2000 },
+    { update_commands,        5,   1000 },
+    { update_logging1,        5,   1000 },
+    { update_logging2,        5,   1000 },
+    { gcs_retry_deferred,     1,   1000 },
+    { gcs_update,             1,   1700 },
+    { gcs_data_stream_send,   1,   3000 },
+    { read_control_switch,   15,   1000 },
+    { read_trim_switch,       5,   1000 },
+    { read_battery,           5,   1000 },
+    { read_receiver_rssi,     5,   1000 },
+    { update_events,          1,   1000 },
+    { check_usb_mux,         15,   1000 },
+    { mount_update,           1,    600 },
+    { gcs_failsafe_check,     5,    600 },
+    { compass_accumulate,     1,    900 },
+    { update_notify,          1,    300 },
+    { one_second_loop,       50,   3000 },
+#if FRSKY_TELEM_ENABLED == ENABLED
+    { telemetry_send,        10,    100 }	
+#endif
+};
+
+
+/*
+  setup is called when the sketch starts
+ */
+void setup() {
+    cliSerial = hal.console;
+
+    // load the default values of variables listed in var_info[]
+    AP_Param::setup_sketch_defaults();
+
+    // rover does not use arming nor pre-arm checks
+    AP_Notify::flags.armed = true;
+    AP_Notify::flags.pre_arm_check = true;
+    AP_Notify::flags.pre_arm_gps_check = true;
+    AP_Notify::flags.failsafe_battery = false;
+
+    notify.init(false);
+
+    battery.init();
+
+    rssi_analog_source = hal.analogin->channel(ANALOG_INPUT_NONE);
+
+	init_ardupilot();
+
+    // initialise the main loop scheduler
+    scheduler.init(&scheduler_tasks[0], sizeof(scheduler_tasks)/sizeof(scheduler_tasks[0]));
+}
+
+/*
+  loop() is called rapidly while the sketch is running
+ */
+void loop()
+{
+    // wait for an INS sample
+    ins.wait_for_sample();
+
+    uint32_t timer = hal.scheduler->micros();
+
+    delta_us_fast_loop	= timer - fast_loopTimer_us;
+    G_Dt                = delta_us_fast_loop * 1.0e-6f;
+    fast_loopTimer_us   = timer;
+
+	if (delta_us_fast_loop > G_Dt_max)
+		G_Dt_max = delta_us_fast_loop;
+
+    mainLoop_count++;
+
+    // tell the scheduler one tick has passed
+    scheduler.tick();
+
+    scheduler.run(19500U);
+}
+
+// update AHRS system
+static void ahrs_update()
+{
+    ahrs.set_armed(hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED);
+
+#if HIL_MODE != HIL_MODE_DISABLED
+    // update hil before AHRS update
+    gcs_update();
+#endif
+
+    // when in reverse we need to tell AHRS not to assume we are a
+    // 'fly forward' vehicle, otherwise it will see a large
+    // discrepancy between the mag and the GPS heading and will try to
+    // correct for it, leading to a large yaw error
+    ahrs.set_fly_forward(!in_reverse);
+
+    ahrs.update();
+
+    // if using the EKF get a speed update now (from accelerometers)
+    Vector3f velocity;
+    if (ahrs.get_velocity_NED(velocity)) {
+        ground_speed = pythagorous2(velocity.x, velocity.y);
+    }
+
+    if (should_log(MASK_LOG_ATTITUDE_FAST))
+        Log_Write_Attitude();
+
+    if (should_log(MASK_LOG_IMU))
+        DataFlash.Log_Write_IMU(ins);
+}
+
+/*
+  update camera mount - 50Hz
+ */
+static void mount_update(void)
+{
+#if MOUNT == ENABLED
+	camera_mount.update_mount_position();
+#endif
+#if CAMERA == ENABLED
+    camera.trigger_pic_cleanup();
+#endif
+}
+
+static void update_alt()
+{
+    barometer.update();
+    if (should_log(MASK_LOG_IMU)) {
+        Log_Write_Baro();
+    }
+}
+
+/*
+  check for GCS failsafe - 10Hz
+ */
+static void gcs_failsafe_check(void)
+{
+	if (g.fs_gcs_enabled) {
+        failsafe_trigger(FAILSAFE_EVENT_GCS, last_heartbeat_ms != 0 && (millis() - last_heartbeat_ms) > 2000);
+    }
+}
+
+/*
+  if the compass is enabled then try to accumulate a reading
+ */
+static void compass_accumulate(void)
+{
+    if (g.compass_enabled) {
+        compass.accumulate();
+    }    
+}
+
+/*
+  check for new compass data - 10Hz
+ */
+static void update_compass(void)
+{
+    if (g.compass_enabled && compass.read()) {
+        ahrs.set_compass(&compass);
+        // update offsets
+        compass.learn_offsets();
+        if (should_log(MASK_LOG_COMPASS)) {
+            Log_Write_Compass();
+        }
+    } else {
+        ahrs.set_compass(NULL);
+    }
+}
+
+/*
+  log some key data - 10Hz
+ */
+static void update_logging1(void)
+{
+    if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST))
+        Log_Write_Attitude();
+    
+    if (should_log(MASK_LOG_CTUN))
+        Log_Write_Control_Tuning();
+
+    if (should_log(MASK_LOG_NTUN))
+        Log_Write_Nav_Tuning();
+}
+
+/*
+  log some key data - 10Hz
+ */
+static void update_logging2(void)
+{
+    if (should_log(MASK_LOG_STEERING)) {
+        if (control_mode == STEERING || control_mode == AUTO || control_mode == RTL || control_mode == GUIDED) {
+            Log_Write_Steering();
+        }
+    }
+
+    if (should_log(MASK_LOG_RC))
+        Log_Write_RC();
+}
+
+
+/*
+  update aux servo mappings
+ */
+static void update_aux(void)
+{
+    RC_Channel_aux::enable_aux_servos();
+        
+#if MOUNT == ENABLED
+    camera_mount.update_mount_type();
+#endif
+}
+
+/*
+  once a second events
+ */
+static void one_second_loop(void)
+{
+	if (should_log(MASK_LOG_CURRENT))
+		Log_Write_Current();
+	// send a heartbeat
+	gcs_send_message(MSG_HEARTBEAT);
+
+    // allow orientation change at runtime to aid config
+    ahrs.set_orientation();
+
+    set_control_channels();
+
+    // cope with changes to aux functions
+    update_aux();
+
+#if MOUNT == ENABLED
+    camera_mount.update_mount_type();
+#endif
+
+    // cope with changes to mavlink system ID
+    mavlink_system.sysid = g.sysid_this_mav;
+
+    static uint8_t counter;
+
+    counter++;
+
+    // write perf data every 20s
+    if (counter % 10 == 0) {
+        if (scheduler.debug() != 0) {
+            hal.console->printf_P(PSTR("G_Dt_max=%lu\n"), (unsigned long)G_Dt_max);
+        }
+        if (should_log(MASK_LOG_PM))
+            Log_Write_Performance();
+        G_Dt_max = 0;
+        resetPerfData();
+    }
+
+    // save compass offsets once a minute
+    if (counter >= 60) {				
+        if (g.compass_enabled) {
+            compass.save_offsets();
+        }
+        counter = 0;
+    }
+}
+
+static void update_GPS_50Hz(void)
+{        
+    static uint32_t last_gps_reading[GPS_MAX_INSTANCES];
+	gps.update();
+
+    for (uint8_t i=0; i<gps.num_sensors(); i++) {
+        if (gps.last_message_time_ms(i) != last_gps_reading[i]) {
+            last_gps_reading[i] = gps.last_message_time_ms(i);
+            if (should_log(MASK_LOG_GPS)) {
+                DataFlash.Log_Write_GPS(gps, i, current_loc.alt);
+            }
+        }
+    }
+}
+
+
+static void update_GPS_10Hz(void)
+{        
+    have_position = ahrs.get_position(current_loc);
+
+	if (have_position && gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
+
+		if (ground_start_count > 1){
+			ground_start_count--;
+
+		} else if (ground_start_count == 1) {
+			// We countdown N number of good GPS fixes
+			// so that the altitude is more accurate
+			// -------------------------------------
+			if (current_loc.lat == 0) {
+				ground_start_count = 20;
+			} else {
+                init_home();
+
+                // set system clock for log timestamps
+                hal.util->set_system_clock(gps.time_epoch_usec());
+
+				if (g.compass_enabled) {
+					// Set compass declination automatically
+					compass.set_initial_location(gps.location().lat, gps.location().lng);
+				}
+				ground_start_count = 0;
+			}
+		}
+        Vector3f velocity;
+        if (ahrs.get_velocity_NED(velocity)) {
+            ground_speed = pythagorous2(velocity.x, velocity.y);
+        } else {
+            ground_speed   = gps.ground_speed();
+        }
+
+#if CAMERA == ENABLED
+        if (camera.update_location(current_loc) == true) {
+            do_take_picture();
+        }
+#endif        
+	}
+}
+
+static void update_current_mode(void)
+{ 
+    switch (control_mode){
+    case AUTO:
+    case RTL:
+    case GUIDED:
+        set_reverse(false);
+        calc_lateral_acceleration();
+        calc_nav_steer();
+        calc_throttle(g.speed_cruise);
+        break;
+
+    case STEERING: {
+        /*
+          in steering mode we control lateral acceleration
+          directly. We first calculate the maximum lateral
+          acceleration at full steering lock for this speed. That is
+          V^2/R where R is the radius of turn. We get the radius of
+          turn from half the STEER2SRV_P.
+         */
+        float max_g_force = ground_speed * ground_speed / steerController.get_turn_radius();
+
+        // constrain to user set TURN_MAX_G
+        max_g_force = constrain_float(max_g_force, 0.1f, g.turn_max_g * GRAVITY_MSS);
+
+        lateral_acceleration = max_g_force * (channel_steer->pwm_to_angle()/4500.0f);
+        calc_nav_steer();
+
+        // and throttle gives speed in proportion to cruise speed, up
+        // to 50% throttle, then uses nudging above that.
+        float target_speed = channel_throttle->pwm_to_angle() * 0.01 * 2 * g.speed_cruise;
+        set_reverse(target_speed < 0);
+        if (in_reverse) {
+            target_speed = constrain_float(target_speed, -g.speed_cruise, 0);
+        } else {
+            target_speed = constrain_float(target_speed, 0, g.speed_cruise);
+        }
+        calc_throttle(target_speed);
+        break;
+    }
+
+    case LEARNING:
+    case MANUAL:
+        /*
+          in both MANUAL and LEARNING we pass through the
+          controls. Setting servo_out here actually doesn't matter, as
+          we set the exact value in set_servos(), but it helps for
+          logging
+         */
+        channel_throttle->servo_out = channel_throttle->control_in;
+        channel_steer->servo_out = channel_steer->pwm_to_angle();
+
+        // mark us as in_reverse when using a negative throttle to
+        // stop AHRS getting off
+        set_reverse(channel_throttle->servo_out < 0);
+        break;
+
+    case HOLD:
+        // hold position - stop motors and center steering
+        channel_throttle->servo_out = 0;
+        channel_steer->servo_out = 0;
+        set_reverse(false);
+        break;
+
+    case INITIALISING:
+        break;
+	}
+}
+
+static void update_navigation()
+{
+    switch (control_mode) {
+    case MANUAL:
+    case HOLD:
+    case LEARNING:
+    case STEERING:
+    case INITIALISING:
+        break;
+
+    case AUTO:
+		mission.update();
+        break;
+
+    case RTL:
+    case GUIDED:
+        // no loitering around the wp with the rover, goes direct to the wp position
+        calc_lateral_acceleration();
+        calc_nav_steer();
+        if (verify_RTL()) {  
+            channel_throttle->servo_out = g.throttle_min.get();
+            set_mode(HOLD);
+        }
+        break;
+	}
+}
+
+AP_HAL_MAIN();

APMrover2/GCS_Mavlink.pde

+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+// default sensors are present and healthy: gyro, accelerometer, rate_control, attitude_stabilization, yaw_position, altitude control, x/y position control, motor_control
+#define MAVLINK_SENSOR_PRESENT_DEFAULT (MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL | MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL | MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION | MAV_SYS_STATUS_SENSOR_YAW_POSITION | MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL | MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS | MAV_SYS_STATUS_AHRS)
+
+// use this to prevent recursion during sensor init
+static bool in_mavlink_delay;
+
+// true if we are out of time in our event timeslice
+static bool	gcs_out_of_time;
+
+// check if a message will fit in the payload space available
+#define CHECK_PAYLOAD_SIZE(id) if (txspace < MAVLINK_NUM_NON_PAYLOAD_BYTES+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)
+{
+    uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
+    uint8_t system_status = MAV_STATE_ACTIVE;
+    uint32_t custom_mode = control_mode;
+    
+    if (failsafe.triggered != 0) {
+        system_status = MAV_STATE_CRITICAL;
+    }
+
+    // work out the base_mode. This value is not very useful
+    // for APM, but we calculate it as best we can so a generic
+    // MAVLink enabled ground station can work out something about
+    // what the MAV is up to. The actual bit values are highly
+    // ambiguous for most of the APM flight modes. In practice, you
+    // only get useful information from the custom_mode, which maps to
+    // the APM flight mode and has a well defined meaning in the
+    // ArduPlane documentation
+    switch (control_mode) {
+    case MANUAL:
+    case LEARNING:
+    case STEERING:
+        base_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
+        break;
+    case AUTO:
+    case RTL:
+    case GUIDED:
+        base_mode = MAV_MODE_FLAG_GUIDED_ENABLED;
+        // note that MAV_MODE_FLAG_AUTO_ENABLED does not match what
+        // APM does in any mode, as that is defined as "system finds its own goal
+        // positions", which APM does not currently do
+        break;
+    case INITIALISING:
+        system_status = MAV_STATE_CALIBRATING;
+        break;
+    case HOLD:
+        system_status = 0;
+        break;
+    }
+
+#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 && ahrs.get_armed()) {
+        base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
+    }
+
+    // indicate we have set a custom mode
+    base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
+
+    mavlink_msg_heartbeat_send(
+        chan,
+        MAV_TYPE_GROUND_ROVER,
+        MAV_AUTOPILOT_ARDUPILOTMEGA,
+        base_mode,
+        custom_mode,
+        system_status);
+}
+
+static NOINLINE void send_attitude(mavlink_channel_t chan)
+{
+    Vector3f omega = ahrs.get_gyro();
+    mavlink_msg_attitude_send(
+        chan,
+        millis(),
+        ahrs.roll,
+        ahrs.pitch,
+        ahrs.yaw,
+        omega.x,
+        omega.y,
+        omega.z);
+}
+
+static NOINLINE void send_extended_status1(mavlink_channel_t chan)
+{
+    uint32_t control_sensors_present;
+    uint32_t control_sensors_enabled;
+    uint32_t control_sensors_health;
+
+    // default sensors present
+    control_sensors_present = MAVLINK_SENSOR_PRESENT_DEFAULT;
+
+    // first what sensors/controllers we have
+    if (g.compass_enabled) {
+        control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG; // compass present
+    }
+    if (gps.status() > AP_GPS::NO_GPS) {
+        control_sensors_present |= MAV_SYS_STATUS_SENSOR_GPS;
+    }
+
+    // all present sensors enabled by default except rate control, attitude stabilization, yaw, altitude, position control and motor output which we will set individually
+    control_sensors_enabled = control_sensors_present & (~MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL & ~MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION & ~MAV_SYS_STATUS_SENSOR_YAW_POSITION & ~MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL & ~MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS);
+
+    switch (control_mode) {
+    case MANUAL:
+    case HOLD:
+        break;
+
+    case LEARNING:
+    case STEERING:
+        control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control
+        control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION; // attitude stabilisation
+        break;
+
+    case AUTO:
+    case RTL:
+    case GUIDED:
+        control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control
+        control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION; // attitude stabilisation
+        control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_YAW_POSITION; // yaw position
+        control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; // X/Y position control
+        break;
+
+    case INITIALISING:
+        break;
+    }
+
+    // set motors outputs as enabled if safety switch is not disarmed (i.e. either NONE or ARMED)
+    if (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED) {
+        control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS;
+    }
+
+    // default to all healthy except compass and gps which we set individually
+    control_sensors_health = control_sensors_present & (~MAV_SYS_STATUS_SENSOR_3D_MAG & ~MAV_SYS_STATUS_SENSOR_GPS);
+    if (g.compass_enabled && compass.healthy(0) && ahrs.use_compass()) {
+        control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG;
+    }
+    if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
+        control_sensors_health |= MAV_SYS_STATUS_SENSOR_GPS;
+    }
+    if (!ins.get_gyro_health_all() || (!g.skip_gyro_cal && !ins.gyro_calibrated_ok_all())) {
+        control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_3D_GYRO;
+    }
+    if (!ins.get_accel_health_all()) {
+        control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_3D_ACCEL;
+    }
+
+    if (ahrs.initialised() && !ahrs.healthy()) {
+        // AHRS subsystem is unhealthy
+        control_sensors_health &= ~MAV_SYS_STATUS_AHRS;
+    }
+
+    int16_t battery_current = -1;
+    int8_t battery_remaining = -1;
+
+    if (battery.has_current()) {
+        battery_remaining = battery.capacity_remaining_pct();
+        battery_current = battery.current_amps() * 100;
+    }
+
+    mavlink_msg_sys_status_send(
+        chan,
+        control_sensors_present,
+        control_sensors_enabled,
+        control_sensors_health,
+        (uint16_t)(scheduler.load_average(20000) * 1000),
+        battery.voltage() * 1000, // mV
+        battery_current,        // in 10mA units
+        battery_remaining,      // in %
+        0, // comm drops %,
+        0, // comm drops in pkts,
+        0, 0, 0, 0);
+
+}
+
+static void NOINLINE send_location(mavlink_channel_t chan)
+{
+    uint32_t fix_time;
+    // if we have a GPS fix, take the time as the last fix time. That
+    // allows us to correctly calculate velocities and extrapolate
+    // positions.
+    // If we don't have a GPS fix then we are dead reckoning, and will
+    // use the current boot time as the fix time.    
+    if (gps.status() >= AP_GPS::GPS_OK_FIX_2D) {
+        fix_time = gps.last_fix_time_ms();
+    } else {
+        fix_time = millis();
+    }
+    const Vector3f &vel = gps.velocity();
+    mavlink_msg_global_position_int_send(
+        chan,
+        fix_time,
+        current_loc.lat,                   // in 1E7 degrees
+        current_loc.lng,                   // in 1E7 degrees
+        gps.location().alt * 10UL,         // millimeters above sea level
+        (current_loc.alt - home.alt) * 10, // millimeters above ground
+        vel.x * 100,  // X speed cm/s (+ve North)
+        vel.y * 100,  // Y speed cm/s (+ve East)
+        vel.z * -100, // Z speed cm/s (+ve up)
+        ahrs.yaw_sensor);
+}
+
+static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
+{
+    mavlink_msg_nav_controller_output_send(
+        chan,
+        lateral_acceleration, // use nav_roll to hold demanded Y accel
+        gps.ground_speed() * ins.get_gyro().z, // use nav_pitch to hold actual Y accel
+        nav_controller->nav_bearing_cd() * 0.01f,
+        nav_controller->target_bearing_cd() * 0.01f,
+        wp_distance,
+        0,
+        groundspeed_error,
+        nav_controller->crosstrack_error());
+}
+
+#if HIL_MODE != HIL_MODE_DISABLED
+static void NOINLINE send_servo_out(mavlink_channel_t chan)
+{
+    // normalized values scaled to -10000 to 10000
+    // This is used for HIL.  Do not change without discussing with
+    // HIL maintainers
+    mavlink_msg_rc_channels_scaled_send(
+        chan,
+        millis(),
+        0, // port 0
+        10000 * channel_steer->norm_output(),
+        0,
+        10000 * channel_throttle->norm_output(),
+        0,
+        0,
+        0,
+        0,
+        0,
+        receiver_rssi);
+}
+#endif
+
+static void NOINLINE send_radio_out(mavlink_channel_t chan)
+{
+#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS
+    mavlink_msg_servo_output_raw_send(
+        chan,
+        micros(),
+        0,     // port
+        hal.rcout->read(0),
+        hal.rcout->read(1),
+        hal.rcout->read(2),
+        hal.rcout->read(3),
+        hal.rcout->read(4),
+        hal.rcout->read(5),
+        hal.rcout->read(6),
+        hal.rcout->read(7));
+#else
+    mavlink_msg_servo_output_raw_send(
+        chan,
+        micros(),
+        0,     // port
+        RC_Channel::rc_channel(0)->radio_out,
+        RC_Channel::rc_channel(1)->radio_out,
+        RC_Channel::rc_channel(2)->radio_out,
+        RC_Channel::rc_channel(3)->radio_out,
+        RC_Channel::rc_channel(4)->radio_out,
+        RC_Channel::rc_channel(5)->radio_out,
+        RC_Channel::rc_channel(6)->radio_out,
+        RC_Channel::rc_channel(7)->radio_out);
+#endif
+}
+
+static void NOINLINE send_vfr_hud(mavlink_channel_t chan)
+{
+    mavlink_msg_vfr_hud_send(
+        chan,
+        gps.ground_speed(),
+        gps.ground_speed(),
+        (ahrs.yaw_sensor / 100) % 360,
+        (uint16_t)(100 * fabsf(channel_throttle->norm_output())),
+        current_loc.alt / 100.0,
+        0);
+}
+
+// report simulator state
+static void NOINLINE send_simstate(mavlink_channel_t chan)
+{
+#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
+    sitl.simstate_send(chan);
+#endif
+}
+
+static void NOINLINE send_hwstatus(mavlink_channel_t chan)
+{
+    mavlink_msg_hwstatus_send(
+        chan,
+        hal.analogin->board_voltage()*1000,
+        hal.i2c->lockup_count());
+}
+
+static void NOINLINE send_rangefinder(mavlink_channel_t chan)
+{
+    if (!sonar.healthy()) {
+        // no sonar to report
+        return;
+    }
+
+    /*
+      report smaller distance of two sonars if more than one enabled
+     */
+    float distance_cm, voltage;
+    if (!sonar.healthy(1)) {
+        distance_cm = sonar.distance_cm(0);
+        voltage = sonar.voltage_mv(0) * 0.001f;
+    } else {
+        float dist1 = sonar.distance_cm(0);
+        float dist2 = sonar.distance_cm(1);
+        if (dist1 <= dist2) {
+            distance_cm = dist1;
+            voltage = sonar.voltage_mv(0) * 0.001f;
+        } else {
+            distance_cm = dist2;
+            voltage = sonar.voltage_mv(1) * 0.001f;
+        }
+    }
+    mavlink_msg_rangefinder_send(
+        chan,
+        distance_cm * 0.01f,
+        voltage);
+}
+
+static void NOINLINE send_current_waypoint(mavlink_channel_t chan)
+{
+    mavlink_msg_mission_current_send(chan, mission.get_current_nav_index());
+}
+
+static void NOINLINE send_statustext(mavlink_channel_t chan)
+{
+    mavlink_statustext_t *s = &gcs[chan-MAVLINK_COMM_0].pending_status;
+    mavlink_msg_statustext_send(
+        chan,
+        s->severity,
+        s->text);
+}
+
+// are we still delaying telemetry to try to avoid Xbee bricking?
+static bool telemetry_delayed(mavlink_channel_t chan)
+{
+    uint32_t tnow = millis() >> 10;
+    if (tnow > (uint32_t)g.telem_delay) {
+        return false;
+    }
+    if (chan == MAVLINK_COMM_0 && hal.gpio->usb_connected()) {
+        // this is USB telemetry, so won't be an Xbee
+        return false;
+    }
+    // we're either on the 2nd UART, or no USB cable is connected
+    // we need to delay telemetry by the TELEM_DELAY time
+    return true;
+}
+
+
+// try to send a message, return false if it won't fit in the serial tx buffer
+bool GCS_MAVLINK::try_send_message(enum ap_message id)
+{
+    uint16_t txspace = comm_get_txspace(chan);
+
+    if (telemetry_delayed(chan)) {
+        return false;
+    }
+
+    // if we don't have at least 1ms remaining before the main loop
+    // wants to fire then don't send a mavlink message. We want to
+    // prioritise the main flight control loop over communications
+    if (!in_mavlink_delay && scheduler.time_available_usec() < 1200) {
+        gcs_out_of_time = true;
+        return false;
+    }
+
+    switch (id) {
+    case MSG_HEARTBEAT:
+        CHECK_PAYLOAD_SIZE(HEARTBEAT);
+        gcs[chan-MAVLINK_COMM_0].last_heartbeat_time = hal.scheduler->millis();        
+        send_heartbeat(chan);
+        return true;
+
+    case MSG_EXTENDED_STATUS1:
+        CHECK_PAYLOAD_SIZE(SYS_STATUS);
+        send_extended_status1(chan);
+        CHECK_PAYLOAD_SIZE(POWER_STATUS);
+        gcs[chan-MAVLINK_COMM_0].send_power_status();
+        break;
+
+    case MSG_EXTENDED_STATUS2:
+        CHECK_PAYLOAD_SIZE(MEMINFO);
+        gcs[chan-MAVLINK_COMM_0].send_meminfo();
+        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:
+        CHECK_PAYLOAD_SIZE(GPS_RAW_INT);
+        gcs[chan-MAVLINK_COMM_0].send_gps_raw(gps);
+        break;
+
+    case MSG_SYSTEM_TIME:
+        CHECK_PAYLOAD_SIZE(SYSTEM_TIME);
+        gcs[chan-MAVLINK_COMM_0].send_system_time(gps);
+        break;
+
+    case MSG_SERVO_OUT:
+#if HIL_MODE != HIL_MODE_DISABLED
+        CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED);
+        send_servo_out(chan);
+#endif
+        break;
+
+    case MSG_RADIO_IN:
+        CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW);
+        gcs[chan-MAVLINK_COMM_0].send_radio_in(receiver_rssi);
+        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;
+
+    case MSG_RAW_IMU1:
+        CHECK_PAYLOAD_SIZE(RAW_IMU);
+        gcs[chan-MAVLINK_COMM_0].send_raw_imu(ins, compass);
+        break;
+
+    case MSG_RAW_IMU3:
+        CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS);
+        gcs[chan-MAVLINK_COMM_0].send_sensor_offsets(ins, compass, barometer);
+        break;
+
+    case MSG_CURRENT_WAYPOINT:
+        CHECK_PAYLOAD_SIZE(MISSION_CURRENT);
+        send_current_waypoint(chan);
+        break;
+
+    case MSG_NEXT_PARAM:
+        CHECK_PAYLOAD_SIZE(PARAM_VALUE);
+        gcs[chan-MAVLINK_COMM_0].queued_param_send();
+        break;
+
+    case MSG_NEXT_WAYPOINT:
+        CHECK_PAYLOAD_SIZE(MISSION_REQUEST);
+        gcs[chan-MAVLINK_COMM_0].queued_waypoint_send();
+        break;
+
+    case MSG_STATUSTEXT:
+        CHECK_PAYLOAD_SIZE(STATUSTEXT);
+        send_statustext(chan);
+        break;
+
+    case MSG_AHRS:
+        CHECK_PAYLOAD_SIZE(AHRS);
+        gcs[chan-MAVLINK_COMM_0].send_ahrs(ahrs);
+        break;
+
+    case MSG_SIMSTATE:
+        CHECK_PAYLOAD_SIZE(SIMSTATE);
+        send_simstate(chan);
+        break;
+
+    case MSG_HWSTATUS:
+        CHECK_PAYLOAD_SIZE(HWSTATUS);
+        send_hwstatus(chan);
+        break;
+
+    case MSG_RANGEFINDER:
+        CHECK_PAYLOAD_SIZE(RANGEFINDER);
+        send_rangefinder(chan);
+        break;
+
+    case MSG_MOUNT_STATUS:
+#if MOUNT == ENABLED
+        CHECK_PAYLOAD_SIZE(MOUNT_STATUS);
+        camera_mount.status_msg(chan);
+#endif // MOUNT == ENABLED
+        break;
+
+    case MSG_RAW_IMU2:
+    case MSG_LIMITS_STATUS:
+    case MSG_FENCE_STATUS:
+    case MSG_WIND:
+        // unused
+        break;
+
+    case MSG_BATTERY2:
+        CHECK_PAYLOAD_SIZE(BATTERY2);
+        gcs[chan-MAVLINK_COMM_0].send_battery2(battery);
+        break;
+
+    case MSG_CAMERA_FEEDBACK:
+#if CAMERA == ENABLED
+        CHECK_PAYLOAD_SIZE(CAMERA_FEEDBACK);
+        camera.send_feedback(chan, gps, ahrs, current_loc);
+#endif
+        break;
+
+    case MSG_RETRY_DEFERRED:
+    case MSG_TERRAIN:
+    case MSG_OPTICAL_FLOW:
+        break; // just here to prevent a warning
+	}
+
+    
+    return true;
+}
+
+/*
+  default stream rates to 1Hz
+ */
+const AP_Param::GroupInfo GCS_MAVLINK::var_info[] PROGMEM = {
+    // @Param: RAW_SENS
+    // @DisplayName: Raw sensor stream rate
+    // @Description: Raw sensor stream rate to ground station
+    // @Units: Hz
+    // @Range: 0 10
+    // @Increment: 1
+    // @User: Advanced
+    AP_GROUPINFO("RAW_SENS", 0, GCS_MAVLINK, streamRates[0],  1),
+
+    // @Param: EXT_STAT
+    // @DisplayName: Extended status stream rate to ground station
+    // @Description: Extended status stream rate to ground station
+    // @Units: Hz
+    // @Range: 0 10
+    // @Increment: 1
+    // @User: Advanced
+    AP_GROUPINFO("EXT_STAT", 1, GCS_MAVLINK, streamRates[1],  1),
+
+    // @Param: RC_CHAN
+    // @DisplayName: RC Channel stream rate to ground station
+    // @Description: RC Channel stream rate to ground station
+    // @Units: Hz
+    // @Range: 0 10
+    // @Increment: 1
+    // @User: Advanced
+    AP_GROUPINFO("RC_CHAN",  2, GCS_MAVLINK, streamRates[2],  1),
+
+    // @Param: RAW_CTRL
+    // @DisplayName: Raw Control stream rate to ground station
+    // @Description: Raw Control stream rate to ground station
+    // @Units: Hz
+    // @Range: 0 10
+    // @Increment: 1
+    // @User: Advanced
+    AP_GROUPINFO("RAW_CTRL", 3, GCS_MAVLINK, streamRates[3],  1),
+
+    // @Param: POSITION
+    // @DisplayName: Position stream rate to ground station
+    // @Description: Position stream rate to ground station
+    // @Units: Hz
+    // @Range: 0 10
+    // @Increment: 1
+    // @User: Advanced
+    AP_GROUPINFO("POSITION", 4, GCS_MAVLINK, streamRates[4],  1),
+
+    // @Param: EXTRA1
+    // @DisplayName: Extra data type 1 stream rate to ground station
+    // @Description: Extra data type 1 stream rate to ground station
+    // @Units: Hz
+    // @Range: 0 10
+    // @Increment: 1
+    // @User: Advanced
+    AP_GROUPINFO("EXTRA1",   5, GCS_MAVLINK, streamRates[5],  1),
+
+    // @Param: EXTRA2
+    // @DisplayName: Extra data type 2 stream rate to ground station
+    // @Description: Extra data type 2 stream rate to ground station
+    // @Units: Hz
+    // @Range: 0 10
+    // @Increment: 1
+    // @User: Advanced
+    AP_GROUPINFO("EXTRA2",   6, GCS_MAVLINK, streamRates[6],  1),
+
+    // @Param: EXTRA3
+    // @DisplayName: Extra data type 3 stream rate to ground station
+    // @Description: Extra data type 3 stream rate to ground station
+    // @Units: Hz
+    // @Range: 0 10
+    // @Increment: 1
+    // @User: Advanced
+    AP_GROUPINFO("EXTRA3",   7, GCS_MAVLINK, streamRates[7],  1),
+
+    // @Param: PARAMS
+    // @DisplayName: Parameter stream rate to ground station
+    // @Description: Parameter stream rate to ground station
+    // @Units: Hz
+    // @Range: 0 10
+    // @Increment: 1
+    // @User: Advanced
+    AP_GROUPINFO("PARAMS",   8, GCS_MAVLINK, streamRates[8],  10),
+    AP_GROUPEND
+};
+
+
+// see if we should send a stream now. Called at 50Hz
+bool GCS_MAVLINK::stream_trigger(enum streams stream_num)
+{
+    if (stream_num >= NUM_STREAMS) {
+        return false;
+    }
+    float rate = (uint8_t)streamRates[stream_num].get();
+
+    // send at a much lower rate while handling waypoints and
+    // parameter sends
+    if ((stream_num != STREAM_PARAMS) && 
+        (waypoint_receiving || _queued_parameter != NULL)) {
+        rate *= 0.25;
+    }
+
+    if (rate <= 0) {
+        return false;
+    }
+
+    if (stream_ticks[stream_num] == 0) {
+        // we're triggering now, setup the next trigger point
+        if (rate > 50) {
+            rate = 50;
+        }
+        stream_ticks[stream_num] = (50 / rate) + stream_slowdown;
+        return true;
+    }
+
+    // count down at 50Hz
+    stream_ticks[stream_num]--;
+    return false;
+}
+
+void
+GCS_MAVLINK::data_stream_send(void)
+{
+    gcs_out_of_time = false;
+
+    if (!in_mavlink_delay) {
+        handle_log_send(DataFlash);
+    }
+
+    if (_queued_parameter != NULL) {
+        if (streamRates[STREAM_PARAMS].get() <= 0) {
+            streamRates[STREAM_PARAMS].set(10);
+        }
+        if (stream_trigger(STREAM_PARAMS)) {
+            send_message(MSG_NEXT_PARAM);
+        }
+    }
+
+    if (gcs_out_of_time) return;
+
+    if (in_mavlink_delay) {
+#if HIL_MODE != HIL_MODE_DISABLED
+        // in HIL we need to keep sending servo values to ensure
+        // the simulator doesn't pause, otherwise our sensor
+        // calibration could stall
+        if (stream_trigger(STREAM_RAW_CONTROLLER)) {
+            send_message(MSG_SERVO_OUT);
+        }
+        if (stream_trigger(STREAM_RC_CHANNELS)) {
+            send_message(MSG_RADIO_OUT);
+        }
+#endif
+        // don't send any other stream types while in the delay callback
+        return;
+    }
+
+    if (gcs_out_of_time) return;
+
+    if (stream_trigger(STREAM_RAW_SENSORS)) {
+        send_message(MSG_RAW_IMU1);
+        send_message(MSG_RAW_IMU3);
+    }
+
+    if (gcs_out_of_time) return;
+
+    if (stream_trigger(STREAM_EXTENDED_STATUS)) {
+        send_message(MSG_EXTENDED_STATUS1);
+        send_message(MSG_EXTENDED_STATUS2);
+        send_message(MSG_CURRENT_WAYPOINT);
+        send_message(MSG_GPS_RAW);            // TODO - remove this message after location message is working
+        send_message(MSG_NAV_CONTROLLER_OUTPUT);
+    }
+
+    if (gcs_out_of_time) return;
+
+    if (stream_trigger(STREAM_POSITION)) {
+        // sent with GPS read
+        send_message(MSG_LOCATION);
+    }
+
+    if (gcs_out_of_time) return;
+
+    if (stream_trigger(STREAM_RAW_CONTROLLER)) {
+        send_message(MSG_SERVO_OUT);
+    }
+
+    if (gcs_out_of_time) return;
+
+    if (stream_trigger(STREAM_RC_CHANNELS)) {
+        send_message(MSG_RADIO_OUT);
+        send_message(MSG_RADIO_IN);
+    }
+
+    if (gcs_out_of_time) return;
+
+    if (stream_trigger(STREAM_EXTRA1)) {
+        send_message(MSG_ATTITUDE);
+        send_message(MSG_SIMSTATE);
+    }
+
+    if (gcs_out_of_time) return;
+
+    if (stream_trigger(STREAM_EXTRA2)) {
+        send_message(MSG_VFR_HUD);
+    }
+
+    if (gcs_out_of_time) return;
+
+    if (stream_trigger(STREAM_EXTRA3)) {
+        send_message(MSG_AHRS);
+        send_message(MSG_HWSTATUS);
+        send_message(MSG_RANGEFINDER);
+        send_message(MSG_SYSTEM_TIME);
+        send_message(MSG_BATTERY2);
+        send_message(MSG_MOUNT_STATUS);
+    }
+}
+
+
+
+void GCS_MAVLINK::handle_guided_request(AP_Mission::Mission_Command &cmd)
+{
+    guided_WP = cmd.content.location;
+
+    set_mode(GUIDED);
+
+    // make any new wp uploaded instant (in case we are already in Guided mode)
+    set_guided_WP();
+}
+
+void GCS_MAVLINK::handle_change_alt_request(AP_Mission::Mission_Command &cmd)
+{
+    // nothing to do
+}
+
+void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
+{
+    switch (msg->msgid) {
+
+    case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
+        {
+            handle_request_data_stream(msg, true);
+            break;
+        }
+
+    case MAVLINK_MSG_ID_COMMAND_LONG:
+        {
+            // decode
+            mavlink_command_long_t packet;
+            mavlink_msg_command_long_decode(msg, &packet);
+
+            uint8_t result = MAV_RESULT_UNSUPPORTED;
+
+            // do command
+            send_text_P(SEVERITY_LOW,PSTR("command received: "));
+
+            switch(packet.command) {
+
+            case MAV_CMD_NAV_RETURN_TO_LAUNCH:
+                set_mode(RTL);
+                result = MAV_RESULT_ACCEPTED;
+                break;
+
+#if MOUNT == ENABLED
+            // Sets the region of interest (ROI) for the camera
+            case MAV_CMD_DO_SET_ROI:
+                Location roi_loc;
+                roi_loc.lat = (int32_t)(packet.param5 * 1.0e7f);
+                roi_loc.lng = (int32_t)(packet.param6 * 1.0e7f);
+                roi_loc.alt = (int32_t)(packet.param7 * 100.0f);
+                if (roi_loc.lat == 0 && roi_loc.lng == 0 && roi_loc.alt == 0) {
+                    // switch off the camera tracking if enabled
+                    if (camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) {
+                        camera_mount.set_mode_to_default();
+                    }
+                } else {
+                    // send the command to the camera mount
+                    camera_mount.set_roi_cmd(&roi_loc);
+                }
+                result = MAV_RESULT_ACCEPTED;
+                break;
+#endif
+
+            case MAV_CMD_MISSION_START:
+                set_mode(AUTO);
+                result = MAV_RESULT_ACCEPTED;
+                break;
+
+            case MAV_CMD_PREFLIGHT_CALIBRATION:
+                if ((packet.param1 == 1 ||
+                     packet.param2 == 1) &&
+                    packet.param3 == 0) {
+                    startup_INS_ground(true);
+                    result = MAV_RESULT_ACCEPTED;
+                } else if (packet.param4 == 1) {
+                    trim_radio();
+                    result = MAV_RESULT_ACCEPTED;
+                } else {
+                    send_text_P(SEVERITY_LOW, PSTR("Unsupported preflight calibration"));
+                }
+                break;
+
+            case MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
+                if (packet.param1 == 2) {
+                    // save first compass's offsets
+                    compass.set_and_save_offsets(0, packet.param2, packet.param3, packet.param4);
+                    result = MAV_RESULT_ACCEPTED;
+                }
+                if (packet.param1 == 5) {
+                    // save secondary compass's offsets
+                    compass.set_and_save_offsets(1, packet.param2, packet.param3, packet.param4);
+                    result = MAV_RESULT_ACCEPTED;
+                }
+                break;
+
+        case MAV_CMD_DO_SET_MODE:
+            switch ((uint16_t)packet.param1) {
+            case MAV_MODE_MANUAL_ARMED:
+            case MAV_MODE_MANUAL_DISARMED:
+                set_mode(MANUAL);
+                result = MAV_RESULT_ACCEPTED;
+                break;
+
+            case MAV_MODE_AUTO_ARMED:
+            case MAV_MODE_AUTO_DISARMED:
+                set_mode(AUTO);
+                result = MAV_RESULT_ACCEPTED;
+                break;
+
+            case MAV_MODE_STABILIZE_DISARMED:
+            case MAV_MODE_STABILIZE_ARMED:
+                set_mode(LEARNING);
+                result = MAV_RESULT_ACCEPTED;
+                break;
+
+            default:
+                result = MAV_RESULT_UNSUPPORTED;
+            }
+            break;
+
+        case MAV_CMD_DO_SET_SERVO:
+            if (ServoRelayEvents.do_set_servo(packet.param1, packet.param2)) {
+                result = MAV_RESULT_ACCEPTED;
+            }
+            break;
+
+        case MAV_CMD_DO_REPEAT_SERVO:
+            if (ServoRelayEvents.do_repeat_servo(packet.param1, packet.param2, packet.param3, packet.param4*1000)) {
+                result = MAV_RESULT_ACCEPTED;
+            }
+            break;
+
+        case MAV_CMD_DO_SET_RELAY:
+            if (ServoRelayEvents.do_set_relay(packet.param1, packet.param2)) {
+                result = MAV_RESULT_ACCEPTED;
+            }
+            break;
+
+        case MAV_CMD_DO_REPEAT_RELAY:
+            if (ServoRelayEvents.do_repeat_relay(packet.param1, packet.param2, packet.param3*1000)) {
+                result = MAV_RESULT_ACCEPTED;
+            }
+            break;
+
+        case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
+            if (packet.param1 == 1 || packet.param1 == 3) {
+                // when packet.param1 == 3 we reboot to hold in bootloader
+                hal.scheduler->reboot(packet.param1 == 3);
+                result = MAV_RESULT_ACCEPTED;
+            }
+            break;
+
+        default:
+                break;
+            }
+
+            mavlink_msg_command_ack_send_buf(
+                msg,
+                chan,
+                packet.command,
+                result);
+
+            break;
+        }
+
+
+    case MAVLINK_MSG_ID_SET_MODE:
+		{
+            handle_set_mode(msg, mavlink_set_mode);
+            break;
+        }
+
+    case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:
+        {
+            handle_mission_request_list(mission, msg);
+            break;
+        }
+
+
+	// XXX read a WP from EEPROM and send it to the GCS
+    case MAVLINK_MSG_ID_MISSION_REQUEST:
+    {
+        handle_mission_request(mission, msg);
+        break;
+    }
+
+
+    case MAVLINK_MSG_ID_MISSION_ACK:
+        {
+            // not used
+            break;
+        }
+
+    case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
+        {
+            // mark the firmware version in the tlog
+            send_text_P(SEVERITY_LOW, PSTR(FIRMWARE_STRING));
+
+#if defined(PX4_GIT_VERSION) && defined(NUTTX_GIT_VERSION)
+            send_text_P(SEVERITY_LOW, PSTR("PX4: " PX4_GIT_VERSION " NuttX: " NUTTX_GIT_VERSION));
+#endif
+            handle_param_request_list(msg);
+            break;
+        }
+
+    case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
+    {
+        handle_param_request_read(msg);
+        break;
+    }
+
+    case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:
+        {
+            handle_mission_clear_all(mission, msg);
+            break;
+        }
+
+    case MAVLINK_MSG_ID_MISSION_SET_CURRENT:
+        {
+            handle_mission_set_current(mission, msg);
+            break;
+        }
+
+    case MAVLINK_MSG_ID_MISSION_COUNT:
+        {
+            handle_mission_count(mission, msg);
+            break;
+        }
+
+    case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST:
+    {
+        handle_mission_write_partial_list(mission, msg);
+        break;
+    }
+
+	// XXX receive a WP from GCS and store in EEPROM
+    case MAVLINK_MSG_ID_MISSION_ITEM:
+        {
+            handle_mission_item(msg, mission);
+            break;
+        }
+
+
+    case MAVLINK_MSG_ID_PARAM_SET:
+        {
+            handle_param_set(msg, &DataFlash);
+            break;
+        }
+
+    case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE:
+    {
+        // allow override of RC channel values for HIL
+        // or for complete GCS control of switch position
+        // and RC PWM values.
+        if(msg->sysid != g.sysid_my_gcs) break;                         // Only accept control from our gcs
+        mavlink_rc_channels_override_t packet;
+        int16_t v[8];
+        mavlink_msg_rc_channels_override_decode(msg, &packet);
+
+        v[0] = packet.chan1_raw;
+        v[1] = packet.chan2_raw;
+        v[2] = packet.chan3_raw;
+        v[3] = packet.chan4_raw;
+        v[4] = packet.chan5_raw;
+        v[5] = packet.chan6_raw;
+        v[6] = packet.chan7_raw;
+        v[7] = packet.chan8_raw;
+
+        hal.rcin->set_overrides(v, 8);
+
+        failsafe.rc_override_timer = millis();
+        failsafe_trigger(FAILSAFE_EVENT_RC, false);
+        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;
+            last_heartbeat_ms = failsafe.rc_override_timer = millis();
+            failsafe_trigger(FAILSAFE_EVENT_GCS, false);
+            break;
+        }
+
+#if HIL_MODE != HIL_MODE_DISABLED
+	case MAVLINK_MSG_ID_HIL_STATE:
+		{
+			mavlink_hil_state_t packet;
+			mavlink_msg_hil_state_decode(msg, &packet);
+			
+            // set gps hil sensor
+            Location loc;
+            loc.lat = packet.lat;
+            loc.lng = packet.lon;
+            loc.alt = packet.alt/10;
+            Vector3f vel(packet.vx, packet.vy, packet.vz);
+            vel *= 0.01f;
+            
+            gps.setHIL(0, AP_GPS::GPS_OK_FIX_3D,
+                       packet.time_usec/1000,
+                       loc, vel, 10, 0, true);
+			
+			// rad/sec
+            Vector3f gyros;
+            gyros.x = packet.rollspeed;
+            gyros.y = packet.pitchspeed;
+            gyros.z = packet.yawspeed;
+
+            // m/s/s
+            Vector3f accels;
+            accels.x = packet.xacc * (GRAVITY_MSS/1000.0f);
+            accels.y = packet.yacc * (GRAVITY_MSS/1000.0f);
+            accels.z = packet.zacc * (GRAVITY_MSS/1000.0f);
+            
+            ins.set_gyro(0, gyros);
+
+            ins.set_accel(0, accels);
+            compass.setHIL(packet.roll, packet.pitch, packet.yaw);
+            break;
+		}
+#endif // HIL_MODE
+
+#if CAMERA == ENABLED
+    case MAVLINK_MSG_ID_DIGICAM_CONFIGURE:
+    {
+        camera.configure_msg(msg);
+        break;
+    }
+
+    case MAVLINK_MSG_ID_DIGICAM_CONTROL:
+    {
+        camera.control_msg(msg);
+        break;
+    }
+#endif // CAMERA == ENABLED
+
+#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;
+		}
+#endif // MOUNT == ENABLED
+
+    case MAVLINK_MSG_ID_RADIO:
+    case MAVLINK_MSG_ID_RADIO_STATUS:
+        {
+            handle_radio_status(msg, DataFlash, should_log(MASK_LOG_PM));
+            break;
+        }
+
+    case MAVLINK_MSG_ID_LOG_REQUEST_DATA:
+    case MAVLINK_MSG_ID_LOG_ERASE:
+        in_log_download = true;
+        // fallthru
+    case MAVLINK_MSG_ID_LOG_REQUEST_LIST:
+        if (!in_mavlink_delay) {
+            handle_log_message(msg, DataFlash);
+        }
+        break;
+    case MAVLINK_MSG_ID_LOG_REQUEST_END:
+        in_log_download = false;
+        if (!in_mavlink_delay) {
+            handle_log_message(msg, DataFlash);
+        }
+        break;
+
+#if HAL_CPU_CLASS > HAL_CPU_CLASS_16
+    case MAVLINK_MSG_ID_SERIAL_CONTROL:
+        handle_serial_control(msg, gps);
+        break;
+#endif
+
+    } // end switch
+} // end handle mavlink
+
+/*
+ *  a delay() callback that processes MAVLink packets. We set this as the
+ *  callback in long running library initialisation routines to allow
+ *  MAVLink to process packets while waiting for the initialisation to
+ *  complete
+ */
+static void mavlink_delay_cb()
+{
+    static uint32_t last_1hz, last_50hz, last_5s;
+    if (!gcs[0].initialised || in_mavlink_delay) return;
+
+    in_mavlink_delay = true;
+
+    uint32_t tnow = millis();
+    if (tnow - last_1hz > 1000) {
+        last_1hz = tnow;
+        gcs_send_message(MSG_HEARTBEAT);
+        gcs_send_message(MSG_EXTENDED_STATUS1);
+    }
+    if (tnow - last_50hz > 20) {
+        last_50hz = tnow;
+        gcs_update();
+        gcs_data_stream_send();
+        notify.update();
+    }
+    if (tnow - last_5s > 5000) {
+        last_5s = tnow;
+        gcs_send_text_P(SEVERITY_LOW, PSTR("Initialising APM..."));
+    }
+    check_usb_mux();
+
+    in_mavlink_delay = false;
+}
+
+/*
+ *  send a message on both GCS links
+ */
+static void gcs_send_message(enum ap_message id)
+{
+    for (uint8_t i=0; i<num_gcs; i++) {
+        if (gcs[i].initialised) {
+            gcs[i].send_message(id);
+        }
+    }
+}
+
+/*
+ *  send data streams in the given rate range on both links
+ */
+static void gcs_data_stream_send(void)
+{
+    for (uint8_t i=0; i<num_gcs; i++) {
+        if (gcs[i].initialised) {
+            gcs[i].data_stream_send();
+        }
+    }
+}
+
+/*
+ *  look for incoming commands on the GCS links
+ */
+static void gcs_update(void)
+{
+    for (uint8_t i=0; i<num_gcs; i++) {
+        if (gcs[i].initialised) {
+#if CLI_ENABLED == ENABLED
+            gcs[i].update(run_cli);
+#else
+            gcs[i].update(NULL);
+#endif
+        }
+    }
+}
+
+static void gcs_send_text_P(gcs_severity severity, const prog_char_t *str)
+{
+    for (uint8_t i=0; i<num_gcs; i++) {
+        if (gcs[i].initialised) {
+            gcs[i].send_text_P(severity, str);
+    }
+    }
+#if LOGGING_ENABLED == ENABLED
+    DataFlash.Log_Write_Message_P(str);
+#endif
+}
+
+/*
+ *  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
+ */
+void gcs_send_text_fmt(const prog_char_t *fmt, ...)
+{
+    va_list arg_list;
+    gcs[0].pending_status.severity = (uint8_t)SEVERITY_LOW;
+    va_start(arg_list, fmt);
+    hal.util->vsnprintf_P((char *)gcs[0].pending_status.text,
+            sizeof(gcs[0].pending_status.text), fmt, arg_list);
+    va_end(arg_list);
+#if LOGGING_ENABLED == ENABLED
+    DataFlash.Log_Write_Message(gcs[0].pending_status.text);
+#endif
+    gcs[0].send_message(MSG_STATUSTEXT);
+    for (uint8_t i=1; i<num_gcs; i++) {
+        if (gcs[i].initialised) {
+            gcs[i].pending_status = gcs[0].pending_status;
+            gcs[i].send_message(MSG_STATUSTEXT);
+        }
+    }
+}
+
+
+/**
+   retry any deferred messages
+ */
+static void gcs_retry_deferred(void)
+{
+    gcs_send_message(MSG_RETRY_DEFERRED);
+}

APMrover2/Log.pde

+// -*- 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
+
+// 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);
+
+// 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)
+{
+	cliSerial->printf_P(PSTR("logs enabled: "));
+
+	if (0 == g.log_bitmask) {
+		cliSerial->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) cliSerial->printf_P(PSTR(" %S"), PSTR(#_s))
+		PLOG(ATTITUDE_FAST);
+		PLOG(ATTITUDE_MED);
+		PLOG(GPS);
+		PLOG(PM);
+		PLOG(CTUN);
+		PLOG(NTUN);
+		PLOG(MODE);
+		PLOG(IMU);
+		PLOG(CMD);
+		PLOG(CURRENT);
+		PLOG(SONAR);
+		PLOG(COMPASS);
+		PLOG(CAMERA);
+		PLOG(STEERING);
+		#undef PLOG
+	}
+
+	cliSerial->println();
+
+    DataFlash.ListAvailableLogs(cliSerial);
+	return(true);
+}
+
+static int8_t
+dump_log(uint8_t argc, const Menu::arg *argv)
+{
+    int16_t dump_log;
+    uint16_t dump_log_start;
+    uint16_t dump_log_end;
+    uint16_t 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) {
+        DataFlash.DumpPageInfo(cliSerial);
+        return(-1);
+    } else if (dump_log <= 0) {
+        cliSerial->printf_P(PSTR("dumping all\n"));
+        Log_Read(0, 1, 0);
+        return(-1);
+    } else if ((argc != 2)
+               || ((uint16_t)dump_log > last_log_num))
+    {
+        cliSerial->printf_P(PSTR("bad log number\n"));
+        return(-1);
+    }
+
+    DataFlash.get_log_boundaries(dump_log, dump_log_start, dump_log_end);
+    Log_Read((uint16_t)dump_log, dump_log_start, dump_log_end);
+    return 0;
+}
+
+
+static void do_erase_logs(void)
+{
+	cliSerial->printf_P(PSTR("\nErasing log...\n"));
+    DataFlash.EraseAll();
+	cliSerial->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) {
+		cliSerial->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(IMU);
+		TARG(CMD);
+		TARG(CURRENT);
+		TARG(SONAR);
+		TARG(COMPASS);
+		TARG(CAMERA);
+		TARG(STEERING);
+		#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;
+}
+
+struct PACKED log_Performance {
+    LOG_PACKET_HEADER;
+    uint32_t time_ms;
+    uint32_t loop_time;
+    uint16_t main_loop_count;
+    uint32_t g_dt_max;
+    int16_t  gyro_drift_x;
+    int16_t  gyro_drift_y;
+    int16_t  gyro_drift_z;
+    uint8_t  i2c_lockup_count;
+    uint16_t ins_error_count;
+};
+
+// Write a performance monitoring packet. Total length : 19 bytes
+static void Log_Write_Performance()
+{
+    struct log_Performance pkt = {
+        LOG_PACKET_HEADER_INIT(LOG_PERFORMANCE_MSG),
+        time_ms         : millis(),
+        loop_time       : millis()- perf_mon_timer,
+        main_loop_count : mainLoop_count,
+        g_dt_max        : G_Dt_max,
+        gyro_drift_x    : (int16_t)(ahrs.get_gyro_drift().x * 1000),
+        gyro_drift_y    : (int16_t)(ahrs.get_gyro_drift().y * 1000),
+        gyro_drift_z    : (int16_t)(ahrs.get_gyro_drift().z * 1000),
+        i2c_lockup_count: hal.i2c->lockup_count(),
+        ins_error_count  : ins.error_count()
+    };
+    DataFlash.WriteBlock(&pkt, sizeof(pkt));
+}
+
+// Write a mission command. Total length : 36 bytes
+static void Log_Write_Cmd(const AP_Mission::Mission_Command &cmd)
+{
+    mavlink_mission_item_t mav_cmd = {};
+    AP_Mission::mission_cmd_to_mavlink(cmd,mav_cmd);
+    DataFlash.Log_Write_MavCmd(mission.num_commands(),mav_cmd);
+}
+
+struct PACKED log_Steering {
+    LOG_PACKET_HEADER;
+    uint32_t time_ms;
+    float demanded_accel;
+    float achieved_accel;
+};
+
+// Write a steering packet
+static void Log_Write_Steering()
+{
+    struct log_Steering pkt = {
+        LOG_PACKET_HEADER_INIT(LOG_STEERING_MSG),
+        time_ms        : hal.scheduler->millis(),
+        demanded_accel : lateral_acceleration,
+        achieved_accel : gps.ground_speed() * ins.get_gyro().z,
+    };
+    DataFlash.WriteBlock(&pkt, sizeof(pkt));
+}
+
+struct PACKED log_Startup {
+    LOG_PACKET_HEADER;
+    uint32_t time_ms;
+    uint8_t startup_type;
+    uint16_t command_total;
+};
+
+static void Log_Write_Startup(uint8_t type)
+{
+    struct log_Startup pkt = {
+        LOG_PACKET_HEADER_INIT(LOG_STARTUP_MSG),
+        time_ms         : millis(),
+        startup_type    : type,
+        command_total   : mission.num_commands()
+    };
+    DataFlash.WriteBlock(&pkt, sizeof(pkt));
+
+    // write all commands to the dataflash as well
+    AP_Mission::Mission_Command cmd;
+    for (uint16_t i = 0; i < mission.num_commands(); i++) {
+        if(mission.read_cmd_from_storage(i,cmd)) {
+            Log_Write_Cmd(cmd);
+        }
+    }
+}
+
+struct PACKED log_Control_Tuning {
+    LOG_PACKET_HEADER;
+    uint32_t time_ms;
+    int16_t steer_out;
+    int16_t roll;
+    int16_t pitch;
+    int16_t throttle_out;
+    float accel_y;
+};
+
+// Write a control tuning packet. Total length : 22 bytes
+static void Log_Write_Control_Tuning()
+{
+    Vector3f accel = ins.get_accel();
+    struct log_Control_Tuning pkt = {
+        LOG_PACKET_HEADER_INIT(LOG_CTUN_MSG),
+        time_ms         : millis(),
+        steer_out       : (int16_t)channel_steer->servo_out,
+        roll            : (int16_t)ahrs.roll_sensor,
+        pitch           : (int16_t)ahrs.pitch_sensor,
+        throttle_out    : (int16_t)channel_throttle->servo_out,
+        accel_y         : accel.y
+    };
+    DataFlash.WriteBlock(&pkt, sizeof(pkt));
+}
+
+struct PACKED log_Nav_Tuning {
+    LOG_PACKET_HEADER;
+    uint32_t time_ms;
+    uint16_t yaw;
+    float    wp_distance;
+    uint16_t target_bearing_cd;
+    uint16_t nav_bearing_cd;
+    int8_t   throttle;
+};
+
+// Write a navigation tuning packet. Total length : 18 bytes
+static void Log_Write_Nav_Tuning()
+{
+    struct log_Nav_Tuning pkt = {
+        LOG_PACKET_HEADER_INIT(LOG_NTUN_MSG),
+        time_ms             : millis(),
+        yaw                 : (uint16_t)ahrs.yaw_sensor,
+        wp_distance         : wp_distance,
+        target_bearing_cd   : (uint16_t)nav_controller->target_bearing_cd(),
+        nav_bearing_cd      : (uint16_t)nav_controller->nav_bearing_cd(),
+        throttle            : (int8_t)(100 * channel_throttle->norm_output())
+    };
+    DataFlash.WriteBlock(&pkt, sizeof(pkt));
+}
+
+struct PACKED log_Attitude {
+    LOG_PACKET_HEADER;
+    uint32_t time_ms;
+    int16_t roll;
+    int16_t pitch;
+    uint16_t yaw;
+};
+
+
+// Write an attitude packet
+static void Log_Write_Attitude()
+{
+    struct log_Attitude pkt = {
+        LOG_PACKET_HEADER_INIT(LOG_ATTITUDE_MSG),
+        time_ms : millis(),
+        roll    : (int16_t)ahrs.roll_sensor,
+        pitch   : (int16_t)ahrs.pitch_sensor,
+        yaw     : (uint16_t)ahrs.yaw_sensor
+    };
+    DataFlash.WriteBlock(&pkt, sizeof(pkt));
+#if AP_AHRS_NAVEKF_AVAILABLE
+ #if OPTFLOW == ENABLED
+    DataFlash.Log_Write_EKF(ahrs,optflow.enabled());
+ #else
+    DataFlash.Log_Write_EKF(ahrs,false);
+ #endif
+    DataFlash.Log_Write_AHRS2(ahrs);
+#endif
+}
+
+struct PACKED log_Mode {
+    LOG_PACKET_HEADER;
+    uint32_t time_ms;
+    uint8_t mode;
+    uint8_t mode_num;
+};
+
+// Write a mode packet
+static void Log_Write_Mode()
+{
+    struct log_Mode pkt = {
+        LOG_PACKET_HEADER_INIT(LOG_MODE_MSG),
+        time_ms         : millis(),
+        mode            : (uint8_t)control_mode,
+        mode_num        : (uint8_t)control_mode
+    };
+    DataFlash.WriteBlock(&pkt, sizeof(pkt));
+}
+
+
+struct PACKED log_Sonar {
+    LOG_PACKET_HEADER;
+    uint32_t time_ms;
+    float    lateral_accel;
+    uint16_t sonar1_distance;
+    uint16_t sonar2_distance;
+    uint16_t detected_count;
+    int8_t   turn_angle;
+    uint16_t turn_time;
+    uint16_t ground_speed;
+    int8_t   throttle;
+};
+
+// Write a sonar packet
+static void Log_Write_Sonar()
+{
+    uint16_t turn_time = 0;
+    if (obstacle.turn_angle != 0) {
+        turn_time = hal.scheduler->millis() - obstacle.detected_time_ms;
+    }
+    struct log_Sonar pkt = {
+        LOG_PACKET_HEADER_INIT(LOG_SONAR_MSG),
+        time_ms         : millis(),
+        lateral_accel   : lateral_acceleration,
+        sonar1_distance : (uint16_t)sonar.distance_cm(0),
+        sonar2_distance : (uint16_t)sonar.distance_cm(1),
+        detected_count  : obstacle.detected_count,
+        turn_angle      : (int8_t)obstacle.turn_angle,
+        turn_time       : turn_time,
+        ground_speed    : (uint16_t)(ground_speed*100),
+        throttle        : (int8_t)(100 * channel_throttle->norm_output())
+    };
+    DataFlash.WriteBlock(&pkt, sizeof(pkt));
+}
+
+struct PACKED log_Current {
+    LOG_PACKET_HEADER;
+    uint32_t time_ms;
+    int16_t throttle_in;
+    int16_t battery_voltage;
+    int16_t current_amps;
+    uint16_t board_voltage;
+    float   current_total;
+};
+
+static void Log_Write_Current()
+{
+    struct log_Current pkt = {
+        LOG_PACKET_HEADER_INIT(LOG_CURRENT_MSG),
+        time_ms                 : millis(),
+        throttle_in             : channel_throttle->control_in,
+        battery_voltage         : (int16_t)(battery.voltage() * 100.0),
+        current_amps            : (int16_t)(battery.current_amps() * 100.0),
+        board_voltage           : (uint16_t)(hal.analogin->board_voltage()*1000),
+        current_total           : battery.current_total_mah()
+    };
+    DataFlash.WriteBlock(&pkt, sizeof(pkt));
+
+    // also write power status
+    DataFlash.Log_Write_Power();
+}
+
+struct PACKED log_Compass {
+    LOG_PACKET_HEADER;
+    uint32_t time_ms;
+    int16_t mag_x;
+    int16_t mag_y;
+    int16_t mag_z;
+    int16_t offset_x;
+    int16_t offset_y;
+    int16_t offset_z;
+    int16_t motor_offset_x;
+    int16_t motor_offset_y;
+    int16_t motor_offset_z;
+};
+
+// Write a Compass packet. Total length : 15 bytes
+static void Log_Write_Compass()
+{
+    const Vector3f &mag_offsets = compass.get_offsets();
+    const Vector3f &mag_motor_offsets = compass.get_motor_offsets();
+    const Vector3f &mag = compass.get_field();
+    struct log_Compass pkt = {
+        LOG_PACKET_HEADER_INIT(LOG_COMPASS_MSG),
+        time_ms         : millis(),
+        mag_x           : (int16_t)mag.x,
+        mag_y           : (int16_t)mag.y,
+        mag_z           : (int16_t)mag.z,
+        offset_x        : (int16_t)mag_offsets.x,
+        offset_y        : (int16_t)mag_offsets.y,
+        offset_z        : (int16_t)mag_offsets.z,
+        motor_offset_x  : (int16_t)mag_motor_offsets.x,
+        motor_offset_y  : (int16_t)mag_motor_offsets.y,
+        motor_offset_z  : (int16_t)mag_motor_offsets.z
+    };
+    DataFlash.WriteBlock(&pkt, sizeof(pkt));
+#if COMPASS_MAX_INSTANCES > 1
+    if (compass.get_count() > 1) {
+        const Vector3f &mag2_offsets = compass.get_offsets(1);
+        const Vector3f &mag2_motor_offsets = compass.get_motor_offsets(1);
+        const Vector3f &mag2 = compass.get_field(1);
+        struct log_Compass pkt2 = {
+            LOG_PACKET_HEADER_INIT(LOG_COMPASS2_MSG),
+            time_ms         : millis(),
+            mag_x           : (int16_t)mag2.x,
+            mag_y           : (int16_t)mag2.y,
+            mag_z           : (int16_t)mag2.z,
+            offset_x        : (int16_t)mag2_offsets.x,
+            offset_y        : (int16_t)mag2_offsets.y,
+            offset_z        : (int16_t)mag2_offsets.z,
+            motor_offset_x  : (int16_t)mag2_motor_offsets.x,
+            motor_offset_y  : (int16_t)mag2_motor_offsets.y,
+            motor_offset_z  : (int16_t)mag2_motor_offsets.z
+        };
+        DataFlash.WriteBlock(&pkt2, sizeof(pkt2));
+    }
+#endif
+#if COMPASS_MAX_INSTANCES > 2
+    if (compass.get_count() > 2) {
+        const Vector3f &mag3_offsets = compass.get_offsets(2);
+        const Vector3f &mag3_motor_offsets = compass.get_motor_offsets(2);
+        const Vector3f &mag3 = compass.get_field(2);
+        struct log_Compass pkt3 = {
+            LOG_PACKET_HEADER_INIT(LOG_COMPASS3_MSG),
+            time_ms         : millis(),
+            mag_x           : (int16_t)mag3.x,
+            mag_y           : (int16_t)mag3.y,
+            mag_z           : (int16_t)mag3.z,
+            offset_x        : (int16_t)mag3_offsets.x,
+            offset_y        : (int16_t)mag3_offsets.y,
+            offset_z        : (int16_t)mag3_offsets.z,
+            motor_offset_x  : (int16_t)mag3_motor_offsets.x,
+            motor_offset_y  : (int16_t)mag3_motor_offsets.y,
+            motor_offset_z  : (int16_t)mag3_motor_offsets.z
+        };
+        DataFlash.WriteBlock(&pkt3, sizeof(pkt3));
+    }
+#endif
+}
+
+
+static void Log_Write_RC(void)
+{
+    DataFlash.Log_Write_RCIN();
+    DataFlash.Log_Write_RCOUT();
+}
+
+static void Log_Write_Baro(void)
+{
+    DataFlash.Log_Write_Baro(barometer);
+}
+
+static const struct LogStructure log_structure[] PROGMEM = {
+    LOG_COMMON_STRUCTURES,
+    { LOG_ATTITUDE_MSG, sizeof(log_Attitude),       
+      "ATT", "IccC",        "TimeMS,Roll,Pitch,Yaw" },
+    { LOG_PERFORMANCE_MSG, sizeof(log_Performance), 
+      "PM",  "IIHIhhhBH", "TimeMS,LTime,MLC,gDt,GDx,GDy,GDz,I2CErr,INSErr" },
+    { LOG_STARTUP_MSG, sizeof(log_Startup),         
+      "STRT", "IBH",        "TimeMS,SType,CTot" },
+    { LOG_CTUN_MSG, sizeof(log_Control_Tuning),     
+      "CTUN", "Ihcchf",     "TimeMS,Steer,Roll,Pitch,ThrOut,AccY" },
+    { LOG_NTUN_MSG, sizeof(log_Nav_Tuning),         
+      "NTUN", "IHfHHb",     "TimeMS,Yaw,WpDist,TargBrg,NavBrg,Thr" },
+    { LOG_SONAR_MSG, sizeof(log_Sonar),             
+      "SONR", "IfHHHbHCb",  "TimeMS,LatAcc,S1Dist,S2Dist,DCnt,TAng,TTim,Spd,Thr" },
+    { LOG_CURRENT_MSG, sizeof(log_Current),             
+      "CURR", "IhhhHf",     "TimeMS,Thr,Volt,Curr,Vcc,CurrTot" },
+    { LOG_MODE_MSG, sizeof(log_Mode),             
+      "MODE", "IMB",        "TimeMS,Mode,ModeNum" },
+    { LOG_COMPASS_MSG, sizeof(log_Compass),             
+      "MAG", "Ihhhhhhhhh",  "TimeMS,MagX,MagY,MagZ,OfsX,OfsY,OfsZ,MOfsX,MOfsY,MOfsZ" },
+    { LOG_COMPASS2_MSG, sizeof(log_Compass),             
+      "MAG2", "Ihhhhhhhhh",   "TimeMS,MagX,MagY,MagZ,OfsX,OfsY,OfsZ,MOfsX,MOfsY,MOfsZ" },
+    { LOG_STEERING_MSG, sizeof(log_Steering),             
+      "STER", "Iff",   "TimeMS,Demanded,Achieved" },
+};
+
+
+// Read the DataFlash log memory : Packet Parser
+static void Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page)
+{
+    cliSerial->printf_P(PSTR("\n" FIRMWARE_STRING
+                             "\nFree RAM: %u\n"),
+                        (unsigned)hal.util->available_memory());
+
+    cliSerial->println_P(PSTR(HAL_BOARD_NAME));
+
+	DataFlash.LogReadProcess(log_num, start_page, end_page, 
+                             print_mode,
+                             cliSerial);
+}
+
+// start a new log
+static void start_logging() 
+{
+    in_mavlink_delay = true;
+    DataFlash.StartNewLog();
+    in_mavlink_delay = false;
+    DataFlash.Log_Write_Message_P(PSTR(FIRMWARE_STRING));
+
+#if defined(PX4_GIT_VERSION) && defined(NUTTX_GIT_VERSION)
+    DataFlash.Log_Write_Message_P(PSTR("PX4: " PX4_GIT_VERSION " NuttX: " NUTTX_GIT_VERSION));
+#endif
+
+    // write system identifier as well if available
+    char sysid[40];
+    if (hal.util->get_system_id(sysid)) {
+        DataFlash.Log_Write_Message(sysid);
+    }
+}
+
+#else // LOGGING_ENABLED
+
+// dummy functions
+static void Log_Write_Startup(uint8_t type) {}
+static void Log_Write_Current() {}
+static void Log_Write_Nav_Tuning() {}
+static void Log_Write_Performance() {}
+static void Log_Write_Cmd(const AP_Mission::Mission_Command &cmd) {}
+static int8_t process_logs(uint8_t argc, const Menu::arg *argv) { return 0; }
+static void Log_Write_Control_Tuning() {}
+static void Log_Write_Sonar() {}
+static void Log_Write_Mode() {}
+static void Log_Write_Attitude() {}
+static void Log_Write_Compass() {}
+static void start_logging() {}
+static void Log_Write_RC(void) {}
+
+#endif // LOGGING_ENABLED
+

APMrover2/Makefile

+include ../mk/apm.mk

APMrover2/Parameters.h

+// -*- 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 = 16;
+    static const uint16_t k_software_type = 20;
+
+    enum {
+        // Layout version number, always key zero.
+        //
+        k_param_format_version = 0,
+		k_param_software_type,
+
+        // Misc
+        //
+        k_param_log_bitmask_old = 10, // unused
+        k_param_num_resets,
+        k_param_reset_switch_chan,
+        k_param_initial_mode,
+        k_param_scheduler,
+        k_param_relay,
+        k_param_BoardConfig,
+        k_param_pivot_turn_angle,
+        k_param_rc_13,
+        k_param_rc_14,
+
+        // IO pins
+        k_param_rssi_pin = 20,
+        k_param_battery_volt_pin,
+        k_param_battery_curr_pin,
+
+        // braking
+        k_param_braking_percent = 30,
+        k_param_braking_speederr,
+
+        // misc2
+        k_param_log_bitmask = 40,
+        k_param_gps,
+        k_param_serial0_baud,
+        k_param_serial1_baud,
+        k_param_serial2_baud,
+
+
+        // 110: Telemetry control
+        //
+        k_param_gcs0 = 110, // stream rates for uartA
+        k_param_gcs1,       // stream rates for uartC
+        k_param_sysid_this_mav,
+        k_param_sysid_my_gcs,
+        k_param_serial0_baud_old,
+        k_param_serial1_baud_old,
+        k_param_telem_delay,
+        k_param_skip_gyro_cal,
+        k_param_gcs2,       // stream rates for uartD
+        k_param_serial2_baud_old,
+        k_param_serial2_protocol,
+
+        //
+        // 130: Sensor parameters
+        //
+        k_param_compass_enabled = 130,
+        k_param_steering_learn, // unused
+        k_param_NavEKF,  // Extended Kalman Filter Inertial Navigation Group
+        k_param_mission, // mission library
+
+        // 140: battery controls
+        k_param_battery_monitoring = 140,   // deprecated, can be deleted
+        k_param_volt_div_ratio,     // deprecated, can be deleted
+        k_param_curr_amp_per_volt,  // deprecated, can be deleted
+        k_param_input_voltage, // deprecated, can be deleted
+        k_param_pack_capacity,      // deprecated, can be deleted
+        k_param_battery,
+
+        //
+        // 150: Navigation parameters
+        //
+        k_param_crosstrack_gain = 150,
+        k_param_crosstrack_entry_angle,
+        k_param_speed_cruise,
+        k_param_speed_turn_gain,
+        k_param_speed_turn_dist,
+        k_param_ch7_option,
+        k_param_auto_trigger_pin,
+        k_param_auto_kickstart,
+        k_param_turn_circle, // unused
+        k_param_turn_max_g,
+
+        //
+        // 160: Radio settings
+        //
+        k_param_rc_1 = 160,
+        k_param_rc_2,
+        k_param_rc_3,
+        k_param_rc_4,
+        k_param_rc_5,
+        k_param_rc_6,
+        k_param_rc_7,
+        k_param_rc_8,
+
+        // throttle control
+        k_param_throttle_min = 170,
+        k_param_throttle_max,
+        k_param_throttle_cruise,
+        k_param_throttle_slewrate,
+        k_param_throttle_reduction,
+        k_param_skid_steer_in,
+        k_param_skid_steer_out,
+
+        // failsafe control
+        k_param_fs_action = 180,
+        k_param_fs_timeout,
+        k_param_fs_throttle_enabled,
+        k_param_fs_throttle_value,
+        k_param_fs_gcs_enabled,
+
+        // obstacle control
+        k_param_sonar_enabled = 190, // deprecated, can be removed
+        k_param_sonar_old, // unused
+        k_param_sonar_trigger_cm,
+        k_param_sonar_turn_angle,
+        k_param_sonar_turn_time,
+        k_param_sonar2_old, // unused
+        k_param_sonar_debounce,
+        k_param_sonar, // sonar object
+        
+        //
+        // 210: driving modes
+        //
+        k_param_mode_channel = 210,
+        k_param_mode1,
+        k_param_mode2,
+        k_param_mode3,
+        k_param_mode4,
+        k_param_mode5,
+        k_param_mode6,
+        k_param_learn_channel,
+
+        //
+        // 220: Waypoint data
+        //
+        k_param_command_total = 220,    // unused
+        k_param_command_index,          // unused
+        k_param_waypoint_radius,
+
+        //
+        // 230: camera control
+        //
+        k_param_camera,
+        k_param_camera_mount,
+        k_param_camera_mount2,
+
+        //
+        // 240: PID Controllers
+        k_param_pidNavSteer = 230,
+        k_param_pidServoSteer, // unused
+        k_param_pidSpeedThrottle,
+
+        // high RC channels
+        k_param_rc_9 = 235,
+        k_param_rc_10,
+        k_param_rc_11,
+        k_param_rc_12,
+
+        // other objects
+        k_param_sitl = 240,
+        k_param_ahrs,
+        k_param_ins,
+        k_param_compass,
+        k_param_rcmap,
+        k_param_L1_controller,
+        k_param_steerController,
+        k_param_barometer,
+
+        // 254,255: reserved
+        };
+
+    AP_Int16    format_version;
+	AP_Int8	    software_type;
+
+    // Misc
+    //
+    AP_Int32    log_bitmask;
+    AP_Int16    num_resets;
+    AP_Int8	    reset_switch_chan;
+    AP_Int8     initial_mode;
+
+    // IO pins
+    AP_Int8     rssi_pin;
+
+    // braking
+    AP_Int8     braking_percent;
+    AP_Float    braking_speederr;
+
+	// Telemetry control
+	//
+	AP_Int16    sysid_this_mav;
+	AP_Int16    sysid_my_gcs;
+    AP_Int16    serial0_baud;
+    AP_Int16    serial1_baud;
+#if MAVLINK_COMM_NUM_BUFFERS > 2
+    AP_Int16    serial2_baud;
+    AP_Int8     serial2_protocol;
+#endif
+    AP_Int8     telem_delay;
+    AP_Int8     skip_gyro_cal;
+
+    // sensor parameters
+    AP_Int8	    compass_enabled; 
+
+    // navigation parameters
+    //
+    AP_Float    speed_cruise;
+    AP_Int8     speed_turn_gain;
+    AP_Float    speed_turn_dist;    
+    AP_Int8	    ch7_option;
+    AP_Int8     auto_trigger_pin;
+    AP_Float    auto_kickstart;
+    AP_Float    turn_max_g;
+    AP_Int16    pivot_turn_angle;
+
+    // RC channels
+    RC_Channel      rc_1;
+    RC_Channel_aux	rc_2;
+    RC_Channel      rc_3;
+    RC_Channel_aux  rc_4;
+    RC_Channel_aux	rc_5;
+    RC_Channel_aux	rc_6;
+    RC_Channel_aux	rc_7;
+    RC_Channel_aux	rc_8;
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+    RC_Channel_aux rc_9;
+#endif
+#if CONFIG_HAL_BOARD == HAL_BOARD_APM2 || CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+    RC_Channel_aux rc_10;
+    RC_Channel_aux rc_11;
+#endif
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+    RC_Channel_aux rc_12;
+    RC_Channel_aux rc_13;
+    RC_Channel_aux rc_14;
+#endif
+
+    // Throttle
+    //
+    AP_Int8     throttle_min;
+    AP_Int8     throttle_max;
+    AP_Int8     throttle_cruise;
+    AP_Int8     throttle_slewrate;
+    AP_Int8     skid_steer_in;
+    AP_Int8     skid_steer_out;
+
+    // failsafe control
+    AP_Int8     fs_action;
+    AP_Float    fs_timeout;
+    AP_Int8     fs_throttle_enabled;
+    AP_Int16    fs_throttle_value;
+	AP_Int8	    fs_gcs_enabled;
+
+    // obstacle control
+    AP_Int16    sonar_trigger_cm;
+    AP_Float    sonar_turn_angle;
+    AP_Float    sonar_turn_time;
+    AP_Int8     sonar_debounce;
+    
+
+    // driving modes
+    //
+    AP_Int8     mode_channel;
+    AP_Int8     mode1;
+    AP_Int8     mode2;
+    AP_Int8     mode3;
+    AP_Int8     mode4;
+    AP_Int8     mode5;
+    AP_Int8     mode6;
+    AP_Int8     learn_channel;
+    
+    // Waypoints
+    //
+    AP_Float    waypoint_radius;
+
+    // PID controllers
+    //
+    PID         pidSpeedThrottle;
+
+    Parameters() :
+        // RC channels
+        rc_1(CH_1),
+        rc_2(CH_2),
+        rc_3(CH_3),
+        rc_4(CH_4),
+        rc_5(CH_5),
+        rc_6(CH_6),
+        rc_7(CH_7),
+        rc_8(CH_8),
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+        rc_9                                    (CH_9),
+#endif
+#if CONFIG_HAL_BOARD == HAL_BOARD_APM2 || CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+        rc_10                                   (CH_10),
+        rc_11                                   (CH_11),
+#endif
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+        rc_12                                   (CH_12),
+        rc_13                                   (CH_13),
+        rc_14                                   (CH_14),
+#endif
+
+        // PID controller    initial P        initial I        initial D        initial imax
+        //-----------------------------------------------------------------------------------
+        pidSpeedThrottle    (0.7,             0.2,             0.2,             4000)
+        {}
+};
+
+extern const AP_Param::Info var_info[];
+
+#endif // PARAMETERS_H
+

APMrover2/Parameters.pde

+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/*
+  APMRover2 parameter definitions
+*/
+
+#define GSCALAR(v, name, def) { g.v.vtype, name, Parameters::k_param_ ## v, &g.v, {def_value:def} }
+#define GGROUP(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &g.v, {group_info:class::var_info} }
+#define GOBJECT(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &v, {group_info:class::var_info} }
+#define GOBJECTN(v, pname, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## pname, &v, {group_info : class::var_info} }
+
+const AP_Param::Info var_info[] PROGMEM = {
+	GSCALAR(format_version,         "FORMAT_VERSION",   1),
+	GSCALAR(software_type,          "SYSID_SW_TYPE",    Parameters::k_software_type),
+
+	// misc
+    // @Param: LOG_BITMASK
+    // @DisplayName: Log bitmask
+    // @Description: Two byte bitmap of log types to enable in dataflash
+    // @Values: 0:Disabled,3950:Default,4078:Default+IMU
+    // @User: Advanced
+	GSCALAR(log_bitmask,            "LOG_BITMASK",      DEFAULT_LOG_BITMASK),
+	GSCALAR(num_resets,             "SYS_NUM_RESETS",   0),
+
+    // @Param: RST_SWITCH_CH
+    // @DisplayName: Reset Switch Channel
+    // @Description: RC channel to use to reset to last flight mode	after geofence takeover.
+    // @User: Advanced
+	GSCALAR(reset_switch_chan,      "RST_SWITCH_CH",    0),
+
+    // @Param: INITIAL_MODE
+    // @DisplayName: Initial driving mode
+    // @Description: This selects the mode to start in on boot. This is useful for when you want to start in AUTO mode on boot without a receiver. Usuallly used in combination with when AUTO_TRIGGER_PIN or AUTO_KICKSTART.
+    // @Values: 0:MANUAL,2:LEARNING,3:STEERING,4:HOLD,10:AUTO,11:RTL,15:GUIDED
+    // @User: Advanced
+	GSCALAR(initial_mode,        "INITIAL_MODE",     MANUAL),
+
+    // @Param: RSSI_PIN
+    // @DisplayName: Receiver RSSI sensing pin
+    // @Description: This selects an analog pin for the receiver RSSI voltage. It assumes the voltage is 5V for max rssi, 0V for minimum
+    // @Values: -1:Disabled, 0:APM2 A0, 1:APM2 A1, 2:APM2 A2, 13:APM2 A13, 103:Pixhawk SBUS
+    // @User: Standard
+    GSCALAR(rssi_pin,            "RSSI_PIN",         -1),
+
+    // @Param: SYSID_THIS_MAV
+    // @DisplayName: MAVLink system ID
+    // @Description: ID used in MAVLink protocol to identify this vehicle
+    // @Range: 1 255
+    // @User: Advanced
+	GSCALAR(sysid_this_mav,         "SYSID_THISMAV",    MAV_SYSTEM_ID),
+
+    // @Param: SYSID_MYGCS
+    // @DisplayName: MAVLink ground station ID
+    // @Description: ID used in MAVLink protocol to identify the controlling ground station
+    // @Range: 1 255
+    // @User: Advanced
+	GSCALAR(sysid_my_gcs,           "SYSID_MYGCS",      255),
+
+    // @Param: SERIAL0_BAUD
+    // @DisplayName: USB Console Baud Rate
+    // @Description: The baud rate used on the USB console. The APM2 can support all baudrates up to 115, and also can support 500. The PX4 can support rates of up to 1500. If you setup a rate you cannot support on APM2 and then can't connect to your board you should load a firmware from a different vehicle type. That will reset all your parameters to defaults.
+    // @Values: 1:1200,2:2400,4:4800,9:9600,19:19200,38:38400,57:57600,111:111100,115:115200,500:500000,921:921600,1500:1500000
+    // @User: Standard
+    GSCALAR(serial0_baud,           "SERIAL0_BAUD",   SERIAL0_BAUD/1000),
+
+    // @Param: SERIAL1_BAUD
+    // @DisplayName: Telemetry Baud Rate
+    // @Description: The baud rate used on the first telemetry port. The APM2 can support all baudrates up to 115, and also can support 500. The PX4 can support rates of up to 1500. If you setup a rate you cannot support on APM2 and then can't connect to your board you should load a firmware from a different vehicle type. That will reset all your parameters to defaults.
+    // @Values: 1:1200,2:2400,4:4800,9:9600,19:19200,38:38400,57:57600,111:111100,115:115200,500:500000,921:921600,1500:1500000
+    // @User: Standard
+    GSCALAR(serial1_baud,           "SERIAL1_BAUD",   SERIAL1_BAUD/1000),
+
+#if MAVLINK_COMM_NUM_BUFFERS > 2
+    // @Param: SERIAL2_BAUD
+    // @DisplayName: Telemetry Baud Rate
+    // @Description: The baud rate used on the second telemetry port. The APM2 can support all baudrates up to 115, and also can support 500. The PX4 can support rates of up to 1500. If you setup a rate you cannot support on APM2 and then can't connect to your board you should load a firmware from a different vehicle type. That will reset all your parameters to defaults.
+    // @Values: 1:1200,2:2400,4:4800,9:9600,19:19200,38:38400,57:57600,111:111100,115:115200,500:500000,921:921600,1500:1500000
+    // @User: Standard
+    GSCALAR(serial2_baud,           "SERIAL2_BAUD",   SERIAL2_BAUD/1000),
+
+#if FRSKY_TELEM_ENABLED == ENABLED
+    // @Param: SERIAL2_PROTOCOL
+    // @DisplayName: SERIAL2 protocol selection
+    // @Description: Control what protocol telemetry 2 port should be used for
+    // @Values: 1:GCS Mavlink,2:Frsky D-PORT
+    // @User: Standard
+    GSCALAR(serial2_protocol,        "SERIAL2_PROTOCOL", SERIAL2_MAVLINK),
+#endif // FRSKY_TELEM_ENABLED
+
+#endif // MAVLINK_COMM_NUM_BUFFERS
+
+
+    // @Param: TELEM_DELAY
+    // @DisplayName: Telemetry startup delay 
+    // @Description: The amount of time (in seconds) to delay radio telemetry to prevent an Xbee bricking on power up
+    // @User: Standard
+    // @Units: seconds
+    // @Range: 0 10
+    // @Increment: 1
+    GSCALAR(telem_delay,            "TELEM_DELAY",     0),
+
+    // @Param: SKIP_GYRO_CAL
+    // @DisplayName: Skip gyro calibration
+    // @Description: When enabled this tells the APM to skip the normal gyroscope calibration at startup, and instead use the saved gyro calibration from the last flight. You should only enable this if you are careful to check that your aircraft has good attitude control before flying, as some boards may have significantly different gyro calibration between boots, especially if the temperature changes a lot. If gyro calibration is skipped then APM relies on using the gyro drift detection code to get the right gyro calibration in the few minutes after it boots. This option is mostly useful where the requirement to hold the vehicle still while it is booting is a significant problem.
+    // @Values: 0:Disabled,1:Enabled
+    // @User: Advanced
+    GSCALAR(skip_gyro_cal,           "SKIP_GYRO_CAL",   0),
+
+    // @Param: MAG_ENABLED
+    // @DisplayName: Magnetometer (compass) enabled
+    // @Description: This should be set to 1 if a compass is installed
+    // @User: Standard
+    // @Values: 0:Disabled,1:Enabled
+	GSCALAR(compass_enabled,        "MAG_ENABLE",       MAGNETOMETER),
+
+	// @Param: AUTO_TRIGGER_PIN
+	// @DisplayName: Auto mode trigger pin
+	// @Description: pin number to use to enable the throttle in auto mode. If set to -1 then don't use a trigger, otherwise this is a pin number which if held low in auto mode will enable the motor to run. If the switch is released while in AUTO then the motor will stop again. This can be used in combination with INITIAL_MODE to give a 'press button to start' rover with no receiver.
+	// @Values: -1:Disabled,0-8:TiggerPin
+	// @User: standard
+	GSCALAR(auto_trigger_pin,        "AUTO_TRIGGER_PIN", -1),
+
+	// @Param: AUTO_KICKSTART
+	// @DisplayName: Auto mode trigger kickstart acceleration
+	// @Description: X acceleration in meters/second/second to use to trigger the motor start in auto mode. If set to zero then auto throttle starts immediately when the mode switch happens, otherwise the rover waits for the X acceleration to go above this value before it will start the motor
+	// @Units: m/s/s
+	// @Range: 0 20
+	// @Increment: 0.1
+	// @User: standard
+	GSCALAR(auto_kickstart,          "AUTO_KICKSTART", 0.0f),
+
+    // @Param: CRUISE_SPEED
+    // @DisplayName: Target cruise speed in auto modes
+    // @Description: The target speed in auto missions.
+    // @Units: m/s
+    // @Range: 0 100
+    // @Increment: 0.1
+    // @User: Standard
+	GSCALAR(speed_cruise,        "CRUISE_SPEED",    5),
+
+    // @Param: SPEED_TURN_GAIN
+    // @DisplayName: Target speed reduction while turning
+    // @Description: The percentage to reduce the throttle while turning. If this is 100% then the target speed is not reduced while turning. If this is 50% then the target speed is reduced in proportion to the turn rate, with a reduction of 50% when the steering is maximally deflected.
+    // @Units: percent
+    // @Range: 0 100
+    // @Increment: 1
+    // @User: Standard
+	GSCALAR(speed_turn_gain,    "SPEED_TURN_GAIN",  50),
+
+    // @Param: SPEED_TURN_DIST
+    // @DisplayName: Distance to turn to start reducing speed
+    // @Description: The distance to the next turn at which the rover reduces its target speed by the SPEED_TURN_GAIN
+    // @Units: meters
+    // @Range: 0 100
+    // @Increment: 0.1
+    // @User: Standard
+	GSCALAR(speed_turn_dist,    "SPEED_TURN_DIST",  2.0f),
+
+    // @Param: BRAKING_PERCENT
+    // @DisplayName: Percentage braking to apply
+    // @Description: The maximum reverse throttle braking percentage to apply when cornering
+    // @Units: percent
+    // @Range: 0 100
+    // @Increment: 1
+    // @User: Standard
+	GSCALAR(braking_percent,    "BRAKING_PERCENT",  0),
+
+    // @Param: BRAKING_SPEEDERR
+    // @DisplayName: Speed error at which to apply braking
+    // @Description: The amount of overspeed error at which to start applying braking
+    // @Units: m/s
+    // @Range: 0 100
+    // @Increment: 1
+    // @User: Standard
+	GSCALAR(braking_speederr,   "BRAKING_SPEEDERR",  3),
+
+    // @Param: PIVOT_TURN_ANGLE
+    // @DisplayName: Pivot turn angle
+    // @Description: Navigation angle threshold in degrees to switch to pivot steering when SKID_STEER_OUT is 1. This allows you to setup a skid steering rover to turn on the spot in auto mode when the angle it needs to turn it greater than this angle. An angle of zero means to disable pivot turning. Note that you will probably also want to set a low value for WP_RADIUS to get neat turns.
+    // @Units: degrees
+    // @Range: 0 360
+    // @Increment: 1
+    // @User: Standard
+	GSCALAR(pivot_turn_angle,   "PIVOT_TURN_ANGLE",  30),
+
+    // @Param: CH7_OPTION
+    // @DisplayName: Channel 7 option
+    // @Description: What to do use channel 7 for
+    // @Values: 0:Nothing,1:LearnWaypoint
+    // @User: Standard
+	GSCALAR(ch7_option,             "CH7_OPTION",          CH7_OPTION),
+
+    // @Group: RC1_
+    // @Path: ../libraries/RC_Channel/RC_Channel.cpp
+	GGROUP(rc_1,                    "RC1_", RC_Channel),
+
+    // @Group: RC2_
+    // @Path: ../libraries/RC_Channel/RC_Channel.cpp
+	GGROUP(rc_2,                    "RC2_", RC_Channel_aux),
+
+    // @Group: RC3_
+    // @Path: ../libraries/RC_Channel/RC_Channel.cpp
+	GGROUP(rc_3,                    "RC3_", RC_Channel),
+
+    // @Group: RC4_
+    // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
+	GGROUP(rc_4,                    "RC4_", RC_Channel_aux),
+
+    // @Group: RC5_
+    // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
+	GGROUP(rc_5,                    "RC5_", RC_Channel_aux),
+
+    // @Group: RC6_
+    // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
+	GGROUP(rc_6,                    "RC6_", RC_Channel_aux),
+
+    // @Group: RC7_
+    // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
+	GGROUP(rc_7,                    "RC7_", RC_Channel_aux),
+
+    // @Group: RC8_
+    // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
+	GGROUP(rc_8,                    "RC8_", RC_Channel_aux),
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+    // @Group: RC9_
+    // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
+    GGROUP(rc_9,                    "RC9_", RC_Channel_aux),
+#endif
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_APM2 || CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+    // @Group: RC10_
+    // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
+    GGROUP(rc_10,                    "RC10_", RC_Channel_aux),
+
+    // @Group: RC11_
+    // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
+    GGROUP(rc_11,                    "RC11_", RC_Channel_aux),
+#endif
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+    // @Group: RC12_
+    // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
+    GGROUP(rc_12,                    "RC12_", RC_Channel_aux),
+
+    // @Group: RC13_
+    // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
+    GGROUP(rc_13,                    "RC13_", RC_Channel_aux),
+
+    // @Group: RC14_
+    // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
+    GGROUP(rc_14,                    "RC14_", RC_Channel_aux),
+#endif
+
+    // @Param: THR_MIN
+    // @DisplayName: Minimum Throttle
+    // @Description: The minimum throttle setting to which the autopilot will apply. This is mostly useful for rovers with internal combustion motors, to prevent the motor from cutting out in auto mode.
+    // @Units: Percent
+    // @Range: 0 100
+    // @Increment: 1
+    // @User: Standard
+	GSCALAR(throttle_min,           "THR_MIN",          THROTTLE_MIN),
+
+    // @Param: THR_MAX
+    // @DisplayName: Maximum Throttle
+    // @Description: The maximum throttle setting to which the autopilot will apply. This can be used to prevent overheating a ESC or motor on an electric rover.
+    // @Units: Percent
+    // @Range: 0 100
+    // @Increment: 1
+    // @User: Standard
+	GSCALAR(throttle_max,           "THR_MAX",          THROTTLE_MAX),
+
+    // @Param: CRUISE_THROTTLE
+    // @DisplayName: Base throttle percentage in auto
+    // @Description: The base throttle percentage to use in auto mode. The CRUISE_SPEED parameter controls the target speed, but the rover starts with the CRUISE_THROTTLE setting as the initial estimate for how much throttle is needed to achieve that speed. It then adjusts the throttle based on how fast the rover is actually going.
+    // @Units: Percent
+    // @Range: 0 100
+    // @Increment: 1
+    // @User: Standard
+	GSCALAR(throttle_cruise,        "CRUISE_THROTTLE",    50),
+
+    // @Param: THR_SLEWRATE
+    // @DisplayName: Throttle slew rate
+    // @Description: maximum percentage change in throttle per second. A setting of 10 means to not change the throttle by more than 10% of the full throttle range in one second. A value of zero means no limit. A value of 100 means the throttle can change over its full range in one second. Note that for some NiMH powered rovers setting a lower value like 40 or 50 may be worthwhile as the sudden current demand on the battery of a big rise in throttle may cause a brownout.
+    // @Units: Percent
+    // @Range: 0 100
+    // @Increment: 1
+    // @User: Standard
+	GSCALAR(throttle_slewrate,      "THR_SLEWRATE",     100),
+
+    // @Param: SKID_STEER_OUT
+    // @DisplayName: Skid steering output
+    // @Description: Set this to 1 for skid steering controlled rovers (tank track style). When enabled, servo1 is used for the left track control, servo3 is used for right track control
+    // @Values: 0:Disabled, 1:SkidSteeringOutput
+    // @User: Standard
+	GSCALAR(skid_steer_out,          "SKID_STEER_OUT",     0),
+
+    // @Param: SKID_STEER_IN
+    // @DisplayName: Skid steering input
+    // @Description: Set this to 1 for skid steering input rovers (tank track style in RC controller). When enabled, servo1 is used for the left track control, servo3 is used for right track control
+    // @Values: 0:Disabled, 1:SkidSteeringInput
+    // @User: Standard
+	GSCALAR(skid_steer_in,           "SKID_STEER_IN",     0),
+
+    // @Param: FS_ACTION
+    // @DisplayName: Failsafe Action
+    // @Description: What to do on a failsafe event
+    // @Values: 0:Nothing,1:RTL,2:HOLD
+    // @User: Standard
+	GSCALAR(fs_action,    "FS_ACTION",     2),
+
+    // @Param: FS_TIMEOUT
+    // @DisplayName: Failsafe timeout
+    // @Description: How long a failsafe event need to happen for before we trigger the failsafe action
+	// @Units: seconds
+    // @User: Standard
+	GSCALAR(fs_timeout,    "FS_TIMEOUT",     5),
+
+    // @Param: FS_THR_ENABLE
+    // @DisplayName: Throttle Failsafe Enable
+    // @Description: The throttle failsafe allows you to configure a software failsafe activated by a setting on the throttle input channel to a low value. This can be used to detect the RC transmitter going out of range. Failsafe will be triggered when the throttle channel goes below the FS_THR_VALUE for FS_TIMEOUT seconds.
+    // @Values: 0:Disabled,1:Enabled
+    // @User: Standard
+	GSCALAR(fs_throttle_enabled,    "FS_THR_ENABLE",     1),
+
+    // @Param: FS_THR_VALUE
+    // @DisplayName: Throttle Failsafe Value
+    // @Description: The PWM level on channel 3 below which throttle sailsafe triggers.
+    // @Range: 925 1100
+    // @Increment: 1
+    // @User: Standard
+	GSCALAR(fs_throttle_value,      "FS_THR_VALUE",     910),
+
+    // @Param: FS_GCS_ENABLE
+    // @DisplayName: GCS failsafe enable
+    // @Description: Enable ground control station telemetry failsafe. When enabled the Rover will execute the FS_ACTION when it fails to receive MAVLink heartbeat packets for FS_TIMEOUT seconds.
+    // @Values: 0:Disabled,1:Enabled
+    // @User: Standard
+	GSCALAR(fs_gcs_enabled, "FS_GCS_ENABLE",   0),
+
+	// @Param: RNGFND_TRIGGR_CM
+	// @DisplayName: Rangefinder trigger distance
+	// @Description: The distance from an obstacle in centimeters at which the rangefinder triggers a turn to avoid the obstacle
+	// @Units: centimeters
+    // @Range: 0 1000
+    // @Increment: 1
+	// @User: Standard
+	GSCALAR(sonar_trigger_cm,   "RNGFND_TRIGGR_CM",    100),
+
+	// @Param: RNGFND_TURN_ANGL
+	// @DisplayName: Rangefinder trigger angle
+	// @Description: The course deviation in degrees to apply while avoiding an obstacle detected with the rangefinder. A positive number means to turn right, and a negative angle means to turn left.
+	// @Units: centimeters
+    // @Range: -45 45
+    // @Increment: 1
+	// @User: Standard
+	GSCALAR(sonar_turn_angle,   "RNGFND_TURN_ANGL",    45),
+
+	// @Param: RNGFND_TURN_TIME
+	// @DisplayName: Rangefinder turn time
+	// @Description: The amount of time in seconds to apply the RNGFND_TURN_ANGL after detecting an obstacle.
+	// @Units: seconds
+    // @Range: 0 100
+    // @Increment: 0.1
+	// @User: Standard
+	GSCALAR(sonar_turn_time,    "RNGFND_TURN_TIME",     1.0f),
+
+	// @Param: RNGFND_DEBOUNCE
+	// @DisplayName: Rangefinder debounce count
+	// @Description: The number of 50Hz rangefinder hits needed to trigger an obstacle avoidance event. If you get a lot of false sonar events then raise this number, but if you make it too large then it will cause lag in detecting obstacles, which could cause you go hit the obstacle.
+    // @Range: 1 100
+    // @Increment: 1
+	// @User: Standard
+	GSCALAR(sonar_debounce,   "RNGFND_DEBOUNCE",    2),
+
+    // @Param: LEARN_CH
+    // @DisplayName: Learning channel
+    // @Description: RC Channel to use for learning waypoints
+    // @User: Advanced
+	GSCALAR(learn_channel,    "LEARN_CH",       7),
+
+    // @Param: MODE_CH
+    // @DisplayName: Mode channel
+    // @Description: RC Channel to use for driving mode control
+    // @User: Advanced
+	GSCALAR(mode_channel,    "MODE_CH",       MODE_CHANNEL),
+
+    // @Param: MODE1
+    // @DisplayName: Mode1
+    // @Values: 0:Manual,2:LEARNING,3:STEERING,4:HOLD,10:Auto,11:RTL,15:Guided
+    // @User: Standard
+    // @Description: Driving mode for switch position 1 (910 to 1230 and above 2049)
+	GSCALAR(mode1,           "MODE1",         MODE_1),
+
+    // @Param: MODE2
+    // @DisplayName: Mode2
+    // @Description: Driving mode for switch position 2 (1231 to 1360)
+    // @Values: 0:Manual,2:LEARNING,3:STEERING,4:HOLD,10:Auto,11:RTL,15:Guided
+    // @User: Standard
+	GSCALAR(mode2,           "MODE2",         MODE_2),
+
+    // @Param: MODE3
+    // @DisplayName: Mode3
+    // @Description: Driving mode for switch position 3 (1361 to 1490)
+    // @Values: 0:Manual,2:LEARNING,3:STEERING,4:HOLD,10:Auto,11:RTL,15:Guided
+    // @User: Standard
+	GSCALAR(mode3,           "MODE3",         MODE_3),
+
+    // @Param: MODE4
+    // @DisplayName: Mode4
+    // @Description: Driving mode for switch position 4 (1491 to 1620)
+    // @Values: 0:Manual,2:LEARNING,3:STEERING,4:HOLD,10:Auto,11:RTL,15:Guided
+    // @User: Standard
+	GSCALAR(mode4,           "MODE4",         MODE_4),
+
+    // @Param: MODE5
+    // @DisplayName: Mode5
+    // @Description: Driving mode for switch position 5 (1621 to 1749)
+    // @Values: 0:Manual,2:LEARNING,3:STEERING,4:HOLD,10:Auto,11:RTL,15:Guided
+    // @User: Standard
+	GSCALAR(mode5,           "MODE5",         MODE_5),
+
+    // @Param: MODE6
+    // @DisplayName: Mode6
+    // @Description: Driving mode for switch position 6 (1750 to 2049)
+    // @Values: 0:Manual,2:LEARNING,3:STEERING,4:HOLD,10:Auto,11:RTL,15:Guided
+    // @User: Standard
+	GSCALAR(mode6,           "MODE6",         MODE_6),
+
+    // @Param: WP_RADIUS
+    // @DisplayName: Waypoint radius
+    // @Description: The distance in meters from a waypoint when we consider the waypoint has been reached. This determines when the rover will turn along the next waypoint path.
+    // @Units: meters
+    // @Range: 0 1000
+    // @Increment: 0.1
+    // @User: Standard
+	GSCALAR(waypoint_radius,        "WP_RADIUS",        2.0f),
+
+    // @Param: TURN_MAX_G
+    // @DisplayName: Turning maximum G force
+    // @Description: The maximum turning acceleration (in units of gravities) that the rover can handle while remaining stable. The navigation code will keep the lateral acceleration below this level to avoid rolling over or slipping the wheels in turns
+    // @Units: gravities
+    // @Range: 0.2 10
+    // @Increment: 0.1
+    // @User: Standard
+	GSCALAR(turn_max_g,             "TURN_MAX_G",      2.0f),
+
+    // @Group: STEER2SRV_
+    // @Path: ../libraries/APM_Control/AP_SteerController.cpp
+	GOBJECT(steerController,        "STEER2SRV_",   AP_SteerController),
+
+	GGROUP(pidSpeedThrottle,        "SPEED2THR_", PID),
+
+	// variables not in the g class which contain EEPROM saved variables
+
+    // @Group: COMPASS_
+    // @Path: ../libraries/AP_Compass/Compass.cpp
+	GOBJECT(compass,                "COMPASS_",	Compass),
+
+    // @Group: SCHED_
+    // @Path: ../libraries/AP_Scheduler/AP_Scheduler.cpp
+    GOBJECT(scheduler, "SCHED_", AP_Scheduler),
+
+    // barometer ground calibration. The GND_ prefix is chosen for
+    // compatibility with previous releases of ArduPlane
+    // @Group: GND_
+    // @Path: ../libraries/AP_Baro/AP_Baro.cpp
+    GOBJECT(barometer, "GND_", AP_Baro),
+
+    // @Group: RELAY_
+    // @Path: ../libraries/AP_Relay/AP_Relay.cpp
+    GOBJECT(relay,                  "RELAY_", AP_Relay),
+
+    // @Group: RCMAP_
+    // @Path: ../libraries/AP_RCMapper/AP_RCMapper.cpp
+    GOBJECT(rcmap,                 "RCMAP_",         RCMapper),
+
+    // @Group: SR0_
+    // @Path: GCS_Mavlink.pde
+    GOBJECTN(gcs[0], gcs0,        "SR0_",     GCS_MAVLINK),
+
+    // @Group: SR1_
+    // @Path: GCS_Mavlink.pde
+    GOBJECTN(gcs[1],  gcs1,       "SR1_",     GCS_MAVLINK),
+
+#if MAVLINK_COMM_NUM_BUFFERS > 2
+    // @Group: SR2_
+    // @Path: GCS_Mavlink.pde
+    GOBJECTN(gcs[2],  gcs2,       "SR2_",     GCS_MAVLINK),
+#endif
+
+    // @Group: NAVL1_
+    // @Path: ../libraries/AP_L1_Control/AP_L1_Control.cpp
+    GOBJECT(L1_controller,         "NAVL1_",   AP_L1_Control),
+
+    // @Group: RNGFND
+    // @Path: ../libraries/AP_RangeFinder/RangeFinder.cpp
+    GOBJECT(sonar,                 "RNGFND", RangeFinder),
+
+    // @Group: INS_
+    // @Path: ../libraries/AP_InertialSensor/AP_InertialSensor.cpp
+    GOBJECT(ins,                            "INS_", AP_InertialSensor),
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
+    // @Group: SIM_
+    // @Path: ../libraries/SITL/SITL.cpp
+    GOBJECT(sitl, "SIM_", SITL),
+#endif
+
+    // @Group: AHRS_
+    // @Path: ../libraries/AP_AHRS/AP_AHRS.cpp
+    GOBJECT(ahrs,                   "AHRS_",    AP_AHRS),
+
+#if CAMERA == ENABLED
+    // @Group: CAM_
+    // @Path: ../libraries/AP_Camera/AP_Camera.cpp
+    GOBJECT(camera,                  "CAM_", AP_Camera),
+#endif
+
+#if MOUNT == ENABLED
+    // @Group: MNT_
+    // @Path: ../libraries/AP_Mount/AP_Mount.cpp
+    GOBJECT(camera_mount,           "MNT_", AP_Mount),
+#endif
+
+    // @Group: BATT_
+    // @Path: ../libraries/AP_BattMonitor/AP_BattMonitor.cpp
+    GOBJECT(battery,                "BATT", AP_BattMonitor),
+
+    // @Group: BRD_
+    // @Path: ../libraries/AP_BoardConfig/AP_BoardConfig.cpp
+    GOBJECT(BoardConfig,            "BRD_",       AP_BoardConfig),
+
+    // GPS driver
+    // @Group: GPS_
+    // @Path: ../libraries/AP_GPS/AP_GPS.cpp
+    GOBJECT(gps, "GPS_", AP_GPS),
+
+#if AP_AHRS_NAVEKF_AVAILABLE
+    // @Group: EKF_
+    // @Path: ../libraries/AP_NavEKF/AP_NavEKF.cpp
+    GOBJECTN(ahrs.get_NavEKF(), NavEKF, "EKF_", NavEKF),
+#endif
+
+    // @Group: MIS_
+    // @Path: ../libraries/AP_Mission/AP_Mission.cpp
+    GOBJECT(mission, "MIS_",       AP_Mission),
+
+	AP_VAREND
+};
+
+/*
+  This is a conversion table from old parameter values to new
+  parameter names. The startup code looks for saved values of the old
+  parameters and will copy them across to the new parameters if the
+  new parameter does not yet have a saved value. It then saves the new
+  value.
+
+  Note that this works even if the old parameter has been removed. It
+  relies on the old k_param index not being removed
+
+  The second column below is the index in the var_info[] table for the
+  old object. This should be zero for top level parameters.
+ */
+const AP_Param::ConversionInfo conversion_table[] PROGMEM = {
+    { Parameters::k_param_battery_monitoring, 0,      AP_PARAM_INT8,  "BATT_MONITOR" },
+    { Parameters::k_param_battery_volt_pin,   0,      AP_PARAM_INT8,  "BATT_VOLT_PIN" },
+    { Parameters::k_param_battery_curr_pin,   0,      AP_PARAM_INT8,  "BATT_CURR_PIN" },
+    { Parameters::k_param_volt_div_ratio,     0,      AP_PARAM_FLOAT, "BATT_VOLT_MULT" },
+    { Parameters::k_param_curr_amp_per_volt,  0,      AP_PARAM_FLOAT, "BATT_AMP_PERVOLT" },
+    { Parameters::k_param_pack_capacity,      0,      AP_PARAM_INT32, "BATT_CAPACITY" },
+};
+
+static void load_parameters(void)
+{
+    if (!AP_Param::check_var_info()) {
+        cliSerial->printf_P(PSTR("Bad var table\n"));        
+        hal.scheduler->panic(PSTR("Bad var table"));
+    }
+
+	if (!g.format_version.load() ||
+	     g.format_version != Parameters::k_format_version) {
+
+		// erase all parameters
+		cliSerial->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);
+		cliSerial->println_P(PSTR("done."));
+    } else {
+	    unsigned long before = micros();
+	    // Load all auto-loaded EEPROM variables
+	    AP_Param::load_all();
+
+	    cliSerial->printf_P(PSTR("load_all took %luus\n"), micros() - before);
+	}
+
+    // set a lower default filter frequency for rovers, due to very
+    // high vibration levels on rough surfaces
+    ins.set_default_filter(5);
+
+    // set a more reasonable default NAVL1_PERIOD for rovers
+    L1_controller.set_default_period(8);
+}

APMrover2/Steering.pde

+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/*****************************************
+* Throttle slew limit
+*****************************************/
+static void throttle_slew_limit(int16_t last_throttle)
+{
+    // if slew limit rate is set to zero then do not slew limit
+    if (g.throttle_slewrate && last_throttle != 0) {                   
+        // limit throttle change by the given percentage per second
+        float temp = g.throttle_slewrate * G_Dt * 0.01f * fabsf(channel_throttle->radio_max - channel_throttle->radio_min);
+        // allow a minimum change of 1 PWM per cycle
+        if (temp < 1) {
+            temp = 1;
+        }
+        channel_throttle->radio_out = constrain_int16(channel_throttle->radio_out, last_throttle - temp, last_throttle + temp);
+    }
+}
+
+/*
+  check for triggering of start of auto mode
+ */
+static bool auto_check_trigger(void)
+{
+    // only applies to AUTO mode
+    if (control_mode != AUTO) {
+        return true;
+    }
+
+    // check for user pressing the auto trigger to off
+    if (auto_triggered && g.auto_trigger_pin != -1 && check_digital_pin(g.auto_trigger_pin) == 1) {
+        gcs_send_text_P(SEVERITY_LOW, PSTR("AUTO triggered off"));
+        auto_triggered = false;
+        return false; 
+    }
+
+    // if already triggered, then return true, so you don't
+    // need to hold the switch down
+    if (auto_triggered) {
+        return true;
+    }
+
+    if (g.auto_trigger_pin == -1 && g.auto_kickstart == 0.0f) {
+        // no trigger configured - let's go!
+        auto_triggered = true;
+        return true;
+    }
+ 
+    if (g.auto_trigger_pin != -1 && check_digital_pin(g.auto_trigger_pin) == 0) {
+        gcs_send_text_P(SEVERITY_LOW, PSTR("Triggered AUTO with pin"));
+        auto_triggered = true;
+        return true;            
+    }
+
+    if (g.auto_kickstart != 0.0f) {
+        float xaccel = ins.get_accel().x;
+        if (xaccel >= g.auto_kickstart) {
+            gcs_send_text_fmt(PSTR("Triggered AUTO xaccel=%.1f"), xaccel);
+            auto_triggered = true;
+            return true;            
+        }
+    }
+
+    return false;   
+}
+
+/*
+  work out if we are going to use pivot steering
+ */
+static bool use_pivot_steering(void)
+{
+    if (control_mode >= AUTO && g.skid_steer_out && g.pivot_turn_angle != 0) {
+        int16_t bearing_error = wrap_180_cd(nav_controller->target_bearing_cd() - ahrs.yaw_sensor) / 100;
+        if (abs(bearing_error) > g.pivot_turn_angle) {
+            return true;
+        }
+    }
+    return false;
+}
+
+
+/*
+  calculate the throtte for auto-throttle modes
+ */
+static void calc_throttle(float target_speed)
+{  
+    if (!auto_check_trigger()) {
+        channel_throttle->servo_out = g.throttle_min.get();
+        return;
+    }
+
+    float throttle_base = (fabsf(target_speed) / g.speed_cruise) * g.throttle_cruise;
+    int throttle_target = throttle_base + throttle_nudge;  
+
+    /*
+      reduce target speed in proportion to turning rate, up to the
+      SPEED_TURN_GAIN percentage.
+    */
+    float steer_rate = fabsf(lateral_acceleration / (g.turn_max_g*GRAVITY_MSS));
+    steer_rate = constrain_float(steer_rate, 0.0, 1.0);
+
+    // use g.speed_turn_gain for a 90 degree turn, and in proportion
+    // for other turn angles
+    int32_t turn_angle = wrap_180_cd(next_navigation_leg_cd - ahrs.yaw_sensor);
+    float speed_turn_ratio = constrain_float(fabsf(turn_angle / 9000.0f), 0, 1);
+    float speed_turn_reduction = (100 - g.speed_turn_gain) * speed_turn_ratio * 0.01f;
+
+    float reduction = 1.0 - steer_rate*speed_turn_reduction;
+    
+    if (control_mode >= AUTO && wp_distance <= g.speed_turn_dist) {
+        // in auto-modes we reduce speed when approaching waypoints
+        float reduction2 = 1.0 - speed_turn_reduction;
+        if (reduction2 < reduction) {
+            reduction = reduction2;
+        }
+    }
+    
+    // reduce the target speed by the reduction factor
+    target_speed *= reduction;
+
+    groundspeed_error = fabsf(target_speed) - ground_speed; 
+    
+    throttle = throttle_target + (g.pidSpeedThrottle.get_pid(groundspeed_error * 100) / 100);
+
+    // also reduce the throttle by the reduction factor. This gives a
+    // much faster response in turns
+    throttle *= reduction;
+
+    if (in_reverse) {
+        channel_throttle->servo_out = constrain_int16(-throttle, -g.throttle_max, -g.throttle_min);
+    } else {
+        channel_throttle->servo_out = constrain_int16(throttle, g.throttle_min, g.throttle_max);
+    }
+
+    if (!in_reverse && g.braking_percent != 0 && groundspeed_error < -g.braking_speederr) {
+        // the user has asked to use reverse throttle to brake. Apply
+        // it in proportion to the ground speed error, but only when
+        // our ground speed error is more than BRAKING_SPEEDERR.
+        //
+        // We use a linear gain, with 0 gain at a ground speed error
+        // of braking_speederr, and 100% gain when groundspeed_error
+        // is 2*braking_speederr
+        float brake_gain = constrain_float(((-groundspeed_error)-g.braking_speederr)/g.braking_speederr, 0, 1);
+        int16_t braking_throttle = g.throttle_max * (g.braking_percent * 0.01f) * brake_gain;
+        channel_throttle->servo_out = constrain_int16(-braking_throttle, -g.throttle_max, -g.throttle_min);
+
+        // temporarily set us in reverse to allow the PWM setting to
+        // go negative
+        set_reverse(true);
+    }
+    
+    if (use_pivot_steering()) {
+        channel_throttle->servo_out = 0;
+    }
+}
+
+/*****************************************
+ * Calculate desired turn angles (in medium freq loop)
+ *****************************************/
+
+static void calc_lateral_acceleration()
+{
+    switch (control_mode) {
+    case AUTO:
+        nav_controller->update_waypoint(prev_WP, next_WP);
+        break;
+
+    case RTL:
+    case GUIDED:
+    case STEERING:
+        nav_controller->update_waypoint(current_loc, next_WP);
+        break;
+    default:
+        return;
+    }
+
+	// Calculate the required turn of the wheels
+
+    // negative error = left turn
+	// positive error = right turn
+    lateral_acceleration = nav_controller->lateral_acceleration();
+    if (use_pivot_steering()) {
+        int16_t bearing_error = wrap_180_cd(nav_controller->target_bearing_cd() - ahrs.yaw_sensor) / 100;
+        if (bearing_error > 0) {
+            lateral_acceleration = g.turn_max_g*GRAVITY_MSS;
+        } else {
+            lateral_acceleration = -g.turn_max_g*GRAVITY_MSS;
+        }
+    }
+}
+
+/*
+  calculate steering angle given lateral_acceleration
+ */
+static void calc_nav_steer()
+{
+    // add in obstacle avoidance
+    lateral_acceleration += (obstacle.turn_angle/45.0f) * g.turn_max_g;
+
+    // constrain to max G force
+    lateral_acceleration = constrain_float(lateral_acceleration, -g.turn_max_g*GRAVITY_MSS, g.turn_max_g*GRAVITY_MSS);
+
+    channel_steer->servo_out = steerController.get_steering_out_lat_accel(lateral_acceleration);
+}
+
+/*****************************************
+* Set the flight control servos based on the current calculated values
+*****************************************/
+static void set_servos(void)
+{
+    static int16_t last_throttle;
+
+    // support a separate steering channel
+    RC_Channel_aux::set_servo_out(RC_Channel_aux::k_steering, channel_steer->pwm_to_angle_dz(0));
+
+	if (control_mode == MANUAL || control_mode == LEARNING) {
+        // do a direct pass through of radio values
+        channel_steer->radio_out       = channel_steer->read();
+        channel_throttle->radio_out    = channel_throttle->read();
+        if (failsafe.bits & FAILSAFE_EVENT_THROTTLE) {
+            // suppress throttle if in failsafe and manual
+            channel_throttle->radio_out = channel_throttle->radio_trim;
+        }
+	} else {       
+        channel_steer->calc_pwm();
+        if (in_reverse) {
+            channel_throttle->servo_out = constrain_int16(channel_throttle->servo_out, 
+                                                          -g.throttle_max,
+                                                          -g.throttle_min);
+        } else {
+            channel_throttle->servo_out = constrain_int16(channel_throttle->servo_out, 
+                                                          g.throttle_min.get(), 
+                                                          g.throttle_max.get());
+        }
+
+        if ((failsafe.bits & FAILSAFE_EVENT_THROTTLE) && control_mode < AUTO) {
+            // suppress throttle if in failsafe
+            channel_throttle->servo_out = 0;
+        }
+
+        // convert 0 to 100% into PWM
+        channel_throttle->calc_pwm();
+
+        // limit throttle movement speed
+        throttle_slew_limit(last_throttle);
+    }
+
+    // record last throttle before we apply skid steering
+    last_throttle = channel_throttle->radio_out;
+
+    if (g.skid_steer_out) {
+        // convert the two radio_out values to skid steering values
+        /*
+          mixing rule:
+          steering = motor1 - motor2
+          throttle = 0.5*(motor1 + motor2)
+          motor1 = throttle + 0.5*steering
+          motor2 = throttle - 0.5*steering
+        */          
+        float steering_scaled = channel_steer->norm_output();
+        float throttle_scaled = channel_throttle->norm_output();
+        float motor1 = throttle_scaled + 0.5*steering_scaled;
+        float motor2 = throttle_scaled - 0.5*steering_scaled;
+        channel_steer->servo_out = 4500*motor1;
+        channel_throttle->servo_out = 100*motor2;
+        channel_steer->calc_pwm();
+        channel_throttle->calc_pwm();
+    }
+
+
+#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS
+	// send values to the PWM timers for output
+	// ----------------------------------------
+    channel_steer->output(); 
+    channel_throttle->output();
+    RC_Channel_aux::output_ch_all();
+#endif
+}
+
+

APMrover2/command_description.txt

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

APMrover2/commands.pde

+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/* Functions in this file:
+	void set_next_WP(const AP_Mission::Mission_Command& cmd)
+	void set_guided_WP(void)
+	void init_home()
+	void restart_nav()
+************************************************************ 
+*/
+
+
+/*
+ *  set_next_WP - sets the target location the vehicle should fly to
+ */
+static void set_next_WP(const struct Location& loc)
+{
+	// copy the current WP into the OldWP slot
+	// ---------------------------------------
+	prev_WP = next_WP;
+
+	// Load the next_WP slot
+	// ---------------------
+	next_WP = loc;
+
+    // are we already past the waypoint? This happens when we jump
+    // waypoints, and it can cause us to skip a waypoint. If we are
+    // past the waypoint when we start on a leg, then use the current
+    // location as the previous waypoint, to prevent immediately
+    // considering the waypoint complete
+    if (location_passed_point(current_loc, prev_WP, next_WP)) {
+        gcs_send_text_P(SEVERITY_LOW, PSTR("Resetting prev_WP"));
+        prev_WP = current_loc;
+    }
+
+	// this is handy for the groundstation
+	wp_totalDistance 	= get_distance(current_loc, next_WP);
+	wp_distance 		= wp_totalDistance;
+}
+
+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;
+
+	// this is handy for the groundstation
+	wp_totalDistance 	= get_distance(current_loc, next_WP);
+	wp_distance 		= wp_totalDistance;
+}
+
+// run this at setup on the ground
+// -------------------------------
+void init_home()
+{
+    if (!have_position) {
+        // we need position information
+        return;
+    }
+
+	gcs_send_text_P(SEVERITY_LOW, PSTR("init home"));
+
+    ahrs.set_home(gps.location());
+	home_is_set = true;
+
+	// Save Home to EEPROM
+	mission.write_home_to_storage();
+
+	// Save prev loc
+	// -------------
+	next_WP = prev_WP = home;
+
+	// Load home for a default guided_WP
+	// -------------
+	guided_WP = home;
+}
+
+static void restart_nav()
+{  
+    g.pidSpeedThrottle.reset_I();
+    prev_WP = current_loc;
+    mission.start_or_resume();
+}

APMrover2/commands_logic.pde

+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+// forward declarations to make compiler happy
+static void do_nav_wp(const AP_Mission::Mission_Command& cmd);
+static void do_wait_delay(const AP_Mission::Mission_Command& cmd);
+static void do_within_distance(const AP_Mission::Mission_Command& cmd);
+static void do_change_speed(const AP_Mission::Mission_Command& cmd);
+static void do_set_home(const AP_Mission::Mission_Command& cmd);
+static bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);
+
+/********************************************************************************/
+// Command Event Handlers
+/********************************************************************************/
+static bool
+start_command(const AP_Mission::Mission_Command& cmd)
+{
+    // log when new commands start
+    if (should_log(MASK_LOG_CMD)) {
+        Log_Write_Cmd(cmd);
+    }
+
+    // exit immediately if not in AUTO mode
+    if (control_mode != AUTO) {
+        return false;
+    }
+
+    gcs_send_text_fmt(PSTR("Executing command ID #%i"),cmd.id);
+
+    // remember the course of our next navigation leg
+    next_navigation_leg_cd = mission.get_next_ground_course_cd(0);
+
+	switch(cmd.id){
+		case MAV_CMD_NAV_WAYPOINT:	// Navigate to Waypoint
+			do_nav_wp(cmd);
+			break;
+
+		case MAV_CMD_NAV_RETURN_TO_LAUNCH:
+			do_RTL();
+			break;
+
+        // Conditional commands
+		case MAV_CMD_CONDITION_DELAY:
+			do_wait_delay(cmd);
+			break;
+
+		case MAV_CMD_CONDITION_DISTANCE:
+			do_within_distance(cmd);
+			break;
+
+        // Do commands
+		case MAV_CMD_DO_CHANGE_SPEED:
+			do_change_speed(cmd);
+			break;
+
+		case MAV_CMD_DO_SET_HOME:
+			do_set_home(cmd);
+			break;
+
+    	case MAV_CMD_DO_SET_SERVO:
+            ServoRelayEvents.do_set_servo(cmd.content.servo.channel, cmd.content.servo.pwm);
+            break;
+
+    	case MAV_CMD_DO_SET_RELAY:
+            ServoRelayEvents.do_set_relay(cmd.content.relay.num, cmd.content.relay.state);
+            break;
+
+    	case MAV_CMD_DO_REPEAT_SERVO:
+            ServoRelayEvents.do_repeat_servo(cmd.content.repeat_servo.channel, cmd.content.repeat_servo.pwm,
+                                             cmd.content.repeat_servo.repeat_count, cmd.content.repeat_servo.cycle_time * 1000.0f);
+            break;
+
+    	case MAV_CMD_DO_REPEAT_RELAY:
+            ServoRelayEvents.do_repeat_relay(cmd.content.repeat_relay.num, cmd.content.repeat_relay.repeat_count,
+                                             cmd.content.repeat_relay.cycle_time * 1000.0f);
+            break;
+
+#if CAMERA == ENABLED
+        case MAV_CMD_DO_CONTROL_VIDEO:                      // Control on-board camera capturing. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty|
+            break;
+
+        case MAV_CMD_DO_DIGICAM_CONFIGURE:                  // Mission command to configure an on-board camera controller system. |Modes: P, TV, AV, M, Etc| Shutter speed: Divisor number for one second| Aperture: F stop number| ISO number e.g. 80, 100, 200, Etc| Exposure type enumerator| Command Identity| Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)|
+            break;
+
+        case MAV_CMD_DO_DIGICAM_CONTROL:                    // Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty|
+            do_take_picture();
+            break;
+
+        case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
+            camera.set_trigger_distance(cmd.content.cam_trigg_dist.meters);
+            break;
+#endif
+
+#if MOUNT == ENABLED
+		// Sets the region of interest (ROI) for a sensor set or the
+		// vehicle itself. This can then be used by the vehicles control
+		// system to control the vehicle attitude and the attitude of various
+		// devices such as cameras.
+		//    |Region of interest mode. (see MAV_ROI enum)| Waypoint index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple cameras etc.)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z|
+        case MAV_CMD_DO_SET_ROI:
+            if (cmd.content.location.alt == 0 && cmd.content.location.lat == 0 && cmd.content.location.lng == 0) {
+                // switch off the camera tracking if enabled
+                if (camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) {
+                    camera_mount.set_mode_to_default();
+                }
+            } else {
+                // send the command to the camera mount
+                camera_mount.set_roi_cmd(&cmd.content.location);
+            }
+            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
+
+		default:
+		    // return false for unhandled commands
+		    return false;
+	}
+
+	// if we got this far we must have been successful
+	return true;
+}
+
+// exit_mission - callback function called from ap-mission when the mission has completed
+//      we double check that the flight mode is AUTO to avoid the possibility of ap-mission triggering actions while we're not in AUTO mode
+static void exit_mission()
+{
+    if (control_mode == AUTO) {
+        gcs_send_text_fmt(PSTR("No commands - setting HOLD"));
+        set_mode(HOLD);
+    }
+}
+
+/********************************************************************************/
+// Verify command Handlers
+//      Returns true if command complete
+/********************************************************************************/
+
+static bool verify_command(const AP_Mission::Mission_Command& cmd)
+{
+    // exit immediately if not in AUTO mode
+    // we return true or we will continue to be called by ap-mission
+    if (control_mode != AUTO) {
+        return true;
+    }
+
+	switch(cmd.id) {
+
+		case MAV_CMD_NAV_WAYPOINT:
+			return verify_nav_wp(cmd);
+
+		case MAV_CMD_NAV_RETURN_TO_LAUNCH:
+			return verify_RTL();
+
+        case MAV_CMD_CONDITION_DELAY:
+            return verify_wait_delay();
+            break;
+
+        case MAV_CMD_CONDITION_DISTANCE:
+            return verify_within_distance();
+            break;
+
+        default:
+            if (cmd.id > MAV_CMD_CONDITION_LAST) {
+                // this is a command that doesn't require verify
+                return true;
+            }
+            gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_conditon: Unsupported command"));
+            return true;
+            break;
+	}
+    return false;
+}
+
+/********************************************************************************/
+//  Nav (Must) commands
+/********************************************************************************/
+
+static void do_RTL(void)
+{
+    prev_WP = current_loc;
+	control_mode 	= RTL;
+	next_WP = home;
+}
+
+static void do_nav_wp(const AP_Mission::Mission_Command& cmd)
+{
+	set_next_WP(cmd.content.location);
+}
+
+/********************************************************************************/
+//  Verify Nav (Must) commands
+/********************************************************************************/
+static bool verify_nav_wp(const AP_Mission::Mission_Command& cmd)
+{
+    if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) {
+        gcs_send_text_fmt(PSTR("Reached Waypoint #%i dist %um"),
+                          (unsigned)cmd.index,
+                          (unsigned)get_distance(current_loc, next_WP));
+        return true;
+    }
+
+    // have we gone past the waypoint?
+    if (location_passed_point(current_loc, prev_WP, next_WP)) {
+        gcs_send_text_fmt(PSTR("Passed Waypoint #%i dist %um"),
+                          (unsigned)cmd.index,
+                          (unsigned)get_distance(current_loc, next_WP));
+        return true;
+    }
+
+    return false;
+}
+
+static bool verify_RTL()
+{
+	if (wp_distance <= g.waypoint_radius) {
+		gcs_send_text_P(SEVERITY_LOW,PSTR("Reached home"));
+                rtl_complete = true;
+		return true;
+	}
+
+    // have we gone past the waypoint?
+    if (location_passed_point(current_loc, prev_WP, next_WP)) {
+        gcs_send_text_fmt(PSTR("Reached Home dist %um"),
+                          (unsigned)get_distance(current_loc, next_WP));
+        return true;
+    }
+
+    return false;
+}
+
+/********************************************************************************/
+//  Condition (May) commands
+/********************************************************************************/
+
+static void do_wait_delay(const AP_Mission::Mission_Command& cmd)
+{
+	condition_start = millis();
+	condition_value  = cmd.content.delay.seconds * 1000;    // convert seconds to milliseconds
+}
+
+static void do_within_distance(const AP_Mission::Mission_Command& cmd)
+{
+	condition_value  = cmd.content.distance.meters;
+}
+
+/********************************************************************************/
+// Verify Condition (May) commands
+/********************************************************************************/
+
+static bool verify_wait_delay()
+{
+	if ((uint32_t)(millis() - condition_start) > (uint32_t)condition_value){
+		condition_value 	= 0;
+		return true;
+	}
+	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_change_speed(const AP_Mission::Mission_Command& cmd)
+{
+	switch (cmd.p1)
+	{
+		case 0:
+			if (cmd.content.speed.target_ms > 0) {
+				g.speed_cruise.set(cmd.content.speed.target_ms);
+                gcs_send_text_fmt(PSTR("Cruise speed: %.1f m/s"), g.speed_cruise.get());
+            }
+			break;
+	}
+
+	if (cmd.content.speed.throttle_pct > 0 && cmd.content.speed.throttle_pct <= 100) {
+		g.throttle_cruise.set(cmd.content.speed.throttle_pct);
+        gcs_send_text_fmt(PSTR("Cruise throttle: %.1f"), g.throttle_cruise.get());
+    }
+}
+
+static void do_set_home(const AP_Mission::Mission_Command& cmd)
+{
+	if(cmd.p1 == 1 && have_position) {
+		init_home();
+	} else {
+        ahrs.set_home(cmd.content.location);
+		home_is_set = true;
+	}
+}
+
+// do_take_picture - take a picture with the camera library
+static void do_take_picture()
+{
+#if CAMERA == ENABLED
+    camera.trigger_pic();
+    gcs_send_message(MSG_CAMERA_FEEDBACK);
+    if (should_log(MASK_LOG_CAMERA)) {
+        DataFlash.Log_Write_Camera(ahrs, gps, current_loc);
+    }
+#endif
+}

APMrover2/commands_process.pde

+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+// called by update navigation at 10Hz
+// --------------------
+static void update_commands(void)
+{
+    if(control_mode == AUTO) {
+        if(home_is_set == true && mission.num_commands() > 1) {
+            mission.update();
+        }
+    }
+}

APMrover2/compat.h

+
+#ifndef __COMPAT_H__
+#define __COMPAT_H__
+
+#define HIGH 1
+#define LOW 0
+
+/* Forward declarations to avoid broken auto-prototyper (coughs on '::'?) */
+static void run_cli(AP_HAL::UARTDriver *port);
+
+#endif // __COMPAT_H__
+

APMrover2/compat.pde

+
+
+static void delay(uint32_t ms)
+{
+    hal.scheduler->delay(ms);
+}
+
+static void mavlink_delay(uint32_t ms)
+{
+    hal.scheduler->delay(ms);
+}
+
+static uint32_t millis()
+{
+    return hal.scheduler->millis();
+}
+
+static uint32_t micros()
+{
+    return hal.scheduler->micros();
+}
+

APMrover2/config.h

+// -*- 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.
+///
+
+#if defined( __AVR_ATmega1280__ )
+ // default choices for a 1280. We can't fit everything in, so we 
+ // make some popular choices by default
+ #define LOGGING_ENABLED DISABLED
+ #ifndef MOUNT
+ # define MOUNT DISABLED
+ #endif
+ #ifndef CAMERA
+ # define CAMERA DISABLED
+ #endif
+#endif
+
+// Just so that it's completely clear...
+#define ENABLED			1
+#define DISABLED		0
+
+//////////////////////////////////////////////////////////////////////////////
+// sensor types
+
+#define CONFIG_BARO     HAL_BARO_DEFAULT
+#define CONFIG_COMPASS  HAL_COMPASS_DEFAULT
+
+#ifdef HAL_SERIAL0_BAUD_DEFAULT
+# define SERIAL0_BAUD HAL_SERIAL0_BAUD_DEFAULT
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// HIL_MODE                                 OPTIONAL
+
+#ifndef HIL_MODE
+ #define HIL_MODE        HIL_MODE_DISABLED
+#endif
+
+#if HIL_MODE != HIL_MODE_DISABLED       // we are in HIL mode
+ #undef CONFIG_BARO
+ #define CONFIG_BARO HAL_BARO_HIL
+ #undef  CONFIG_COMPASS
+ #define CONFIG_COMPASS HAL_COMPASS_HIL
+#endif
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
+# define BATTERY_PIN_1	  0
+# define CURRENT_PIN_1	  1
+#elif CONFIG_HAL_BOARD == HAL_BOARD_APM2
+# define BATTERY_PIN_1	  1
+# define CURRENT_PIN_1	  2
+#elif CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
+# define BATTERY_PIN_1	  1
+# define CURRENT_PIN_1	  2
+#elif CONFIG_HAL_BOARD == HAL_BOARD_PX4
+# define BATTERY_PIN_1	  -1
+# define CURRENT_PIN_1	  -1
+#elif CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE
+# define BATTERY_PIN_1     20
+# define CURRENT_PIN_1	   19
+#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX
+# define BATTERY_PIN_1     -1
+# define CURRENT_PIN_1	   -1
+#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+# define BATTERY_PIN_1	  -1
+# define CURRENT_PIN_1	  -1
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// HIL_MODE                                 OPTIONAL
+
+#ifndef HIL_MODE
+#define HIL_MODE	HIL_MODE_DISABLED
+#endif
+
+#ifndef MAV_SYSTEM_ID
+# define MAV_SYSTEM_ID		1
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// Serial port speeds.
+//
+#ifndef SERIAL0_BAUD
+# define SERIAL0_BAUD			115200
+#endif
+#ifndef SERIAL1_BAUD
+# define SERIAL1_BAUD			 57600
+#endif
+#ifndef SERIAL2_BAUD
+# define SERIAL2_BAUD			 57600
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// FrSky telemetry support
+//
+
+#ifndef FRSKY_TELEM_ENABLED
+#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_APM2
+ # define FRSKY_TELEM_ENABLED DISABLED
+#else
+ # define FRSKY_TELEM_ENABLED ENABLED
+#endif
+#endif
+
+
+#ifndef CH7_OPTION
+# define CH7_OPTION		          CH7_SAVE_WP
+#endif
+
+#ifndef TUNING_OPTION
+# define TUNING_OPTION		          TUN_NONE
+#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			ENABLED
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// MODE
+// MODE_CHANNEL
+//
+#ifndef MODE_CHANNEL
+# define MODE_CHANNEL	8
+#endif
+#if (MODE_CHANNEL != 5) && (MODE_CHANNEL != 6) && (MODE_CHANNEL != 7) && (MODE_CHANNEL != 8)
+# error XXX
+# error XXX You must set MODE_CHANNEL to 5, 6, 7 or 8
+# error XXX
+#endif
+
+#if !defined(MODE_1)
+# define MODE_1			LEARNING
+#endif
+#if !defined(MODE_2)
+# define MODE_2			LEARNING
+#endif
+#if !defined(MODE_3)
+# define MODE_3			LEARNING
+#endif
+#if !defined(MODE_4)
+# define MODE_4			LEARNING
+#endif
+#if !defined(MODE_5)
+# define MODE_5			LEARNING
+#endif
+#if !defined(MODE_6)
+# define MODE_6			MANUAL
+#endif
+
+
+//////////////////////////////////////////////////////////////////////////////
+// failsafe defaults
+#ifndef THROTTLE_FAILSAFE
+# define THROTTLE_FAILSAFE		ENABLED
+#endif
+#ifndef THROTTLE_FS_VALUE
+# define THROTTLE_FS_VALUE		950
+#endif
+#ifndef LONG_FAILSAFE_ACTION
+# define LONG_FAILSAFE_ACTION		0
+#endif
+#ifndef GCS_HEARTBEAT_FAILSAFE
+# define GCS_HEARTBEAT_FAILSAFE		DISABLED
+#endif
+
+
+//////////////////////////////////////////////////////////////////////////////
+// THROTTLE_OUT
+//
+#ifndef THROTTE_OUT
+# define THROTTLE_OUT			ENABLED
+#endif
+
+
+//////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////
+// STARTUP BEHAVIOUR
+//////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////
+
+//////////////////////////////////////////////////////////////////////////////
+// GROUND_START_DELAY
+//
+#ifndef GROUND_START_DELAY
+# define GROUND_START_DELAY		0
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// MOUNT (ANTENNA OR CAMERA)
+//
+#ifndef MOUNT
+# define MOUNT		ENABLED
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// CAMERA control
+//
+#ifndef CAMERA
+# define CAMERA ENABLED
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// AIRSPEED_CRUISE
+//
+#ifndef SPEED_CRUISE
+# define SPEED_CRUISE		3 // 3 m/s
+#endif
+
+#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
+
+//////////////////////////////////////////////////////////////////////////////
+// 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			100
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// Attitude control gains
+//
+#ifndef SERVO_STEER_P
+# define SERVO_STEER_P         0.4
+#endif
+#ifndef SERVO_STEER_I
+# define SERVO_STEER_I         0.0
+#endif
+#ifndef SERVO_STEER_D
+# define SERVO_STEER_D         0.0
+#endif
+#ifndef SERVO_STEER_INT_MAX
+# define SERVO_STEER_INT_MAX   5
+#endif
+#define SERVO_STEER_INT_MAX_CENTIDEGREE SERVO_STEER_INT_MAX*100
+
+
+//////////////////////////////////////////////////////////////////////////////
+// Crosstrack compensation
+//
+#ifndef XTRACK_GAIN
+# define XTRACK_GAIN          1 // deg/m
+#endif
+#ifndef XTRACK_ENTRY_ANGLE
+# define XTRACK_ENTRY_ANGLE   50 // deg
+#endif
+# define XTRACK_GAIN_SCALED XTRACK_GAIN*100
+# define XTRACK_ENTRY_ANGLE_CENTIDEGREE XTRACK_ENTRY_ANGLE*100
+
+//////////////////////////////////////////////////////////////////////////////
+// Dataflash logging control
+//
+#ifndef LOGGING_ENABLED
+# define LOGGING_ENABLED		ENABLED
+#endif
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_APM2
+#define DEFAULT_LOG_BITMASK     \
+    MASK_LOG_ATTITUDE_MED | \
+    MASK_LOG_GPS | \
+    MASK_LOG_PM | \
+    MASK_LOG_CTUN | \
+    MASK_LOG_NTUN | \
+    MASK_LOG_MODE | \
+    MASK_LOG_CMD | \
+    MASK_LOG_SONAR | \
+    MASK_LOG_COMPASS | \
+    MASK_LOG_CURRENT | \
+    MASK_LOG_STEERING | \
+    MASK_LOG_CAMERA
+#else
+// other systems have plenty of space for full logs
+#define DEFAULT_LOG_BITMASK   0xffff
+#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
+
+// 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
+
+#ifndef BOOSTER
+# define BOOSTER              2    // booster factor x1 = 1 or x2 = 2
+#endif
+
+#ifndef SONAR_ENABLED
+# define SONAR_ENABLED       DISABLED
+#endif
+
+/*
+  build a firmware version string.
+  GIT_VERSION comes from Makefile builds
+*/
+#ifndef GIT_VERSION
+#define FIRMWARE_STRING THISFIRMWARE
+#else
+#define FIRMWARE_STRING THISFIRMWARE " (" GIT_VERSION ")"
+#endif

APMrover2/control_modes.pde

+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+static void read_control_switch()
+{
+	
+	uint8_t switchPosition = readSwitch();
+	
+	// If switchPosition = 255 this indicates that the mode control channel input was out of range
+	// If we get this value we do not want to change modes.
+	if(switchPosition == 255) return;
+
+    if (hal.scheduler->millis() - failsafe.last_valid_rc_ms > 100) {
+        // only use signals that are less than 0.1s old.
+        return;
+    }
+
+    // we look for changes in the switch position. If the
+    // RST_SWITCH_CH parameter is set, then it is a switch that can be
+    // used to force re-reading of the control switch. This is useful
+    // when returning to the previous mode after a failsafe or fence
+    // breach. This channel is best used on a momentary switch (such
+    // as a spring loaded trainer switch).
+	if (oldSwitchPosition != switchPosition ||
+        (g.reset_switch_chan != 0 && 
+         hal.rcin->read(g.reset_switch_chan-1) > RESET_SWITCH_CHAN_PWM)) {
+
+		set_mode((enum mode)modes[switchPosition].get());
+
+		oldSwitchPosition = switchPosition;
+		prev_WP = current_loc;
+
+		// reset speed integrator
+        g.pidSpeedThrottle.reset_I();
+	}
+
+}
+
+static uint8_t readSwitch(void){
+    uint16_t pulsewidth = hal.rcin->read(g.mode_channel - 1);
+	if (pulsewidth <= 900 || pulsewidth >= 2200) 	return 255;	// This is an error condition
+	if (pulsewidth > 1230 && pulsewidth <= 1360) 	return 1;
+	if (pulsewidth > 1360 && pulsewidth <= 1490) 	return 2;
+	if (pulsewidth > 1490 && pulsewidth <= 1620) 	return 3;
+	if (pulsewidth > 1620 && pulsewidth <= 1749) 	return 4;	// Software Manual
+	if (pulsewidth >= 1750) 						return 5;	// Hardware Manual
+	return 0;
+}
+
+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()
+{
+    switch ((enum ch7_option)g.ch7_option.get()) {
+    case CH7_DO_NOTHING:
+        break;
+    case CH7_SAVE_WP:
+		if (channel_learn->radio_in > CH_7_PWM_TRIGGER) {
+            // switch is engaged
+			ch7_flag = true;
+		} else { // switch is disengaged
+			if (ch7_flag) {
+				ch7_flag = false;
+
+				if (control_mode == MANUAL) {
+                    hal.console->println_P(PSTR("Erasing waypoints"));
+                    // if SW7 is ON in MANUAL = Erase the Flight Plan
+					mission.clear();
+                    if (channel_steer->control_in > 3000) {
+						// if roll is full right store the current location as home
+                        init_home();
+                    }
+					return;
+				} else if (control_mode == LEARNING || control_mode == STEERING) {    
+                    // if SW7 is ON in LEARNING = record the Wp
+
+				    // create new mission command
+				    AP_Mission::Mission_Command cmd = {};
+
+				    // set new waypoint to current location
+				    cmd.content.location = current_loc;
+
+				    // make the new command to a waypoint
+				    cmd.id = MAV_CMD_NAV_WAYPOINT;
+
+				    // save command
+				    if(mission.add_cmd(cmd)) {
+                        hal.console->printf_P(PSTR("Learning waypoint %u"), (unsigned)mission.num_commands());
+				    }
+                } else if (control_mode == AUTO) {    
+                    // if SW7 is ON in AUTO = set to RTL  
+                    set_mode(RTL);
+                }
+            }
+        }
+        break;
+    }
+}
+