Copter: version to 3.3.4-pixracer

this avoids a bus locking issue on Pixracer

.gitmodules

@@ -7,3 +7,6 @@
 [submodule "modules/uavcan"]
 	path = modules/uavcan
 	url = git://github.com/diydrones/uavcan.git
+[submodule "modules/mavlink"]
+	path = modules/mavlink
+	url = git://github.com/diydrones/mavlink

.travis.yml

@@ -1,11 +1,26 @@
 language: cpp
 sudo: required
 
-before_install:
-  -  APMDIR=$(pwd) && pushd .. && $APMDIR/Tools/scripts/install-travis-env.sh -y && . ~/.profile && popd
+cache:
+  directories:
+    - $HOME/opt
 
-script: 
-  - Tools/scripts/build_all_travis.sh
+addons:
+  coverity_scan:
+    project:
+      name: "diydrones/ardupilot"
+      description: "Build submitted via Travis CI"
+    notification_email: andrew-scan@tridgell.net
+    build_command_prepend: "make clean"
+    build_command:   "make"
+    branch_pattern: coverity_scan
+
+before_install:
+  - APMDIR=$(pwd) && pushd .. && $APMDIR/Tools/scripts/configure-ci.sh && . ~/.profile && popd
+
+script:
+  - python Tools/autotest/param_metadata/param_parse.py
+  - Tools/scripts/build_ci.sh
 
 notifications:
   webhooks:
@@ -21,15 +36,5 @@ env:
     #   via the "travis encrypt" command using the project repo's public key
     - secure: "FjIwqZQV2FhNPWYITX5LZXTE38yYqBaQdbm3QmbEg/30wnPTm1ZOLIU7o/aSvX615ImR8kHoryvFPDQDWc6wWfqTEs3Ytq2kIvcIJS2Y5l/0PFfpWJoH5gRd6hDThnoi+1oVMLvj1+bhn4yFlCCQ2vT/jxoGfiQqqgvHtv4fLzI="
   matrix:
-    - TRAVIS_BUILD_TARGET="px4-v2"
-    - TRAVIS_BUILD_TARGET="sitl linux apm2 navio"
-
-addons:
-  coverity_scan:
-    project:
-      name: "diydrones/ardupilot"
-      description: "Build submitted via Travis CI"
-    notification_email: andrew-scan@tridgell.net
-    build_command_prepend: "make clean"
-    build_command:   "make"
-    branch_pattern: coverity_scan
+    - CI_BUILD_TARGET="px4-v2 sitl linux"
+    - CI_BUILD_TARGET="navio raspilot minlure bebop"

APMrover2/APMrover2.cpp

@@ -20,9 +20,9 @@
    ArduPlane by Jean-Louis Naudin (JLN), and then rewritten after the
    AP_HAL merge by Andrew Tridgell
 
-   Maintainer: Andrew Tridgell
+   Maintainer: Grant Morphett
 
-   Authors:    Doug Weibel, Jose Julio, Jordi Munoz, Jason Short, Andrew Tridgell, Randy Mackay, Pat Hickey, John Arne Birkeland, Olivier Adler, Jean-Louis Naudin
+   Authors:    Doug Weibel, Jose Julio, Jordi Munoz, Jason Short, Andrew Tridgell, Randy Mackay, Pat Hickey, John Arne Birkeland, Olivier Adler, Jean-Louis Naudin, Grant Morphett
 
    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 
 
@@ -124,7 +124,17 @@ void Rover::loop()
 
     // tell the scheduler one tick has passed
     scheduler.tick();
-    scheduler.run(19500U);
+
+    // run all the tasks that are due to run. Note that we only
+    // have to call this once per loop, as the tasks are scheduled
+    // in multiples of the main loop tick. So if they don't run on
+    // the first call to the scheduler they won't run on a later
+    // call until scheduler.tick() is called again
+    uint32_t remaining = (timer + 20000) - micros();
+    if (remaining > 19500) {
+        remaining = 19500;
+    }
+    scheduler.run(remaining);
 }
 
 // update AHRS system
@@ -376,17 +386,15 @@ void Rover::update_current_mode(void)
 
     case GUIDED:
         set_reverse(false);
-        if (!rtl_complete) {
-            if (verify_RTL()) {
-                // we have reached destination so stop where we are
-                channel_throttle->servo_out = g.throttle_min.get();
-                channel_steer->servo_out = 0;
-                lateral_acceleration = 0;
-            } else {
-                calc_lateral_acceleration();
-                calc_nav_steer();
-                calc_throttle(g.speed_cruise);
-            }
+        if (rtl_complete || verify_RTL()) {
+            // we have reached destination so stop where we are
+            channel_throttle->servo_out = g.throttle_min.get();
+            channel_steer->servo_out = 0;
+            lateral_acceleration = 0;
+        } else {
+            calc_lateral_acceleration();
+            calc_nav_steer();
+            calc_throttle(g.speed_cruise);
         }
         break;
 
@@ -475,13 +483,11 @@ void Rover::update_navigation()
         // no loitering around the wp with the rover, goes direct to the wp position
         calc_lateral_acceleration();
         calc_nav_steer();
-        if (!rtl_complete) {
-            if (verify_RTL()) {
-                // we have reached destination so stop where we are
-                channel_throttle->servo_out = g.throttle_min.get();
-                channel_steer->servo_out = 0;
-                lateral_acceleration = 0;
-            }
+        if (rtl_complete || verify_RTL()) {
+            // we have reached destination so stop where we are
+            channel_throttle->servo_out = g.throttle_min.get();
+            channel_steer->servo_out = 0;
+            lateral_acceleration = 0;
         }
         break;
     }

APMrover2/GCS_Mavlink.cpp

@@ -5,10 +5,6 @@
 // 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)
 
-// check if a message will fit in the payload space available
-#define HAVE_PAYLOAD_SPACE(chan, id) (comm_get_txspace(chan) >= MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_ ## id ## _LEN)
-#define CHECK_PAYLOAD_SIZE(id) if (txspace < MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_## id ##_LEN) return false
-
 void Rover::send_heartbeat(mavlink_channel_t chan)
 {
     uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
@@ -169,6 +165,16 @@ void Rover::send_extended_status1(mavlink_channel_t chan)
         battery_current = battery.current_amps() * 100;
     }
 
+    if (sonar.num_sensors() > 0) {
+        control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
+        if (g.sonar_trigger_cm > 0) {
+            control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
+        }
+        if (sonar.has_data()) {
+            control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
+        }
+    }
+
     if (AP_Notify::flags.initialising) {
         // while initialising the gyros and accels are not enabled
         control_sensors_enabled &= ~(MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL);
@@ -406,8 +412,6 @@ bool Rover::telemetry_delayed(mavlink_channel_t chan)
 // 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 (rover.telemetry_delayed(chan)) {
         return false;
     }
@@ -580,10 +584,16 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
         rover.send_pid_tuning(chan);
         break;
 
+    case MSG_MISSION_ITEM_REACHED:
+        CHECK_PAYLOAD_SIZE(MISSION_ITEM_REACHED);
+        mavlink_msg_mission_item_reached_send(chan, mission_item_reached_index);
+        break;
+
     case MSG_RETRY_DEFERRED:
     case MSG_TERRAIN:
     case MSG_OPTICAL_FLOW:
     case MSG_GIMBAL_REPORT:
+    case MSG_RPM:
         break; // just here to prevent a warning
 	}
 
@@ -1016,7 +1026,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 
         case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: {
             if (is_equal(packet.param1,1.0f)) {
-                rover.gcs[chan-MAVLINK_COMM_0].send_autopilot_version();
+                rover.gcs[chan-MAVLINK_COMM_0].send_autopilot_version(FIRMWARE_VERSION);
                 result = MAV_RESULT_ACCEPTED;
             }
             break;
@@ -1258,7 +1268,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 #endif
 
     case MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST:
-        rover.gcs[chan-MAVLINK_COMM_0].send_autopilot_version();
+        rover.gcs[chan-MAVLINK_COMM_0].send_autopilot_version(FIRMWARE_VERSION);
         break;
 
     } // end switch
@@ -1310,6 +1320,19 @@ void Rover::gcs_send_message(enum ap_message id)
     }
 }
 
+/*
+ *  send a mission item reached message and load the index before the send attempt in case it may get delayed
+ */
+void Rover::gcs_send_mission_item_reached_message(uint16_t mission_index)
+{
+    for (uint8_t i=0; i<num_gcs; i++) {
+        if (gcs[i].initialised) {
+            gcs[i].mission_item_reached_index = mission_index;
+            gcs[i].send_message(MSG_MISSION_ITEM_REACHED);
+        }
+    }
+}
+
 /*
  *  send data streams in the given rate range on both links
  */

APMrover2/Parameters.cpp

@@ -32,7 +32,7 @@ const AP_Param::Info Rover::var_info[] PROGMEM = {
 
     // @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.
+    // @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. Usually 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),
@@ -310,7 +310,7 @@ const AP_Param::Info Rover::var_info[] PROGMEM = {
 
     // @Param: FS_THR_VALUE
     // @DisplayName: Throttle Failsafe Value
-    // @Description: The PWM level on channel 3 below which throttle sailsafe triggers.
+    // @Description: The PWM level on channel 3 below which throttle failsafe triggers.
     // @Range: 925 1100
     // @Increment: 1
     // @User: Standard

APMrover2/Parameters.h

@@ -3,7 +3,7 @@
 #ifndef PARAMETERS_H
 #define PARAMETERS_H
 
-#include <AP_Common.h>
+#include <AP_Common/AP_Common.h>
 
 // Global parameter class.
 //

APMrover2/Rover.cpp

@@ -32,7 +32,7 @@ Rover::Rover(void) :
     steerController(ahrs),
     mission(ahrs,
             FUNCTOR_BIND_MEMBER(&Rover::start_command, bool, const AP_Mission::Mission_Command&),
-            FUNCTOR_BIND_MEMBER(&Rover::verify_command, bool, const AP_Mission::Mission_Command&),
+            FUNCTOR_BIND_MEMBER(&Rover::verify_command_callback, bool, const AP_Mission::Mission_Command&),
             FUNCTOR_BIND_MEMBER(&Rover::exit_mission, void)),
     num_gcs(MAVLINK_COMM_NUM_BUFFERS),
     ServoRelayEvents(relay),

APMrover2/Rover.h

@@ -20,69 +20,70 @@
 #ifndef _ROVER_H_
 #define _ROVER_H_
 
-#define THISFIRMWARE "ArduRover v2.50"
+#define THISFIRMWARE "ArduRover v2.51-beta"
+#define FIRMWARE_VERSION 2,51,0,FIRMWARE_VERSION_TYPE_BETA
 
 #include <math.h>
 #include <stdarg.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_SerialManager.h>   // Serial manager library
-#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 <AP_Common/AP_Common.h>
+#include <AP_Progmem/AP_Progmem.h>
+#include <AP_HAL/AP_HAL.h>
+#include <AP_Menu/AP_Menu.h>
+#include <AP_Param/AP_Param.h>
+#include <StorageManager/StorageManager.h>
+#include <AP_GPS/AP_GPS.h>         // ArduPilot GPS library
+#include <AP_ADC/AP_ADC.h>         // ArduPilot Mega Analog to Digital Converter Library
+#include <AP_ADC_AnalogSource/AP_ADC_AnalogSource.h>
+#include <AP_Baro/AP_Baro.h>
+#include <AP_Compass/AP_Compass.h>     // ArduPilot Mega Magnetometer Library
+#include <AP_Math/AP_Math.h>        // ArduPilot Mega Vector/Matrix math Library
+#include <AP_InertialSensor/AP_InertialSensor.h> // Inertial Sensor (uncalibated IMU) Library
+#include <AP_AHRS/AP_AHRS.h>         // ArduPilot Mega DCM Library
+#include <AP_NavEKF/AP_NavEKF.h>
+#include <AP_Mission/AP_Mission.h>     // Mission command library
+#include <AP_Rally/AP_Rally.h>
+#include <AP_Terrain/AP_Terrain.h>
+#include <PID/PID.h>            // PID library
+#include <RC_Channel/RC_Channel.h>     // RC Channel Library
+#include <AP_RangeFinder/AP_RangeFinder.h>	// Range finder library
+#include <Filter/Filter.h>			// Filter library
+#include <Filter/Butter.h>			// Filter library - butterworth filter
+#include <AP_Buffer/AP_Buffer.h>      // FIFO buffer library
+#include <Filter/ModeFilter.h>		// Mode Filter from Filter library
+#include <Filter/AverageFilter.h>	// Mode Filter from Filter library
+#include <AP_Relay/AP_Relay.h>       // APM relay
+#include <AP_ServoRelayEvents/AP_ServoRelayEvents.h>
+#include <AP_Mount/AP_Mount.h>		// Camera/Antenna mount
+#include <AP_Camera/AP_Camera.h>		// Camera triggering
+#include <GCS_MAVLink/GCS_MAVLink.h>    // MAVLink GCS definitions
+#include <AP_SerialManager/AP_SerialManager.h>   // Serial manager library
+#include <AP_Airspeed/AP_Airspeed.h>    // needed for AHRS build
+#include <AP_Vehicle/AP_Vehicle.h>     // needed for AHRS build
+#include <DataFlash/DataFlash.h>
+#include <AP_RCMapper/AP_RCMapper.h>        // RC input mapping library
+#include <SITL/SITL.h>
+#include <AP_Scheduler/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_Navigation/AP_Navigation.h>
+#include <APM_Control/APM_Control.h>
+#include <AP_L1_Control/AP_L1_Control.h>
+#include <AP_BoardConfig/AP_BoardConfig.h>
+#include <AP_Frsky_Telem/AP_Frsky_Telem.h>
 
-#include <AP_HAL_AVR.h>
-#include <AP_HAL_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 <AP_HAL_AVR/AP_HAL_AVR.h>
+#include <AP_HAL_SITL/AP_HAL_SITL.h>
+#include <AP_HAL_PX4/AP_HAL_PX4.h>
+#include <AP_HAL_VRBRAIN/AP_HAL_VRBRAIN.h>
+#include <AP_HAL_FLYMAPLE/AP_HAL_FLYMAPLE.h>
+#include <AP_HAL_Linux/AP_HAL_Linux.h>
+#include <AP_HAL_Empty/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
+#include <AP_Notify/AP_Notify.h>      // Notify library
+#include <AP_BattMonitor/AP_BattMonitor.h> // Battery monitor library
+#include <AP_OpticalFlow/AP_OpticalFlow.h>     // Optical Flow library
 
 // Configuration
 #include "config.h"
@@ -90,9 +91,9 @@
 // Local modules
 #include "defines.h"
 #include "Parameters.h"
-#include "GCS.h"
+#include <GCS_MAVLink/GCS.h>
 
-#include <AP_Declination.h> // ArduPilot Mega Declination Helper Library
+#include <AP_Declination/AP_Declination.h> // ArduPilot Mega Declination Helper Library
 
 class Rover {
 public:
@@ -358,6 +359,12 @@ private:
     static const AP_Param::Info var_info[];
     static const LogStructure log_structure[];
 
+    // Loiter control
+    uint16_t loiter_time_max; // How long we should loiter at the nav_waypoint (time in seconds)
+    uint32_t loiter_time;     // How long have we been loitering - The start time in millis
+
+    float distance_past_wp; // record the distance we have gone past the wp
+
 private:
     // private member functions
     void ahrs_update();
@@ -390,6 +397,7 @@ private:
     void send_statustext(mavlink_channel_t chan);
     bool telemetry_delayed(mavlink_channel_t chan);
     void gcs_send_message(enum ap_message id);
+    void gcs_send_mission_item_reached_message(uint16_t mission_index);
     void gcs_data_stream_send(void);
     void gcs_update(void);
     void gcs_send_text_P(gcs_severity severity, const prog_char_t *str);
@@ -484,6 +492,7 @@ private:
     void print_mode(AP_HAL::BetterStream *port, uint8_t mode);
     bool start_command(const AP_Mission::Mission_Command& cmd);
     bool verify_command(const AP_Mission::Mission_Command& cmd);
+    bool verify_command_callback(const AP_Mission::Mission_Command& cmd);
     void do_nav_wp(const AP_Mission::Mission_Command& cmd);
     bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);
     void do_wait_delay(const AP_Mission::Mission_Command& cmd);
@@ -492,6 +501,7 @@ private:
     void do_set_home(const AP_Mission::Mission_Command& cmd);
     void do_digicam_configure(const AP_Mission::Mission_Command& cmd);
     void do_digicam_control(const AP_Mission::Mission_Command& cmd);
+    void init_capabilities(void);
 
 public:
     bool print_log_menu(void);

APMrover2/Steering.cpp

@@ -85,8 +85,10 @@ bool Rover::use_pivot_steering(void)
   calculate the throtte for auto-throttle modes
  */
 void Rover::calc_throttle(float target_speed)
-{  
-    if (!auto_check_trigger()) {
+{
+    // If not autostarting OR we are loitering at a waypoint
+    // then set the throttle to minimum
+    if (!auto_check_trigger() || ((loiter_time > 0) && (control_mode == AUTO))) {
         channel_throttle->servo_out = g.throttle_min.get();
         return;
     }
@@ -196,6 +198,12 @@ void Rover::calc_lateral_acceleration()
  */
 void Rover::calc_nav_steer()
 {
+    // check to see if the rover is loitering
+    if ((loiter_time > 0) && (control_mode == AUTO)) {
+        channel_steer->servo_out = 0;
+        return;
+    }
+
     // add in obstacle avoidance
     lateral_acceleration += (obstacle.turn_angle/45.0f) * g.turn_max_g;
 

APMrover2/capabilities.cpp

@@ -0,0 +1,9 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#include "Rover.h"
+
+void Rover::init_capabilities(void)
+{
+    hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT);
+    hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT);
+}

APMrover2/commands_logic.cpp

@@ -122,6 +122,22 @@ void Rover::exit_mission()
     }
 }
 
+// verify_command_callback - callback function called from ap-mission at 10hz or higher when a command is being run
+//      we double check that the flight mode is AUTO to avoid the possibility of ap-mission triggering actions while we're not in AUTO mode
+bool Rover::verify_command_callback(const AP_Mission::Mission_Command& cmd)
+{
+    if (control_mode == AUTO) {
+        bool cmd_complete = verify_command(cmd);
+
+        // send message to GCS
+        if (cmd_complete) {
+            gcs_send_mission_item_reached_message(cmd.index);
+        }
+
+        return cmd_complete;
+    }
+    return false;
+}
 /********************************************************************************/
 // Verify command Handlers
 //      Returns true if command complete
@@ -129,12 +145,6 @@ void Rover::exit_mission()
 
 bool Rover::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:
@@ -145,11 +155,9 @@ bool Rover::verify_command(const AP_Mission::Mission_Command& cmd)
 
         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) {
@@ -158,7 +166,6 @@ bool Rover::verify_command(const AP_Mission::Mission_Command& cmd)
             }
             gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_conditon: Unsupported command"));
             return true;
-            break;
 	}
     return false;
 }
@@ -176,6 +183,14 @@ void Rover::do_RTL(void)
 
 void Rover::do_nav_wp(const AP_Mission::Mission_Command& cmd)
 {
+    // this will be used to remember the time in millis after we reach or pass the WP.
+    loiter_time = 0;
+    // this is the delay, stored in seconds
+    loiter_time_max = abs(cmd.p1);
+
+    // this is the distance we travel past the waypoint - not there yet so 0 initially
+    distance_past_wp = 0;
+
 	set_next_WP(cmd.content.location);
 }
 
@@ -185,17 +200,45 @@ void Rover::do_nav_wp(const AP_Mission::Mission_Command& cmd)
 bool Rover::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));
+        // Check if we need to loiter at this waypoint
+        if (loiter_time_max > 0) {
+            if (loiter_time == 0) {  // check if we are just starting loiter
+                gcs_send_text_fmt(PSTR("Reached Waypoint #%i - Loiter for %i seconds"),
+                                  (unsigned)cmd.index,
+                                  (unsigned)loiter_time_max);
+                // record the current time i.e. start timer
+                loiter_time = millis();
+            }
+            // Check if we have loiter long enough
+            if (((millis() - loiter_time) / 1000) < loiter_time_max) {
+                return false;
+            }
+        } else {
+            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?
+    // We should always go through the waypoint i.e. the above code
+    // first before we go past it.
     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));
+        // check if we have gone futher past the wp then last time and output new message if we have
+        if ((uint32_t)distance_past_wp != (uint32_t)get_distance(current_loc, next_WP)) {
+            distance_past_wp = get_distance(current_loc, next_WP);
+            gcs_send_text_fmt(PSTR("Passed Waypoint #%i dist %um"),
+                              (unsigned)cmd.index,
+                              (unsigned)distance_past_wp);
+        }
+        // Check if we need to loiter at this waypoint
+        if (loiter_time_max > 0) {
+            if (((millis() - loiter_time) / 1000) < loiter_time_max) {
+                return false;
+            }
+        }
+        distance_past_wp = 0;
         return true;
     }
 

APMrover2/radio.cpp

@@ -55,8 +55,13 @@ void Rover::read_radio()
 
 	channel_throttle->servo_out = channel_throttle->control_in;
 
-	if (abs(channel_throttle->servo_out) > 50) {
-        throttle_nudge = (g.throttle_max - g.throttle_cruise) * ((fabsf(channel_throttle->norm_input())-0.5f) / 0.5f);
+    // Check if the throttle value is above 50% and we need to nudge
+    // Make sure its above 50% in the direction we are travelling
+	if ((abs(channel_throttle->servo_out) > 50) &&
+        (((channel_throttle->servo_out < 0) && in_reverse) ||
+         ((channel_throttle->servo_out > 0) && !in_reverse))) {
+            throttle_nudge = (g.throttle_max - g.throttle_cruise) *
+                ((fabsf(channel_throttle->norm_input())-0.5f) / 0.5f);
 	} else {
 		throttle_nudge = 0;
 	}

APMrover2/system.cpp

@@ -210,6 +210,8 @@ void Rover::init_ardupilot()
     }
 #endif
 
+	init_capabilities();
+
 	startup_ground();
     Log_Write_Startup(TYPE_GROUNDSTART_MSG);
 
@@ -277,6 +279,11 @@ void Rover::set_mode(enum mode mode)
 		// don't switch modes if we are already in the correct mode.
 		return;
 	}
+
+    // If we are changing out of AUTO mode reset the loiter timer
+    if (control_mode == AUTO)
+        loiter_time = 0;
+
 	control_mode = mode;
     throttle_last = 0;
     throttle = 500;

AntennaTracker/GCS_Mavlink.cpp

@@ -8,9 +8,6 @@
 // use this to prevent recursion during sensor init
 static bool in_mavlink_delay;
 
-// 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!!
  *
@@ -165,11 +162,19 @@ void Tracker::send_simstate(mavlink_channel_t chan)
 #endif
 }
 
+void GCS_MAVLINK::handle_guided_request(AP_Mission::Mission_Command&)
+{
+    // do nothing
+}
+
+void GCS_MAVLINK::handle_change_alt_request(AP_Mission::Mission_Command&)
+{
+    // do nothing
+}
 
 // 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);
     switch (id) {
     case MSG_HEARTBEAT:
         CHECK_PAYLOAD_SIZE(HEARTBEAT);
@@ -276,6 +281,8 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
     case MSG_EKF_STATUS_REPORT:
     case MSG_PID_TUNING:
     case MSG_VIBRATION:
+    case MSG_RPM:
+    case MSG_MISSION_ITEM_REACHED:
         break; // just here to prevent a warning
     }
     return true;
@@ -684,7 +691,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 
             case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: {
                 if (is_equal(packet.param1,1.0f)) {
-                    tracker.gcs[chan-MAVLINK_COMM_0].send_autopilot_version();
+                    tracker.gcs[chan-MAVLINK_COMM_0].send_autopilot_version(FIRMWARE_VERSION);
                     result = MAV_RESULT_ACCEPTED;
                 }
                 break;
@@ -849,7 +856,7 @@ mission_failed:
 #endif
 
     case MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST:
-        tracker.gcs[chan-MAVLINK_COMM_0].send_autopilot_version();
+        tracker.gcs[chan-MAVLINK_COMM_0].send_autopilot_version(FIRMWARE_VERSION);
         break;
 
     } // end switch
@@ -968,4 +975,3 @@ void Tracker::gcs_retry_deferred(void)
 {
     gcs_send_message(MSG_RETRY_DEFERRED);
 }
-

AntennaTracker/Parameters.h

@@ -3,7 +3,7 @@
 #ifndef PARAMETERS_H
 #define PARAMETERS_H
 
-#include <AP_Common.h>
+#include <AP_Common/AP_Common.h>
 
 // Global parameter class.
 //

AntennaTracker/ReleaseNotes.txt

@@ -1,5 +1,9 @@
 Antenna Tracker Release Notes:
 ------------------------------------------------------------------
+AntennaTracker 0.7.2 1-Aug-2015
+Changes from 0.7.1
+1) Fixed Pitch
+------------------------------------------------------------------
 AntennaTracker 0.7.1 29-May-2015
 Changes from 0.7
 1) Added support for continuous rotation (CR) servos

AntennaTracker/Tracker.h

@@ -1,6 +1,8 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
-#define THISFIRMWARE "AntennaTracker V0.7.1"
+#define THISFIRMWARE "AntennaTracker V0.7.2"
+#define FIRMWARE_VERSION 0,7,2,FIRMWARE_VERSION_TYPE_DEV
+
 /*
    Lead developers: Matthew Ridley and Andrew Tridgell
  
@@ -28,57 +30,57 @@
 #include <stdarg.h>
 #include <stdio.h>
 
-#include <AP_Common.h>
-#include <AP_Progmem.h>
-#include <AP_HAL.h>
-#include <AP_Param.h>
-#include <StorageManager.h>
-#include <AP_GPS.h>         // ArduPilot GPS library
-#include <AP_Baro.h>        // ArduPilot barometer library
-#include <AP_Compass.h>     // ArduPilot Mega Magnetometer Library
-#include <AP_Math.h>        // ArduPilot Mega Vector/Matrix math Library
-#include <AP_ADC.h>         // ArduPilot Mega Analog to Digital Converter Library
-#include <AP_ADC_AnalogSource.h>
-#include <AP_InertialSensor.h> // Inertial Sensor Library
-#include <AP_AHRS.h>         // ArduPilot Mega DCM Library
-#include <Filter.h>                     // Filter library
-#include <AP_Buffer.h>      // APM FIFO Buffer
-#include <memcheck.h>
+#include <AP_Common/AP_Common.h>
+#include <AP_Progmem/AP_Progmem.h>
+#include <AP_HAL/AP_HAL.h>
+#include <AP_Param/AP_Param.h>
+#include <StorageManager/StorageManager.h>
+#include <AP_GPS/AP_GPS.h>         // ArduPilot GPS library
+#include <AP_Baro/AP_Baro.h>        // ArduPilot barometer library
+#include <AP_Compass/AP_Compass.h>     // ArduPilot Mega Magnetometer Library
+#include <AP_Math/AP_Math.h>        // ArduPilot Mega Vector/Matrix math Library
+#include <AP_ADC/AP_ADC.h>         // ArduPilot Mega Analog to Digital Converter Library
+#include <AP_ADC_AnalogSource/AP_ADC_AnalogSource.h>
+#include <AP_InertialSensor/AP_InertialSensor.h> // Inertial Sensor Library
+#include <AP_AHRS/AP_AHRS.h>         // ArduPilot Mega DCM Library
+#include <Filter/Filter.h>                     // Filter library
+#include <AP_Buffer/AP_Buffer.h>      // APM FIFO Buffer
+#include <AP_HAL_AVR/memcheck.h>
 
-#include <GCS_MAVLink.h>    // MAVLink GCS definitions
-#include <AP_SerialManager.h>   // Serial manager library
-#include <AP_Declination.h> // ArduPilot Mega Declination Helper Library
-#include <DataFlash.h>
-#include <SITL.h>
-#include <PID.h>
-#include <AP_Scheduler.h>       // main loop scheduler
-#include <AP_NavEKF.h>
+#include <GCS_MAVLink/GCS_MAVLink.h>    // MAVLink GCS definitions
+#include <AP_SerialManager/AP_SerialManager.h>   // Serial manager library
+#include <AP_Declination/AP_Declination.h> // ArduPilot Mega Declination Helper Library
+#include <DataFlash/DataFlash.h>
+#include <SITL/SITL.h>
+#include <PID/PID.h>
+#include <AP_Scheduler/AP_Scheduler.h>       // main loop scheduler
+#include <AP_NavEKF/AP_NavEKF.h>
 
-#include <AP_Vehicle.h>
-#include <AP_Mission.h>
-#include <AP_Terrain.h>
-#include <AP_Rally.h>
-#include <AP_Notify.h>      // Notify library
-#include <AP_BattMonitor.h> // Battery monitor library
-#include <AP_Airspeed.h>
-#include <RC_Channel.h>
-#include <AP_BoardConfig.h>
-#include <AP_OpticalFlow.h>
-#include <AP_RangeFinder.h>
+#include <AP_Vehicle/AP_Vehicle.h>
+#include <AP_Mission/AP_Mission.h>
+#include <AP_Terrain/AP_Terrain.h>
+#include <AP_Rally/AP_Rally.h>
+#include <AP_Notify/AP_Notify.h>      // Notify library
+#include <AP_BattMonitor/AP_BattMonitor.h> // Battery monitor library
+#include <AP_Airspeed/AP_Airspeed.h>
+#include <RC_Channel/RC_Channel.h>
+#include <AP_BoardConfig/AP_BoardConfig.h>
+#include <AP_OpticalFlow/AP_OpticalFlow.h>
+#include <AP_RangeFinder/AP_RangeFinder.h>
 
 // Configuration
 #include "config.h"
 #include "defines.h"
 
 #include "Parameters.h"
-#include "GCS.h"
+#include <GCS_MAVLink/GCS.h>
 
-#include <AP_HAL_AVR.h>
-#include <AP_HAL_SITL.h>
-#include <AP_HAL_PX4.h>
-#include <AP_HAL_FLYMAPLE.h>
-#include <AP_HAL_Linux.h>
-#include <AP_HAL_Empty.h>
+#include <AP_HAL_AVR/AP_HAL_AVR.h>
+#include <AP_HAL_SITL/AP_HAL_SITL.h>
+#include <AP_HAL_PX4/AP_HAL_PX4.h>
+#include <AP_HAL_FLYMAPLE/AP_HAL_FLYMAPLE.h>
+#include <AP_HAL_Linux/AP_HAL_Linux.h>
+#include <AP_HAL_Empty/AP_HAL_Empty.h>
 
 class Tracker {
 public:
@@ -165,7 +167,7 @@ private:
         bool need_altitude_calibration  : 1;// true if tracker altitude has not been determined (true after startup)
         bool scan_reverse_pitch         : 1;// controls direction of pitch movement in SCAN mode
         bool scan_reverse_yaw           : 1;// controls direction of yaw movement in SCAN mode
-    } nav_status = {0.0f, 0.0f, 0.0f, 0.0f, false, false, true, false, false};
+    } nav_status = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, false, false, true, false, false};
 
     // Servo state
     struct {
@@ -244,6 +246,7 @@ private:
     void tracking_manual_control(const mavlink_manual_control_t &msg);
     void update_armed_disarmed();
     void gcs_send_text_fmt(const prog_char_t *fmt, ...);
+    void init_capabilities(void);
 
 public:
     void mavlink_snoop(const mavlink_message_t* msg);

AntennaTracker/capabilities.cpp

@@ -0,0 +1,8 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#include "Tracker.h"
+
+void Tracker::init_capabilities(void)
+{
+    hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT);
+}

AntennaTracker/readme.txt

@@ -0,0 +1,4 @@
+AntennaTracker README
+
+User Manual: http://antennatracker.ardupilot.com/
+Developer Manual: http://dev.ardupilot.com/

AntennaTracker/system.cpp

@@ -102,6 +102,8 @@ void Tracker::init_tracker()
         get_home_eeprom(current_loc);
     }
 
+    init_capabilities();
+
     gcs_send_text_P(SEVERITY_LOW,PSTR("\nReady to track."));
     hal.scheduler->delay(1000); // Why????
 

ArduCopter/ArduCopter.cpp

@@ -128,6 +128,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] PROGMEM = {
     { SCHED_TASK(full_rate_logging_loop),1,    100 },
     { SCHED_TASK(perf_update),        4000,     75 },
     { SCHED_TASK(read_receiver_rssi),   40,     75 },
+    { SCHED_TASK(rpm_update),           40,    200 },
 #if FRSKY_TELEM_ENABLED == ENABLED
     { SCHED_TASK(frsky_telemetry_send), 80,     75 },
 #endif
@@ -463,10 +464,16 @@ void Copter::one_hz_loop()
         // make it possible to change ahrs orientation at runtime during initial config
         ahrs.set_orientation();
 
+#if FRAME_CONFIG == HELI_FRAME
+        // helicopters are always using motor interlock
+        set_using_interlock(true);
+#else
         // check the user hasn't updated the frame orientation
         motors.set_frame_orientation(g.frame_orientation);
 
-#if FRAME_CONFIG != HELI_FRAME
+        // check if we are using motor interlock control on an aux switch
+        set_using_interlock(check_if_auxsw_mode_used(AUXSW_MOTOR_INTERLOCK));
+
         // set all throttle channel settings
         motors.set_throttle_range(g.throttle_min, channel_throttle->radio_min, channel_throttle->radio_max);
         // set hover throttle

ArduCopter/Attitude.cpp

@@ -36,9 +36,9 @@ void Copter::get_pilot_desired_lean_angles(float roll_in, float pitch_in, float
     pitch_out = pitch_in;
 }
 
-// get_pilot_desired_heading - transform pilot's yaw input into a desired heading
-// returns desired angle in centi-degrees
-// To-Do: return heading as a float?
+// get_pilot_desired_heading - transform pilot's yaw input into a
+// desired yaw rate
+// returns desired yaw rate in centi-degrees per second
 float Copter::get_pilot_desired_yaw_rate(int16_t stick_angle)
 {
     // convert pilot input to the desired yaw rate
@@ -308,3 +308,12 @@ void Copter::update_poscon_alt_max()
     // pass limit to pos controller
     pos_control.set_alt_max(alt_limit_cm);
 }
+
+// rotate vector from vehicle's perspective to North-East frame
+void Copter::rotate_body_frame_to_NE(float &x, float &y)
+{
+    float ne_x = x*ahrs.cos_yaw() - y*ahrs.sin_yaw();
+    float ne_y = x*ahrs.sin_yaw() + y*ahrs.cos_yaw();
+    x = ne_x;
+    y = ne_y;
+}

ArduCopter/Copter.cpp

@@ -31,7 +31,10 @@ Copter::Copter(void) :
             FUNCTOR_BIND_MEMBER(&Copter::exit_mission, void)),
     control_mode(STABILIZE),
 #if FRAME_CONFIG == HELI_FRAME  // helicopter constructor requires more arguments
-    motors(g.rc_7, g.rc_8, g.heli_servo_1, g.heli_servo_2, g.heli_servo_3, g.heli_servo_4, MAIN_LOOP_RATE),
+    motors(g.rc_7, g.heli_servo_rsc, g.heli_servo_1, g.heli_servo_2, g.heli_servo_3, g.heli_servo_4, MAIN_LOOP_RATE),
+
+    // ToDo: Input Manager is only used by Heli for 3.3, but will be used by all frames for 3.4
+    input_manager(MAIN_LOOP_RATE),
 #elif FRAME_CONFIG == TRI_FRAME  // tri constructor requires additional rc_7 argument to allow tail servo reversing
     motors(MAIN_LOOP_RATE),
 #elif FRAME_CONFIG == SINGLE_FRAME  // single constructor requires extra servos for flaps
@@ -50,6 +53,7 @@ Copter::Copter(void) :
     guided_mode(Guided_TakeOff),
     rtl_state(RTL_InitialClimb),
     rtl_state_complete(false),
+    rtl_alt(0.0f),
     circle_pilot_yaw_override(false),
     simple_cos_yaw(1.0f),
     simple_sin_yaw(0.0f),
@@ -122,7 +126,7 @@ Copter::Copter(void) :
 #endif
     in_mavlink_delay(false),
     gcs_out_of_time(false),
-    param_loader(var_info)
+    param_loader(var_info)    
 {
     memset(&current_loc, 0, sizeof(current_loc));
 

ArduCopter/Copter.h

@@ -1,6 +1,8 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
-#define THISFIRMWARE "APM:Copter V3.4-dev"
+#define THISFIRMWARE "APM:Copter V3.3.4-pixracer"
+#define FIRMWARE_VERSION 3,3,4,FIRMWARE_VERSION_TYPE_OFFICIAL
+
 /*
    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
@@ -28,80 +30,83 @@
 #include <stdarg.h>
 
 // Common dependencies
-#include <AP_Common.h>
-#include <AP_Progmem.h>
-#include <AP_Menu.h>
-#include <AP_Param.h>
-#include <StorageManager.h>
+#include <AP_Common/AP_Common.h>
+#include <AP_Progmem/AP_Progmem.h>
+#include <AP_Menu/AP_Menu.h>
+#include <AP_Param/AP_Param.h>
+#include <StorageManager/StorageManager.h>
 // AP_HAL
-#include <AP_HAL.h>
-#include <AP_HAL_AVR.h>
-#include <AP_HAL_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 <AP_HAL/AP_HAL.h>
+#include <AP_HAL_AVR/AP_HAL_AVR.h>
+#include <AP_HAL_SITL/AP_HAL_SITL.h>
+#include <AP_HAL_PX4/AP_HAL_PX4.h>
+#include <AP_HAL_VRBRAIN/AP_HAL_VRBRAIN.h>
+#include <AP_HAL_FLYMAPLE/AP_HAL_FLYMAPLE.h>
+#include <AP_HAL_Linux/AP_HAL_Linux.h>
+#include <AP_HAL_Empty/AP_HAL_Empty.h>
 
 // Application dependencies
-#include <GCS.h>
-#include <GCS_MAVLink.h>        // MAVLink GCS definitions
-#include <AP_SerialManager.h>   // Serial manager library
-#include <AP_GPS.h>             // ArduPilot GPS library
-#include <DataFlash.h>          // ArduPilot Mega Flash Memory 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_Curve.h>           // Curve used to linearlise throttle pwm to thrust
-#include <AP_InertialSensor.h>  // ArduPilot Mega Inertial Sensor (accel & gyro) Library
-#include <AP_AHRS.h>
-#include <AP_NavEKF.h>
-#include <AP_Mission.h>         // Mission command library
-#include <AP_Rally.h>           // Rally point library
-#include <AC_PID.h>             // PID library
-#include <AC_PI_2D.h>           // PID library (2-axis)
-#include <AC_HELI_PID.h>        // Heli specific Rate PID library
-#include <AC_P.h>               // P library
-#include <AC_AttitudeControl_Multi.h> // Attitude control library
-#include <AC_AttitudeControl_Heli.h> // Attitude control library for traditional helicopter
-#include <AC_PosControl.h>      // Position control library
-#include <RC_Channel.h>         // RC Channel Library
-#include <AP_Motors.h>          // AP Motors library
-#include <AP_RangeFinder.h>     // Range finder library
-#include <AP_OpticalFlow.h>     // Optical Flow library
-#include <Filter.h>             // Filter library
-#include <AP_Buffer.h>          // APM FIFO Buffer
-#include <AP_Relay.h>           // APM relay
-#include <AP_ServoRelayEvents.h>
-#include <AP_Camera.h>          // Photo or video camera
-#include <AP_Mount.h>           // Camera/Antenna mount
-#include <AP_Airspeed.h>        // needed for AHRS build
-#include <AP_Vehicle.h>         // needed for AHRS build
-#include <AP_InertialNav.h>     // ArduPilot Mega inertial navigation library
-#include <AC_WPNav.h>           // ArduCopter waypoint navigation library
-#include <AC_Circle.h>          // circle navigation library
-#include <AP_Declination.h>     // ArduPilot Mega Declination Helper Library
-#include <AC_Fence.h>           // Arducopter Fence library
-#include <SITL.h>               // software in the loop support
-#include <AP_Scheduler.h>       // main loop scheduler
-#include <AP_RCMapper.h>        // RC input mapping library
-#include <AP_Notify.h>          // Notify library
-#include <AP_BattMonitor.h>     // Battery monitor library
-#include <AP_BoardConfig.h>     // board configuration library
-#include <AP_Frsky_Telem.h>
+#include <GCS_MAVLink/GCS.h>
+#include <GCS_MAVLink/GCS_MAVLink.h>        // MAVLink GCS definitions
+#include <AP_SerialManager/AP_SerialManager.h>   // Serial manager library
+#include <AP_GPS/AP_GPS.h>             // ArduPilot GPS library
+#include <DataFlash/DataFlash.h>          // ArduPilot Mega Flash Memory Library
+#include <AP_ADC/AP_ADC.h>             // ArduPilot Mega Analog to Digital Converter Library
+#include <AP_ADC_AnalogSource/AP_ADC_AnalogSource.h>
+#include <AP_Baro/AP_Baro.h>
+#include <AP_Compass/AP_Compass.h>         // ArduPilot Mega Magnetometer Library
+#include <AP_Math/AP_Math.h>            // ArduPilot Mega Vector/Matrix math Library
+#include <AP_Curve/AP_Curve.h>           // Curve used to linearlise throttle pwm to thrust
+#include <AP_InertialSensor/AP_InertialSensor.h>  // ArduPilot Mega Inertial Sensor (accel & gyro) Library
+#include <AP_AHRS/AP_AHRS.h>
+#include <AP_NavEKF/AP_NavEKF.h>
+#include <AP_Mission/AP_Mission.h>         // Mission command library
+#include <AP_Rally/AP_Rally.h>           // Rally point library
+#include <AC_PID/AC_PID.h>             // PID library
+#include <AC_PID/AC_PI_2D.h>           // PID library (2-axis)
+#include <AC_PID/AC_HELI_PID.h>        // Heli specific Rate PID library
+#include <AC_PID/AC_P.h>               // P library
+#include <AC_AttitudeControl/AC_AttitudeControl_Multi.h> // Attitude control library
+#include <AC_AttitudeControl/AC_AttitudeControl_Heli.h> // Attitude control library for traditional helicopter
+#include <AC_AttitudeControl/AC_PosControl.h>      // Position control library
+#include <RC_Channel/RC_Channel.h>         // RC Channel Library
+#include <AP_Motors/AP_Motors.h>          // AP Motors library
+#include <AP_RangeFinder/AP_RangeFinder.h>     // Range finder library
+#include <AP_OpticalFlow/AP_OpticalFlow.h>     // Optical Flow library
+#include <Filter/Filter.h>             // Filter library
+#include <AP_Buffer/AP_Buffer.h>          // APM FIFO Buffer
+#include <AP_Relay/AP_Relay.h>           // APM relay
+#include <AP_ServoRelayEvents/AP_ServoRelayEvents.h>
+#include <AP_Camera/AP_Camera.h>          // Photo or video camera
+#include <AP_Mount/AP_Mount.h>           // Camera/Antenna mount
+#include <AP_Airspeed/AP_Airspeed.h>        // needed for AHRS build
+#include <AP_Vehicle/AP_Vehicle.h>         // needed for AHRS build
+#include <AP_InertialNav/AP_InertialNav.h>     // ArduPilot Mega inertial navigation library
+#include <AC_WPNav/AC_WPNav.h>           // ArduCopter waypoint navigation library
+#include <AC_WPNav/AC_Circle.h>          // circle navigation library
+#include <AP_Declination/AP_Declination.h>     // ArduPilot Mega Declination Helper Library
+#include <AC_Fence/AC_Fence.h>           // Arducopter Fence library
+#include <SITL/SITL.h>               // software in the loop support
+#include <AP_Scheduler/AP_Scheduler.h>       // main loop scheduler
+#include <AP_RCMapper/AP_RCMapper.h>        // RC input mapping library
+#include <AP_Notify/AP_Notify.h>          // Notify library
+#include <AP_BattMonitor/AP_BattMonitor.h>     // Battery monitor library
+#include <AP_BoardConfig/AP_BoardConfig.h>     // board configuration library
+#include <AP_Frsky_Telem/AP_Frsky_Telem.h>
 #if SPRAYER == ENABLED
-#include <AC_Sprayer.h>         // crop sprayer library
+#include <AC_Sprayer/AC_Sprayer.h>         // crop sprayer library
 #endif
 #if EPM_ENABLED == ENABLED
-#include <AP_EPM.h>             // EPM cargo gripper stuff
+#include <AP_EPM/AP_EPM.h>             // EPM cargo gripper stuff
 #endif
 #if PARACHUTE == ENABLED
-#include <AP_Parachute.h>       // Parachute release library
+#include <AP_Parachute/AP_Parachute.h>       // Parachute release library
 #endif
-#include <AP_LandingGear.h>     // Landing Gear library
-#include <AP_Terrain.h>
+#include <AP_LandingGear/AP_LandingGear.h>     // Landing Gear library
+#include <AP_Terrain/AP_Terrain.h>
+#include <AP_RPM/AP_RPM.h>
+#include <AC_InputManager/AC_InputManager.h>        // Pilot input handling library
+#include <AC_InputManager/AC_InputManager_Heli.h>   // Heli specific pilot input handling library
 
 // AP_HAL to Arduino compatibility layer
 // Configuration
@@ -168,10 +173,12 @@ private:
     AP_InertialSensor ins;
 
 #if CONFIG_SONAR == ENABLED
-    RangeFinder sonar;
+    RangeFinder sonar{serial_manager};
     bool sonar_enabled; // enable user switch for sonar
 #endif
 
+    AP_RPM rpm_sensor;
+
     // Inertial Navigation EKF
     NavEKF EKF{&ahrs, barometer, sonar};
     AP_AHRS_NavEKF ahrs{ins, barometer, gps, sonar, EKF};
@@ -291,7 +298,7 @@ private:
 #elif FRAME_CONFIG == OCTA_QUAD_FRAME
  #define MOTOR_CLASS AP_MotorsOctaQuad
 #elif FRAME_CONFIG == HELI_FRAME
- #define MOTOR_CLASS AP_MotorsHeli
+ #define MOTOR_CLASS AP_MotorsHeli_Single
 #elif FRAME_CONFIG == SINGLE_FRAME
  #define MOTOR_CLASS AP_MotorsSingle
 #elif FRAME_CONFIG == COAX_FRAME
@@ -325,6 +332,7 @@ private:
     // RTL
     RTLState rtl_state;  // records state of rtl (initial climb, returning home, etc)
     bool rtl_state_complete; // set to true if the current state is completed
+    float rtl_alt;     // altitude the vehicle is returning at
 
     // Circle
     bool circle_pilot_yaw_override; // true if pilot is overriding yaw
@@ -485,6 +493,13 @@ private:
     AP_Terrain terrain;
 #endif
 
+    // Pilot Input Management Library
+    // Only used for Helicopter for AC3.3, to be expanded to include Multirotor
+    // child class for AC3.4
+#if FRAME_CONFIG == HELI_FRAME
+    AC_InputManager_Heli input_manager;
+#endif
+
     // use this to prevent recursion during sensor init
     bool in_mavlink_delay;
 
@@ -562,6 +577,7 @@ private:
     float get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt);
     void set_accel_throttle_I_from_pilot_throttle(int16_t pilot_throttle);
     void update_poscon_alt_max();
+    void rotate_body_frame_to_NE(float &x, float &y);
     void gcs_send_heartbeat(void);
     void gcs_send_deferred(void);
     void send_heartbeat(mavlink_channel_t chan);
@@ -577,14 +593,16 @@ private:
     void send_vfr_hud(mavlink_channel_t chan);
     void send_current_waypoint(mavlink_channel_t chan);
     void send_rangefinder(mavlink_channel_t chan);
+    void send_rpm(mavlink_channel_t chan);
+    void rpm_update();
     void send_pid_tuning(mavlink_channel_t chan);
     void send_statustext(mavlink_channel_t chan);
     bool telemetry_delayed(mavlink_channel_t chan);
     void gcs_send_message(enum ap_message id);
+    void gcs_send_mission_item_reached_message(uint16_t mission_index);
     void gcs_data_stream_send(void);
     void gcs_check_input(void);
     void gcs_send_text_P(gcs_severity severity, const prog_char_t *str);
-    void gcs_send_mission_item_reached(uint16_t seq);
     void do_erase_logs(void);
     void Log_Write_AutoTune(uint8_t axis, uint8_t tune_step, float meas_target, float meas_min, float meas_max, float new_gain_rp, float new_gain_rd, float new_gain_sp, float new_ddt);
     void Log_Write_AutoTuneDetails(float angle_cd, float rate_cds);
@@ -788,7 +806,6 @@ private:
     bool mode_allows_arming(uint8_t mode, bool arming_from_gcs);
     void notify_flight_mode(uint8_t mode);
     void heli_init();
-    int16_t get_pilot_desired_collective(int16_t control_in);
     void check_dynamic_flight(void);
     void update_heli_control_dynamics(void);
     void heli_update_landing_swash();
@@ -946,6 +963,7 @@ private:
     void print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode);
     void log_init(void);
     void run_cli(AP_HAL::UARTDriver *port);
+    void init_capabilities(void);
 
 public:
     void mavlink_delay_cb();

ArduCopter/GCS_Mavlink.cpp

@@ -5,10 +5,6 @@
 // default sensors are present and healthy: gyro, accelerometer, barometer, rate_control, attitude_stabilization, yaw_position, altitude control, x/y position control, motor_control
 #define MAVLINK_SENSOR_PRESENT_DEFAULT (MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL | MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE | MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL | MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION | MAV_SYS_STATUS_SENSOR_YAW_POSITION | MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL | MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL | MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS | MAV_SYS_STATUS_AHRS)
 
-// check if a message will fit in the payload space available
-#define HAVE_PAYLOAD_SPACE(chan, id) (comm_get_txspace(chan) >= MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_ ## id ## _LEN)
-#define CHECK_PAYLOAD_SIZE(id) if (txspace < MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_ ## id ## _LEN) return false
-
 void Copter::gcs_send_heartbeat(void)
 {
     gcs_send_message(MSG_HEARTBEAT);
@@ -414,6 +410,19 @@ void NOINLINE Copter::send_rangefinder(mavlink_channel_t chan)
 }
 #endif
 
+/*
+  send RPM packet
+ */
+void NOINLINE Copter::send_rpm(mavlink_channel_t chan)
+{
+    if (rpm_sensor.healthy(0) || rpm_sensor.healthy(1)) {
+        mavlink_msg_rpm_send(
+            chan,
+            rpm_sensor.get_rpm(0),
+            rpm_sensor.get_rpm(1));
+    }
+}
+
 
 /*
   send PID tuning message
@@ -505,8 +514,6 @@ bool Copter::telemetry_delayed(mavlink_channel_t chan)
 // 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 (copter.telemetry_delayed(chan)) {
         return false;
     }
@@ -629,6 +636,11 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
 #endif
         break;
 
+    case MSG_RPM:
+        CHECK_PAYLOAD_SIZE(RPM);
+        copter.send_rpm(chan);
+        break;
+
     case MSG_TERRAIN:
 #if AP_TERRAIN_AVAILABLE
         CHECK_PAYLOAD_SIZE(TERRAIN_REQUEST);
@@ -720,6 +732,11 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
         send_vibration(copter.ins);
         break;
 
+    case MSG_MISSION_ITEM_REACHED:
+        CHECK_PAYLOAD_SIZE(MISSION_ITEM_REACHED);
+        mavlink_msg_mission_item_reached_send(chan, mission_item_reached_index);
+        break;
+
     case MSG_RETRY_DEFERRED:
         break; // just here to prevent a warning
     }
@@ -945,6 +962,7 @@ GCS_MAVLINK::data_stream_send(void)
         send_message(MSG_GIMBAL_REPORT);
         send_message(MSG_EKF_STATUS_REPORT);
         send_message(MSG_VIBRATION);
+        send_message(MSG_RPM);
     }
 }
 
@@ -1193,6 +1211,10 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
                     result = MAV_RESULT_ACCEPTED;
                 }
             } else {
+                // sanity check location
+                if (fabsf(packet.param5) > 90.0f || fabsf(packet.param6) > 180.0f) {
+                    break;
+                }
                 Location new_home_loc;
                 new_home_loc.lat = (int32_t)(packet.param5 * 1.0e7f);
                 new_home_loc.lng = (int32_t)(packet.param6 * 1.0e7f);
@@ -1205,6 +1227,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             }
             break;
 
+        case MAV_CMD_DO_FLIGHTTERMINATION:
+            if (packet.param1 > 0.5f) {
+                copter.init_disarm_motors();
+                result = MAV_RESULT_ACCEPTED;
+            }
+            break;
+
         case MAV_CMD_DO_SET_ROI:
             // param1 : regional of interest mode (not supported)
             // param2 : mission index/ target id (not supported)
@@ -1212,6 +1241,10 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             // param5 : x / lat
             // param6 : y / lon
             // param7 : z / alt
+            // sanity check location
+            if (fabsf(packet.param5) > 90.0f || fabsf(packet.param6) > 180.0f) {
+                break;
+            }
             Location roi_loc;
             roi_loc.lat = (int32_t)(packet.param5 * 1.0e7f);
             roi_loc.lng = (int32_t)(packet.param6 * 1.0e7f);
@@ -1297,6 +1330,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
                     result = MAV_RESULT_ACCEPTED;
                 }
             } else if (is_zero(packet.param1) && (copter.mode_has_manual_throttle(copter.control_mode) || copter.ap.land_complete || is_equal(packet.param2,21196.0f)))  {
+                // force disarming by setting param2 = 21196 is deprecated
                 copter.init_disarm_motors();
                 result = MAV_RESULT_ACCEPTED;
             } else {
@@ -1416,7 +1450,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 
         case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: {
             if (is_equal(packet.param1,1.0f)) {
-                copter.gcs[chan-MAVLINK_COMM_0].send_autopilot_version();
+                copter.gcs[chan-MAVLINK_COMM_0].send_autopilot_version(FIRMWARE_VERSION);
                 result = MAV_RESULT_ACCEPTED;
             }
             break;
@@ -1450,6 +1484,14 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             break;
         }
 
+        // check for supported coordinate frames
+        if (packet.coordinate_frame != MAV_FRAME_LOCAL_NED &&
+            packet.coordinate_frame != MAV_FRAME_LOCAL_OFFSET_NED &&
+            packet.coordinate_frame != MAV_FRAME_BODY_NED &&
+            packet.coordinate_frame != MAV_FRAME_BODY_OFFSET_NED) {
+            break;
+        }
+
         bool pos_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
         bool vel_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE;
         bool acc_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE;
@@ -1461,16 +1503,45 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
          * bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;
          */
 
+        // prepare position
+        Vector3f pos_vector;
+        if (!pos_ignore) {
+            // convert to cm
+            pos_vector = Vector3f(packet.x * 100.0f, packet.y * 100.0f, -packet.z * 100.0f);
+            // rotate to body-frame if necessary
+            if (packet.coordinate_frame == MAV_FRAME_BODY_NED ||
+                packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
+                copter.rotate_body_frame_to_NE(pos_vector.x, pos_vector.y);
+            }
+            // add body offset if necessary
+            if (packet.coordinate_frame == MAV_FRAME_LOCAL_OFFSET_NED ||
+                packet.coordinate_frame == MAV_FRAME_BODY_NED ||
+                packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
+                pos_vector += copter.inertial_nav.get_position();
+            } else {
+                // convert from alt-above-home to alt-above-ekf-origin
+                pos_vector.z = copter.pv_alt_above_origin(pos_vector.z);
+            }
+        }
+
+        // prepare velocity
+        Vector3f vel_vector;
+        if (!vel_ignore) {
+            // convert to cm
+            vel_vector = Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f);
+            // rotate to body-frame if necessary
+            if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
+                copter.rotate_body_frame_to_NE(vel_vector.x, vel_vector.y);
+            }
+        }
+
+        // send request
         if (!pos_ignore && !vel_ignore && acc_ignore) {
-            Vector3f pos_ned = Vector3f(packet.x * 100.0f, packet.y * 100.0f, -packet.z * 100.0f);
-            pos_ned.z = copter.pv_alt_above_origin(pos_ned.z);
-            copter.guided_set_destination_posvel(pos_ned, Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f));
+            copter.guided_set_destination_posvel(pos_vector, vel_vector);
         } else if (pos_ignore && !vel_ignore && acc_ignore) {
-            copter.guided_set_velocity(Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f));
+            copter.guided_set_velocity(vel_vector);
         } else if (!pos_ignore && vel_ignore && acc_ignore) {
-            Vector3f pos_ned = Vector3f(packet.x * 100.0f, packet.y * 100.0f, -packet.z * 100.0f);
-            pos_ned.z = copter.pv_alt_above_origin(pos_ned.z);
-            copter.guided_set_destination(pos_ned);
+            copter.guided_set_destination(pos_vector);
         } else {
             result = MAV_RESULT_FAILED;
         }
@@ -1489,6 +1560,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             break;
         }
 
+        // check for supported coordinate frames
+        if (packet.coordinate_frame != MAV_FRAME_GLOBAL_INT &&
+            packet.coordinate_frame != MAV_FRAME_GLOBAL_RELATIVE_ALT_INT &&
+            packet.coordinate_frame != MAV_FRAME_GLOBAL_TERRAIN_ALT_INT) {
+            break;
+        }
+
         bool pos_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
         bool vel_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE;
         bool acc_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE;
@@ -1508,17 +1586,14 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             loc.lng = packet.lon_int;
             loc.alt = packet.alt*100;
             switch (packet.coordinate_frame) {
-                case MAV_FRAME_GLOBAL_RELATIVE_ALT:
                 case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT:
                     loc.flags.relative_alt = true;
                     loc.flags.terrain_alt = false;
                     break;
-                case MAV_FRAME_GLOBAL_TERRAIN_ALT:
                 case MAV_FRAME_GLOBAL_TERRAIN_ALT_INT:
                     loc.flags.relative_alt = true;
                     loc.flags.terrain_alt = true;
                     break;
-                case MAV_FRAME_GLOBAL:
                 case MAV_FRAME_GLOBAL_INT:
                 default:
                     loc.flags.relative_alt = false;
@@ -1714,7 +1789,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 #endif // AC_RALLY == ENABLED
 
     case MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST:
-        copter.gcs[chan-MAVLINK_COMM_0].send_autopilot_version();
+        copter.gcs[chan-MAVLINK_COMM_0].send_autopilot_version(FIRMWARE_VERSION);
         break;
 
     case MAVLINK_MSG_ID_LED_CONTROL:
@@ -1773,6 +1848,19 @@ void Copter::gcs_send_message(enum ap_message id)
     }
 }
 
+/*
+ *  send a mission item reached message and load the index before the send attempt in case it may get delayed
+ */
+void Copter::gcs_send_mission_item_reached_message(uint16_t mission_index)
+{
+    for (uint8_t i=0; i<num_gcs; i++) {
+        if (gcs[i].initialised) {
+            gcs[i].mission_item_reached_index = mission_index;
+            gcs[i].send_message(MSG_MISSION_ITEM_REACHED);
+        }
+    }
+}
+
 /*
  *  send data streams in the given rate range on both links
  */
@@ -1831,15 +1919,3 @@ void Copter::gcs_send_text_fmt(const prog_char_t *fmt, ...)
         }
     }
 }
-
-/*
- *  send mission_item_reached message to all GCSs
- */
-void Copter::gcs_send_mission_item_reached(uint16_t seq)
-{
-    for (uint8_t i=0; i<num_gcs; i++) {
-        if (gcs[i].initialised) {
-            gcs[i].send_mission_item_reached(seq);
-        }
-    }
-}

ArduCopter/Log.cpp

@@ -664,7 +664,7 @@ struct PACKED log_Heli {
     LOG_PACKET_HEADER;
     uint64_t time_us;
     int16_t   desired_rotor_speed;
-    int16_t   estimated_rotor_speed;
+    int16_t   main_rotor_speed;
 };
 
 #if FRAME_CONFIG == HELI_FRAME
@@ -675,7 +675,7 @@ void Copter::Log_Write_Heli()
         LOG_PACKET_HEADER_INIT(LOG_HELI_MSG),
         time_us                 : hal.scheduler->micros64(),
         desired_rotor_speed     : motors.get_desired_rotor_speed(),
-        estimated_rotor_speed   : motors.get_estimated_rotor_speed(),
+        main_rotor_speed        : motors.get_main_rotor_speed(),
     };
     DataFlash.WriteBlock(&pkt_heli, sizeof(pkt_heli));
 }
@@ -781,4 +781,51 @@ void Copter::log_init(void)
     }
 }
 
-#endif // LOGGING_DISABLED
+#else // LOGGING_ENABLED
+
+#if CLI_ENABLED == ENABLED
+bool Copter::print_log_menu(void) { return true; }
+int8_t Copter::dump_log(uint8_t argc, const Menu::arg *argv) { return 0; }
+int8_t Copter::erase_logs(uint8_t argc, const Menu::arg *argv) { return 0; }
+int8_t Copter::select_logs(uint8_t argc, const Menu::arg *argv) { return 0; }
+int8_t Copter::process_logs(uint8_t argc, const Menu::arg *argv) { return 0; }
+void Copter::Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page) {}
+#endif // CLI_ENABLED == ENABLED
+
+void Copter::do_erase_logs(void) {}
+void Copter::Log_Write_AutoTune(uint8_t axis, uint8_t tune_step, float meas_target, \
+                                float meas_min, float meas_max, float new_gain_rp, \
+                                float new_gain_rd, float new_gain_sp, float new_ddt) {}
+void Copter::Log_Write_AutoTuneDetails(float angle_cd, float rate_cds) {}
+void Copter::Log_Write_Current() {}
+void Copter::Log_Write_Nav_Tuning() {}
+void Copter::Log_Write_Control_Tuning() {}
+void Copter::Log_Write_Performance() {}
+void Copter::Log_Write_Attitude(void) {}
+void Copter::Log_Write_Rate() {}
+void Copter::Log_Write_MotBatt() {}
+void Copter::Log_Write_Startup() {}
+void Copter::Log_Write_Event(uint8_t id) {}
+void Copter::Log_Write_Data(uint8_t id, int32_t value) {}
+void Copter::Log_Write_Data(uint8_t id, uint32_t value) {}
+void Copter::Log_Write_Data(uint8_t id, int16_t value) {}
+void Copter::Log_Write_Data(uint8_t id, uint16_t value) {}
+void Copter::Log_Write_Data(uint8_t id, float value) {}
+void Copter::Log_Write_Error(uint8_t sub_system, uint8_t error_code) {}
+void Copter::Log_Write_Baro(void) {}
+void Copter::Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, int16_t control_in, int16_t tune_low, int16_t tune_high) {}
+void Copter::Log_Write_Home_And_Origin() {}
+void Copter::Log_Sensor_Health() {}
+
+#if FRAME_CONFIG == HELI_FRAME
+void Copter::Log_Write_Heli() {}
+#endif
+
+#if OPTFLOW == ENABLED
+void Copter::Log_Write_Optflow() {}
+#endif
+
+void Copter::start_logging() {}
+void Copter::log_init(void) {}
+
+#endif // LOGGING_ENABLED

ArduCopter/Parameters.cpp

@@ -87,7 +87,7 @@ const AP_Param::Info Copter::var_info[] PROGMEM = {
     // @DisplayName: Takeoff trigger deadzone
     // @Description: Offset from mid stick at which takeoff is triggered
     // @User: Standard
-    // @Range 0.0 500.0
+    // @Range: 0.0 500.0
     // @Increment: 10
     GSCALAR(takeoff_trigger_dz, "PILOT_TKOFF_DZ", THR_DZ_DEFAULT),
 
@@ -229,7 +229,7 @@ const AP_Param::Info Copter::var_info[] PROGMEM = {
 
     // @Param: RTL_LOIT_TIME
     // @DisplayName: RTL loiter time
-    // @Description: Time (in milliseconds) to loiter above home before begining final descent
+    // @Description: Time (in milliseconds) to loiter above home before beginning final descent
     // @Units: ms
     // @Range: 0 60000
     // @Increment: 1000
@@ -447,11 +447,19 @@ const AP_Param::Info Copter::var_info[] PROGMEM = {
     // @User: Standard
     GSCALAR(arming_check, "ARMING_CHECK",           ARMING_CHECK_ALL),
 
+    // @Param: DISARM_DELAY
+    // @DisplayName: Disarm delay
+    // @Description: Delay before automatic disarm in seconds. A value of zero disables auto disarm.
+    // @Units: Seconds
+    // @Range: 0 127
+    // @User: Advanced
+    GSCALAR(disarm_delay, "DISARM_DELAY",           AUTO_DISARMING_DELAY),
+    
     // @Param: ANGLE_MAX
     // @DisplayName: Angle Max
     // @Description: Maximum lean angle in all flight modes
     // @Units: Centi-degrees
-    // @Range 1000 8000
+    // @Range: 1000 8000
     // @User: Advanced
     ASCALAR(angle_max, "ANGLE_MAX",                 DEFAULT_ANGLE_MAX),
 
@@ -516,24 +524,9 @@ const AP_Param::Info Copter::var_info[] PROGMEM = {
     // @Group: HS4_
     // @Path: ../libraries/RC_Channel/RC_Channel.cpp
     GGROUP(heli_servo_4,    "HS4_", RC_Channel),
-
-    // @Param: H_STAB_COL_MIN
-    // @DisplayName: Heli Stabilize Throttle Collective Minimum
-    // @Description: Helicopter's minimum collective position while pilot directly controls collective in stabilize mode
-    // @Range: 0 500
-    // @Units: Percent*10
-    // @Increment: 1
-    // @User: Standard
-    GSCALAR(heli_stab_col_min, "H_STAB_COL_MIN", HELI_STAB_COLLECTIVE_MIN_DEFAULT),
-
-    // @Param: H_STAB_COL_MAX
-    // @DisplayName: Stabilize Throttle Maximum
-    // @Description: Helicopter's maximum collective position while pilot directly controls collective in stabilize mode
-    // @Range: 500 1000
-    // @Units: Percent*10
-    // @Increment: 1
-    // @User: Standard
-    GSCALAR(heli_stab_col_max, "H_STAB_COL_MAX", HELI_STAB_COLLECTIVE_MAX_DEFAULT),
+    // @Group: H_RSC_
+    // @Path: ../libraries/RC_Channel/RC_Channel.cpp
+    GGROUP(heli_servo_rsc,    "H_RSC_", RC_Channel),
 #endif
 
     // RC channel
@@ -802,6 +795,13 @@ const AP_Param::Info Copter::var_info[] PROGMEM = {
     // @Description: Throttle acceleration controller D gain.  Compensates for short-term change in desired vertical acceleration vs actual acceleration
     // @Range: 0.000 0.400
     // @User: Standard
+
+    // @Param: ACCEL_Z_FILT_HZ
+    // @DisplayName: Throttle acceleration filter
+    // @Description: Filter applied to acceleration to reduce noise.  Lower values reduce noise but add delay.
+    // @Range: 1.000 100.000
+    // @Units: Hz
+    // @User: Standard
     GGROUP(pid_accel_z, "ACCEL_Z_", AC_PID),
 
     // P controllers
@@ -869,6 +869,12 @@ const AP_Param::Info Copter::var_info[] PROGMEM = {
     // @Path: ../libraries/AP_LandingGear/AP_LandingGear.cpp
     GOBJECT(landinggear,    "LGR_", AP_LandingGear),
 
+#if FRAME_CONFIG == HELI_FRAME
+    // @Group: IM_
+    // @Path: ../libraries/AC_InputManager_Heli.cpp
+    GOBJECT(input_manager, "IM_", AC_InputManager_Heli),
+#endif
+
     // @Group: COMPASS_
     // @Path: ../libraries/AP_Compass/Compass.cpp
     GOBJECT(compass,        "COMPASS_", Compass),
@@ -886,8 +892,6 @@ const AP_Param::Info Copter::var_info[] PROGMEM = {
     GOBJECT(circle_nav, "CIRCLE_",  AC_Circle),
 
 #if FRAME_CONFIG == HELI_FRAME
-    // @Group: ATC_
-    // @Path: ../libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp
     GOBJECT(attitude_control, "ATC_", AC_AttitudeControl_Heli),
 #else
     // @Group: ATC_
@@ -929,7 +933,7 @@ const AP_Param::Info Copter::var_info[] PROGMEM = {
     GOBJECT(camera_mount,           "MNT",  AP_Mount),
 #endif
 
-    // @Group: BATT_
+    // @Group: BATT
     // @Path: ../libraries/AP_BattMonitor/AP_BattMonitor.cpp
     GOBJECT(battery,                "BATT",         AP_BattMonitor),
 
@@ -974,8 +978,8 @@ const AP_Param::Info Copter::var_info[] PROGMEM = {
 
 #if FRAME_CONFIG ==     HELI_FRAME
     // @Group: H_
-    // @Path: ../libraries/AP_Motors/AP_MotorsHeli.cpp
-    GOBJECT(motors, "H_",           AP_MotorsHeli),
+    // @Path: ../libraries/AP_Motors/AP_MotorsHeli_Single.cpp
+    GOBJECT(motors, "H_",           AP_MotorsHeli_Single),
 
 #elif FRAME_CONFIG == SINGLE_FRAME
     // @Group: SS1_
@@ -1012,7 +1016,7 @@ const AP_Param::Info Copter::var_info[] PROGMEM = {
 
 #else
     // @Group: MOT_
-    // @Path: ../libraries/AP_Motors/AP_Motors_Class.cpp
+    // @Path: ../libraries/AP_Motors/AP_MotorsMulticopter.cpp
     GOBJECT(motors, "MOT_",         AP_MotorsMulticopter),
 #endif
 
@@ -1046,7 +1050,11 @@ const AP_Param::Info Copter::var_info[] PROGMEM = {
     GOBJECT(optflow,   "FLOW", OpticalFlow),
 #endif
 
-    // @Param: AUTOTUNE_AXIS_BITMASK
+    // @Group: RPM
+    // @Path: ../libraries/AP_RPM/AP_RPM.cpp
+    GOBJECT(rpm_sensor, "RPM", AP_RPM),
+
+    // @Param: AUTOTUNE_AXES
     // @DisplayName: Autotune axis bitmask
     // @Description: 1-byte bitmap of axes to autotune
     // @Values: 7:All,1:Roll Only,2:Pitch Only,4:Yaw Only,3:Roll and Pitch,5:Roll and Yaw,6:Pitch and Yaw
@@ -1054,13 +1062,20 @@ const AP_Param::Info Copter::var_info[] PROGMEM = {
     // @User: Standard
     GSCALAR(autotune_axis_bitmask, "AUTOTUNE_AXES", 7),  // AUTOTUNE_AXIS_BITMASK_DEFAULT
 
-    // @Param: AUTOTUNE_AGGRESSIVENESS
-    // @DisplayName: autotune_aggressiveness
-    // @Description: autotune_aggressiveness. Defines the bounce back used to detect size of the D term.
+    // @Param: AUTOTUNE_AGGR
+    // @DisplayName: Autotune aggressiveness
+    // @Description: Autotune aggressiveness. Defines the bounce back used to detect size of the D term.
     // @Range: 0.05 0.10
     // @User: Standard
     GSCALAR(autotune_aggressiveness, "AUTOTUNE_AGGR", 0.1f),
 
+    // @Param: AUTOTUNE_MIN_D
+    // @DisplayName: AutoTune minimum D
+    // @Description: Defines the minimum D gain
+    // @Range: 0.001 0.006
+    // @User: Standard
+    GSCALAR(autotune_min_d, "AUTOTUNE_MIN_D", 0.004f),
+
     AP_VAREND
 };
 

ArduCopter/Parameters.h

@@ -3,7 +3,7 @@
 #ifndef PARAMETERS_H
 #define PARAMETERS_H
 
-#include <AP_Common.h>
+#include <AP_Common/AP_Common.h>
 
 // Global parameter class.
 //
@@ -82,6 +82,9 @@ public:
         // Landing gear object
         k_param_landinggear,    // 18
 
+        // Input Management object
+        k_param_input_manager,  // 19 FULL!
+
         // Misc
         //
         k_param_log_bitmask_old = 20,           // Deprecated
@@ -159,13 +162,15 @@ public:
         k_param_heli_pitch_ff,      // remove
         k_param_heli_roll_ff,       // remove
         k_param_heli_yaw_ff,        // remove
-        k_param_heli_stab_col_min,
-        k_param_heli_stab_col_max,  // 88
+        k_param_heli_stab_col_min,  // remove
+        k_param_heli_stab_col_max,  // remove
+        k_param_heli_servo_rsc,     // 89 = full!
 
         //
         // 90: Motors
         //
         k_param_motors = 90,
+        k_param_disarm_delay,
 
         //
         // 100: Inertial Nav
@@ -336,7 +341,9 @@ public:
         k_param_autotune_aggressiveness,
         k_param_pi_vel_xy,
         k_param_fs_ekf_action,
-        k_param_rtl_climb_min,  // 249
+        k_param_rtl_climb_min,
+        k_param_rpm_sensor,
+        k_param_autotune_min_d, // 251
 
         // 254,255: reserved
     };
@@ -421,6 +428,7 @@ public:
     AP_Int8         ch11_option;
     AP_Int8         ch12_option;
     AP_Int8         arming_check;
+    AP_Int8         disarm_delay;
 
     AP_Int8         land_repositioning;
     AP_Int8         fs_ekf_action;
@@ -430,8 +438,7 @@ public:
 #if FRAME_CONFIG ==     HELI_FRAME
     // Heli
     RC_Channel      heli_servo_1, heli_servo_2, heli_servo_3, heli_servo_4;     // servos for swash plate and tail
-    AP_Int16        heli_stab_col_min;                                          // min collective while pilot directly controls collective in stabilize mode
-    AP_Int16        heli_stab_col_max;                                          // min collective while pilot directly controls collective in stabilize mode
+    RC_Channel      heli_servo_rsc;                                             // servo for rotor speed control output
 #endif
 #if FRAME_CONFIG ==     SINGLE_FRAME
     // Single
@@ -497,6 +504,7 @@ public:
     // Autotune
     AP_Int8                 autotune_axis_bitmask;
     AP_Float                autotune_aggressiveness;
+    AP_Float                autotune_min_d;
 
     // Note: keep initializers here in the same order as they are declared
     // above.
@@ -507,6 +515,7 @@ public:
         heli_servo_2        (CH_2),
         heli_servo_3        (CH_3),
         heli_servo_4        (CH_4),
+        heli_servo_rsc      (CH_8),
 #endif
 #if FRAME_CONFIG ==     SINGLE_FRAME
         single_servo_1        (CH_1),

ArduCopter/ReleaseNotes.txt

@@ -1,5 +1,100 @@
 APM:Copter Release Notes:
 ------------------------------------------------------------------
+Copter 3.3.4-pixracer 25-Feb-2016 / 3.3.4-rc3-pixracer 23-Feb-2016
+Changes from 3.3.4-rc2-pixracer
+1) fix parameter save issue caused by bus locking
+------------------------------------------------------------------
+Copter 3.3.4-rc2-pixracer 20-Feb-2016
+Changes from 3.3.4-rc1-pixracers
+1) fix SBUS input
+------------------------------------------------------------------
+Copter 3.3.4-rc1-pixracer 19-Feb-2016
+Changes from 3.3.3
+1) Ignores accel and gyro errors during first 2 seconds of startup
+------------------------------------------------------------------
+Copter 3.3.3-rc2 27-Jan-2016
+Changes from 3.3.3-rc1
+1) bug fix to Guided mode's velocity controller to run at 400hz
+2) bug fix to MAVLink routing to allow camera messages to reach MAVLink enabled cameras and gimbals (including SToRM32)
+------------------------------------------------------------------
+Copter 3.3.3-rc1 4-Jan-2016
+Changes from 3.3.2
+1) Restrict mode changes in helicopter when rotor is not at speed
+2) add ATC_ANGLE_BOOST param to allow disabling angle boost for all flight modes
+3) add LightWare range finder support
+------------------------------------------------------------------
+Copter 3.3.2 01-Dec-2015 / 3.3.2-rc2 18-Nov-2015
+Changes from 3.3.2-rc1
+1) Bug fix for desired climb rate initialisation that could lead to drop when entering AltHold, Loiter, PosHold
+2) Fix to hard landings when WPNAV_SPEED_DN set high in RTL, Auto (resolved by using non-feedforward alt hold)
+3) Reduce Bad AHRS by filtering innovations
+4) Allow arming without GPS if using Optical Flow
+5) Smoother throttle output in Guided mode's velocity control (z-axis now 400hz) 
+------------------------------------------------------------------
+Copter 3.3.2-rc1 4-Nov-2015
+Changes from 3.3.1
+1) Helicopter Improvements:
+    a) Fix Arming race condition
+    b) Fix servos to move after arming in Stabilize and Acro
+    c) Implement Pirouette Compensation
+    d) Add Rate I-Leak-Min functionality
+    e) Add new Stab Collective and Acro Expo Col functions
+    f) Add circular swashplate limits (Cyclic Ring)
+    g) Add new H_SV_Man functions
+    h) Add Hover Roll Trim function
+    i) Add Engine Run Enable Aux Channel function
+    j) Add servo boot test function
+    h) Add Disarm Delay parameter
+------------------------------------------------------------------
+Copter 3.3.1 26-Oct-2015 / 3.3.1-rc1 20-Oct-2015
+Changes from 3.3
+1) Bug fix to prevent potential crash if Follow-Me is used after an aborted takeoff
+2) compiler upgraded to 4.9.3 (runs slightly faster than 4.7.2 which was used previously)
+------------------------------------------------------------------
+Copter 3.3 29-Sep-2015 / 3.3-rc12 22-Sep-2015
+Changes from 3.3-rc11
+1) EKF recovers from pre-arm "Compass variance" failure if compasses are consistent
+------------------------------------------------------------------
+Copter 3.3-rc11 10-Sep-2015
+Changes from 3.3-rc10
+1) PreArm "Need 3D Fix" message replaced with detailed reason from EKF
+------------------------------------------------------------------
+Copter 3.3-rc10 28-Aug-2015
+Changes from 3.3-rc9
+1) EKF improvements:
+    a) simpler optical flow takeoff check
+2) Bug Fixes/Minor enhancements:
+    a) fix INS3_USE parameter eeprom location
+    b) fix SToRM32 serial protocol driver to work with recent versions
+    c) increase motor pwm->thrust conversion (aka MOT_THST_EXPO) to 0.65 (was 0.50)
+    d) Firmware version sent to GCS in AUTOPILOT_VERSION message
+3) Safety:
+    a) pre-arm check of compass variance if arming in Loiter, PosHold, Guided
+    b) always check GPS before arming in Loiter (previously could be disabled if ARMING_CHECK=0)
+    c) sanity check locations received from GCS for follow-me, do-set-home, do-set-ROI
+    d) fix optical flow failsafe (was not always triggering LAND when optical flow failed)
+    e) failsafe RTL vs LAND decision based on hardcoded 5m from home check (previously used WPNAV_RADIUS parameter)
+------------------------------------------------------------------
+Copter 3.3-rc9 19-Aug-2015
+Changes from 3.3-rc8
+1) EKF improvements:
+    a) IMU weighting based on vibration levels (previously used accel clipping)
+    b) fix blended acceleration (used for altitude control) in cases where first IMU fails
+    c) ensure unhealthy barometer values are never consumed
+2) TradHeli: remove acceleration feed forward
+3) Safety:
+    a) check accel, gyro and baro are healthy when arming (previously was only checked pre-arm)
+    b) Guided mode velocity controller timeout (vehicle stops) after 3 seconds with no update from GCS
+4) Minor enhancements:
+    a) fix for AUAV board's usb-connected detection
+    b) add Lidar-Lite-V2 support
+    c) MOT_THR_MIN_MAX param added to control prioritisation of throttle vs attitude during dynamic flight
+    d) RALLY_INCL_HOME param allows always including home when using rally points
+    e) DO_FLIGHT_TERMINATION message from GCS acts as kill switch
+5) Bug Fixes:
+    a) fix to ensure motors start slowly on 2nd spin-up
+    b) fix RTL_CLIMB_MIN feature (vehicle climbed too high above home)
+------------------------------------------------------------------
 Copter 3.3-rc8 25-Jul-2015
 Changes from 3.3-rc7
 1) EKF improvements:
@@ -16,6 +111,7 @@ Changes from 3.3-rc7
     d) Circle rate adjustment with ch6 takes effect immediately
     e) log home and origin
     f) pre-arm check of battery voltage and fence
+    g) RTL_CLIMB_MIN parameter forces vehicle to climb at least this many cm when RTL is engaged (default is zero)
 5) Bug fixes:
     a) fix THR_MIN being incorrectly scaled as pwm value during low-throttle check
     b) fence distance calculated from home (was incorrectly calculated from ekf-origin)
@@ -26,7 +122,7 @@ Changes from 3.3-rc7
     g) fix initialisation of mount's mode
     h) start-up logging so parameters only logged once, mission always written
 6) Linux:
-    a) bebop support 
+    a) bebop support
 ------------------------------------------------------------------
 Copter 3.3-rc7 28-Jun-2015
 Changes from 3.3-rc6

ArduCopter/capabilities.cpp

@@ -0,0 +1,12 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#include "Copter.h"
+
+void Copter::init_capabilities(void)
+{
+    hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT);
+    hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT);
+    hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED);
+    hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_GLOBAL_INT);
+    hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_FLIGHT_TERMINATION);
+}

ArduCopter/commands_logic.cpp

@@ -168,7 +168,7 @@ bool Copter::verify_command_callback(const AP_Mission::Mission_Command& cmd)
 
         // send message to GCS
         if (cmd_complete) {
-            gcs_send_mission_item_reached(cmd.index);
+            gcs_send_mission_item_reached_message(cmd.index);
         }
 
         return cmd_complete;
@@ -189,40 +189,31 @@ bool Copter::verify_command(const AP_Mission::Mission_Command& cmd)
     //
     case MAV_CMD_NAV_TAKEOFF:
         return verify_takeoff();
-        break;
 
     case MAV_CMD_NAV_WAYPOINT:
         return verify_nav_wp(cmd);
-        break;
 
     case MAV_CMD_NAV_LAND:
         return verify_land();
-        break;
 
     case MAV_CMD_NAV_LOITER_UNLIM:
         return verify_loiter_unlimited();
-        break;
 
     case MAV_CMD_NAV_LOITER_TURNS:
         return verify_circle(cmd);
-        break;
 
     case MAV_CMD_NAV_LOITER_TIME:
         return verify_loiter_time();
-        break;
 
     case MAV_CMD_NAV_RETURN_TO_LAUNCH:
         return verify_RTL();
-        break;
 
     case MAV_CMD_NAV_SPLINE_WAYPOINT:
         return verify_spline_wp(cmd);
-        break;
 
 #if NAV_GUIDED == ENABLED
     case MAV_CMD_NAV_GUIDED_ENABLE:
         return verify_nav_guided_enable(cmd);
-        break;
 #endif
 
     ///
@@ -230,29 +221,23 @@ bool Copter::verify_command(const AP_Mission::Mission_Command& cmd)
     ///
     case MAV_CMD_CONDITION_DELAY:
         return verify_wait_delay();
-        break;
 
     case MAV_CMD_CONDITION_DISTANCE:
         return verify_within_distance();
-        break;
 
     case MAV_CMD_CONDITION_CHANGE_ALT:
         return verify_change_alt();
-        break;
 
     case MAV_CMD_CONDITION_YAW:
         return verify_yaw();
-        break;
 
     case MAV_CMD_DO_PARACHUTE:
         // assume parachute was released successfully
         return true;
-        break;
 
     default:
-        // return true if we do not recognise the command so that we move on to the next command
+        // return true if we do not recognize the command so that we move on to the next command
         return true;
-        break;
     }
 }
 

ArduCopter/config.h

@@ -122,8 +122,6 @@
   # define RATE_YAW_IMAX                        4500
   # define RATE_YAW_FF                          0.02
   # define RATE_YAW_FILT_HZ                     20.0f
-  # define HELI_STAB_COLLECTIVE_MIN_DEFAULT     0
-  # define HELI_STAB_COLLECTIVE_MAX_DEFAULT     1000
   # define THR_MIN_DEFAULT                      0
   # define AUTOTUNE_ENABLED                     DISABLED
 #endif
@@ -238,6 +236,11 @@
  # define FS_GCS_TIMEOUT_MS             5000    // gcs failsafe triggers after 5 seconds with no GCS heartbeat
 #endif
 
+// possible values for FS_GCS parameter
+#define FS_GCS_DISABLED                     0
+#define FS_GCS_ENABLED_ALWAYS_RTL           1
+#define FS_GCS_ENABLED_CONTINUE_MISSION     2
+
 // Radio failsafe while using RC_override
 #ifndef FS_RADIO_RC_OVERRIDE_TIMEOUT_MS
  # define FS_RADIO_RC_OVERRIDE_TIMEOUT_MS  1000    // RC Radio failsafe triggers after 1 second while using RC_override from ground station
@@ -248,10 +251,9 @@
  #define FS_RADIO_TIMEOUT_MS            500     // RC Radio Failsafe triggers after 500 miliseconds with No RC Input
 #endif
 
-// possible values for FS_GCS parameter
-#define FS_GCS_DISABLED                     0
-#define FS_GCS_ENABLED_ALWAYS_RTL           1
-#define FS_GCS_ENABLED_CONTINUE_MISSION     2
+#ifndef FS_CLOSE_TO_HOME_CM
+ # define FS_CLOSE_TO_HOME_CM               200 // if vehicle within 2m of home, vehicle will LAND instead of RTL during some failsafes
+#endif
 
 // pre-arm baro vs inertial nav max alt disparity
 #ifndef PREARM_MAX_ALT_DISPARITY_CM
@@ -303,11 +305,6 @@
  #endif
 #endif
 
-// arming check's maximum acceptable vector difference between internal and external compass after vectors are normalized to field length of 1.0
-#ifndef COMPASS_ACCEPTABLE_VECTOR_DIFF
-  #define COMPASS_ACCEPTABLE_VECTOR_DIFF    0.75f    // pre arm compass check will fail if internal vs external compass direction differ by more than 45 degrees
- #endif
-
 //////////////////////////////////////////////////////////////////////////////
 //  OPTICAL_FLOW
 #ifndef OPTFLOW
@@ -691,6 +688,10 @@
  # define ALT_HOLD_ACCEL_MAX 250    // if you change this you must also update the duplicate declaration in AC_WPNav.h
 #endif
 
+#ifndef AUTO_DISARMING_DELAY
+# define AUTO_DISARMING_DELAY  10
+#endif
+
 //////////////////////////////////////////////////////////////////////////////
 // Dataflash logging control
 //

ArduCopter/control_acro.cpp

@@ -23,6 +23,10 @@ void Copter::acro_run()
     // if motors not running reset angle targets
     if(!motors.armed() || ap.throttle_zero) {
         attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
+        // slow start if landed
+        if (ap.land_complete) {
+            motors.slow_start(true);
+        }
         return;
     }
 

ArduCopter/control_althold.cpp

@@ -9,12 +9,21 @@
 // althold_init - initialise althold controller
 bool Copter::althold_init(bool ignore_checks)
 {
+#if FRAME_CONFIG == HELI_FRAME
+    // do not allow helis to enter Alt Hold if the Rotor Runup is not complete and current control mode has manual throttle control,
+    // as this will force the helicopter to descend.
+    if (!ignore_checks && mode_has_manual_throttle(control_mode) && !motors.rotor_runup_complete()){
+        return false;
+    }
+#endif
+
     // initialize vertical speeds and leash lengths
     pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
     pos_control.set_accel_z(g.pilot_accel_z);
 
-    // initialise altitude target to stopping point
-    pos_control.set_target_to_stopping_point_z();
+    // initialise position and desired velocity
+    pos_control.set_alt_target(inertial_nav.get_altitude());
+    pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
 
     // stop takeoff if running
     takeoff_stop();
@@ -51,8 +60,10 @@ void Copter::althold_run()
 #endif
 
     // Alt Hold State Machine Determination
-    if(!ap.auto_armed || !motors.get_interlock()) {
+    if(!ap.auto_armed) {
         althold_state = AltHold_Disarmed;
+    } else if (!motors.get_interlock()){
+        althold_state = AltHold_MotorStop;
     } else if (takeoff_state.running || takeoff_triggered){
         althold_state = AltHold_Takeoff;
     } else if (ap.land_complete){
@@ -77,6 +88,22 @@ void Copter::althold_run()
         pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);
         break;
 
+    case AltHold_MotorStop:
+
+#if FRAME_CONFIG == HELI_FRAME    
+        // helicopters are capable of flying even with the motor stopped, therefore we will attempt to keep flying
+        // call attitude controller
+        attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
+
+        // force descent rate and call position controller
+        pos_control.set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false);
+        pos_control.update_z_controller();
+#else   // Multicopter do not stabilize roll/pitch/yaw when motor are stopped
+        attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
+        pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);
+#endif  // HELI_FRAME
+        break;
+
     case AltHold_Takeoff:
 
         // initiate take-off
@@ -95,7 +122,7 @@ void Copter::althold_run()
         attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
 
         // call position controller
-        pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false);
+        pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
         pos_control.add_takeoff_climb_rate(takeoff_climb_rate, G_Dt);
         pos_control.update_z_controller();
         break;
@@ -124,7 +151,7 @@ void Copter::althold_run()
         }
 
         // call position controller
-        pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false);
+        pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
         pos_control.update_z_controller();
         break;
     }

ArduCopter/control_auto.cpp

@@ -22,6 +22,14 @@
 // auto_init - initialise auto controller
 bool Copter::auto_init(bool ignore_checks)
 {
+#if FRAME_CONFIG == HELI_FRAME
+    // do not allow helis to enter Alt Hold if the Rotor Runup is not complete and current control mode has manual throttle control,
+    // as this will force the helicopter to descend.
+    if (!ignore_checks && mode_has_manual_throttle(control_mode) && !motors.rotor_runup_complete()){
+        return false;
+    }
+#endif
+
     if ((position_ok() && mission.num_commands() > 1) || ignore_checks) {
         auto_mode = Auto_Loiter;
 

ArduCopter/control_autotune.cpp

@@ -57,7 +57,6 @@
 #define AUTOTUNE_PI_RATIO_FOR_TESTING      0.1f     // I is set 10x smaller than P during testing
 #define AUTOTUNE_PI_RATIO_FINAL            1.0f     // I is set 1x P after testing
 #define AUTOTUNE_YAW_PI_RATIO_FINAL        0.1f     // I is set 1x P after testing
-#define AUTOTUNE_RD_MIN                  0.004f     // minimum Rate D value
 #define AUTOTUNE_RD_MAX                  0.050f     // maximum Rate D value
 #define AUTOTUNE_RLPF_MIN                  1.0f     // minimum Rate Yaw filter value
 #define AUTOTUNE_RLPF_MAX                  5.0f     // maximum Rate Yaw filter value
@@ -165,6 +164,11 @@ static float    tune_yaw_rp, tune_yaw_rLPF, tune_yaw_sp, tune_yaw_accel;
 // autotune_init - should be called when autotune mode is selected
 bool Copter::autotune_init(bool ignore_checks)
 {
+#if FRAME_CONFIG == HELI_FRAME
+    // Autotune mode not available for helicopters
+    return false;
+#endif
+
     bool success = true;
 
     switch (autotune_state.mode) {
@@ -248,8 +252,9 @@ bool Copter::autotune_start(bool ignore_checks)
     pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
     pos_control.set_accel_z(g.pilot_accel_z);
 
-    // initialise altitude target to stopping point
-    pos_control.set_target_to_stopping_point_z();
+    // initialise position and desired velocity
+    pos_control.set_alt_target(inertial_nav.get_altitude());
+    pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
 
     return true;
 }
@@ -326,7 +331,7 @@ void Copter::autotune_run()
         }
 
         // call position controller
-        pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false);
+        pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
         pos_control.update_z_controller();
     }
 }
@@ -552,10 +557,10 @@ void Copter::autotune_attitude_control()
         case AUTOTUNE_TYPE_RD_UP:
             switch (autotune_state.axis) {
             case AUTOTUNE_AXIS_ROLL:
-                autotune_updating_d_up(tune_roll_rd, AUTOTUNE_RD_MIN, AUTOTUNE_RD_MAX, AUTOTUNE_RD_STEP, tune_roll_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, autotune_target_rate, autotune_test_min, autotune_test_max);
+                autotune_updating_d_up(tune_roll_rd, g.autotune_min_d, AUTOTUNE_RD_MAX, AUTOTUNE_RD_STEP, tune_roll_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, autotune_target_rate, autotune_test_min, autotune_test_max);
                 break;
             case AUTOTUNE_AXIS_PITCH:
-                autotune_updating_d_up(tune_pitch_rd, AUTOTUNE_RD_MIN, AUTOTUNE_RD_MAX, AUTOTUNE_RD_STEP, tune_pitch_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, autotune_target_rate, autotune_test_min, autotune_test_max);
+                autotune_updating_d_up(tune_pitch_rd, g.autotune_min_d, AUTOTUNE_RD_MAX, AUTOTUNE_RD_STEP, tune_pitch_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, autotune_target_rate, autotune_test_min, autotune_test_max);
                 break;
             case AUTOTUNE_AXIS_YAW:
                 autotune_updating_d_up(tune_yaw_rLPF, AUTOTUNE_RLPF_MIN, AUTOTUNE_RLPF_MAX, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, autotune_target_rate, autotune_test_min, autotune_test_max);
@@ -566,10 +571,10 @@ void Copter::autotune_attitude_control()
         case AUTOTUNE_TYPE_RD_DOWN:
             switch (autotune_state.axis) {
             case AUTOTUNE_AXIS_ROLL:
-                autotune_updating_d_down(tune_roll_rd, AUTOTUNE_RD_MIN, AUTOTUNE_RD_STEP, tune_roll_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, autotune_target_rate, autotune_test_min, autotune_test_max);
+                autotune_updating_d_down(tune_roll_rd, g.autotune_min_d, AUTOTUNE_RD_STEP, tune_roll_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, autotune_target_rate, autotune_test_min, autotune_test_max);
                 break;
             case AUTOTUNE_AXIS_PITCH:
-                autotune_updating_d_down(tune_pitch_rd, AUTOTUNE_RD_MIN, AUTOTUNE_RD_STEP, tune_pitch_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, autotune_target_rate, autotune_test_min, autotune_test_max);
+                autotune_updating_d_down(tune_pitch_rd, g.autotune_min_d, AUTOTUNE_RD_STEP, tune_pitch_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, autotune_target_rate, autotune_test_min, autotune_test_max);
                 break;
             case AUTOTUNE_AXIS_YAW:
                 autotune_updating_d_down(tune_yaw_rLPF, AUTOTUNE_RLPF_MIN, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, autotune_target_rate, autotune_test_min, autotune_test_max);
@@ -580,10 +585,10 @@ void Copter::autotune_attitude_control()
         case AUTOTUNE_TYPE_RP_UP:
             switch (autotune_state.axis) {
             case AUTOTUNE_AXIS_ROLL:
-                autotune_updating_p_up_d_down(tune_roll_rd, AUTOTUNE_RD_MIN, AUTOTUNE_RD_STEP, tune_roll_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, autotune_target_rate, autotune_test_min, autotune_test_max);
+                autotune_updating_p_up_d_down(tune_roll_rd, g.autotune_min_d, AUTOTUNE_RD_STEP, tune_roll_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, autotune_target_rate, autotune_test_min, autotune_test_max);
                 break;
             case AUTOTUNE_AXIS_PITCH:
-                autotune_updating_p_up_d_down(tune_pitch_rd, AUTOTUNE_RD_MIN, AUTOTUNE_RD_STEP, tune_pitch_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, autotune_target_rate, autotune_test_min, autotune_test_max);
+                autotune_updating_p_up_d_down(tune_pitch_rd, g.autotune_min_d, AUTOTUNE_RD_STEP, tune_pitch_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, autotune_target_rate, autotune_test_min, autotune_test_max);
                 break;
             case AUTOTUNE_AXIS_YAW:
                 autotune_updating_p_up_d_down(tune_yaw_rLPF, AUTOTUNE_RLPF_MIN, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, autotune_target_rate, autotune_test_min, autotune_test_max);
@@ -635,11 +640,11 @@ void Copter::autotune_attitude_control()
                 autotune_state.tune_type = AutoTuneTuneType(autotune_state.tune_type + 1);
                 switch (autotune_state.axis) {
                 case AUTOTUNE_AXIS_ROLL:
-                    tune_roll_rd = max(AUTOTUNE_RD_MIN, tune_roll_rd * AUTOTUNE_RD_BACKOFF);
+                    tune_roll_rd = max(g.autotune_min_d, tune_roll_rd * AUTOTUNE_RD_BACKOFF);
                     tune_roll_rp = max(AUTOTUNE_RP_MIN, tune_roll_rp * AUTOTUNE_RD_BACKOFF);
                     break;
                 case AUTOTUNE_AXIS_PITCH:
-                    tune_pitch_rd = max(AUTOTUNE_RD_MIN, tune_pitch_rd * AUTOTUNE_RD_BACKOFF);
+                    tune_pitch_rd = max(g.autotune_min_d, tune_pitch_rd * AUTOTUNE_RD_BACKOFF);
                     tune_pitch_rp = max(AUTOTUNE_RP_MIN, tune_pitch_rp * AUTOTUNE_RD_BACKOFF);
                     break;
                 case AUTOTUNE_AXIS_YAW:

ArduCopter/control_brake.cpp

@@ -9,7 +9,15 @@
 // brake_init - initialise brake controller
 bool Copter::brake_init(bool ignore_checks)
 {
-    if (position_ok() || optflow_position_ok() || ignore_checks) {
+#if FRAME_CONFIG == HELI_FRAME
+    // do not allow helis to enter Alt Hold if the Rotor Runup is not complete and current control mode has manual throttle control,
+    // as this will force the helicopter to descend.
+    if (!ignore_checks && mode_has_manual_throttle(control_mode) && !motors.rotor_runup_complete()){
+        return false;
+    }
+#endif
+
+    if (position_ok() || ignore_checks) {
 
         // set desired acceleration to zero
         wp_nav.clear_pilot_desired_acceleration();
@@ -21,8 +29,9 @@ bool Copter::brake_init(bool ignore_checks)
         pos_control.set_speed_z(BRAKE_MODE_SPEED_Z, BRAKE_MODE_SPEED_Z);
         pos_control.set_accel_z(BRAKE_MODE_DECEL_RATE);
 
-        // initialise altitude target to stopping point
-        pos_control.set_target_to_stopping_point_z();
+        // initialise position and desired velocity
+        pos_control.set_alt_target(inertial_nav.get_altitude());
+        pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
 
         return true;
     }else{
@@ -67,6 +76,6 @@ void Copter::brake_run()
     // body-frame rate controller is run directly from 100hz loop
 
     // update altitude target and call position controller
-    pos_control.set_alt_target_from_climb_rate(0.0f, G_Dt, false);
+    pos_control.set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false);
     pos_control.update_z_controller();
 }

ArduCopter/control_circle.cpp

@@ -9,6 +9,14 @@
 // circle_init - initialise circle controller flight mode
 bool Copter::circle_init(bool ignore_checks)
 {
+#if FRAME_CONFIG == HELI_FRAME
+    // do not allow helis to enter Alt Hold if the Rotor Runup is not complete and current control mode has manual throttle control,
+    // as this will force the helicopter to descend.
+    if (!ignore_checks && mode_has_manual_throttle(control_mode) && !motors.rotor_runup_complete()){
+        return false;
+    }
+#endif
+
     if (position_ok() || ignore_checks) {
         circle_pilot_yaw_override = false;
 

ArduCopter/control_drift.cpp

@@ -31,6 +31,14 @@
 // drift_init - initialise drift controller
 bool Copter::drift_init(bool ignore_checks)
 {
+#if FRAME_CONFIG == HELI_FRAME
+    // do not allow helis to enter Alt Hold if the Rotor Runup is not complete and current control mode has manual throttle control,
+    // as this will force the helicopter to descend.
+    if (!ignore_checks && mode_has_manual_throttle(control_mode) && !motors.rotor_runup_complete()){
+        return false;
+    }
+#endif
+
     if (position_ok() || ignore_checks) {
         return true;
     }else{

ArduCopter/control_flip.cpp

@@ -41,6 +41,12 @@ int8_t    flip_pitch_dir;           // pitch direction (-1 = pitch forward, 1 =
 // flip_init - initialise flip controller
 bool Copter::flip_init(bool ignore_checks)
 {
+
+#if FRAME_CONFIG == HELI_FRAME
+    // Flip mode not available for helis as it is untested.
+    return false;
+#endif
+
     // only allow flip from ACRO, Stabilize, AltHold or Drift flight modes
     if (control_mode != ACRO && control_mode != STABILIZE && control_mode != ALT_HOLD) {
         return false;

ArduCopter/control_guided.cpp

@@ -15,6 +15,7 @@
 static Vector3f posvel_pos_target_cm;
 static Vector3f posvel_vel_target_cms;
 static uint32_t posvel_update_time_ms;
+static uint32_t vel_update_time_ms;
 
 struct Guided_Limit {
     uint32_t timeout_ms;  // timeout (in seconds) from the time that guided is invoked
@@ -28,6 +29,14 @@ struct Guided_Limit {
 // guided_init - initialise guided controller
 bool Copter::guided_init(bool ignore_checks)
 {
+#if FRAME_CONFIG == HELI_FRAME
+    // do not allow helis to enter Alt Hold if the Rotor Runup is not complete and current control mode has manual throttle control,
+    // as this will force the helicopter to descend.
+    if (!ignore_checks && mode_has_manual_throttle(control_mode) && !motors.rotor_runup_complete()){
+        return false;
+    }
+#endif
+
     if (position_ok() || ignore_checks) {
         // initialise yaw
         set_auto_yaw_mode(get_default_auto_yaw_mode(false));
@@ -138,6 +147,8 @@ void Copter::guided_set_velocity(const Vector3f& velocity)
         guided_vel_control_start();
     }
 
+    vel_update_time_ms = millis();
+
     // set position controller velocity target
     pos_control.set_desired_velocity(velocity);
 }
@@ -160,20 +171,6 @@ void Copter::guided_set_destination_posvel(const Vector3f& destination, const Ve
 // should be called at 100hz or more
 void Copter::guided_run()
 {
-    // if not auto armed or motors not enabled set throttle to zero and exit immediately
-    if(!ap.auto_armed || !motors.get_interlock()) {
-        // To-Do: reset waypoint controller?
-#if FRAME_CONFIG == HELI_FRAME  // Helicopters always stabilize roll/pitch/yaw
-        // call attitude controller
-        attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(0, 0, 0, get_smoothing_gain());
-        attitude_control.set_throttle_out(0,false,g.throttle_filt);
-#else   // multicopters do not stabilize roll/pitch/yaw when disarmed
-        attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
-#endif
-        // To-Do: handle take-offs - these may not only be immediately after auto_armed becomes true
-        return;
-    }
-
     // call the correct auto controller
     switch (guided_mode) {
 
@@ -203,20 +200,15 @@ void Copter::guided_run()
 //      called by guided_run at 100hz or more
 void Copter::guided_takeoff_run()
 {
-    // if not auto armed or motors interlock not enabled set throttle to zero and exit immediately
-    if(!ap.auto_armed || !motors.get_interlock()) {
-        // initialise wpnav targets
-        wp_nav.shift_wp_origin_to_current_pos();
+    // if not auto armed or motors not enabled set throttle to zero and exit immediately
+    if (!ap.auto_armed || !motors.get_interlock()) {
 #if FRAME_CONFIG == HELI_FRAME  // Helicopters always stabilize roll/pitch/yaw
         // call attitude controller
         attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(0, 0, 0, get_smoothing_gain());
         attitude_control.set_throttle_out(0,false,g.throttle_filt);
 #else   // multicopters do not stabilize roll/pitch/yaw when disarmed
-        // reset attitude control targets
         attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
 #endif
-        // clear i term when we're taking off
-        set_throttle_takeoff();
         return;
     }
 
@@ -241,6 +233,18 @@ void Copter::guided_takeoff_run()
 // called from guided_run
 void Copter::guided_pos_control_run()
 {
+    // if not auto armed or motors not enabled set throttle to zero and exit immediately
+    if (!ap.auto_armed || !motors.get_interlock() || ap.land_complete) {
+#if FRAME_CONFIG == HELI_FRAME  // Helicopters always stabilize roll/pitch/yaw
+        // call attitude controller
+        attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(0, 0, 0, get_smoothing_gain());
+        attitude_control.set_throttle_out(0,false,g.throttle_filt);
+#else   // multicopters do not stabilize roll/pitch/yaw when disarmed
+        attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
+#endif
+        return;
+    }
+
     // process pilot's yaw input
     float target_yaw_rate = 0;
     if (!failsafe.radio) {
@@ -271,6 +275,20 @@ void Copter::guided_pos_control_run()
 // called from guided_run
 void Copter::guided_vel_control_run()
 {
+    // if not auto armed or motors not enabled set throttle to zero and exit immediately
+    if (!ap.auto_armed || !motors.get_interlock() || ap.land_complete) {
+        // initialise velocity controller
+        pos_control.init_vel_controller_xyz();
+#if FRAME_CONFIG == HELI_FRAME  // Helicopters always stabilize roll/pitch/yaw
+        // call attitude controller
+        attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(0, 0, 0, get_smoothing_gain());
+        attitude_control.set_throttle_out(0,false,g.throttle_filt);
+#else   // multicopters do not stabilize roll/pitch/yaw when disarmed
+        attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
+#endif
+        return;
+    }
+
     // process pilot's yaw input
     float target_yaw_rate = 0;
     if (!failsafe.radio) {
@@ -281,20 +299,15 @@ void Copter::guided_vel_control_run()
         }
     }
 
-    // calculate dt
-    float dt = pos_control.time_since_last_xy_update();
-
-    // update at poscontrol update rate
-    if (dt >= pos_control.get_dt_xy()) {
-        // sanity check dt
-        if (dt >= 0.2f) {
-            dt = 0.0f;
-        }
-
-        // call velocity controller which includes z axis controller
-        pos_control.update_vel_controller_xyz(ekfNavVelGainScaler);
+    // set velocity to zero if no updates received for 3 seconds
+    uint32_t tnow = millis();
+    if (tnow - vel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS && !pos_control.get_desired_velocity().is_zero()) {
+        pos_control.set_desired_velocity(Vector3f(0,0,0));
     }
 
+    // call velocity controller which includes z axis controller
+    pos_control.update_vel_controller_xyz(ekfNavVelGainScaler);
+
     // call attitude controller
     if (auto_yaw_mode == AUTO_YAW_HOLD) {
         // roll & pitch from waypoint controller, yaw rate from pilot
@@ -309,6 +322,21 @@ void Copter::guided_vel_control_run()
 // called from guided_run
 void Copter::guided_posvel_control_run()
 {
+    // if not auto armed or motors not enabled set throttle to zero and exit immediately
+    if (!ap.auto_armed || !motors.get_interlock() || ap.land_complete) {
+        // set target position and velocity to current position and velocity
+        pos_control.set_pos_target(inertial_nav.get_position());
+        pos_control.set_desired_velocity(Vector3f(0,0,0));
+#if FRAME_CONFIG == HELI_FRAME  // Helicopters always stabilize roll/pitch/yaw
+        // call attitude controller
+        attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(0, 0, 0, get_smoothing_gain());
+        attitude_control.set_throttle_out(0,false,g.throttle_filt);
+#else   // multicopters do not stabilize roll/pitch/yaw when disarmed
+        attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
+#endif
+        return;
+    }
+
     // process pilot's yaw input
     float target_yaw_rate = 0;
 

ArduCopter/control_loiter.cpp

@@ -9,7 +9,15 @@
 // loiter_init - initialise loiter controller
 bool Copter::loiter_init(bool ignore_checks)
 {
-    if (position_ok() || optflow_position_ok() || ignore_checks) {
+#if FRAME_CONFIG == HELI_FRAME
+    // do not allow helis to enter Alt Hold if the Rotor Runup is not complete and current control mode has manual throttle control,
+    // as this will force the helicopter to descend.
+    if (!ignore_checks && mode_has_manual_throttle(control_mode) && !motors.rotor_runup_complete()){
+        return false;
+    }
+#endif
+
+    if (position_ok() || ignore_checks) {
 
         // set target to current position
         wp_nav.init_loiter_target();
@@ -18,8 +26,9 @@ bool Copter::loiter_init(bool ignore_checks)
         pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
         pos_control.set_accel_z(g.pilot_accel_z);
 
-        // initialise altitude target to stopping point
-        pos_control.set_target_to_stopping_point_z();
+        // initialise position and desired velocity
+        pos_control.set_alt_target(inertial_nav.get_altitude());
+        pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
 
         return true;
     }else{
@@ -61,8 +70,10 @@ void Copter::loiter_run()
     }
 
     // Loiter State Machine Determination
-    if(!ap.auto_armed || !motors.get_interlock()) {
+    if(!ap.auto_armed) {
         loiter_state = Loiter_Disarmed;
+    } else if (!motors.get_interlock()){
+        loiter_state = Loiter_MotorStop;
     } else if (takeoff_state.running || (ap.land_complete && (channel_throttle->control_in > get_takeoff_trigger_throttle()))){
         loiter_state = Loiter_Takeoff;
     } else if (ap.land_complete){
@@ -87,6 +98,27 @@ void Copter::loiter_run()
         pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);
         break;
 
+    case Loiter_MotorStop:
+
+#if FRAME_CONFIG == HELI_FRAME
+        // helicopters are capable of flying even with the motor stopped, therefore we will attempt to keep flying
+        // run loiter controller
+        wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
+
+        // call attitude controller
+        attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
+
+        // force descent rate and call position controller
+        pos_control.set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false);
+        pos_control.update_z_controller();
+#else
+        wp_nav.init_loiter_target();
+        // multicopters do not stabilize roll/pitch/yaw when motors are stopped
+        attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
+        pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);
+#endif  // HELI_FRAME
+        break;
+
     case Loiter_Takeoff:
 
         if (!takeoff_state.running) {
@@ -107,7 +139,7 @@ void Copter::loiter_run()
         attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
 
         // update altitude target and call position controller
-        pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false);
+        pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
         pos_control.add_takeoff_climb_rate(takeoff_climb_rate, G_Dt);
         pos_control.update_z_controller();
         break;
@@ -141,7 +173,7 @@ void Copter::loiter_run()
         }
 
         // update altitude target and call position controller
-        pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false);
+        pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
         pos_control.update_z_controller();
         break;
     }

ArduCopter/control_poshold.cpp

@@ -77,6 +77,14 @@ static struct {
 // poshold_init - initialise PosHold controller
 bool Copter::poshold_init(bool ignore_checks)
 {
+#if FRAME_CONFIG == HELI_FRAME
+    // do not allow helis to enter Alt Hold if the Rotor Runup is not complete and current control mode has manual throttle control,
+    // as this will force the helicopter to descend.
+    if (!ignore_checks && mode_has_manual_throttle(control_mode) && !motors.rotor_runup_complete()){
+        return false;
+    }
+#endif
+
     // fail to initialise PosHold mode if no GPS lock
     if (!position_ok() && !ignore_checks) {
         return false;
@@ -86,8 +94,9 @@ bool Copter::poshold_init(bool ignore_checks)
     pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
     pos_control.set_accel_z(g.pilot_accel_z);
 
-    // initialise altitude target to stopping point
-    pos_control.set_target_to_stopping_point_z();
+    // initialise position and desired velocity
+    pos_control.set_alt_target(inertial_nav.get_altitude());
+    pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
 
     // initialise lean angles to current attitude
     poshold.pilot_roll = 0;
@@ -510,7 +519,7 @@ void Copter::poshold_run()
             target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control.get_alt_target(), G_Dt);
         }
         // update altitude target and call position controller
-        pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false);
+        pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
         pos_control.add_takeoff_climb_rate(takeoff_climb_rate, G_Dt);
         pos_control.update_z_controller();
     }

ArduCopter/control_rtl.cpp

@@ -12,6 +12,14 @@
 // rtl_init - initialise rtl controller
 bool Copter::rtl_init(bool ignore_checks)
 {
+#if FRAME_CONFIG == HELI_FRAME
+    // do not allow helis to enter Alt Hold if the Rotor Runup is not complete and current control mode has manual throttle control,
+    // as this will force the helicopter to descend.
+    if (!ignore_checks && mode_has_manual_throttle(control_mode) && !motors.rotor_runup_complete()){
+        return false;
+    }
+#endif
+
     if (position_ok() || ignore_checks) {
         rtl_climb_start();
         return true;
@@ -79,6 +87,7 @@ void Copter::rtl_climb_start()
 {
     rtl_state = RTL_InitialClimb;
     rtl_state_complete = false;
+    rtl_alt = get_RTL_alt();
 
     // initialise waypoint and spline controller
     wp_nav.wp_and_spline_init();
@@ -89,12 +98,12 @@ void Copter::rtl_climb_start()
 
 #if AC_RALLY == ENABLED
     // rally_point.alt will be the altitude of the nearest rally point or the RTL_ALT. uses absolute altitudes
-    Location rally_point = rally.calc_best_rally_or_home_location(current_loc, get_RTL_alt()+ahrs.get_home().alt);
+    Location rally_point = rally.calc_best_rally_or_home_location(current_loc, rtl_alt+ahrs.get_home().alt);
     rally_point.alt -= ahrs.get_home().alt; // convert to altitude above home
     rally_point.alt = max(rally_point.alt, current_loc.alt);    // ensure we do not descend before reaching home
     destination.z = pv_alt_above_origin(rally_point.alt);
 #else
-    destination.z = pv_alt_above_origin(get_RTL_alt());
+    destination.z = pv_alt_above_origin(rtl_alt);
 #endif
 
     // set the destination
@@ -114,13 +123,13 @@ void Copter::rtl_return_start()
     // set target to above home/rally point
 #if AC_RALLY == ENABLED
     // rally_point will be the nearest rally point or home.  uses absolute altitudes
-    Location rally_point = rally.calc_best_rally_or_home_location(current_loc, get_RTL_alt()+ahrs.get_home().alt);
+    Location rally_point = rally.calc_best_rally_or_home_location(current_loc, rtl_alt+ahrs.get_home().alt);
     rally_point.alt -= ahrs.get_home().alt; // convert to altitude above home
     rally_point.alt = max(rally_point.alt, current_loc.alt);    // ensure we do not descend before reaching home
     Vector3f destination = pv_location_to_vector(rally_point);
 #else
     Vector3f destination = pv_location_to_vector(ahrs.get_home());
-    destination.z = pv_alt_above_origin(get_RTL_alt());
+    destination.z = pv_alt_above_origin(rtl_alt));
 #endif
 
     wp_nav.set_wp_destination(destination);
@@ -414,16 +423,16 @@ void Copter::rtl_land_run()
 float Copter::get_RTL_alt()
 {
     // maximum of current altitude + climb_min and rtl altitude
-    float rtl_alt = max(current_loc.alt + max(0, g.rtl_climb_min), g.rtl_altitude);
-    rtl_alt = max(rtl_alt, RTL_ALT_MIN);
+    float ret = max(current_loc.alt + max(0, g.rtl_climb_min), g.rtl_altitude);
+    ret = max(ret, RTL_ALT_MIN);
 
 #if AC_FENCE == ENABLED
     // ensure not above fence altitude if alt fence is enabled
     if ((fence.get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) != 0) {
-        rtl_alt = min(rtl_alt, fence.get_safe_alt()*100.0f);
+        ret = min(ret, fence.get_safe_alt()*100.0f);
     }
 #endif
 
-    return rtl_alt;
+    return ret;
 }
 

ArduCopter/control_sport.cpp

@@ -9,12 +9,21 @@
 // sport_init - initialise sport controller
 bool Copter::sport_init(bool ignore_checks)
 {
+#if FRAME_CONFIG == HELI_FRAME
+    // do not allow helis to enter Alt Hold if the Rotor Runup is not complete and current control mode has manual throttle control,
+    // as this will force the helicopter to descend.
+    if (!ignore_checks && mode_has_manual_throttle(control_mode) && !motors.rotor_runup_complete()){
+        return false;
+    }
+#endif
+
     // initialize vertical speed and accelerationj
     pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
     pos_control.set_accel_z(g.pilot_accel_z);
 
-    // initialise altitude target to stopping point
-    pos_control.set_target_to_stopping_point_z();
+    // initialise position and desired velocity
+    pos_control.set_alt_target(inertial_nav.get_altitude());
+    pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
 
     return true;
 }
@@ -101,7 +110,7 @@ void Copter::sport_run()
         }
 
         // call position controller
-        pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false);
+        pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
         pos_control.add_takeoff_climb_rate(takeoff_climb_rate, G_Dt);
         pos_control.update_z_controller();
     }

ArduCopter/control_stabilize.cpp

@@ -28,6 +28,10 @@ void Copter::stabilize_run()
     // if not armed or throttle at zero, set throttle to zero and exit immediately
     if(!motors.armed() || ap.throttle_zero) {
         attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
+        // slow start if landed
+        if (ap.land_complete) {
+            motors.slow_start(true);
+        }
         return;
     }
 

ArduCopter/defines.h

@@ -3,7 +3,7 @@
 #ifndef _DEFINES_H
 #define _DEFINES_H
 
-#include <AP_HAL_Boards.h>
+#include <AP_HAL/AP_HAL_Boards.h>
 
 // Just so that it's completely clear...
 #define ENABLED                 1
@@ -199,6 +199,7 @@ enum RTLState {
 // Alt_Hold states
 enum AltHoldModeState {
     AltHold_Disarmed,
+    AltHold_MotorStop,
     AltHold_Takeoff,
     AltHold_Flying,
     AltHold_Landed
@@ -207,6 +208,7 @@ enum AltHoldModeState {
 // Loiter states
 enum LoiterModeState {
     Loiter_Disarmed,
+    Loiter_MotorStop,
     Loiter_Takeoff,
     Loiter_Flying,
     Loiter_Landed

ArduCopter/events.cpp

@@ -26,7 +26,7 @@ void Copter::failsafe_radio_on_event()
                 set_mode_land_with_pause();
 
             // if far from home then RTL
-            }else if(home_distance > wp_nav.get_wp_radius()) {
+            } else if(home_distance > FS_CLOSE_TO_HOME_CM) {
                 // switch to RTL or if that fails, LAND
                 set_mode_RTL_or_land_with_pause();
 
@@ -47,7 +47,7 @@ void Copter::failsafe_radio_on_event()
 
             // if failsafe_throttle is FS_THR_ENABLED_ALWAYS_RTL do RTL
             } else if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_RTL) {
-                if(home_distance > wp_nav.get_wp_radius()) {
+                if (home_distance > FS_CLOSE_TO_HOME_CM) {
                     // switch to RTL or if that fails, LAND
                     set_mode_RTL_or_land_with_pause();
                 }else{
@@ -75,7 +75,7 @@ void Copter::failsafe_radio_on_event()
                 set_mode_land_with_pause();
 
             // if far from home then RTL
-            }else if(home_distance > wp_nav.get_wp_radius()) {
+            } else if(home_distance > FS_CLOSE_TO_HOME_CM) {
                 // switch to RTL or if that fails, LAND
                 set_mode_RTL_or_land_with_pause();
             }else{
@@ -117,7 +117,7 @@ void Copter::failsafe_battery_event(void)
                     init_disarm_motors();
                 }else{
                     // set mode to RTL or LAND
-                    if (g.failsafe_battery_enabled == FS_BATT_RTL && home_distance > wp_nav.get_wp_radius()) {
+                    if (g.failsafe_battery_enabled == FS_BATT_RTL && home_distance > FS_CLOSE_TO_HOME_CM) {
                         // switch to RTL or if that fails, LAND
                         set_mode_RTL_or_land_with_pause();
                     }else{
@@ -131,7 +131,7 @@ void Copter::failsafe_battery_event(void)
                     init_disarm_motors();
 
                 // set mode to RTL or LAND
-                } else if (home_distance > wp_nav.get_wp_radius()) {
+                } else if (home_distance > FS_CLOSE_TO_HOME_CM) {
                     // switch to RTL or if that fails, LAND
                     set_mode_RTL_or_land_with_pause();
                 } else {
@@ -145,7 +145,7 @@ void Copter::failsafe_battery_event(void)
                     init_disarm_motors();
 
                 // set mode to RTL or LAND
-                } else if (g.failsafe_battery_enabled == FS_BATT_RTL && home_distance > wp_nav.get_wp_radius()) {
+                } else if (g.failsafe_battery_enabled == FS_BATT_RTL && home_distance > FS_CLOSE_TO_HOME_CM) {
                     // switch to RTL or if that fails, LAND
                     set_mode_RTL_or_land_with_pause();
                 } else {
@@ -212,7 +212,7 @@ void Copter::failsafe_gcs_check()
             // if throttle is zero disarm motors
             if (ap.throttle_zero || ap.land_complete) {
                 init_disarm_motors();
-            }else if(home_distance > wp_nav.get_wp_radius()) {
+            }else if(home_distance > FS_CLOSE_TO_HOME_CM) {
                 // switch to RTL or if that fails, LAND
                 set_mode_RTL_or_land_with_pause();
             }else{
@@ -226,7 +226,7 @@ void Copter::failsafe_gcs_check()
                 init_disarm_motors();
             // if g.failsafe_gcs is 1 do RTL, 2 means continue with the mission
             } else if (g.failsafe_gcs == FS_GCS_ENABLED_ALWAYS_RTL) {
-                if (home_distance > wp_nav.get_wp_radius()) {
+                if (home_distance > FS_CLOSE_TO_HOME_CM) {
                     // switch to RTL or if that fails, LAND
                     set_mode_RTL_or_land_with_pause();
                 }else{
@@ -241,7 +241,7 @@ void Copter::failsafe_gcs_check()
             // if landed disarm
             if (ap.land_complete) {
                 init_disarm_motors();
-            } else if (home_distance > wp_nav.get_wp_radius()) {
+            } else if (home_distance > FS_CLOSE_TO_HOME_CM) {
                 // switch to RTL or if that fails, LAND
                 set_mode_RTL_or_land_with_pause();
             }else{

ArduCopter/flight_mode.cpp

@@ -243,6 +243,17 @@ void Copter::exit_mode(uint8_t old_control_mode, uint8_t new_control_mode)
         attitude_control.use_flybar_passthrough(false, false);
     }
 
+    // if we are changing from a mode that did not use manual throttle,
+    // stab col ramp value should be pre-loaded to the correct value to avoid a twitch
+    // heli_stab_col_ramp should really only be active switching between Stabilize and Acro modes
+    if (!mode_has_manual_throttle(old_control_mode)){
+        if (new_control_mode == STABILIZE){
+            input_manager.set_stab_col_ramp(1.0);
+        } else if (new_control_mode == ACRO){
+            input_manager.set_stab_col_ramp(0.0);
+        }
+    }
+
     // reset RC Passthrough to motors
     motors.reset_radio_passthrough();
 #endif //HELI_FRAME

ArduCopter/heli.cpp

@@ -16,22 +16,25 @@ static int8_t heli_dynamic_flight_counter;
 // heli_init - perform any special initialisation required for the tradheli
 void Copter::heli_init()
 {
-    // Nothing in here for now.  To-Do: Eliminate this function completely?
-}
+    // helicopters are always using motor interlock
+    set_using_interlock(true);
 
-// get_pilot_desired_collective - converts pilot input (from 0 ~ 1000) to a value that can be fed into the channel_throttle->servo_out function
-int16_t Copter::get_pilot_desired_collective(int16_t control_in)
-{
-    // return immediately if reduce collective range for manual flight has not been configured
-    if (g.heli_stab_col_min == 0 && g.heli_stab_col_max == 1000) {
-        return control_in;
+    /*
+      automatically set H_RSC_MIN and H_RSC_MAX from RC8_MIN and
+      RC8_MAX so that when users upgrade from tradheli version 3.2 to
+      3.3 they get the same throttle range as in previous versions of
+      the code
+     */
+    if (!g.heli_servo_rsc.radio_min.load()) {
+        g.heli_servo_rsc.radio_min.set_and_save(g.rc_8.radio_min.get());
+    }
+    if (!g.heli_servo_rsc.radio_max.load()) {
+        g.heli_servo_rsc.radio_max.set_and_save(g.rc_8.radio_max.get());
     }
 
-    // scale pilot input to reduced collective range
-    float scalar = ((float)(g.heli_stab_col_max - g.heli_stab_col_min))/1000.0f;
-    int16_t collective_out = g.heli_stab_col_min + control_in * scalar;
-    collective_out = constrain_int16(collective_out, 0, 1000);
-    return collective_out;
+    // pre-load stab col values as mode is initialized as Stabilize, but stabilize_init() function is not run on start-up.
+    input_manager.set_use_stab_col(true);
+    input_manager.set_stab_col_ramp(1.0);
 }
 
 // heli_check_dynamic_flight - updates the dynamic_flight flag based on our horizontal velocity
@@ -82,10 +85,22 @@ void Copter::check_dynamic_flight(void)
 // should be run between the rate controller and the servo updates.
 void Copter::update_heli_control_dynamics(void)
 {
+    static int16_t hover_roll_trim_scalar_slew = 0;
+
     // Use Leaky_I if we are not moving fast
     attitude_control.use_leaky_i(!heli_flags.dynamic_flight);
-    
-    // To-Do: Update dynamic phase angle of swashplate
+
+    if (ap.land_complete || (motors.get_desired_rotor_speed() == 0)){
+        // if we are landed or there is no rotor power demanded, decrement slew scalar
+        hover_roll_trim_scalar_slew--;        
+    } else {
+        // if we are not landed and motor power is demanded, increment slew scalar
+        hover_roll_trim_scalar_slew++;
+    }
+    hover_roll_trim_scalar_slew = constrain_int16(hover_roll_trim_scalar_slew, 0, MAIN_LOOP_RATE);
+
+    // set hover roll trim scalar, will ramp from 0 to 1 over 1 second after we think helicopter has taken off
+    attitude_control.set_hover_roll_trim_scalar((float)(hover_roll_trim_scalar_slew/MAIN_LOOP_RATE));
 }
 
 // heli_update_landing_swash - sets swash plate flag so higher minimum is used when landed or landing
@@ -140,27 +155,39 @@ void Copter::heli_update_rotor_speed_targets()
 
     rsc_control_deglitched = rotor_speed_deglitch_filter.apply(g.rc_8.control_in);
 
-    if (motors.armed()) {
-        switch (rsc_control_mode) {
-            case AP_MOTORS_HELI_RSC_MODE_NONE:
-                // even though pilot passes rotors speed directly to rotor ESC via receiver, motor lib needs to know if
-                // rotor is spinning in case we are using direct drive tailrotor which must be spun up at same time
-            case AP_MOTORS_HELI_RSC_MODE_CH8_PASSTHROUGH:
-                // pass through pilot desired rotor speed
+
+    switch (rsc_control_mode) {
+        case AP_MOTORS_HELI_RSC_MODE_CH8_PASSTHROUGH:
+            // pass through pilot desired rotor speed if control input is higher than 10, creating a deadband at the bottom
+            if (rsc_control_deglitched > 10) {
+                motors.set_interlock(true);
                 motors.set_desired_rotor_speed(rsc_control_deglitched);
-                break;
-            case AP_MOTORS_HELI_RSC_MODE_SETPOINT:
-                // pass setpoint through as desired rotor speed
-                if (rsc_control_deglitched > 0) {
-                    motors.set_desired_rotor_speed(motors.get_rsc_setpoint());
-                }else{
-                    motors.set_desired_rotor_speed(0);
-                }
-                break;
-        }
-    } else {
-        // if disarmed, force desired_rotor_speed to Zero
-        motors.set_desired_rotor_speed(0);
+            } else {
+                motors.set_interlock(false);
+                motors.set_desired_rotor_speed(0);
+            }
+            break;
+        case AP_MOTORS_HELI_RSC_MODE_SETPOINT:
+            // pass setpoint through as desired rotor speed
+            if (rsc_control_deglitched > 0) {
+                motors.set_interlock(true);
+                motors.set_desired_rotor_speed(motors.get_rsc_setpoint());
+            }else{
+                motors.set_interlock(false);
+                motors.set_desired_rotor_speed(0);
+            }
+            break;
+        case AP_MOTORS_HELI_RSC_MODE_THROTTLE_CURVE:
+            // pass setpoint through as desired rotor speed, this is almost pointless as the Setpoint serves no function in this mode
+            // other than being used to create a crude estimate of rotor speed
+            if (rsc_control_deglitched > 0) {
+                motors.set_interlock(true);
+                motors.set_desired_rotor_speed(motors.get_rsc_setpoint());
+            }else{
+                motors.set_interlock(false);
+                motors.set_desired_rotor_speed(0);
+            }
+            break;
     }
 
     // when rotor_runup_complete changes to true, log event

ArduCopter/heli_control_acro.cpp

@@ -11,7 +11,10 @@
 bool Copter::heli_acro_init(bool ignore_checks)
 {
     // if heli is equipped with a flybar, then tell the attitude controller to pass through controls directly to servos
-    attitude_control.use_flybar_passthrough(motors.has_flybar(), motors.tail_type() == AP_MOTORS_HELI_TAILTYPE_SERVO_EXTGYRO);
+    attitude_control.use_flybar_passthrough(motors.has_flybar(), motors.supports_yaw_passthrough());
+
+    // set stab collective false to use full collective pitch range
+    input_manager.set_use_stab_col(false);
 
     // always successfully enter acro
     return true;
@@ -22,6 +25,7 @@ bool Copter::heli_acro_init(bool ignore_checks)
 void Copter::heli_acro_run()
 {
     float target_roll, target_pitch, target_yaw;
+    int16_t pilot_throttle_scaled;
     
     // Tradheli should not reset roll, pitch, yaw targets when motors are not runup, because
     // we may be in autorotation flight.  These should be reset only when transitioning from disarmed
@@ -35,7 +39,6 @@ void Copter::heli_acro_run()
     }
     
     if(motors.armed() && heli_flags.init_targets_on_arming) {
-        attitude_control.relax_bf_rate_controller();
         attitude_control.set_yaw_target_to_current_heading();
         if (motors.rotor_speed_above_critical()) {
             heli_flags.init_targets_on_arming=false;
@@ -57,8 +60,11 @@ void Copter::heli_acro_run()
         attitude_control.passthrough_bf_roll_pitch_rate_yaw(channel_roll->control_in, channel_pitch->control_in, target_yaw);
     }
 
+    // get pilot's desired throttle
+    pilot_throttle_scaled = input_manager.get_pilot_desired_collective(channel_throttle->control_in);
+
     // output pilot's throttle without angle boost
-    attitude_control.set_throttle_out(channel_throttle->control_in, false, g.throttle_filt);
+    attitude_control.set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt);
 }
 
 // get_pilot_desired_yaw_rate - transform pilot's yaw input into a desired yaw angle rate

ArduCopter/heli_control_stabilize.cpp

@@ -13,6 +13,10 @@ bool Copter::heli_stabilize_init(bool ignore_checks)
     // set target altitude to zero for reporting
     // To-Do: make pos controller aware when it's active/inactive so it can always report the altitude error?
     pos_control.set_alt_target(0);
+
+    // set stab collective true to use stabilize scaled collective pitch range
+    input_manager.set_use_stab_col(true);
+
     return true;
 }
 
@@ -36,7 +40,6 @@ void Copter::heli_stabilize_run()
     }
     
     if(motors.armed() && heli_flags.init_targets_on_arming) {
-        attitude_control.relax_bf_rate_controller();
         attitude_control.set_yaw_target_to_current_heading();
         if (motors.rotor_speed_above_critical()) {
             heli_flags.init_targets_on_arming=false;
@@ -57,7 +60,7 @@ void Copter::heli_stabilize_run()
     target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);
 
     // get pilot's desired throttle
-    pilot_throttle_scaled = get_pilot_desired_collective(channel_throttle->control_in);
+    pilot_throttle_scaled = input_manager.get_pilot_desired_collective(channel_throttle->control_in);
 
     // call attitude controller
     attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());

ArduCopter/make.inc

@@ -63,3 +63,5 @@ LIBRARIES += AP_EPM
 LIBRARIES += AP_Parachute
 LIBRARIES += AP_LandingGear
 LIBRARIES += AP_Terrain
+LIBRARIES += AP_RPM
+LIBRARIES += AC_InputManager

ArduCopter/motors.cpp

@@ -5,8 +5,6 @@
 #define ARM_DELAY               20  // called at 10hz so 2 seconds
 #define DISARM_DELAY            20  // called at 10hz so 2 seconds
 #define AUTO_TRIM_DELAY         100 // called at 10hz so 10 seconds
-#define AUTO_DISARMING_DELAY_LONG   15  // called at 1hz so 15 seconds
-#define AUTO_DISARMING_DELAY_SHORT   5  // called at 1hz so 5 seconds
 #define LOST_VEHICLE_DELAY      10  // called at 10hz so 1 second
 
 static uint8_t auto_disarming_counter;
@@ -75,33 +73,43 @@ void Copter::arm_motors_check()
 // called at 1hz
 void Copter::auto_disarm_check()
 {
+    uint8_t disarm_delay = constrain_int16(g.disarm_delay, 0, 127);
 
-    uint8_t disarm_delay;
-
-    // exit immediately if we are already disarmed or throttle output is not zero,
-    if (!motors.armed() || !ap.throttle_zero) {
+    // exit immediately if we are already disarmed, or if auto
+    // disarming is disabled
+    if (!motors.armed() || disarm_delay == 0) {
         auto_disarming_counter = 0;
         return;
     }
 
-    // allow auto disarm in manual flight modes or Loiter/AltHold if we're landed
     // always allow auto disarm if using interlock switch or motors are Emergency Stopped
-    if (mode_has_manual_throttle(control_mode) || ap.land_complete || (ap.using_interlock && !motors.get_interlock()) || ap.motor_emergency_stop) {
+    if ((ap.using_interlock && !motors.get_interlock()) || ap.motor_emergency_stop) {
         auto_disarming_counter++;
-
         // use a shorter delay if using throttle interlock switch or Emergency Stop, because it is less
         // obvious the copter is armed as the motors will not be spinning
-        if (ap.using_interlock || ap.motor_emergency_stop){
-            disarm_delay = AUTO_DISARMING_DELAY_SHORT;
+        disarm_delay /= 2;
+    } else {
+        bool sprung_throttle_stick = (g.throttle_behavior & THR_BEHAVE_FEEDBACK_FROM_MID_STICK) != 0;
+        bool thr_low;
+        if (mode_has_manual_throttle(control_mode) || !sprung_throttle_stick) {
+            thr_low = ap.throttle_zero;
         } else {
-            disarm_delay = AUTO_DISARMING_DELAY_LONG;
+            float deadband_top = g.rc_3.get_control_mid() + g.throttle_deadzone;
+            thr_low = g.rc_3.control_in <= deadband_top;
         }
 
-        if(auto_disarming_counter >= disarm_delay) {
-            init_disarm_motors();
+        if (thr_low && ap.land_complete) {
+            // increment counter
+            auto_disarming_counter++;
+        } else {
+            // reset counter
             auto_disarming_counter = 0;
         }
-    }else{
+    }
+
+    // disarm once counter expires
+    if (auto_disarming_counter >= disarm_delay) {
+        init_disarm_motors();
         auto_disarming_counter = 0;
     }
 }
@@ -176,27 +184,6 @@ bool Copter::init_arm_motors(bool arming_from_gcs)
         did_ground_start = true;
     }
 
-    // check if we are using motor interlock control on an aux switch
-    set_using_interlock(check_if_auxsw_mode_used(AUXSW_MOTOR_INTERLOCK));
-
-    // if we are using motor interlock switch and it's enabled, fail to arm
-    if (ap.using_interlock && motors.get_interlock()){
-        gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Motor Interlock Enabled"));
-        AP_Notify::flags.armed = false;
-        return false;
-    }
-
-    // if we are not using Emergency Stop switch option, force Estop false to ensure motors
-    // can run normally
-    if (!check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP)){
-        set_motor_emergency_stop(false);
-    // if we are using motor Estop switch, it must not be in Estop position
-    } else if (check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP) && ap.motor_emergency_stop){
-        gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Motor Emergency Stopped"));
-        AP_Notify::flags.armed = false;
-        return false;
-    }
-
     // enable gps velocity based centrefugal force compensation
     ahrs.set_correct_centrifugal(true);
     hal.util->set_soft_armed(true);
@@ -252,27 +239,6 @@ bool Copter::pre_arm_checks(bool display_failure)
         return false;
     }
 
-    // check if motor interlock aux switch is in use
-    // if it is, switch needs to be in disabled position to arm
-    // otherwise exit immediately.  This check to be repeated, 
-    // as state can change at any time.
-    set_using_interlock(check_if_auxsw_mode_used(AUXSW_MOTOR_INTERLOCK));
-    if (ap.using_interlock && motors.get_interlock()){
-        if (display_failure) {
-            gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Motor Interlock Enabled"));
-        }
-        return false;
-    }
-
-    // if we are using Motor Emergency Stop aux switch, check it is not enabled 
-    // and warn if it is
-    if (check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP) && ap.motor_emergency_stop){
-        if (display_failure) {
-            gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Motor Emergency Stopped"));
-        }
-        return false;
-    }
-
     // exit immediately if we've already successfully performed the pre-arm check
     if (ap.pre_arm_check) {
         // run gps checks because results may change and affect LED colour
@@ -356,25 +322,13 @@ bool Copter::pre_arm_checks(bool display_failure)
             return false;
         }
 
-#if COMPASS_MAX_INSTANCES > 1
         // check all compasses point in roughly same direction
-        if (compass.get_count() > 1) {
-            Vector3f prime_mag_vec = compass.get_field();
-            prime_mag_vec.normalize();
-            for(uint8_t i=0; i<compass.get_count(); i++) {
-                // get next compass
-                Vector3f mag_vec = compass.get_field(i);
-                mag_vec.normalize();
-                Vector3f vec_diff = mag_vec - prime_mag_vec;
-                if (compass.use_for_yaw(i) && vec_diff.length() > COMPASS_ACCEPTABLE_VECTOR_DIFF) {
+        if (!compass.consistent()) {
                     if (display_failure) {
                         gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: inconsistent compasses"));
                     }
                     return false;
                 }
-            }
-        }
-#endif
 
     }
 
@@ -618,12 +572,6 @@ bool Copter::pre_arm_gps_checks(bool display_failure)
         return false;
     }
 
-    // return true immediately if gps check is disabled
-    if (!(g.arming_check == ARMING_CHECK_ALL || g.arming_check & ARMING_CHECK_GPS)) {
-        AP_Notify::flags.pre_arm_gps_check = true;
-        return true;
-    }
-
     // check if flight mode requires GPS
     bool gps_required = mode_requires_GPS(control_mode);
 
@@ -643,12 +591,29 @@ bool Copter::pre_arm_gps_checks(bool display_failure)
     // ensure GPS is ok
     if (!position_ok()) {
         if (display_failure) {
-            gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Need 3D Fix"));
+            const char *reason = ahrs.prearm_failure_reason();
+            if (reason) {
+                GCS_MAVLINK::send_statustext_all(SEVERITY_HIGH, PSTR("PreArm: %s"), reason);
+            } else {
+                gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Need 3D Fix"));
+        }
         }
         AP_Notify::flags.pre_arm_gps_check = false;
         return false;
     }
 
+    // check EKF compass variance is below failsafe threshold
+    float vel_variance, pos_variance, hgt_variance, tas_variance;
+    Vector3f mag_variance;
+    Vector2f offset;
+    ahrs.get_NavEKF().getVariances(vel_variance, pos_variance, hgt_variance, mag_variance, tas_variance, offset);
+    if (mag_variance.length() >= g.fs_ekf_thresh) {
+        if (display_failure) {
+            gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: EKF compass variance"));
+        }
+        return false;
+    }
+
     // check home and EKF origin are not too far
     if (far_from_EKF_origin(ahrs.get_home())) {
         if (display_failure) {
@@ -658,6 +623,12 @@ bool Copter::pre_arm_gps_checks(bool display_failure)
         return false;
     }
 
+    // return true immediately if gps check is disabled
+    if (!(g.arming_check == ARMING_CHECK_ALL || g.arming_check & ARMING_CHECK_GPS)) {
+        AP_Notify::flags.pre_arm_gps_check = true;
+        return true;
+    }
+
     // warn about hdop separately - to prevent user confusion with no gps lock
     if (gps.get_hdop() > g.gps_hdop_good) {
         if (display_failure) {
@@ -682,6 +653,22 @@ bool Copter::arm_checks(bool display_failure, bool arming_from_gcs)
     start_logging();
 #endif
 
+    // check accels and gyro are healthy
+    if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_INS)) {
+        if(!ins.get_accel_health_all()) {
+            if (display_failure) {
+                gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Accelerometers not healthy"));
+            }
+            return false;
+        }
+        if(!ins.get_gyro_health_all()) {
+            if (display_failure) {
+                gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Gyros not healthy"));
+            }
+            return false;
+        }
+    }
+
     // always check if inertial nav has started and is ready
     if(!ahrs.healthy()) {
         if (display_failure) {
@@ -698,29 +685,59 @@ bool Copter::arm_checks(bool display_failure, bool arming_from_gcs)
         return false;
     }
 
-    // always check if rotor is spinning on heli
+    // always check gps
+    if (!pre_arm_gps_checks(display_failure)) {
+        return false;
+    }
+
 #if FRAME_CONFIG == HELI_FRAME
-    // heli specific arming check
-    if ((rsc_control_deglitched > 0) || !motors.allow_arming()){
+	// heli specific arming check
+    // check if rotor is spinning on heli because this could disrupt gyro calibration
+    if (!motors.allow_arming()){
         if (display_failure) {
-            gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Rotor Control Engaged"));
+            gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Rotor is Spinning"));
         }
         return false;
     }
-#endif  // HELI_FRAME
+#endif
+
+    // if we are using motor interlock switch and it's enabled, fail to arm
+    if (ap.using_interlock && motors.get_interlock()){
+        gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Motor Interlock Enabled"));
+        AP_Notify::flags.armed = false;
+        return false;
+    }
+
+    // if we are not using Emergency Stop switch option, force Estop false to ensure motors
+    // can run normally
+    if (!check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP)){
+        set_motor_emergency_stop(false);
+    // if we are using motor Estop switch, it must not be in Estop position
+    } else if (check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP) && ap.motor_emergency_stop){
+        gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Motor Emergency Stopped"));
+        return false;
+    }
 
     // succeed if arming checks are disabled
     if (g.arming_check == ARMING_CHECK_NONE) {
         return true;
     }
 
-    // Check baro & inav alt are within 1m if EKF is operating in an absolute position mode.
-    // Do not check if intending to operate in a ground relative height mode as EKF will output a ground relative height
-    // that may differ from the baro height due to baro drift.
-    nav_filter_status filt_status = inertial_nav.get_filter_status();
-    bool using_baro_ref = (!filt_status.flags.pred_horiz_pos_rel && filt_status.flags.pred_horiz_pos_abs);
-    if (((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_BARO)) && using_baro_ref) {
-        if (fabsf(inertial_nav.get_altitude() - baro_alt) > PREARM_MAX_ALT_DISPARITY_CM) {
+    // baro checks
+    if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_BARO)) {
+        // baro health check
+        if (!barometer.all_healthy()) {
+            if (display_failure) {
+                gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Barometer not healthy"));
+            }
+            return false;
+        }
+        // Check baro & inav alt are within 1m if EKF is operating in an absolute position mode.
+        // Do not check if intending to operate in a ground relative height mode as EKF will output a ground relative height
+        // that may differ from the baro height due to baro drift.
+        nav_filter_status filt_status = inertial_nav.get_filter_status();
+        bool using_baro_ref = (!filt_status.flags.pred_horiz_pos_rel && filt_status.flags.pred_horiz_pos_abs);
+        if (using_baro_ref && (fabsf(inertial_nav.get_altitude() - baro_alt) > PREARM_MAX_ALT_DISPARITY_CM)) {
             if (display_failure) {
                 gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Altitude disparity"));
             }
@@ -728,11 +745,6 @@ bool Copter::arm_checks(bool display_failure, bool arming_from_gcs)
         }
     }
 
-    // check gps
-    if (!pre_arm_gps_checks(display_failure)) {
-        return false;
-    }
-
 #if AC_FENCE == ENABLED
     // check vehicle is within fence
     if(!fence.pre_arm_check()) {

ArduCopter/sensors.cpp

@@ -69,6 +69,19 @@ int16_t Copter::read_sonar(void)
 #endif
 }
 
+/*
+  update RPM sensors
+ */
+void Copter::rpm_update(void)
+{
+    rpm_sensor.update();
+    if (rpm_sensor.healthy(0) || rpm_sensor.healthy(1)) {
+        if (should_log(MASK_LOG_RCIN)) {
+            DataFlash.Log_Write_RPM(rpm_sensor);
+        }
+    }
+}
+
 // initialise compass
 void Copter::init_compass()
 {

ArduCopter/system.cpp

@@ -165,6 +165,11 @@ void Copter::init_ardupilot()
     log_init();
 #endif
 
+#if FRAME_CONFIG == HELI_FRAME
+    // trad heli specific initialisation
+    heli_init();
+#endif
+    
     init_rc_in();               // sets up rc channels from radio
     init_rc_out();              // sets up motors and output to escs
 
@@ -240,6 +245,9 @@ void Copter::init_ardupilot()
     init_sonar();
 #endif
 
+    // initialise AP_RPM library
+    rpm_sensor.init();
+
     // initialise mission library
     mission.init();
 
@@ -248,11 +256,6 @@ void Copter::init_ardupilot()
     reset_control_switch();
     init_aux_switches();
 
-#if FRAME_CONFIG == HELI_FRAME
-    // trad heli specific initialisation
-    heli_init();
-#endif
-
     startup_ground(true);
 
     // we don't want writes to the serial port to cause us to pause
@@ -266,6 +269,9 @@ void Copter::init_ardupilot()
     ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW));
     ins.set_dataflash(&DataFlash);
 
+    // init vehicle capabilties
+    init_capabilities();
+
     cliSerial->print_P(PSTR("\nReady to FLY "));
 
     // flag that initialisation has completed
@@ -311,7 +317,7 @@ bool Copter::position_ok()
     }
 
     // check ekf position estimate
-    return ekf_position_ok();
+    return (ekf_position_ok() || optflow_position_ok());
 }
 
 // ekf_position_ok - returns true if the ekf claims it's horizontal absolute position estimate is ok and home position is set
@@ -347,7 +353,13 @@ bool Copter::optflow_position_ok()
 
     // get filter status from EKF
     nav_filter_status filt_status = inertial_nav.get_filter_status();
-    return (filt_status.flags.horiz_pos_rel || filt_status.flags.pred_horiz_pos_rel);
+
+    // if disarmed we accept a predicted horizontal relative position
+    if (!motors.armed()) {
+        return (filt_status.flags.pred_horiz_pos_rel);
+    } else {
+        return (filt_status.flags.horiz_pos_rel && !filt_status.flags.const_pos_mode);
+    }
 #endif
 }
 

ArduPlane/ArduPlane.cpp

@@ -137,10 +137,12 @@ void Plane::ahrs_update()
     hal.util->set_soft_armed(arming.is_armed() &&
                    hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED);
 
+#if HIL_SUPPORT
     if (g.hil_mode == 1) {
         // update hil before AHRS update
         gcs_update();
     }
+#endif
 
     ahrs.update();
 
@@ -150,7 +152,9 @@ void Plane::ahrs_update()
 
     if (should_log(MASK_LOG_IMU)) {
         Log_Write_IMU();
+#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
         DataFlash.Log_Write_IMUDT(ins);
+#endif
     }
 
     // calculate a scaled roll limit based on current pitch
@@ -253,8 +257,10 @@ void Plane::update_logging2(void)
     if (should_log(MASK_LOG_RC))
         Log_Write_RC();
 
+#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
     if (should_log(MASK_LOG_IMU))
         DataFlash.Log_Write_Vibration(ins);
+#endif
 }
 
 
@@ -315,7 +321,9 @@ void Plane::one_second_loop()
         Log_Write_Status();
     }
 
+#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
     ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW));
+#endif
 }
 
 void Plane::log_perf_info()
@@ -325,8 +333,10 @@ void Plane::log_perf_info()
                           (unsigned long)G_Dt_max, 
                           (unsigned long)G_Dt_min);
     }
+#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
     if (should_log(MASK_LOG_PM))
         Log_Write_Performance();
+#endif
     G_Dt_max = 0;
     G_Dt_min = 0;
     resetPerfData();
@@ -542,6 +552,7 @@ void Plane::update_flight_mode(void)
     case TRAINING: {
         training_manual_roll = false;
         training_manual_pitch = false;
+        update_load_factor();
         
         // if the roll is past the set roll limit, then
         // we set target roll to the limit

ArduPlane/Attitude.cpp

@@ -995,6 +995,7 @@ void Plane::set_servos(void)
     obc.check_crash_plane();
 #endif
 
+#if HIL_SUPPORT
     if (g.hil_mode == 1) {
         // get the servos to the GCS immediately for HIL
         if (comm_get_txspace(MAVLINK_COMM_0) >= 
@@ -1005,6 +1006,7 @@ void Plane::set_servos(void)
             return;
         }
     }
+#endif
 
     // send values to the PWM timers for output
     // ----------------------------------------
@@ -1080,6 +1082,7 @@ void Plane::update_load_factor(void)
         // our airspeed is below the minimum airspeed. Limit roll to
         // 25 degrees
         nav_roll_cd = constrain_int32(nav_roll_cd, -2500, 2500);
+        roll_limit_cd = constrain_int32(roll_limit_cd, -2500, 2500);
     } else if (max_load_factor < aerodynamic_load_factor) {
         // the demanded nav_roll would take us past the aerodymamic
         // load limit. Limit our roll to a bank angle that will keep
@@ -1092,5 +1095,6 @@ void Plane::update_load_factor(void)
             roll_limit = 2500;
         }
         nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit, roll_limit);
+        roll_limit_cd = constrain_int32(roll_limit_cd, -roll_limit, roll_limit);
     }    
 }

ArduPlane/GCS_Mavlink.cpp

@@ -5,8 +5,6 @@
 // default sensors are present and healthy: gyro, accelerometer, barometer, rate_control, attitude_stabilization, yaw_position, altitude control, x/y position control, motor_control
 #define MAVLINK_SENSOR_PRESENT_DEFAULT (MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL | MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE | MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL | MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION | MAV_SYS_STATUS_SENSOR_YAW_POSITION | MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL | MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL | MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS | MAV_SYS_STATUS_AHRS)
 
-#define HAVE_PAYLOAD_SPACE(chan, id) (comm_get_txspace(chan) >= MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_ ## id ## _LEN)
-
 void Plane::send_heartbeat(mavlink_channel_t chan)
 {
     uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
@@ -69,9 +67,11 @@ void Plane::send_heartbeat(mavlink_channel_t chan)
         base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
     }
 
+#if HIL_SUPPORT
     if (g.hil_mode == 1) {
         base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
     }
+#endif
 
     // we are armed if we are not initialising
     if (control_mode != INITIALISING && hal.util->get_soft_armed()) {
@@ -366,6 +366,7 @@ void Plane::send_servo_out(mavlink_channel_t chan)
 
 void Plane::send_radio_out(mavlink_channel_t chan)
 {
+#if HIL_SUPPORT
     if (g.hil_mode==1 && !g.hil_servos) {
         mavlink_msg_servo_output_raw_send(
             chan,
@@ -381,6 +382,7 @@ void Plane::send_radio_out(mavlink_channel_t chan)
             RC_Channel::rc_channel(7)->radio_out);
         return;
     }
+#endif
     mavlink_msg_servo_output_raw_send(
         chan,
         micros(),
@@ -416,14 +418,16 @@ void Plane::send_vfr_hud(mavlink_channel_t chan)
 /*
   keep last HIL_STATE message to allow sending SIM_STATE
  */
+#if HIL_SUPPORT
 static mavlink_hil_state_t last_hil_state;
+#endif
 
 // report simulator state
 void Plane::send_simstate(mavlink_channel_t chan)
 {
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
     sitl.simstate_send(chan);
-#else
+#elif HIL_SUPPORT
     if (g.hil_mode == 1) {
         mavlink_msg_simstate_send(chan,
                                   last_hil_state.roll,
@@ -564,15 +568,10 @@ bool Plane::telemetry_delayed(mavlink_channel_t chan)
     return true;
 }
 
-// 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
-#define CHECK_PAYLOAD_SIZE2(id) if (!HAVE_PAYLOAD_SPACE(chan, id)) return false
 
 // 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 (plane.telemetry_delayed(chan)) {
         return false;
     }
@@ -637,10 +636,12 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
         break;
 
     case MSG_SERVO_OUT:
+#if HIL_SUPPORT
         if (plane.g.hil_mode == 1) {
             CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED);
             plane.send_servo_out(chan);
         }
+#endif
         break;
 
     case MSG_RADIO_IN:
@@ -790,6 +791,15 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
         CHECK_PAYLOAD_SIZE(VIBRATION);
         send_vibration(plane.ins);
         break;
+
+    case MSG_RPM:
+        // unused
+        break;
+
+    case MSG_MISSION_ITEM_REACHED:
+        CHECK_PAYLOAD_SIZE(MISSION_ITEM_REACHED);
+        mavlink_msg_mission_item_reached_send(chan, mission_item_reached_index);
+        break;
     }
     return true;
 }
@@ -937,6 +947,7 @@ GCS_MAVLINK::data_stream_send(void)
     if (plane.gcs_out_of_time) return;
 
     if (plane.in_mavlink_delay) {
+#if HIL_SUPPORT
         if (plane.g.hil_mode == 1) {
             // in HIL we need to keep sending servo values to ensure
             // the simulator doesn't pause, otherwise our sensor
@@ -948,6 +959,7 @@ GCS_MAVLINK::data_stream_send(void)
                 send_message(MSG_RADIO_OUT);
             }
         }
+#endif
         // don't send any other stream types while in the delay callback
         return;
     }
@@ -1330,7 +1342,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 
         case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: {
             if (is_equal(packet.param1,1.0f)) {
-                plane.gcs[chan-MAVLINK_COMM_0].send_autopilot_version();
+                plane.gcs[chan-MAVLINK_COMM_0].send_autopilot_version(FIRMWARE_VERSION);
                 result = MAV_RESULT_ACCEPTED;
             }
             break;
@@ -1606,6 +1618,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 
     case MAVLINK_MSG_ID_HIL_STATE:
     {
+#if HIL_SUPPORT
         if (plane.g.hil_mode != 1) {
             break;
         }
@@ -1656,6 +1669,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
              wrap_PI(fabsf(packet.yaw - plane.ahrs.yaw)) > ToRad(plane.g.hil_err_limit))) {
             plane.ahrs.reset_attitude(packet.roll, packet.pitch, packet.yaw);
         }
+#endif
         break;
     }
 
@@ -1729,7 +1743,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
         break;
 
     case MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST:
-        plane.gcs[chan-MAVLINK_COMM_0].send_autopilot_version();
+        plane.gcs[chan-MAVLINK_COMM_0].send_autopilot_version(FIRMWARE_VERSION);
         break;
         
     } // end switch
@@ -1781,6 +1795,19 @@ void Plane::gcs_send_message(enum ap_message id)
     }
 }
 
+/*
+ *  send a mission item reached message and load the index before the send attempt in case it may get delayed
+ */
+void Plane::gcs_send_mission_item_reached_message(uint16_t mission_index)
+{
+    for (uint8_t i=0; i<num_gcs; i++) {
+        if (gcs[i].initialised) {
+            gcs[i].mission_item_reached_index = mission_index;
+            gcs[i].send_message(MSG_MISSION_ITEM_REACHED);
+        }
+    }
+}
+
 /*
  *  send data streams in the given rate range on both links
  */
@@ -1868,4 +1895,3 @@ void Plane::gcs_retry_deferred(void)
 {
     gcs_send_message(MSG_RETRY_DEFERRED);
 }
-

ArduPlane/Log.cpp

@@ -539,10 +539,42 @@ void Plane::log_init(void)
 
 #else // LOGGING_ENABLED
 
-int8_t Plane::process_logs(uint8_t argc, const Menu::arg *argv) 
-{
-    return 0;
-}
+ #if CLI_ENABLED == ENABLED
+bool Plane::print_log_menu(void) { return true; }
+int8_t Plane::dump_log(uint8_t argc, const Menu::arg *argv) { return 0; }
+int8_t Plane::erase_logs(uint8_t argc, const Menu::arg *argv) { return 0; }
+int8_t Plane::select_logs(uint8_t argc, const Menu::arg *argv) { return 0; }
+int8_t Plane::process_logs(uint8_t argc, const Menu::arg *argv) { return 0; }
+ #endif // CLI_ENABLED == ENABLED
 
+void Plane::do_erase_logs(void) {}
+void Plane::Log_Write_Attitude(void) {}
+void Plane::Log_Write_Performance() {}
+void Plane::Log_Write_Startup(uint8_t type) {}
+void Plane::Log_Write_Control_Tuning() {}
+void Plane::Log_Write_TECS_Tuning(void) {}
+void Plane::Log_Write_Nav_Tuning() {}
+void Plane::Log_Write_Status() {}
+void Plane::Log_Write_Sonar() {}
+
+ #if OPTFLOW == ENABLED
+void Plane::Log_Write_Optflow() {}
+ #endif
+
+void Plane::Log_Write_Current() {}
+void Plane::Log_Arm_Disarm() {}
+void Plane::Log_Write_GPS(uint8_t instance) {}
+void Plane::Log_Write_IMU() {}
+void Plane::Log_Write_RC(void) {}
+void Plane::Log_Write_Baro(void) {}
+void Plane::Log_Write_Airspeed(void) {}
+void Plane::Log_Write_Home_And_Origin() {}
+
+ #if CLI_ENABLED == ENABLED
+void Plane::Log_Read(uint16_t log_num, int16_t start_page, int16_t end_page) {}
+ #endif // CLI_ENABLED
+
+void Plane::start_logging() {}
+void Plane::log_init(void) {}
 
 #endif // LOGGING_ENABLED

ArduPlane/Parameters.cpp

@@ -55,7 +55,7 @@ const AP_Param::Info Plane::var_info[] PROGMEM = {
 
     // @Param: AUTOTUNE_LEVEL
     // @DisplayName: Autotune level
-    // @Description: Level of agressiveness for autotune. When autotune is run a lower AUTOTUNE_LEVEL will result in a 'softer' tune, with less agressive gains. For most users a level of 6 is recommended.
+    // @Description: Level of aggressiveness for autotune. When autotune is run a lower AUTOTUNE_LEVEL will result in a 'softer' tune, with less agressive gains. For most users a level of 6 is recommended.
     // @Range: 1 10
     // @Increment: 1
     // @User: Standard
@@ -171,7 +171,7 @@ const AP_Param::Info Plane::var_info[] PROGMEM = {
 
     // @Param: TKOFF_TDRAG_ELEV
     // @DisplayName: Takeoff tail dragger elevator
-    // @Description: This parameter sets the amount of elevator to apply during the initial stage of a takeoff. It is used to hold the tail wheel of a taildragger on the ground during the initial takeoff stage to give maximum steering. This option should be conbined with the TKOFF_TDRAG_SPD1 option and the GROUND_STEER_ALT option along with tuning of the ground steering controller. A value of zero means to bypass the initial "tail hold" stage of takeoff. Set to zero for hand and catapult launch. For tail-draggers you should normally set this to 100, meaning full up elevator during the initial stage of takeoff. For most tricycle undercarriage aircraft a value of zero will work well, but for some tricycle aircraft a small negative value (say around -20 to -30) will apply down elevator which will hold the nose wheel firmly on the ground during initial acceleration. Only use a negative value if you find that the nosewheel doesn't grip well during takeoff. Too much down elevator on a tricycle undercarriage may cause instability in steering as the plane pivots around the nosewheel. Add down elevator 10 percent at a time.
+    // @Description: This parameter sets the amount of elevator to apply during the initial stage of a takeoff. It is used to hold the tail wheel of a taildragger on the ground during the initial takeoff stage to give maximum steering. This option should be combined with the TKOFF_TDRAG_SPD1 option and the GROUND_STEER_ALT option along with tuning of the ground steering controller. A value of zero means to bypass the initial "tail hold" stage of takeoff. Set to zero for hand and catapult launch. For tail-draggers you should normally set this to 100, meaning full up elevator during the initial stage of takeoff. For most tricycle undercarriage aircraft a value of zero will work well, but for some tricycle aircraft a small negative value (say around -20 to -30) will apply down elevator which will hold the nose wheel firmly on the ground during initial acceleration. Only use a negative value if you find that the nosewheel doesn't grip well during takeoff. Too much down elevator on a tricycle undercarriage may cause instability in steering as the plane pivots around the nosewheel. Add down elevator 10 percent at a time.
     // @Units: Percent
     // @Range: -100 100
     // @Increment: 1
@@ -262,14 +262,14 @@ const AP_Param::Info Plane::var_info[] PROGMEM = {
 
 	// @Param: NAV_CONTROLLER
 	// @DisplayName: Navigation controller selection
-	// @Description: Which navigation controller to enable. Currently the only navigation controller available is L1. From time to time other experimental conrtrollers will be added which are selected using this parameter.
+	// @Description: Which navigation controller to enable. Currently the only navigation controller available is L1. From time to time other experimental controllers will be added which are selected using this parameter.
 	// @Values: 0:Default,1:L1Controller
 	// @User: Standard
 	GSCALAR(nav_controller,          "NAV_CONTROLLER",   AP_Navigation::CONTROLLER_L1),
 
     // @Param: ALT_MIX
     // @DisplayName: GPS to Baro Mix
-    // @Description: The percent of mixing between GPS altitude and baro altitude. 0 = 100% gps, 1 = 100% baro. It is highly recommend that you not change this from the default of 1, as GPS altitude is notoriously unreliable. The only time I would recommend changing this is if you have a high altitude enabled GPS, and you are dropping a plane from a high altitude baloon many kilometers off the ground.
+    // @Description: The percent of mixing between GPS altitude and baro altitude. 0 = 100% gps, 1 = 100% baro. It is highly recommend that you not change this from the default of 1, as GPS altitude is notoriously unreliable. The only time I would recommend changing this is if you have a high altitude enabled GPS, and you are dropping a plane from a high altitude balloon many kilometers off the ground.
     // @Units: Percent
     // @Range: 0 1
     // @Increment: 0.1
@@ -278,7 +278,7 @@ const AP_Param::Info Plane::var_info[] PROGMEM = {
 
     // @Param: ALT_CTRL_ALG
     // @DisplayName: Altitude control algorithm
-    // @Description: This sets what algorithm will be used for altitude control. The default is zero, which selects the most appropriate algorithm for your airframe. Currently the default is to use TECS (total energy control system). From time to time we will add other experimental altitude control algorithms which will be seleted using this parameter.
+    // @Description: This sets what algorithm will be used for altitude control. The default is zero, which selects the most appropriate algorithm for your airframe. Currently the default is to use TECS (total energy control system). From time to time we will add other experimental altitude control algorithms which will be selected using this parameter.
     // @Values: 0:Automatic
     // @User: Advanced
     GSCALAR(alt_control_algorithm, "ALT_CTRL_ALG",    ALT_CONTROL_DEFAULT),
@@ -294,7 +294,7 @@ const AP_Param::Info Plane::var_info[] PROGMEM = {
 
     // @Param: WP_RADIUS
     // @DisplayName: Waypoint Radius
-    // @Description: Defines the maximum distance from a waypoint that when crossed indicates the waypoint may be complete. To avoid the aircraft looping around the waypoint in case it misses by more than the WP_RADIUS an additional check is made to see if the aircraft has crossed a "finish line" passing through the waypoint and perpendicular to the flight path from the previous waypoint. If that finish line is crossed then the waypoint is considered complete. Note that the navigation controller may decide to turn later than WP_RADIUS before a waypoint, based on how sharp the turn is and the speed of the aircraft. It is safe to set WP_RADIUS much larger than the usual turn radius of your aircaft and the navigation controller will work out when to turn. If you set WP_RADIUS too small then you will tend to overshoot the turns.
+    // @Description: Defines the maximum distance from a waypoint that when crossed indicates the waypoint may be complete. To avoid the aircraft looping around the waypoint in case it misses by more than the WP_RADIUS an additional check is made to see if the aircraft has crossed a "finish line" passing through the waypoint and perpendicular to the flight path from the previous waypoint. If that finish line is crossed then the waypoint is considered complete. Note that the navigation controller may decide to turn later than WP_RADIUS before a waypoint, based on how sharp the turn is and the speed of the aircraft. It is safe to set WP_RADIUS much larger than the usual turn radius of your aircraft and the navigation controller will work out when to turn. If you set WP_RADIUS too small then you will tend to overshoot the turns.
     // @Units: Meters
     // @Range: 1 32767
     // @Increment: 1
@@ -368,7 +368,7 @@ const AP_Param::Info Plane::var_info[] PROGMEM = {
 
     // @Param: FENCE_AUTOENABLE
     // @DisplayName: Fence automatic enable
-    // @Description: When set to 1, gefence automatically enables after an auto takeoff and automatically disables at the beginning of an auto landing.  When on the ground before takeoff the fence is disabled.  When set to 2, the fence autoenables after an auto takeoff, but only disables the fence floor during landing. It is highly recommended to not use this option for line of sight flying and use a fence enable channel instead.
+    // @Description: When set to 1, geofence automatically enables after an auto takeoff and automatically disables at the beginning of an auto landing.  When on the ground before takeoff the fence is disabled.  When set to 2, the fence autoenables after an auto takeoff, but only disables the fence floor during landing. It is highly recommended to not use this option for line of sight flying and use a fence enable channel instead.
     // @Values: 0:NoAutoEnable,1:AutoEnable,2:AutoEnableDisableFloorOnly
     // @User: Standard
     GSCALAR(fence_autoenable,       "FENCE_AUTOENABLE", 0),
@@ -485,7 +485,7 @@ const AP_Param::Info Plane::var_info[] PROGMEM = {
 
     // @Param: THR_SUPP_MAN
     // @DisplayName: Throttle suppress manual passthru
-    // @Description: When throttle is supressed in auto mode it is normally forced to zero. If you enable this option, then while suppressed it will be manual throttle. This is useful on petrol engines to hold the idle throttle manually while waiting for takeoff
+    // @Description: When throttle is suppressed in auto mode it is normally forced to zero. If you enable this option, then while suppressed it will be manual throttle. This is useful on petrol engines to hold the idle throttle manually while waiting for takeoff
 	// @Values: 0:Disabled,1:Enabled
     // @User: Advanced
     GSCALAR(throttle_suppress_manual,"THR_SUPP_MAN",   0),
@@ -507,7 +507,7 @@ const AP_Param::Info Plane::var_info[] PROGMEM = {
 
     // @Param: THR_FS_VALUE
     // @DisplayName: Throttle Failsafe Value
-    // @Description: The PWM level on channel 3 below which throttle sailsafe triggers
+    // @Description: The PWM level on channel 3 below which throttle failsafe triggers
     // @Range: 925 1100
     // @Increment: 1
     // @User: Standard
@@ -539,7 +539,7 @@ const AP_Param::Info Plane::var_info[] PROGMEM = {
 
     // @Param: FS_SHORT_TIMEOUT
     // @DisplayName: Short failsafe timeout
-    // @Description: The time in seconds that a failsafe condition has to persist before a short failsafe event will occor. This defaults to 1.5 seconds
+    // @Description: The time in seconds that a failsafe condition has to persist before a short failsafe event will occur. This defaults to 1.5 seconds
     // @Units: seconds
     // @Range: 1 100
     // @Increment: 0.5
@@ -555,7 +555,7 @@ const AP_Param::Info Plane::var_info[] PROGMEM = {
 
     // @Param: FS_LONG_TIMEOUT
     // @DisplayName: Long failsafe timeout
-    // @Description: The time in seconds that a failsafe condition has to persist before a long failsafe event will occor. This defaults to 5 seconds
+    // @Description: The time in seconds that a failsafe condition has to persist before a long failsafe event will occur. This defaults to 5 seconds.
     // @Units: seconds
     // @Range: 1 300
     // @Increment: 0.5
@@ -564,7 +564,7 @@ const AP_Param::Info Plane::var_info[] PROGMEM = {
 
     // @Param: FS_BATT_VOLTAGE
     // @DisplayName: Failsafe battery voltage
-    // @Description: Battery voltage to trigger failsafe. Set to 0 to disable battery voltage failsafe. If the battery voltage drops below this voltage continuously for 10 seconds then the plane will switch to RTL mode
+    // @Description: Battery voltage to trigger failsafe. Set to 0 to disable battery voltage failsafe. If the battery voltage drops below this voltage continuously for 10 seconds then the plane will switch to RTL mode.
     // @Units: Volts
     // @Increment: 0.1
     // @User: Standard
@@ -572,7 +572,7 @@ const AP_Param::Info Plane::var_info[] PROGMEM = {
 
     // @Param: FS_BATT_MAH
     // @DisplayName: Failsafe battery milliAmpHours
-    // @Description: Battery capacity remaining to trigger failsafe. Set to 0 to disable battery remaining failsafe. If the battery remaining drops below this level then the plane will switch to RTL mode immediately
+    // @Description: Battery capacity remaining to trigger failsafe. Set to 0 to disable battery remaining failsafe. If the battery remaining drops below this level then the plane will switch to RTL mode immediately.
     // @Units: mAh
     // @Increment: 50
     // @User: Standard
@@ -580,7 +580,7 @@ const AP_Param::Info Plane::var_info[] PROGMEM = {
 
     // @Param: FS_GCS_ENABL
     // @DisplayName: GCS failsafe enable
-    // @Description: Enable ground control station telemetry failsafe. Failsafe will trigger after FS_LONG_TIMEOUT seconds of no MAVLink heartbeat messages. There are two possible enabled settings. Seeing FS_GCS_ENABL to 1 means that GCS failsafe will be triggered when the aircraft has not received a MAVLink HEARTBEAT message. Setting FS_GCS_ENABL to 2 means that GCS failsafe will be triggerded on either a loss of HEARTBEAT messages, or a RADIO_STATUS message from a MAVLink enabled 3DR radio indicating that the ground station is not receiving status updates from the aircraft, which is indicated by the RADIO_STATUS.remrssi field being zero (this may happen if you have a one way link due to asymmetric noise on the ground station and aircraft radios).Setting FS_GCS_ENABL to 3 means that GCS failsafe will be triggerded by Hearbeat(like option one), but only in AUTO mode. WARNING: Enabling this option opens up the possibility of your plane going into failsafe mode and running the motor on the ground it it loses contact with your ground station. If this option is enabled on an electric plane then you should enable ARMING_REQUIRED.
+    // @Description: Enable ground control station telemetry failsafe. Failsafe will trigger after FS_LONG_TIMEOUT seconds of no MAVLink heartbeat messages. There are two possible enabled settings. Seeing FS_GCS_ENABL to 1 means that GCS failsafe will be triggered when the aircraft has not received a MAVLink HEARTBEAT message. Setting FS_GCS_ENABL to 2 means that GCS failsafe will be triggered on either a loss of HEARTBEAT messages, or a RADIO_STATUS message from a MAVLink enabled 3DR radio indicating that the ground station is not receiving status updates from the aircraft, which is indicated by the RADIO_STATUS.remrssi field being zero (this may happen if you have a one way link due to asymmetric noise on the ground station and aircraft radios).Setting FS_GCS_ENABL to 3 means that GCS failsafe will be triggered by Heartbeat(like option one), but only in AUTO mode. WARNING: Enabling this option opens up the possibility of your plane going into failsafe mode and running the motor on the ground it it loses contact with your ground station. If this option is enabled on an electric plane then you should enable ARMING_REQUIRED.
     // @Values: 0:Disabled,1:Heartbeat,2:HeartbeatAndREMRSSI,3:HeartbeatAndAUTO
     // @User: Standard
     GSCALAR(gcs_heartbeat_fs_enabled, "FS_GCS_ENABL", GCS_FAILSAFE_OFF),
@@ -719,7 +719,7 @@ const AP_Param::Info Plane::var_info[] PROGMEM = {
 
     // @Param: ELEVON_MIXING
     // @DisplayName: Elevon mixing
-    // @Description: This enables an older form of elevon mixing which is now deprecated. Please see the ELEVON_OUTPUT option for setting up elevons. The ELEVON_MIXING option should be set to 0 for elevon planes except for backwards compatibilty with older setups.
+    // @Description: This enables an older form of elevon mixing which is now deprecated. Please see the ELEVON_OUTPUT option for setting up elevons. The ELEVON_MIXING option should be set to 0 for elevon planes except for backwards compatibility with older setups.
     // @Values: 0:Disabled,1:Enabled
     // @User: User
     GSCALAR(mix_mode,               "ELEVON_MIXING",  ELEVON_MIXING),
@@ -830,7 +830,7 @@ const AP_Param::Info Plane::var_info[] PROGMEM = {
 
     // @Param: ALT_HOLD_RTL
     // @DisplayName: RTL altitude
-    // @Description: Return to launch target altitude. This is the altitude the plane will aim for and loiter at when returning home. If this is negative (usually -1) then the plane will use the current altitude at the time of entering RTL. Note that when transiting to a Rally Point the alitude of the Rally Point is used instead of ALT_HOLD_RTL.
+    // @Description: Return to launch target altitude. This is the altitude the plane will aim for and loiter at when returning home. If this is negative (usually -1) then the plane will use the current altitude at the time of entering RTL. Note that when transiting to a Rally Point the altitude of the Rally Point is used instead of ALT_HOLD_RTL.
     // @Units: centimeters
     // @User: User
     GSCALAR(RTL_altitude_cm,        "ALT_HOLD_RTL",   ALT_HOLD_HOME_CM),
@@ -857,7 +857,7 @@ const AP_Param::Info Plane::var_info[] PROGMEM = {
 
     // @Param: FLAPERON_OUTPUT
     // @DisplayName: Flaperon output
-    // @Description: Enable flaperon output in software. If enabled then the APM will provide software flaperon mixing on the FLAPERON1 and FLAPERON2 output channels specified using the FUNCTION on two auxillary channels. There are 4 different mixing modes available, which refer to the 4 ways the flap and aileron outputs can be mapped to the two flaperon servos. Note that you must not use flaperon output mixing with hardware pass-through of RC values, such as with channel 8 manual control on an APM1. So if you use an APM1 then set FLTMODE_CH to something other than 8 before you enable FLAPERON_OUTPUT. Please also see the MIXING_GAIN parameter for the output gain of the mixer. FLAPERON_OUTPUT cannot be combined with ELEVON_OUTPUT or ELEVON_MIXING.
+    // @Description: Enable flaperon output in software. If enabled then the APM will provide software flaperon mixing on the FLAPERON1 and FLAPERON2 output channels specified using the FUNCTION on two auxiliary channels. There are 4 different mixing modes available, which refer to the 4 ways the flap and aileron outputs can be mapped to the two flaperon servos. Note that you must not use flaperon output mixing with hardware pass-through of RC values, such as with channel 8 manual control on an APM1. So if you use an APM1 then set FLTMODE_CH to something other than 8 before you enable FLAPERON_OUTPUT. Please also see the MIXING_GAIN parameter for the output gain of the mixer. FLAPERON_OUTPUT cannot be combined with ELEVON_OUTPUT or ELEVON_MIXING.
     // @Values: 0:Disabled,1:UpUp,2:UpDown,3:DownUp,4:DownDown
     // @User: User
     GSCALAR(flaperon_output,        "FLAPERON_OUTPUT",  0),
@@ -907,7 +907,7 @@ const AP_Param::Info Plane::var_info[] PROGMEM = {
 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4
     // @Param: OVERRIDE_CHAN
     // @DisplayName: PX4IO override channel
-    // @Description: If set to a non-zero value then this is an RC input channel number to use for testing manual control in case the main FMU microcontroller on a PX4 or Pixhawk fails. When this RC input channel goes above 1750 the FMU will stop sending servo controls to the PX4IO board, which will trigger the PX4IO board to start using its failsafe override behaviour, which should give you manual control of the aircraft. That allows you to test for correct manual behaviour without actually crashing the FMU. This parameter is normally only set to a non-zero value for ground testing purposes. When the override channel is used it also forces the PX4 safety switch into an armed state. This allows it to be used as a way to re-arm a plane after an in-flight reboot. Use in that way is considered a developer option, for people testing unstable developer code. Note that you may set OVERRIDE_CHAN to the same channel as FLTMODE_CH to get PX4IO based override when in flight mode 6. Note that when override is triggered the 6 auxillary output channels on Pixhawk will no longer be updated, so all the flight controls you need must be assigned to the first 8 channels.
+    // @Description: If set to a non-zero value then this is an RC input channel number to use for testing manual control in case the main FMU microcontroller on a PX4 or Pixhawk fails. When this RC input channel goes above 1750 the FMU will stop sending servo controls to the PX4IO board, which will trigger the PX4IO board to start using its failsafe override behaviour, which should give you manual control of the aircraft. That allows you to test for correct manual behaviour without actually crashing the FMU. This parameter is normally only set to a non-zero value for ground testing purposes. When the override channel is used it also forces the PX4 safety switch into an armed state. This allows it to be used as a way to re-arm a plane after an in-flight reboot. Use in that way is considered a developer option, for people testing unstable developer code. Note that you may set OVERRIDE_CHAN to the same channel as FLTMODE_CH to get PX4IO based override when in flight mode 6. Note that when override is triggered the 6 auxiliary output channels on Pixhawk will no longer be updated, so all the flight controls you need must be assigned to the first 8 channels.
     // @User: Advanced
     GSCALAR(override_channel,      "OVERRIDE_CHAN",  0),
 #endif
@@ -929,17 +929,19 @@ const AP_Param::Info Plane::var_info[] PROGMEM = {
 
     // @Param: INVERTEDFLT_CH
     // @DisplayName: Inverted flight channel
-    // @Description: A RC input channel number to enable inverted flight. If this is non-zero then the APM will monitor the correcponding RC input channel and will enable inverted flight when the channel goes above 1750.
+    // @Description: A RC input channel number to enable inverted flight. If this is non-zero then the APM will monitor the corresponding RC input channel and will enable inverted flight when the channel goes above 1750.
     // @Values: 0:Disabled,1:Channel1,2:Channel2,3:Channel3,4:Channel4,5:Channel5,6:Channel6,7:Channel7,8:Channel8
     // @User: Standard
     GSCALAR(inverted_flight_ch,     "INVERTEDFLT_CH", 0),
 
+#if HIL_SUPPORT
     // @Param: HIL_MODE
     // @DisplayName: HIL mode enable
     // @Description: This enables and disables hardware in the loop mode. If HIL_MODE is 1 then on the next reboot all sensors are replaced with HIL sensors which come from the GCS.
     // @Values: 0:Disabled,1:Enabled
     // @User: Advanced
     GSCALAR(hil_mode,               "HIL_MODE",      0),
+#endif
 
     // @Param: HIL_SERVOS
     // @DisplayName: HIL Servos enable
@@ -969,7 +971,7 @@ const AP_Param::Info Plane::var_info[] PROGMEM = {
     // @Description: Automatically set roll/pitch trim from Tx at ground start. This makes the assumption that the RC transmitter has not been altered since trims were last captured.
     // @Values: 0:Disable,1:Enable
     // @User: Standard
-    GSCALAR(trim_rc_at_start,     "TRIM_RC_AT_START",    1), 
+    GSCALAR(trim_rc_at_start,     "TRIM_RC_AT_START",    0), 
 
     // barometer ground calibration. The GND_ prefix is chosen for
     // compatibility with previous releases of ArduPlane

ArduPlane/Parameters.h

@@ -3,7 +3,7 @@
 #ifndef PARAMETERS_H
 #define PARAMETERS_H
 
-#include <AP_Common.h>
+#include <AP_Common/AP_Common.h>
 
 // Global parameter class.
 //
@@ -448,7 +448,9 @@ public:
     AP_Int16 pitch_trim_cd;
     AP_Int16 FBWB_min_altitude_cm;
     AP_Int8  hil_servos;
+#if HIL_SUPPORT
     AP_Int8  hil_mode;
+#endif
 
     AP_Int8 compass_enabled;
     AP_Int8 flap_1_percent;

ArduPlane/Plane.h

@@ -1,6 +1,8 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
-#define THISFIRMWARE "ArduPlane V3.3.1beta1"
+#define THISFIRMWARE "ArduPlane V3.4.0beta1"
+#define FIRMWARE_VERSION 3,4,0,FIRMWARE_VERSION_TYPE_BETA
+
 /*
    Lead developer: Andrew Tridgell
  
@@ -31,62 +33,62 @@
 #include <stdarg.h>
 #include <stdio.h>
 
-#include <AP_HAL.h>
-#include <AP_Common.h>
-#include <AP_Progmem.h>
-#include <AP_Menu.h>
-#include <AP_Param.h>
-#include <StorageManager.h>
-#include <AP_GPS.h>         // ArduPilot GPS library
-#include <AP_Baro.h>        // ArduPilot barometer library
-#include <AP_Compass.h>     // ArduPilot Mega Magnetometer Library
-#include <AP_Math.h>        // ArduPilot Mega Vector/Matrix math Library
-#include <AP_ADC.h>         // ArduPilot Mega Analog to Digital Converter Library
-#include <AP_ADC_AnalogSource.h>
-#include <AP_InertialSensor.h> // Inertial Sensor Library
-#include <AP_AHRS.h>         // ArduPilot Mega DCM Library
-#include <RC_Channel.h>     // RC Channel Library
-#include <AP_RangeFinder.h>     // Range finder library
-#include <Filter.h>                     // Filter library
-#include <AP_Buffer.h>      // APM FIFO Buffer
-#include <AP_Relay.h>       // APM relay
-#include <AP_Camera.h>          // Photo or video camera
-#include <AP_Airspeed.h>
-#include <AP_Terrain.h>
+#include <AP_HAL/AP_HAL.h>
+#include <AP_Common/AP_Common.h>
+#include <AP_Progmem/AP_Progmem.h>
+#include <AP_Menu/AP_Menu.h>
+#include <AP_Param/AP_Param.h>
+#include <StorageManager/StorageManager.h>
+#include <AP_GPS/AP_GPS.h>         // ArduPilot GPS library
+#include <AP_Baro/AP_Baro.h>        // ArduPilot barometer library
+#include <AP_Compass/AP_Compass.h>     // ArduPilot Mega Magnetometer Library
+#include <AP_Math/AP_Math.h>        // ArduPilot Mega Vector/Matrix math Library
+#include <AP_ADC/AP_ADC.h>         // ArduPilot Mega Analog to Digital Converter Library
+#include <AP_ADC_AnalogSource/AP_ADC_AnalogSource.h>
+#include <AP_InertialSensor/AP_InertialSensor.h> // Inertial Sensor Library
+#include <AP_AHRS/AP_AHRS.h>         // ArduPilot Mega DCM Library
+#include <RC_Channel/RC_Channel.h>     // RC Channel Library
+#include <AP_RangeFinder/AP_RangeFinder.h>     // Range finder library
+#include <Filter/Filter.h>                     // Filter library
+#include <AP_Buffer/AP_Buffer.h>      // APM FIFO Buffer
+#include <AP_Relay/AP_Relay.h>       // APM relay
+#include <AP_Camera/AP_Camera.h>          // Photo or video camera
+#include <AP_Airspeed/AP_Airspeed.h>
+#include <AP_Terrain/AP_Terrain.h>
 
-#include <APM_OBC.h>
-#include <APM_Control.h>
-#include <AP_AutoTune.h>
-#include <GCS.h>
-#include <GCS_MAVLink.h>    // MAVLink GCS definitions
-#include <AP_SerialManager.h>   // Serial manager library
-#include <AP_Mount.h>           // Camera/Antenna mount
-#include <AP_Declination.h> // ArduPilot Mega Declination Helper Library
-#include <DataFlash.h>
-#include <SITL.h>
-#include <AP_Scheduler.h>       // main loop scheduler
+#include <APM_OBC/APM_OBC.h>
+#include <APM_Control/APM_Control.h>
+#include <APM_Control/AP_AutoTune.h>
+#include <GCS_MAVLink/GCS.h>
+#include <GCS_MAVLink/GCS_MAVLink.h>    // MAVLink GCS definitions
+#include <AP_SerialManager/AP_SerialManager.h>   // Serial manager library
+#include <AP_Mount/AP_Mount.h>           // Camera/Antenna mount
+#include <AP_Declination/AP_Declination.h> // ArduPilot Mega Declination Helper Library
+#include <DataFlash/DataFlash.h>
+#include <SITL/SITL.h>
+#include <AP_Scheduler/AP_Scheduler.h>       // main loop scheduler
 
-#include <AP_Navigation.h>
-#include <AP_L1_Control.h>
-#include <AP_RCMapper.h>        // RC input mapping library
+#include <AP_Navigation/AP_Navigation.h>
+#include <AP_L1_Control/AP_L1_Control.h>
+#include <AP_RCMapper/AP_RCMapper.h>        // RC input mapping library
 
-#include <AP_Vehicle.h>
-#include <AP_SpdHgtControl.h>
-#include <AP_TECS.h>
-#include <AP_NavEKF.h>
-#include <AP_Mission.h>     // Mission command library
+#include <AP_Vehicle/AP_Vehicle.h>
+#include <AP_SpdHgtControl/AP_SpdHgtControl.h>
+#include <AP_TECS/AP_TECS.h>
+#include <AP_NavEKF/AP_NavEKF.h>
+#include <AP_Mission/AP_Mission.h>     // Mission command library
 
-#include <AP_Notify.h>      // Notify library
-#include <AP_BattMonitor.h> // Battery monitor library
+#include <AP_Notify/AP_Notify.h>      // Notify library
+#include <AP_BattMonitor/AP_BattMonitor.h> // Battery monitor library
 
-#include <AP_Arming.h>
-#include <AP_BoardConfig.h>
-#include <AP_Frsky_Telem.h>
-#include <AP_ServoRelayEvents.h>
+#include <AP_Arming/AP_Arming.h>
+#include <AP_BoardConfig/AP_BoardConfig.h>
+#include <AP_Frsky_Telem/AP_Frsky_Telem.h>
+#include <AP_ServoRelayEvents/AP_ServoRelayEvents.h>
 
-#include <AP_Rally.h>
+#include <AP_Rally/AP_Rally.h>
 
-#include <AP_OpticalFlow.h>     // Optical Flow library
+#include <AP_OpticalFlow/AP_OpticalFlow.h>     // Optical Flow library
 
 // Configuration
 #include "config.h"
@@ -96,13 +98,13 @@
 
 #include "Parameters.h"
 
-#include <AP_HAL_AVR.h>
-#include <AP_HAL_SITL.h>
-#include <AP_HAL_PX4.h>
-#include <AP_HAL_FLYMAPLE.h>
-#include <AP_HAL_Linux.h>
-#include <AP_HAL_Empty.h>
-#include <AP_HAL_VRBRAIN.h>
+#include <AP_HAL_AVR/AP_HAL_AVR.h>
+#include <AP_HAL_SITL/AP_HAL_SITL.h>
+#include <AP_HAL_PX4/AP_HAL_PX4.h>
+#include <AP_HAL_FLYMAPLE/AP_HAL_FLYMAPLE.h>
+#include <AP_HAL_Linux/AP_HAL_Linux.h>
+#include <AP_HAL_Empty/AP_HAL_Empty.h>
+#include <AP_HAL_VRBRAIN/AP_HAL_VRBRAIN.h>
 
 /*
   a plane specific arming class
@@ -466,6 +468,9 @@ private:
         
         // last time is_flying() returned true in milliseconds
         uint32_t last_flying_ms;
+
+        // once landed, post some landing statistics to the GCS
+        bool post_landing_stats;
     } auto_state;
 
     // true if we are in an auto-throttle mode, which means
@@ -667,6 +672,7 @@ private:
     void send_statustext(mavlink_channel_t chan);
     bool telemetry_delayed(mavlink_channel_t chan);
     void gcs_send_message(enum ap_message id);
+    void gcs_send_mission_item_reached_message(uint16_t mission_index);
     void gcs_data_stream_send(void);
     void gcs_update(void);
     void gcs_send_text_P(gcs_severity severity, const prog_char_t *str);
@@ -717,7 +723,6 @@ private:
     float lookahead_adjustment(void);
     float rangefinder_correction(void);
     void rangefinder_height_update(void);
-    void add_altitude_data(unsigned long xl, long y);
     void set_next_WP(const struct Location &loc);
     void set_guided_WP(void);
     void init_home();
@@ -915,6 +920,7 @@ private:
     void log_init();
     uint32_t millis() const;
     uint32_t micros() const;
+    void init_capabilities(void);
 
 public:
     void mavlink_delay_cb();

ArduPlane/altitude.cpp

@@ -398,8 +398,8 @@ bool Plane::above_location_current(const Location &loc)
 #endif
 
     float loc_alt_cm = loc.alt;
-    if (!loc.flags.relative_alt) {
-        loc_alt_cm -= home.alt;
+    if (loc.flags.relative_alt) {
+        loc_alt_cm += home.alt;
     }
     return current_loc.alt > loc_alt_cm;
 }

ArduPlane/capabilities.cpp

@@ -0,0 +1,9 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#include "Plane.h"
+
+void Plane::init_capabilities(void)
+{
+    hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT);
+    hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT);
+}

ArduPlane/climb_rate.cpp

@@ -1,62 +0,0 @@
-// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
-
-#include "Plane.h"
-
-#if 0 // currently unused
-
-struct DataPoint {
-    unsigned long x;
-    long y;
-};
-
-DataPoint history[ALTITUDE_HISTORY_LENGTH];               // Collection of (x,y) points to regress a rate of change from
-unsigned char hindex;   // Index in history for the current data point
-
-unsigned long xoffset;
-unsigned char n;
-
-// Intermediate variables for regression calculation
-long xi;
-long yi;
-long xiyi;
-unsigned long xi2;
-
-void add_altitude_data(unsigned long xl, long y)
-{
-    //Reset the regression if our X variable overflowed
-    if (xl < xoffset)
-        n = 0;
-
-    //To allow calculation of sum(xi*yi), make sure X hasn't exceeded 2^32/2^15/length
-    if (xl - xoffset > 131072/ALTITUDE_HISTORY_LENGTH)
-        n = 0;
-
-    if (n == ALTITUDE_HISTORY_LENGTH) {
-        xi -= history[hindex].x;
-        yi -= history[hindex].y;
-        xiyi -= (long)history[hindex].x * history[hindex].y;
-        xi2 -= history[hindex].x * history[hindex].x;
-    } else {
-        if (n == 0) {
-            xoffset = xl;
-            xi = 0;
-            yi = 0;
-            xiyi = 0;
-            xi2 = 0;
-        }
-        n++;
-    }
-
-    history[hindex].x = xl - xoffset;
-    history[hindex].y = y;
-
-    xi += history[hindex].x;
-    yi += history[hindex].y;
-    xiyi += (long)history[hindex].x * history[hindex].y;
-    xi2 += history[hindex].x * history[hindex].x;
-
-    if (++hindex >= ALTITUDE_HISTORY_LENGTH)
-        hindex = 0;
-}
-#endif
-

ArduPlane/commands_logic.cpp

@@ -28,6 +28,9 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
         // start non-idle
         auto_state.idle_mode = false;
         
+        // once landed, post some landing statistics to the GCS
+        auto_state.post_landing_stats = false;
+
         gcs_send_text_fmt(PSTR("Executing nav command ID #%i"),cmd.id);
     } else {
         gcs_send_text_fmt(PSTR("Executing command ID #%i"),cmd.id);
@@ -245,15 +248,12 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd)        // Ret
 
     case MAV_CMD_CONDITION_DELAY:
         return verify_wait_delay();
-        break;
 
     case MAV_CMD_CONDITION_DISTANCE:
         return verify_within_distance();
-        break;
 
     case MAV_CMD_CONDITION_CHANGE_ALT:
         return verify_change_alt();
-        break;
 
     // do commands (always return true)
     case MAV_CMD_DO_CHANGE_SPEED:
@@ -831,10 +831,12 @@ void Plane::do_take_picture()
 // log_picture - log picture taken and send feedback to GCS
 void Plane::log_picture()
 {
+#if CAMERA == ENABLED
     gcs_send_message(MSG_CAMERA_FEEDBACK);
     if (should_log(MASK_LOG_CAMERA)) {
         DataFlash.Log_Write_Camera(ahrs, gps, current_loc);
     }
+#endif
 }
 
 // start_command_callback - callback function called from ap-mission when it begins a new mission command
@@ -852,7 +854,14 @@ bool Plane::start_command_callback(const AP_Mission::Mission_Command &cmd)
 bool Plane::verify_command_callback(const AP_Mission::Mission_Command& cmd)
 {
     if (control_mode == AUTO) {
-        return verify_command(cmd);
+        bool cmd_complete = verify_command(cmd);
+
+        // send message to GCS
+        if (cmd_complete) {
+            gcs_send_mission_item_reached_message(cmd.index);
+        }
+
+        return cmd_complete;
     }
     return false;
 }

ArduPlane/config.h

@@ -420,11 +420,7 @@
 
 // use this to disable geo-fencing
 #ifndef GEOFENCE_ENABLED
-#if HAL_CPU_CLASS > HAL_CPU_CLASS_16
  # define GEOFENCE_ENABLED ENABLED
-#else
- # define GEOFENCE_ENABLED DISABLE
-#endif
 #endif
 
 // pwm value on FENCE_CHANNEL to use to enable fenced mode
@@ -459,6 +455,12 @@
 #define CLI_ENABLED DISABLED
 #endif
 
+#if HAL_CPU_CLASS < HAL_CPU_CLASS_75
+#define HIL_SUPPORT DISABLED
+#else
+#define HIL_SUPPORT ENABLED
+#endif
+
 /*
   build a firmware version string.
   GIT_VERSION comes from Makefile builds

ArduPlane/control_modes.cpp

@@ -44,7 +44,6 @@ void Plane::read_control_switch()
         set_mode((enum FlightMode)(flight_modes[switchPosition].get()));
 
         oldSwitchPosition = switchPosition;
-        prev_WP_loc = current_loc;
     }
 
     if (g.reset_mission_chan != 0 &&

ArduPlane/geofence.cpp

@@ -94,7 +94,8 @@ void Plane::geofence_load(void)
     uint8_t i;
 
     if (geofence_state == NULL) {
-        if (hal.util->available_memory() < 512 + sizeof(struct GeofenceState)) {
+        uint16_t boundary_size = sizeof(Vector2l) * max_fencepoints();
+        if (hal.util->available_memory() < 100 + boundary_size + sizeof(struct GeofenceState)) {
             // too risky to enable as we could run out of stack
             goto failed;
         }
@@ -104,7 +105,7 @@ void Plane::geofence_load(void)
             goto failed;
         }
 
-        geofence_state->boundary = (Vector2l *)calloc(sizeof(Vector2l), max_fencepoints());
+        geofence_state->boundary = (Vector2l *)calloc(1, boundary_size);
         if (geofence_state->boundary == NULL) {
             free(geofence_state);
             geofence_state = NULL;

ArduPlane/landing.cpp

@@ -49,6 +49,7 @@ bool Plane::verify_land()
         (fabsf(auto_state.sink_rate) < 0.2f && !is_flying())) {
 
         if (!auto_state.land_complete) {
+            auto_state.post_landing_stats = true;
             if (!is_flying() && (millis()-auto_state.last_flying_ms) > 3000) {
                 gcs_send_text_fmt(PSTR("Flare crash detected: speed=%.1f"), (double)gps.ground_speed());
             } else {
@@ -80,6 +81,13 @@ bool Plane::verify_land()
                     get_distance(prev_WP_loc, current_loc) + 200);
     nav_controller->update_waypoint(prev_WP_loc, land_WP_loc);
 
+    // once landed and stationary, post some statistics
+    // this is done before disarm_if_autoland_complete() so that it happens on the next loop after the disarm
+    if (auto_state.post_landing_stats && !arming.is_armed()) {
+        auto_state.post_landing_stats = false;
+        gcs_send_text_fmt(PSTR("Distance from LAND point=%.2fm"), get_distance(current_loc, next_WP_loc));
+    }
+
     // check if we should auto-disarm after a confirmed landing
     disarm_if_autoland_complete();
 
@@ -131,6 +139,12 @@ void Plane::setup_landing_glide_slope(void)
         int32_t land_bearing_cd = get_bearing_cd(prev_WP_loc, next_WP_loc);
         float total_distance = get_distance(prev_WP_loc, next_WP_loc);
 
+        // If someone mistakenly puts all 0's in their LAND command then total_distance
+        // will be calculated as 0 and cause a divide by 0 error below.  Lets avoid that.
+        if (total_distance < 1) {
+            total_distance = 1;
+        }
+
         // height we need to sink for this WP
         float sink_height = (prev_WP_loc.alt - next_WP_loc.alt)*0.01f;
 

ArduPlane/sensors.cpp

@@ -75,7 +75,9 @@ void Plane::read_battery(void)
     battery.read();
     compass.set_current(battery.current_amps());
 
-    if (!usb_connected && battery.exhausted(g.fs_batt_voltage, g.fs_batt_mah)) {
+    if (!usb_connected && 
+        hal.util->get_soft_armed() &&
+        battery.exhausted(g.fs_batt_voltage, g.fs_batt_mah)) {
         low_battery_event();
     }
 }

ArduPlane/system.cpp

@@ -95,12 +95,14 @@ void Plane::init_ardupilot()
     //
     load_parameters();
 
+#if HIL_SUPPORT
     if (g.hil_mode == 1) {
         // set sensors to HIL mode
         ins.set_hil_mode();
         compass.set_hil_mode();
         barometer.set_hil_mode();
     }
+#endif
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4
     // this must be before BoardConfig.init() so if
@@ -233,6 +235,8 @@ void Plane::init_ardupilot()
     }
 #endif // CLI_ENABLED
 
+    init_capabilities();
+
     startup_ground();
     Log_Write_Startup(TYPE_GROUNDSTART_MSG);
 
@@ -359,6 +363,9 @@ void Plane::set_mode(enum FlightMode mode)
     // disable taildrag takeoff on mode change
     auto_state.fbwa_tdrag_takeoff_mode = false;
 
+    // start with previous WP at current location
+    prev_WP_loc = current_loc;
+
     switch(control_mode)
     {
     case INITIALISING:
@@ -536,6 +543,7 @@ void Plane::check_short_failsafe()
 
 void Plane::startup_INS_ground(void)
 {
+#if HIL_SUPPORT
     if (g.hil_mode == 1) {
         while (barometer.get_last_update() == 0) {
             // the barometer begins updating when we get the first
@@ -544,6 +552,7 @@ void Plane::startup_INS_ground(void)
             hal.scheduler->delay(1000);
         }
     }
+#endif
 
     AP_InertialSensor::Start_style style;
     if (g.skip_gyro_cal) {
@@ -679,12 +688,14 @@ void Plane::print_comma(void)
  */
 void Plane::servo_write(uint8_t ch, uint16_t pwm)
 {
+#if HIL_SUPPORT
     if (g.hil_mode==1 && !g.hil_servos) {
         if (ch < 8) {
             RC_Channel::rc_channel(ch)->radio_out = pwm;
         }
         return;
     }
+#endif
     hal.rcout->enable_ch(ch);
     hal.rcout->write(ch, pwm);
 }

README.md

@@ -77,6 +77,8 @@
 >>   - ***Subsystem***: Linux
 >> - [Peter Barker](https://github.com/peterbarker)
 >>   - ***Subsystem***: DataFlash
+>> - [Michael du Breuil](https://github.com/WickedShell)
+>>   - ***Subsystem***: uBlox GPS
 >> - [Víctor Mayoral Vilches](https://github.com/vmayoral)
 >>   - ***Board***: PXF
 >> - [Mirko Denecke](https://github.com/mirkix)

Tools/ArduPPM/WorkBasket/Jeti_Duplex/Jetibox/JetiBox.h

@@ -24,7 +24,7 @@
 #define JetiBox_h
 
 #include <inttypes.h>
-#include "Print.h"
+#include <AP_HAL/utility/Print.h>
 
 #define jbox_key_up 0b0010
 #define jbox_key_right 0b0001

Tools/CPUInfo/CPUInfo.cpp

@@ -5,39 +5,39 @@
   Andrew Tridgell September 2011
 */
 
-#include <AP_HAL.h>
-#include <AP_HAL_AVR.h>
-#include <AP_HAL_SITL.h>
-#include <AP_HAL_PX4.h>
-#include <AP_HAL_Empty.h>
-#include <AP_Common.h>
-#include <AP_Scheduler.h>
-#include <AP_Baro.h>
-#include <AP_ADC.h>
-#include <AP_GPS.h>
-#include <AP_InertialSensor.h>
-#include <AP_Notify.h>
-#include <DataFlash.h>
-#include <GCS_MAVLink.h>
-#include <AP_Compass.h>
-#include <AP_Declination.h>
-#include <SITL.h>
-#include <Filter.h>
-#include <AP_Param.h>
-#include <StorageManager.h>
-#include <AP_Progmem.h>
-#include <AP_Math.h>
-#include <AP_AHRS.h>
-#include <AP_NavEKF.h>
-#include <AP_Airspeed.h>
-#include <AP_Vehicle.h>
-#include <AP_Mission.h>
-#include <AP_Terrain.h>
-#include <AP_BattMonitor.h>
-#include <AP_Rally.h>
-#include <AP_ADC_AnalogSource.h>
-#include <AP_OpticalFlow.h>
-#include <AP_RangeFinder.h>
+#include <AP_HAL/AP_HAL.h>
+#include <AP_HAL_AVR/AP_HAL_AVR.h>
+#include <AP_HAL_SITL/AP_HAL_SITL.h>
+#include <AP_HAL_PX4/AP_HAL_PX4.h>
+#include <AP_HAL_Empty/AP_HAL_Empty.h>
+#include <AP_Common/AP_Common.h>
+#include <AP_Scheduler/AP_Scheduler.h>
+#include <AP_Baro/AP_Baro.h>
+#include <AP_ADC/AP_ADC.h>
+#include <AP_GPS/AP_GPS.h>
+#include <AP_InertialSensor/AP_InertialSensor.h>
+#include <AP_Notify/AP_Notify.h>
+#include <DataFlash/DataFlash.h>
+#include <GCS_MAVLink/GCS_MAVLink.h>
+#include <AP_Compass/AP_Compass.h>
+#include <AP_Declination/AP_Declination.h>
+#include <SITL/SITL.h>
+#include <Filter/Filter.h>
+#include <AP_Param/AP_Param.h>
+#include <StorageManager/StorageManager.h>
+#include <AP_Progmem/AP_Progmem.h>
+#include <AP_Math/AP_Math.h>
+#include <AP_AHRS/AP_AHRS.h>
+#include <AP_NavEKF/AP_NavEKF.h>
+#include <AP_Airspeed/AP_Airspeed.h>
+#include <AP_Vehicle/AP_Vehicle.h>
+#include <AP_Mission/AP_Mission.h>
+#include <AP_Terrain/AP_Terrain.h>
+#include <AP_BattMonitor/AP_BattMonitor.h>
+#include <AP_Rally/AP_Rally.h>
+#include <AP_ADC_AnalogSource/AP_ADC_AnalogSource.h>
+#include <AP_OpticalFlow/AP_OpticalFlow.h>
+#include <AP_RangeFinder/AP_RangeFinder.h>
 
 const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
 

Tools/Failsafe/Failsafe.pde

@@ -9,8 +9,8 @@ version 2.1 of the License, or (at your option) any later version.
 
 // Libraries
 #include <FastSerial.h>
-#include <AP_Common.h>
-#include <AP_Math.h>
+#include <AP_Common/AP_Common.h>
+#include <AP_Math/AP_Math.h>
 #include <Arduino_Mega_ISR_Registry.h>
 #include <APM_RC.h>         // ArduPilot Mega RC Library
 

Tools/FlightGear/FGShim.c

@@ -59,12 +59,12 @@
 #include <unistd.h>
 
 #pragma pack(1)
-#include "mavlink_types.h"
+#include <GCS_MAVLink/include/mavlink/v1.0/mavlink_types.h>
 
 mavlink_system_t         mavlink_system;
 void                    comm_send_ch(mavlink_channel_t chan, uint8_t ch);
 #define MAVLINK_USE_CONVENIENCE_FUNCTIONS
-#include "ardupilotmega/mavlink.h"
+#include <GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/mavlink.h>
 
 #define TARGET_SYSTEM           7       /* XXX what should these really be? */
 #define TARGET_COMPONENT        1

Tools/Hello/Hello.cpp

@@ -5,30 +5,30 @@
   Andrew Tridgell September 2011
 */
 
-#include <AP_Common.h>
-#include <AP_Progmem.h>
-#include <AP_HAL.h>
-#include <AP_HAL_AVR.h>
-#include <AP_HAL_SITL.h>
-#include <AP_HAL_PX4.h>
-#include <AP_HAL_Empty.h>
-#include <AP_Math.h>
-#include <Filter.h>
-#include <AP_InertialSensor.h>
-#include <AP_ADC.h>
-#include <GCS_MAVLink.h>
-#include <AP_Param.h>
-#include <AP_Baro.h>
-#include <AP_Compass.h>
-#include <AP_Declination.h>
-#include <SITL.h>
-#include <DataFlash.h>
-#include <AP_GPS.h>
-#include <AP_AHRS.h>
-#include <AP_Airspeed.h>
-#include <AP_Vehicle.h>
-#include <AP_ADC_AnalogSource.h>
-#include <AP_Notify.h>
+#include <AP_Common/AP_Common.h>
+#include <AP_Progmem/AP_Progmem.h>
+#include <AP_HAL/AP_HAL.h>
+#include <AP_HAL_AVR/AP_HAL_AVR.h>
+#include <AP_HAL_SITL/AP_HAL_SITL.h>
+#include <AP_HAL_PX4/AP_HAL_PX4.h>
+#include <AP_HAL_Empty/AP_HAL_Empty.h>
+#include <AP_Math/AP_Math.h>
+#include <Filter/Filter.h>
+#include <AP_InertialSensor/AP_InertialSensor.h>
+#include <AP_ADC/AP_ADC.h>
+#include <GCS_MAVLink/GCS_MAVLink.h>
+#include <AP_Param/AP_Param.h>
+#include <AP_Baro/AP_Baro.h>
+#include <AP_Compass/AP_Compass.h>
+#include <AP_Declination/AP_Declination.h>
+#include <SITL/SITL.h>
+#include <DataFlash/DataFlash.h>
+#include <AP_GPS/AP_GPS.h>
+#include <AP_AHRS/AP_AHRS.h>
+#include <AP_Airspeed/AP_Airspeed.h>
+#include <AP_Vehicle/AP_Vehicle.h>
+#include <AP_ADC_AnalogSource/AP_ADC_AnalogSource.h>
+#include <AP_Notify/AP_Notify.h>
 
 const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
 

Tools/Replay/DataFlashFileReader.h

@@ -1,7 +1,7 @@
 #ifndef REPLAY_DATAFLASHREADER_H
 #define REPLAY_DATAFLASHREADER_H
 
-#include <DataFlash.h>
+#include <DataFlash/DataFlash.h>
 
 class DataFlashFileReader
 {

Tools/Replay/LR_MsgHandler.cpp

@@ -296,7 +296,7 @@ void LR_MsgHandler_MAG::process_message(uint8_t *msg)
     update_from_msg_compass(0, msg);
 }
 
-#include <AP_AHRS.h>
+#include <AP_AHRS/AP_AHRS.h>
 #include "VehicleType.h"
 
 void LR_MsgHandler_MSG::process_message(uint8_t *msg)

Tools/Replay/LogReader.cpp

@@ -1,13 +1,13 @@
-#include <AP_HAL.h>
-#include <AP_Common.h>
-#include <AP_Math.h>
-#include <AP_Airspeed.h>
-#include <AP_Compass.h>
-#include <AP_GPS.h>
-#include <AP_Compass.h>
-#include <AP_Baro.h>
-#include <AP_InertialSensor.h>
-#include <DataFlash.h>
+#include <AP_HAL/AP_HAL.h>
+#include <AP_Common/AP_Common.h>
+#include <AP_Math/AP_Math.h>
+#include <AP_Airspeed/AP_Airspeed.h>
+#include <AP_Compass/AP_Compass.h>
+#include <AP_GPS/AP_GPS.h>
+#include <AP_Compass/AP_Compass.h>
+#include <AP_Baro/AP_Baro.h>
+#include <AP_InertialSensor/AP_InertialSensor.h>
+#include <DataFlash/DataFlash.h>
 
 #include "LogReader.h"
 #include <stdio.h>

Tools/Replay/MsgHandler.h

@@ -1,7 +1,7 @@
 #ifndef AP_MSGHANDLER_H
 #define AP_MSGHANDLER_H
 
-#include <DataFlash.h>
+#include <DataFlash/DataFlash.h>
 #include "VehicleType.h"
 
 #include <stdio.h>

Tools/Replay/Parameters.h

@@ -3,7 +3,7 @@
 #ifndef PARAMETERS_H
 #define PARAMETERS_H
 
-#include <AP_Common.h>
+#include <AP_Common/AP_Common.h>
 
 // Global parameter class.
 //

Tools/Replay/Replay.cpp

@@ -14,48 +14,48 @@
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include <AP_Common.h>
-#include <AP_Progmem.h>
-#include <AP_Param.h>
-#include <StorageManager.h>
+#include <AP_Common/AP_Common.h>
+#include <AP_Progmem/AP_Progmem.h>
+#include <AP_Param/AP_Param.h>
+#include <StorageManager/StorageManager.h>
 #include <fenv.h>
-#include <AP_Math.h>
-#include <AP_HAL.h>
-#include <AP_HAL_AVR.h>
-#include <AP_HAL_SITL.h>
-#include <AP_HAL_Linux.h>
-#include <AP_HAL_Empty.h>
-#include <AP_ADC.h>
-#include <AP_Declination.h>
-#include <AP_ADC_AnalogSource.h>
-#include <Filter.h>
-#include <AP_Buffer.h>
-#include <AP_Airspeed.h>
-#include <AP_Vehicle.h>
-#include <AP_Notify.h>
-#include <DataFlash.h>
-#include <GCS_MAVLink.h>
-#include <AP_GPS.h>
-#include <AP_AHRS.h>
-#include <SITL.h>
-#include <AP_Compass.h>
-#include <AP_Baro.h>
-#include <AP_InertialSensor.h>
-#include <AP_InertialNav.h>
-#include <AP_NavEKF.h>
-#include <AP_Mission.h>
-#include <AP_Rally.h>
-#include <AP_BattMonitor.h>
-#include <AP_Terrain.h>
-#include <AP_OpticalFlow.h>
-#include <AP_SerialManager.h>
-#include <RC_Channel.h>
-#include <AP_RangeFinder.h>
+#include <AP_Math/AP_Math.h>
+#include <AP_HAL/AP_HAL.h>
+#include <AP_HAL_AVR/AP_HAL_AVR.h>
+#include <AP_HAL_SITL/AP_HAL_SITL.h>
+#include <AP_HAL_Linux/AP_HAL_Linux.h>
+#include <AP_HAL_Empty/AP_HAL_Empty.h>
+#include <AP_ADC/AP_ADC.h>
+#include <AP_Declination/AP_Declination.h>
+#include <AP_ADC_AnalogSource/AP_ADC_AnalogSource.h>
+#include <Filter/Filter.h>
+#include <AP_Buffer/AP_Buffer.h>
+#include <AP_Airspeed/AP_Airspeed.h>
+#include <AP_Vehicle/AP_Vehicle.h>
+#include <AP_Notify/AP_Notify.h>
+#include <DataFlash/DataFlash.h>
+#include <GCS_MAVLink/GCS_MAVLink.h>
+#include <AP_GPS/AP_GPS.h>
+#include <AP_AHRS/AP_AHRS.h>
+#include <SITL/SITL.h>
+#include <AP_Compass/AP_Compass.h>
+#include <AP_Baro/AP_Baro.h>
+#include <AP_InertialSensor/AP_InertialSensor.h>
+#include <AP_InertialNav/AP_InertialNav.h>
+#include <AP_NavEKF/AP_NavEKF.h>
+#include <AP_Mission/AP_Mission.h>
+#include <AP_Rally/AP_Rally.h>
+#include <AP_BattMonitor/AP_BattMonitor.h>
+#include <AP_Terrain/AP_Terrain.h>
+#include <AP_OpticalFlow/AP_OpticalFlow.h>
+#include <AP_SerialManager/AP_SerialManager.h>
+#include <RC_Channel/RC_Channel.h>
+#include <AP_RangeFinder/AP_RangeFinder.h>
 #include <stdio.h>
 #include <errno.h>
 #include <signal.h>
 #include <unistd.h>
-#include <utility/getopt_cpp.h>
+#include <AP_HAL/utility/getopt_cpp.h>
 #include "Parameters.h"
 #include "VehicleType.h"
 #include "MsgHandler.h"

Tools/autotest/Helicopter.parm

@@ -0,0 +1,75 @@
+BATT_MONITOR 4.0000
+COMPASS_OFS2_X 5.0000
+COMPASS_OFS2_Y 13.0000
+COMPASS_OFS2_Z -18.0000
+COMPASS_OFS3_X 1.0000
+COMPASS_OFS3_Y 1.0000
+COMPASS_OFS3_Z 1.0000
+COMPASS_OFS_X 2.8749
+COMPASS_OFS_Y -21.8262
+COMPASS_OFS_Z -21.8422
+FLTMODE1 7.0000
+FLTMODE2 9.0000
+FLTMODE3 6.0000
+FLTMODE4 3.0000
+FLTMODE5 5.0000
+# we need small INS_ACC offsets so INS is recognised as being calibrated
+INS_ACCOFFS_X   0.001
+INS_ACCOFFS_Y   0.001
+INS_ACCOFFS_Z   0.001
+INS_ACCSCAL_X   1.001
+INS_ACCSCAL_Y   1.001
+INS_ACCSCAL_Z   1.001
+INS_ACC2OFFS_X   0.001
+INS_ACC2OFFS_Y   0.001
+INS_ACC2OFFS_Z   0.001
+INS_ACC2SCAL_X   1.001
+INS_ACC2SCAL_Y   1.001
+INS_ACC2SCAL_Z   1.001
+INS_ACC3OFFS_X   0.000
+INS_ACC3OFFS_Y   0.000
+INS_ACC3OFFS_Z   0.000
+INS_ACC3SCAL_X   1.000
+INS_ACC3SCAL_Y   1.000
+INS_ACC3SCAL_Z   1.000
+RATE_PIT_D 0.0500
+RATE_PIT_I 12.5000
+RATE_PIT_IMAX 4500.0000
+RATE_PIT_P 5.0000
+RATE_PIT_VFF 0.220000
+RATE_RLL_D 0.0500
+RATE_RLL_I 12.5000
+RATE_RLL_IMAX 4500.0000
+RATE_RLL_P 5.0000
+RATE_RLL_VFF 0.220000
+RATE_YAW_D 0.0200
+RATE_YAW_I 0.0975
+RATE_YAW_VFF 0.200000
+STB_PIT_P 12.5368
+STB_RLL_P 15.2386
+STB_YAW_P 7.6965
+RC1_MAX         2000.000000
+RC1_MIN         1000.000000
+RC1_TRIM        1500.000000
+RC2_MAX         2000.000000
+RC2_MIN         1000.000000
+RC2_TRIM        1500.000000
+RC3_MAX         2000.000000
+RC3_MIN         1000.000000
+RC3_TRIM        1500.000000
+RC4_MAX         2000.000000
+RC4_MIN         1000.000000
+RC4_TRIM        1500.000000
+RC5_MAX         2000.000000
+RC5_MIN         1000.000000
+RC5_TRIM        1500.000000
+RC6_MAX         2000.000000
+RC6_MIN         1000.000000
+RC6_TRIM        1500.000000
+RC7_MAX         2000.000000
+RC7_MIN         1000.000000
+RC7_TRIM        1500.000000
+RC8_MAX         2000.000000
+RC8_MIN         1000.000000
+RC8_TRIM        1500.000000
+WP_YAW_BEHAVIOR 2

Tools/autotest/run_cmd.sh

@@ -1,6 +0,0 @@
-#!/bin/bash
-
-# This is a helper script for run_in_terminal_window.sh.  I'm not sure why Ardupilot.elf exits if not run from inside of a script
-echo running $*
-( $* )
-echo cmd exited

Tools/autotest/run_in_terminal_window.sh

@@ -21,6 +21,6 @@ else
   echo "Window access not found, logging to $filename"
   cmd="$1"
   shift
-  ( $AUTOTEST/run_cmd.sh $cmd $* &>$filename < /dev/null ) &
+  ( $cmd $* &>$filename < /dev/null ) &
 fi
 exit 0

Tools/autotest/sim_vehicle.sh

@@ -188,7 +188,12 @@ SIMOUT_PORT="127.0.0.1:"$((5501+10*$INSTANCE))
 FG_PORT="127.0.0.1:"$((5503+10*$INSTANCE))
 
 [ -z "$VEHICLE" ] && {
-    VEHICLE=$(basename $PWD)
+    CDIR="$PWD"
+    rpath=$(which realpath)
+    [ -n "rpath" ] && {
+        CDIR=$(realpath $CDIR)
+    }
+    VEHICLE=$(basename $CDIR)
 }
 
 [ -z "$FRAME" -a "$VEHICLE" = "APMrover2" ] && {
@@ -247,6 +252,16 @@ case $FRAME in
 	BUILD_TARGET="sitl-heli"
         MODEL="heli"
 	;;
+    heli-dual)
+  BUILD_TARGET="sitl-heli-dual"
+        EXTRA_SIM="$EXTRA_SIM --frame=heli-dual"
+        MODEL="heli-dual"
+  ;;
+    heli-compound)
+  BUILD_TARGET="sitl-heli-compound"
+        EXTRA_SIM="$EXTRA_SIM --frame=heli-compound"
+        MODEL="heli-compound"
+  ;;
     IrisRos)
 	BUILD_TARGET="sitl"
 	;;
@@ -287,8 +302,12 @@ autotest="../Tools/autotest"
     # the location of the sim_vehicle.sh script to find the path
     autotest=$(dirname $(readlink -e $0))
 }
-pushd $autotest/../../$VEHICLE || {
-    echo "Failed to change to vehicle directory for $VEHICLE"
+VEHICLEDIR="$autotest/../../$VEHICLE"
+[ -d "$VEHICLEDIR" ] || {
+    VEHICLEDIR=$(dirname $(readlink -e $VEHICLEDIR))
+}
+pushd $VEHICLEDIR || {
+    echo "Failed to change to vehicle directory for $VEHICLEDIR"
     usage
     exit 1
 }

Tools/scripts/build_all.sh

@@ -8,9 +8,12 @@
 set -e
 set -x
 
+export BUILDROOT="/tmp/all.build"
+rm -rf $BUILDROOT
+
 echo "Testing ArduPlane build"
 pushd ArduPlane
-for b in all apm2 sitl linux; do
+for b in sitl linux; do
     pwd
     make clean
     make $b -j4
@@ -28,7 +31,7 @@ popd
 
 echo "Testing APMRover build"
 pushd APMrover2
-for b in all apm2 sitl linux; do
+for b in sitl linux; do
     pwd
     make clean
     make $b -j4
@@ -37,7 +40,7 @@ popd
 
 echo "Testing AntennaTracker build"
 pushd AntennaTracker
-for b in apm2 sitl; do
+for b in sitl; do
     pwd
     make clean
     make $b -j4
@@ -50,15 +53,13 @@ examples="Tools/CPUInfo"
 for d in $examples; do
     pushd $d
     make clean
-    make apm2 -j4
-    make clean
     make sitl -j4
     popd
 done
 
 test -d ../libmaple && {
 echo "Testing flymaple build"
-for d in ArduPlane APMrover2; do
+for d in APMrover2; do
     pushd $d
     make clean
     make flymaple -j4

Tools/scripts/build_all_fast.sh

@@ -0,0 +1,36 @@
+#!/bin/bash
+# useful script to test all the different build types that we support.
+# This helps when doing large merges
+# Andrew Tridgell, November 2011
+
+. config.mk
+
+set -e
+set -x
+
+echo "Testing ArduPlane build"
+pushd ArduPlane
+for b in all apm2 sitl linux; do
+    pwd
+    make clean
+    make $b -j4
+done
+popd
+
+for d in ArduCopter APMrover2 ArduPlane AntennaTracker; do
+    pushd $d
+    make clean
+    make sitl -j4
+    make clean
+    make linux -j4
+    make clean
+    make px4-v2
+    popd
+done
+
+pushd Tools/Replay
+make clean
+make linux -j4
+popd
+
+exit 0

Tools/scripts/build_autotest.sh

@@ -1,6 +1,6 @@
 #!/bin/bash
 
-export PATH=$HOME/.local/bin:/usr/local/bin:$HOME/prefix/bin:$HOME/APM/px4/gcc-arm-none-eabi-4_7-2014q2/bin:$PATH
+export PATH=$HOME/.local/bin:/usr/local/bin:$HOME/prefix/bin:$HOME/APM/px4/gcc-arm-none-eabi-4_9-2015q3/bin:$PATH
 export PYTHONUNBUFFERED=1
 export PYTHONPATH=$HOME/APM
 
@@ -40,7 +40,7 @@ lock_file() {
 
         if test -f "$lck" && kill -0 $pid 2> /dev/null; then
 	    LOCKAGE=$(($(date +%s) - $(stat -c '%Y' "build.lck")))
-	    test $LOCKAGE -gt 7200 && {
+	    test $LOCKAGE -gt 30000 && {
                 echo "old lock file $lck is valid for $pid with age $LOCKAGE seconds"
 	    }
             return 1
@@ -88,6 +88,9 @@ report_pull_failure() {
 oldhash=$(cd APM && git rev-parse HEAD)
 
 pushd APM
+git checkout -f master
+git fetch origin
+git reset --hard origin/master
 git pull || report_pull_failure
 git clean -f -f -x -d -d
 git tag autotest-$(date '+%Y-%m-%d-%H%M%S') -m "test tag `date`"
@@ -148,7 +151,7 @@ popd
 githash=$(cd APM && git rev-parse HEAD)
 hdate=$(date +"%Y-%m-%d-%H:%m")
 
-for d in ArduPlane ArduCopter APMrover2; do
+for d in ArduPlane ArduCopter APMrover2 AntennaTracker; do
     pushd APM/$d
     rm -rf ../../buildlogs/$d.build
     (date && TMPDIR=../../buildlogs make) > ../../buildlogs/$d.txt 2>&1
@@ -177,6 +180,6 @@ killall -9 JSBSim || /bin/true
 # raise core limit
 ulimit -c 10000000
 
-timelimit 12000 APM/Tools/autotest/autotest.py --timeout=11500 > buildlogs/autotest-output.txt 2>&1
+timelimit 22000 APM/Tools/autotest/autotest.py --timeout=20000 > buildlogs/autotest-output.txt 2>&1
 
 ) >> build.log 2>&1

Tools/scripts/build_binaries.sh

@@ -22,34 +22,63 @@ error_count=0
 
 . config.mk
 
+board_branch() {
+    board="$1"
+    case $board in
+        apm1|apm2)
+            echo "-AVR"
+            ;;
+        *)
+            echo ""
+            ;;
+    esac
+}
+
 # checkout the right version of the tree
 checkout() {
     vehicle="$1"
     tag="$2"
     board="$3"
+    frame="$4"
+    echo "Trying checkout $vehicle $tag $board $frame"
     git stash
     if [ "$tag" = "latest" ]; then
 	vtag="master"
-	vtag2="master"
     else
-	vtag="$vehicle-$tag-$board"
-	vtag2="$vehicle-$tag"
+	vtag="$vehicle-$tag"
     fi
 
-    echo "FORCING NON-BOARD SPECIFIC BUILD"
-    vtag=$vtag2
+    # try frame specific tag
+    if [ -n "$frame" ]; then
+        vtag2="$vtag-$frame"
 
-    echo "Checkout for $vehicle for $board with tag $tag"
+        git checkout -f "$vtag2" && {
+            echo "Using frame specific tag $vtag2"
+            [ -f $BASEDIR/.gitmodules ] && git submodule update
+            git log -1
+            return 0
+        }
+    fi
 
-    git checkout -f "$vtag" || git checkout -f "$vtag2" || {
-        return 1
+    # try board type specific branch extension
+    vtag2="$vtag"$(board_branch $board)
+
+    git checkout -f "$vtag2" && {
+        echo "Using board specific tag $vtag2"
         [ -f $BASEDIR/.gitmodules ] && git submodule update
+        git log -1
+        return 0
     }
 
-    [ -f $BASEDIR/.gitmodules ] && git submodule update
-    git log -1
+    git checkout -f "$vtag" && {
+        echo "Using generic tag $vtag"
+        [ -f $BASEDIR/.gitmodules ] && git submodule update
+        git log -1
+        return 0
+    }
 
-    return 0
+    echo "Failed to find tag for $vehicle $tag $board $frame"
+    return 1
 }
 
 # check if we should skip this build because we have already
@@ -126,7 +155,7 @@ build_arduplane() {
     echo "Building ArduPlane $tag binaries from $(pwd)"
     pushd ArduPlane
     for b in apm1 apm2 navio pxf; do
-        checkout ArduPlane $tag $b || {
+        checkout ArduPlane $tag $b "" || {
             echo "Failed checkout of ArduPlane $b $tag"
             error_count=$((error_count+1))
             continue
@@ -141,15 +170,15 @@ build_arduplane() {
             continue
         }
         extension=$(board_extension $b)
-	copyit $TMPDIR/ArduPlane.build/ArduPlane.$extension $ddir $tag
+	copyit $BUILDROOT/ArduPlane.$extension $ddir $tag
 	touch $binaries/Plane/$tag
     done
     echo "Building ArduPlane PX4 binaries"
     ddir=$binaries/Plane/$hdate/PX4
-    checkout ArduPlane $tag PX4 || {
+    checkout ArduPlane $tag PX4 "" || {
         echo "Failed checkout of ArduPlane PX4 $tag"
         error_count=$((error_count+1))
-        checkout ArduPlane "latest" ""
+        checkout ArduPlane "latest" "" ""
         popd
         return
     }
@@ -157,12 +186,13 @@ build_arduplane() {
 	make px4 || {
             echo "Failed build of ArduPlane PX4 $tag"
             error_count=$((error_count+1))
-            checkout ArduPlane "latest" ""
+            checkout ArduPlane "latest" "" ""
             popd
             return
         }
 	copyit ArduPlane-v1.px4 $ddir $tag &&
-	copyit ArduPlane-v2.px4 $ddir $tag
+	copyit ArduPlane-v2.px4 $ddir $tag &&
+	test ! -f ArduPlane-v4.px4 || copyit ArduPlane-v4.px4 $ddir $tag
         if [ "$tag" = "latest" ]; then
 	    copyit px4io-v1.bin $binaries/PX4IO/$hdate/PX4IO $tag
 	    copyit px4io-v1.elf $binaries/PX4IO/$hdate/PX4IO $tag
@@ -170,7 +200,7 @@ build_arduplane() {
 	    copyit px4io-v2.elf $binaries/PX4IO/$hdate/PX4IO $tag
         fi
     }
-    checkout ArduPlane "latest" ""
+    checkout ArduPlane "latest" "" ""
     popd
 }
 
@@ -182,12 +212,12 @@ build_arducopter() {
     frames="quad tri hexa y6 octa octa-quad heli"
     for b in navio pxf; do
         for f in $frames; do
-            checkout ArduCopter $tag $b || {
-                echo "Failed checkout of ArduCopter $b $tag"
+            checkout ArduCopter $tag $b $f || {
+                echo "Failed checkout of ArduCopter $b $tag $f"
                 error_count=$((error_count+1))
                 continue
             }
-	    echo "Building ArduCopter $b binaries"
+	    echo "Building ArduCopter $b binaries $f"
 	    ddir=$binaries/Copter/$hdate/$b-$f
 	    skip_build $tag $ddir && continue
 	    make clean || continue
@@ -197,19 +227,18 @@ build_arducopter() {
                 continue
             }
             extension=$(board_extension $b)
-	    copyit $TMPDIR/ArduCopter.build/ArduCopter.$extension $ddir $tag
+	    copyit $BUILDROOT/ArduCopter.$extension $ddir $tag
 	    touch $binaries/Copter/$tag
         done
     done
-    checkout ArduCopter $tag PX4 || {
-        echo "Failed checkout of ArduCopter PX4 $tag"
-        error_count=$((error_count+1))
-        checkout ArduCopter "latest" ""
-        popd
-        return
-    }
-    rm -rf ../Build.ArduCopter
     for f in $frames; do
+        checkout ArduCopter $tag PX4 $f || {
+            echo "Failed checkout of ArduCopter PX4 $tag $f"
+            error_count=$((error_count+1))
+            checkout ArduCopter "latest" "" ""
+            continue
+        }
+        rm -rf ../Build.ArduCopter
 	echo "Building ArduCopter PX4-$f binaries"
 	ddir="$binaries/Copter/$hdate/PX4-$f"
 	skip_build $tag $ddir && continue
@@ -219,9 +248,10 @@ build_arducopter() {
             continue
         }
 	copyit ArduCopter-v1.px4 $ddir $tag &&
-	copyit ArduCopter-v2.px4 $ddir $tag
+	copyit ArduCopter-v2.px4 $ddir $tag &&
+	test ! -f ArduCopter-v4.px4 || copyit ArduCopter-v4.px4 $ddir $tag
     done
-    checkout ArduCopter "latest" ""
+    checkout ArduCopter "latest" "" ""
     popd
 }
 
@@ -232,7 +262,7 @@ build_rover() {
     pushd APMrover2
     for b in apm1 apm2 navio pxf; do
 	echo "Building APMrover2 $b binaries"
-        checkout APMrover2 $tag $b || continue
+        checkout APMrover2 $tag $b "" || continue
 	ddir=$binaries/Rover/$hdate/$b
 	skip_build $tag $ddir && continue
 	make clean || continue
@@ -242,13 +272,13 @@ build_rover() {
             continue
         }
         extension=$(board_extension $b)
-	copyit $TMPDIR/APMrover2.build/APMrover2.$extension $ddir $tag
+	copyit $BUILDROOT/APMrover2.$extension $ddir $tag
 	touch $binaries/Rover/$tag
     done
     echo "Building APMrover2 PX4 binaries"
     ddir=$binaries/Rover/$hdate/PX4
-    checkout APMrover2 $tag PX4 || {
-        checkout APMrover2 "latest" ""
+    checkout APMrover2 $tag PX4 "" || {
+        checkout APMrover2 "latest" "" ""
         popd
         return
     }
@@ -256,14 +286,15 @@ build_rover() {
 	make px4 || {
             echo "Failed build of APMrover2 PX4 $tag"
             error_count=$((error_count+1))
-            checkout APMrover2 "latest" ""
+            checkout APMrover2 "latest" "" ""
             popd
             return
         }
 	copyit APMrover2-v1.px4 $binaries/Rover/$hdate/PX4 $tag &&
-	copyit APMrover2-v2.px4 $binaries/Rover/$hdate/PX4 $tag 
+	copyit APMrover2-v2.px4 $binaries/Rover/$hdate/PX4 $tag &&
+	test ! -f APMrover2-v4.px4 || copyit APMrover2-v4.px4 $binaries/Rover/$hdate/PX4 $tag 
     }
-    checkout APMrover2 "latest" ""
+    checkout APMrover2 "latest" "" ""
     popd
 }
 
@@ -274,7 +305,7 @@ build_antennatracker() {
     pushd AntennaTracker
     for b in apm2; do
 	echo "Building AntennaTracker $b binaries"
-        checkout AntennaTracker $tag $b || continue
+        checkout AntennaTracker $tag $b "" || continue
 	ddir=$binaries/AntennaTracker/$hdate/$b
 	skip_build $tag $ddir && continue
 	make clean || continue
@@ -284,13 +315,13 @@ build_antennatracker() {
             continue
         }
         extension=$(board_extension $b)
-	copyit $TMPDIR/AntennaTracker.build/AntennaTracker.$extension $ddir $tag
+	copyit $BUILDROOT/AntennaTracker.$extension $ddir $tag
 	touch $binaries/AntennaTracker/$tag
     done
     echo "Building AntennaTracker PX4 binaries"
     ddir=$binaries/AntennaTracker/$hdate/PX4
-    checkout AntennaTracker $tag PX4 || {
-        checkout AntennaTracker "latest" ""
+    checkout AntennaTracker $tag PX4 "" || {
+        checkout AntennaTracker "latest" "" ""
         popd
         return
     }
@@ -298,14 +329,15 @@ build_antennatracker() {
 	make px4 || {
             echo "Failed build of AntennaTracker PX4 $tag"
             error_count=$((error_count+1))
-            checkout AntennaTracker "latest" ""
+            checkout AntennaTracker "latest" "" ""
             popd
             return
         }
 	copyit AntennaTracker-v1.px4 $binaries/AntennaTracker/$hdate/PX4 $tag &&
-	copyit AntennaTracker-v2.px4 $binaries/AntennaTracker/$hdate/PX4 $tag 
+	copyit AntennaTracker-v2.px4 $binaries/AntennaTracker/$hdate/PX4 $tag &&
+	test ! -f AntennaTracker-v4.px4 || copyit AntennaTracker-v4.px4 $binaries/AntennaTracker/$hdate/PX4 $tag 
     }
-    checkout AntennaTracker "latest" ""
+    checkout AntennaTracker "latest" "" ""
     popd
 }
 
@@ -314,10 +346,15 @@ build_antennatracker() {
     git submodule update
 }
 
+export BUILDROOT="$TMPDIR/binaries.build"
+rm -rf $BUILDROOT
+
 # make sure PX4 is rebuilt from scratch
-pushd ArduPlane
-make px4-clean || exit 1
-popd
+for d in ArduPlane ArduCopter APMrover2 AntennaTracker; do
+         pushd $d
+         make px4-clean || exit 1
+         popd
+done
 
 for build in stable beta latest; do
     build_arduplane $build

Tools/scripts/build_ci.sh

@@ -0,0 +1,75 @@
+#!/bin/bash
+# useful script to test all the different build types that we support.
+# This helps when doing large merges
+# Andrew Tridgell, November 2011
+
+set -ex
+
+. ~/.profile
+
+# CXX and CC are exported by default by travis
+unset CXX CC
+
+export BUILDROOT=/tmp/travis.build.$$
+rm -rf $BUILDROOT
+
+# If CI_BUILD_TARGET is not set, default to all of them
+if [ -z "$CI_BUILD_TARGET" ]; then
+    CI_BUILD_TARGET="sitl linux navio raspilot minlure bebop px4-v2 px4-v4"
+fi
+
+declare -A build_platforms
+declare -A build_concurrency
+declare -A build_extra_clean
+declare -A waf_supported_boards
+
+build_platforms=(["ArduCopter"]="px4-v2 px4-v4")
+
+build_concurrency=(["navio"]="-j2"
+                   ["raspilot"]="-j2"
+                   ["minlure"]="-j2"
+                   ["bebop"]="-j2"
+                   ["sitl"]="-j2"
+                   ["linux"]="-j2"
+                   ["px4-v2"]=""
+                   ["px4-v4"]="")
+
+build_extra_clean=(["px4-v2"]="make px4-cleandep")
+
+waf=modules/waf/waf-light
+
+# get list of boards supported by the waf build
+for board in $($waf list_boards | head -n1); do waf_supported_boards[$board]=1; done
+
+echo "Targets: $CI_BUILD_TARGET"
+for t in $CI_BUILD_TARGET; do
+    echo "Starting make based build for target ${t}..."
+    for v in ${!build_platforms[@]}; do
+        if [[ ${build_platforms[$v]} != *$t* ]]; then
+            continue
+        fi
+        echo "Building $v for ${t}..."
+
+        pushd $v
+        make clean
+        if [ ${build_extra_clean[$t]+_} ]; then
+            ${build_extra_clean[$t]}
+        fi
+
+        make $t ${build_concurrency[$t]}
+        popd
+    done
+
+    if [[ -n ${waf_supported_boards[$t]} ]]; then
+        echo "Starting waf build for board ${t}..."
+        $waf configure --board $t
+        $waf clean
+        $waf ${build_concurrency[$t]} all
+        if [[ $t == linux ]]; then
+            $waf check
+        fi
+    fi
+done
+
+echo build OK
+exit 0

Tools/scripts/build_examples.sh

@@ -6,7 +6,7 @@
 set -e
 set -x
 
-targets="clean apm2"
+targets="navio"
 
 [ $# -gt 0 ] && {
     targets="$*"
@@ -16,6 +16,9 @@ export PATH=/usr/lib/ccache:$PATH
 
 TESTS=$(find libraries -name 'Makefile' | grep -v FLYMAPLE | xargs -i dirname '{}')
 
+export BUILDROOT="/tmp/examples.build"
+rm -rf $BUILDROOT
+
 for b in $TESTS; do
     echo "TESTING $b"
     pushd $b

Tools/scripts/configure-ci.sh

@@ -0,0 +1,66 @@
+#!/bin/bash
+# Install dependencies and configure the environment for CI build testing
+
+set -ex
+
+PKGS="build-essential gawk ccache genromfs libc6-i386 \
+      python-argparse python-empy python-serial zlib1g-dev gcc-4.9 g++-4.9"
+
+ARM_ROOT="gcc-arm-none-eabi-4_9-2015q3"
+ARM_TARBALL="$ARM_ROOT-20150921-linux.tar.bz2"
+
+RPI_ROOT="master"
+RPI_TARBALL="$RPI_ROOT.tar.gz"
+
+sudo add-apt-repository ppa:ubuntu-toolchain-r/test -y
+sudo apt-get -qq -y update
+sudo apt-get -qq -y install $PKGS
+sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-4.9 90 \
+    --slave /usr/bin/g++ g++ /usr/bin/g++-4.9
+
+pushd $HOME
+
+mkdir -p $HOME/opt
+pushd $HOME/opt
+
+# PX4 toolchain
+compiler=$ARM_ROOT
+if [ ! -d "$HOME/opt/$compiler" ]; then
+  wget http://firmware.diydrones.com/Tools/PX4-tools/$ARM_TARBALL
+  tar -xf $ARM_TARBALL
+fi
+
+# RPi/BBB toolchain
+compiler="tools-master/arm-bcm2708/gcc-linaro-arm-linux-gnueabihf-raspbian-x64"
+if [ ! -d "$HOME/opt/$compiler" ]; then
+  wget http://firmware.diydrones.com/Tools/Travis/NavIO/$RPI_TARBALL
+  tar -xf $RPI_TARBALL
+fi
+
+popd
+
+mkdir -p $HOME/bin
+
+# configure ccache
+ln -s /usr/bin/ccache ~/bin/g++
+ln -s /usr/bin/ccache ~/bin/gcc
+ln -s /usr/bin/ccache ~/bin/arm-none-eabi-g++
+ln -s /usr/bin/ccache ~/bin/arm-none-eabi-gcc
+ln -s /usr/bin/ccache ~/bin/arm-linux-gnueabihf-g++
+ln -s /usr/bin/ccache ~/bin/arm-linux-gnueabihf-gcc
+
+exportline="export PATH=$HOME/bin:"
+exportline="${exportline}:$HOME/opt/gcc-arm-none-eabi-4_9-2015q3/bin:"
+exportline="${exportline}:$HOME/opt/tools-master/arm-bcm2708/gcc-linaro-arm-linux-gnueabihf-raspbian-x64/bin:"
+exportline="${exportline}:\$PATH"
+
+if grep -Fxq "$exportline" ~/.profile; then
+    echo nothing to do;
+else
+    echo $exportline >> ~/.profile;
+fi
+
+. ~/.profile
+
+popd
+

Tools/scripts/fix_libraries_includes.sh

@@ -0,0 +1,238 @@
+#!/bin/bash
+
+src=$(realpath $(dirname $BASH_SOURCE)/../../)
+base=$src/libraries
+declare -A header_dirs
+
+arg_verbose=false
+arg_create_commits=false
+
+usage(){
+    cat <<EOF
+Usage: $(basename $BASH_SOURCE) [OPTIONS] [--] [<pathspec>...]
+
+Fix includes of libraries headers in source files to be as the following:
+
+ - If the header is in the same directory the source belongs to, then the
+ notation #include "" is used with the path relative to the directory
+ containing the source.
+
+ - If the header is outside the directory containing the source, then we use
+ the notation #include <> with the path relative to libraries folder.
+
+If pathspec is given then it's an argument passed directly to git-grep. See
+git-grep(1) for more information on its format. In this case the changes will
+apply only to files that match the pathspec. Otherwise changes will be made to
+the entire repository.
+
+The output is a log of the process.
+
+OPTIONS:
+    -h,--help
+        Display this help message.
+
+    -v,--verbose
+        Not only log errors and warnings but also substitutions.
+
+    -c,--create-commits
+        Create commits in the end.
+
+    --commit
+        Assume that the user have run the substitutions beforehand - only
+        create the commits.
+EOF
+}
+
+create_commits(){
+    for f in $(git diff-files --name-only); do
+        if [[ ${f%%/*} == "libraries" ]]; then
+            echo $f | sed "s,\(libraries/[^/]*\)/.*,\1,"
+        else
+            echo ${f%%/*}
+        fi
+    done | uniq | while read d; do
+        if [[ $d == libraries/* ]]; then
+            commit_base=${d#libraries/}
+        else
+            commit_base=$d
+        fi
+        cat >/tmp/commit_msg <<EOF
+$commit_base: standardize inclusion of libaries headers
+
+This commit changes the way libraries headers are included in source files:
+
+ - If the header is in the same directory the source belongs to, so the
+ notation '#include ""' is used with the path relative to the directory
+ containing the source.
+
+ - If the header is outside the directory containing the source, then we use
+ the notation '#include <>' with the path relative to libraries folder.
+
+Some of the advantages of such approach:
+
+ - Only one search path for libraries headers.
+
+ - OSs like Windows may have a better lookup time.
+EOF
+        git add -u $d
+        git commit -F /tmp/commit_msg
+    done
+}
+
+replace_include(){
+    local file=$1
+    local n=$2
+    local new_path=$3
+    local old_path=$4
+    local regex="\(#\s*include\s*\)[<\"].\+[>\"]"
+
+    [[ $new_path == $old_path ]] && return
+
+    $arg_verbose && echo "$file:$n: $old_path -->  $new_path"
+    if ! sed -i "${n}s,$regex,\1$new_path," $file; then
+        echo Error on executing command: sed -i "${n}s,$regex,\1$new_path," $file >&2
+        kill -SIGINT $$
+    fi
+}
+
+fix_includes(){
+    local file=$1
+    local header=$2
+    local dirs=(${header_dirs[$header]})
+    local num_dirs=${#dirs[@]}
+    local regex="^\s*#\s*include\s*[<\"]\(.*/\)\?$header[>\"]"
+
+    grep -ahno $regex $file | while IFS=":" read n match; do
+        path=$(echo $match | sed "s/^\s*#\s*include\s*//g")
+        delim=${path:0:1}
+        path=${path:1:(${#path}-2)}
+        file_dir=$(realpath $(dirname $file))
+
+        if [[ $delim == "\"" ]]; then
+            localpath=$file_dir/$path
+            if [[ -f $localpath ]]; then
+                # verify if file is under to the file dir
+                localpath=$(realpath $localpath)
+                [[ $localpath == $file_dir* ]] && continue
+
+                # if not under file dir, check if $localpath is under $base
+                if [[ $localpath == $base* ]]; then
+                    new_path=${localpath#$base/}
+                    replace_include $file $n \<$new_path\> \"$path\"
+                    continue
+                fi
+            fi
+        fi
+
+        match_count=0
+        possible_paths=()
+        for dir in "${dirs[@]}"; do
+            if [[ $dir/$header == *$path ]]; then
+                ((match_count++))
+                new_path=$dir/$header
+                possible_paths[${#possible_paths[@]}]=$new_path
+            fi
+        done
+
+        if [[ $match_count -eq 0 ]]; then
+            echo "$file:$n: couldn't find a match for inclusion of $path"
+        elif [[ $match_count -eq 1 ]]; then
+            # check if included header is under file dir
+            if [[ -f $file_dir/$path ]]; then
+                new_path=\"$(realpath $file_dir/$path --relative-to $file_dir)\"
+            else
+                new_path=\<$new_path\>
+            fi
+            if [[ $delim == '"' ]]; then path=\"$path\"; else path=\<$path\>; fi
+            replace_include $file $n $new_path $path
+        else
+            echo "$file:$n: more than one match for inclusion of $path"
+            echo "    possible paths:"
+            for p in "${possible_paths[@]}"; do
+                echo "    $p"
+            done
+        fi
+    done
+}
+
+trap_reset_tree(){
+    echo
+    echo Process killed or interrupted! Reseting tree...
+    git -C $src reset --hard
+    exit 1
+}
+
+# parse args
+while [[ -n $1 ]]; do
+    case "$1" in
+    -h|--help)
+        usage
+        exit 0
+        ;;
+    -v|--verbose)
+        arg_verbose=true
+        ;;
+    -c|--create-commits)
+        arg_create_commits=true
+        ;;
+    --commit)
+        create_commits
+        exit $?
+        ;;
+    --)
+        # remaining args are pathspecs
+        shift
+        break
+        ;;
+    -*)
+        usage >&2
+        exit 1
+        ;;
+    *)
+        # this and the remaining args are pathspecs
+        break
+    esac
+    shift
+done
+
+trap trap_reset_tree SIGINT SIGKILL
+
+if ! git -C $src diff-files --quiet --exit-code; then
+    echo You have unstaged changes, please commit or stash them beforehand >&2
+    exit 1
+fi
+
+pushd $src > /dev/null
+
+# collect all headers
+git -C $base ls-files *.h > /tmp/headers
+total=$(cat /tmp/headers | wc -l)
+header_max_len=0
+while read f; do
+    header=$(basename $f)
+    dir=$(dirname $f)
+    if [[ -z ${header_dirs[$header]} ]]; then
+        header_dirs[$header]=$dir
+    else
+        header_dirs[$header]+=" $dir"
+    fi
+    printf "\rCollecting header files paths... $((++i))/$total" >&2
+    [[ ${#header} -gt $header_max_len ]] && header_max_len=${#header}
+done </tmp/headers
+echo
+
+total=${#header_dirs[@]}
+i=0
+for header in "${!header_dirs[@]}"; do
+    regex="#\s*include\s*[<\"]\(.*/\)\?$header[>\"]"
+    printf "\r($((++i))/$total) Fixing includes for header %-${header_max_len}s" $header >&2
+
+    # for each file that includes $header
+    git grep -l $regex -- "$@" | while read f; do
+        fix_includes $f $header
+    done
+done
+
+$arg_create_commits && create_commits
+
+popd > /dev/null

Tools/scripts/install-prereqs-arch.sh

@@ -0,0 +1,83 @@
+#!/bin/bash
+set -e
+
+command -v yaourt >/dev/null 2>&1 || { echo >&2 "Please install yaourt first. Aborting."; exit 1; }
+
+CWD=$(pwd)
+OPT="/opt"
+
+BASE_PKGS="wget curl base-devel git-core tk gsfonts"
+SITL_PKGS="python2-pip python-pip wxpython2.8 opencv python2-numpy python2-scipy ccache"
+PX4_PKGS="lib32-glibc zip zlib ncurses"
+
+PYTHON2_PKGS="pymavlink MAVProxy droneapi argparse matplotlib pyparsing catkin_pkg"
+PYTHON3_PKGS="pyserial empy"
+ARCH_AUR_PKGS="genromfs"
+
+# GNU Tools for ARM Embedded Processors
+# (see https://launchpad.net/gcc-arm-embedded/)
+ARM_ROOT="gcc-arm-none-eabi-4_9-2015q3"
+ARM_TARBALL="$ARM_ROOT-20150921-linux.tar.bz2"
+ARM_TARBALL_URL="http://firmware.diydrones.com/Tools/PX4-tools/$ARM_TARBALL"
+
+# Ardupilot Tools
+ARDUPILOT_TOOLS="ardupilot/Tools/autotest"
+
+function prompt_user() {
+      read -p "$1"
+      if [[ $REPLY =~ ^[Yy]$ ]]; then
+          return 0
+      else
+          return 1
+      fi
+}
+
+sudo usermod -a -G uucp $USER
+
+sudo pacman -S --noconfirm $BASE_PKGS $SITL_PKGS $PX4_PKGS
+sudo pip2 -q install $PYTHON2_PKGS
+sudo pip3 -q install $PYTHON3_PKGS
+yaourt -S --noconfirm $ARCH_AUR_PKGS
+
+(
+ cd /usr/lib/ccache
+ sudo ln -s /usr/bin/ccache arm-none-eabi-g++
+ sudo ln -s /usr/bin/ccache arm-none-eabi-gcc
+)
+
+if [ ! -d $OPT/$ARM_ROOT ]; then
+    (
+        cd $OPT;
+        sudo wget $ARM_TARBALL_URL;
+        sudo tar xjf ${ARM_TARBALL};
+        sudo rm ${ARM_TARBALL};
+    )
+fi
+
+exportline="export PATH=$OPT/$ARM_ROOT/bin:\$PATH";
+if ! grep -Fxq "$exportline" ~/.bashrc ; then
+    if prompt_user "Add $OPT/$ARM_ROOT/bin to your PATH [Y/n]?" ; then
+        echo "$exportline" >> ~/.bashrc
+        . ~/.bashrc
+    else
+        echo "Skipping adding $OPT/$ARM_ROOT/bin to PATH."
+    fi
+fi
+
+exportline2="export PATH=$CWD/$ARDUPILOT_TOOLS:\$PATH";
+if  ! grep -Fxq "$exportline2" ~/.bashrc ; then
+    if prompt_user "Add $CWD/$ARDUPILOT_TOOLS to your PATH [Y/n]?" ; then
+        echo "$exportline2" >> ~/.bashrc
+        . ~/.bashrc
+    else
+        echo "Skipping adding $CWD/$ARDUPILOT_TOOLS to PATH."
+    fi
+fi
+
+(
+ cd ./ardupilot
+ git submodule init
+ git submodule update
+)
+
+echo "Done. Please log out and log in again."

Tools/scripts/install-prereqs-ubuntu.sh

@@ -5,20 +5,19 @@ CWD=$(pwd)
 OPT="/opt"
 
 BASE_PKGS="gawk make git arduino-core curl"
-SITL_PKGS="g++ python-pip python-matplotlib python-serial python-wxgtk2.8 python-scipy python-opencv python-numpy python-pyparsing ccache"
-AVR_PKGS="gcc-avr binutils-avr avr-libc"
-PYTHON_PKGS="pymavlink MAVProxy droneapi"
+SITL_PKGS="g++ python-pip python-matplotlib python-serial python-wxgtk2.8 python-scipy python-opencv python-numpy python-pyparsing ccache realpath"
+PYTHON_PKGS="pymavlink MAVProxy droneapi catkin_pkg"
 PX4_PKGS="python-serial python-argparse openocd flex bison libncurses5-dev \
           autoconf texinfo build-essential libftdi-dev libtool zlib1g-dev \
-          zip genromfs"
+          zip genromfs python-empy"
 BEBOP_PKGS="g++-arm-linux-gnueabihf"
 UBUNTU64_PKGS="libc6:i386 libgcc1:i386 gcc-4.6-base:i386 libstdc++5:i386 libstdc++6:i386"
 ASSUME_YES=false
 
 # GNU Tools for ARM Embedded Processors
 # (see https://launchpad.net/gcc-arm-embedded/)
-ARM_ROOT="gcc-arm-none-eabi-4_7-2014q2"
-ARM_TARBALL="$ARM_ROOT-20140408-linux.tar.bz2"
+ARM_ROOT="gcc-arm-none-eabi-4_9-2015q3"
+ARM_TARBALL="$ARM_ROOT-20150921-linux.tar.bz2"
 ARM_TARBALL_URL="http://firmware.diydrones.com/Tools/PX4-tools/$ARM_TARBALL"
 
 # Ardupilot Tools
@@ -59,7 +58,7 @@ sudo usermod -a -G dialout $USER
 
 $APT_GET remove modemmanager
 $APT_GET update
-$APT_GET install $BASE_PKGS $SITL_PKGS $PX4_PKGS $BEBOP_PKGS $UBUNTU64_PKGS $AVR_PKGS
+$APT_GET install $BASE_PKGS $SITL_PKGS $PX4_PKGS $BEBOP_PKGS $UBUNTU64_PKGS
 sudo pip2 -q install $PYTHON_PKGS
 
 if [ ! -d $OPT/$ARM_ROOT ]; then

eclipse.cproject

@@ -0,0 +1,159 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<?fileVersion 4.0.0?><cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
+	<storageModule moduleId="org.eclipse.cdt.core.settings">
+		<cconfiguration id="cdt.managedbuild.toolchain.gnu.cross.base.354374513">
+			<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.cross.base.354374513" moduleId="org.eclipse.cdt.core.settings" name="Default">
+				<externalSettings/>
+				<extensions>
+					<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
+					<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+				</extensions>
+			</storageModule>
+			<storageModule moduleId="cdtBuildSystem" version="4.0.0">
+				<configuration buildProperties="" id="cdt.managedbuild.toolchain.gnu.cross.base.354374513" name="Default" parent="org.eclipse.cdt.build.core.emptycfg">
+					<folderInfo id="cdt.managedbuild.toolchain.gnu.cross.base.354374513.892177930" name="/" resourcePath="">
+						<toolChain id="cdt.managedbuild.toolchain.gnu.cross.base.526035570" name="cdt.managedbuild.toolchain.gnu.cross.base" superClass="cdt.managedbuild.toolchain.gnu.cross.base">
+							<option id="cdt.managedbuild.option.gnu.cross.prefix.458442836" name="Prefix" superClass="cdt.managedbuild.option.gnu.cross.prefix"/>
+							<option id="cdt.managedbuild.option.gnu.cross.path.1219552670" name="Path" superClass="cdt.managedbuild.option.gnu.cross.path"/>
+							<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF" id="cdt.managedbuild.targetPlatform.gnu.cross.737405680" isAbstract="false" osList="all" superClass="cdt.managedbuild.targetPlatform.gnu.cross"/>
+							<builder id="cdt.managedbuild.builder.gnu.cross.778361658" managedBuildOn="false" name="Gnu Make Builder.Default" superClass="cdt.managedbuild.builder.gnu.cross"/>
+							<tool id="cdt.managedbuild.tool.gnu.cross.c.compiler.768396405" name="Cross GCC Compiler" superClass="cdt.managedbuild.tool.gnu.cross.c.compiler">
+								<inputType id="cdt.managedbuild.tool.gnu.c.compiler.input.281326927" superClass="cdt.managedbuild.tool.gnu.c.compiler.input"/>
+							</tool>
+							<tool id="cdt.managedbuild.tool.gnu.cross.cpp.compiler.1282124949" name="Cross G++ Compiler" superClass="cdt.managedbuild.tool.gnu.cross.cpp.compiler">
+								<inputType id="cdt.managedbuild.tool.gnu.cpp.compiler.input.1533331666" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.input"/>
+							</tool>
+							<tool id="cdt.managedbuild.tool.gnu.cross.c.linker.1121841264" name="Cross GCC Linker" superClass="cdt.managedbuild.tool.gnu.cross.c.linker"/>
+							<tool id="cdt.managedbuild.tool.gnu.cross.cpp.linker.1530575069" name="Cross G++ Linker" superClass="cdt.managedbuild.tool.gnu.cross.cpp.linker">
+								<inputType id="cdt.managedbuild.tool.gnu.cpp.linker.input.1550437110" superClass="cdt.managedbuild.tool.gnu.cpp.linker.input">
+									<additionalInput kind="additionalinputdependency" paths="$(USER_OBJS)"/>
+									<additionalInput kind="additionalinput" paths="$(LIBS)"/>
+								</inputType>
+							</tool>
+							<tool id="cdt.managedbuild.tool.gnu.cross.archiver.1425628759" name="Cross GCC Archiver" superClass="cdt.managedbuild.tool.gnu.cross.archiver"/>
+							<tool id="cdt.managedbuild.tool.gnu.cross.assembler.941717398" name="Cross GCC Assembler" superClass="cdt.managedbuild.tool.gnu.cross.assembler">
+								<inputType id="cdt.managedbuild.tool.gnu.assembler.input.968458728" superClass="cdt.managedbuild.tool.gnu.assembler.input"/>
+							</tool>
+						</toolChain>
+					</folderInfo>
+				</configuration>
+			</storageModule>
+			<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
+		</cconfiguration>
+	</storageModule>
+	<storageModule moduleId="cdtBuildSystem" version="4.0.0">
+		<project id="ardupilot.null.967258070" name="ardupilot"/>
+	</storageModule>
+	<storageModule moduleId="scannerConfiguration">
+		<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
+	</storageModule>
+	<storageModule moduleId="org.eclipse.cdt.core.LanguageSettingsProviders"/>
+	<storageModule moduleId="refreshScope"/>
+	<storageModule moduleId="org.eclipse.cdt.make.core.buildtargets">
+		<buildTargets>
+			<target name="px4-v2" path="ArduCopter" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildTarget>px4-v2</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="px4-cleandep" path="ArduCopter" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments/>
+				<buildTarget>px4-cleandep</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="clean" path="ArduCopter" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments/>
+				<buildTarget>clean</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="px4-clean" path="ArduCopter" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildTarget>px4-clean</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="px4-v2" path="ArduPlane" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildTarget>px4-v2</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="clean" path="ArduPlane" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments/>
+				<buildTarget>clean</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="px4-clean" path="ArduPlane" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments/>
+				<buildTarget>px4-clean</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="px4-cleandep" path="ArduPlane" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments/>
+				<buildTarget>px4-cleandep</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="px4-v2" path="AntennaTracker" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildTarget>px4-v2</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="px4-v2" path="APMrover2" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildTarget>px4-v2</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="clean" path="APMrover2" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments/>
+				<buildTarget>clean</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="px4-clean" path="APMrover2" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments/>
+				<buildTarget>px4-clean</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="px4-cleandep" path="APMrover2" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments/>
+				<buildTarget>px4-cleandep</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+		</buildTargets>
+	</storageModule>
+</cproject>