Copter: update version to AC3.1.5

This update applies to pixhawk only

.gitignore

@@ -1,11 +1,13 @@
 *~
 *.bin
+*.cproject
 *.d
 *.dll
 *.exe
 *.lst
 *.o
 *.obj
+*.project
 *.px4
 *.pyc
 *.tlog

.project

@@ -1,11 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<projectDescription>
-	<name>ArduPilotMega-Source@ardupilotone</name>
-	<comment></comment>
-	<projects>
-	</projects>
-	<buildSpec>
-	</buildSpec>
-	<natures>
-	</natures>
-</projectDescription>

APMrover2/APMrover2.pde

@@ -252,7 +252,7 @@ AP_InertialSensor_Oilpan ins( &adc );
   #error Unrecognised CONFIG_INS_TYPE setting.
 #endif // CONFIG_INS_TYPE
 
-AP_AHRS_DCM ahrs(&ins, g_gps);
+AP_AHRS_DCM ahrs(ins, g_gps);
 
 static AP_L1_Control L1_controller(ahrs);
 
@@ -523,52 +523,56 @@ static float G_Dt						= 0.02;
 // Timer used to accrue data and trigger recording of the performanc monitoring log message
 static int32_t 	perf_mon_timer;
 // The maximum main loop execution time recorded in the current performance monitoring interval
-static int16_t 	G_Dt_max = 0;
+static uint32_t 	G_Dt_max;
 // The number of gps fixes recorded in the current performance monitoring interval
 static uint8_t 	gps_fix_count = 0;
-// A variable used by developers to track performanc metrics.
-// Currently used to record the number of GCS heartbeat messages received
-static int16_t pmTest1 = 0;
-
 
 ////////////////////////////////////////////////////////////////////////////////
 // System Timers
 ////////////////////////////////////////////////////////////////////////////////
-// Time in miliseconds of start of main control loop.  Milliseconds
+// Time in microseconds of start of main control loop. 
-static uint32_t 	fast_loopTimer;
+static uint32_t 	fast_loopTimer_us;
-// Time Stamp when fast loop was complete.  Milliseconds
-static uint32_t 	fast_loopTimeStamp;
 // Number of milliseconds used in last main loop cycle
-static uint8_t 		delta_ms_fast_loop;
+static uint32_t		delta_us_fast_loop;
 // Counter of main loop executions.  Used for performance monitoring and failsafe processing
 static uint16_t			mainLoop_count;
 
+// set if we are driving backwards
+static bool in_reverse;
+
 ////////////////////////////////////////////////////////////////////////////////
 // Top-level logic
 ////////////////////////////////////////////////////////////////////////////////
 
 /*
-  scheduler table - all regular tasks apart from the fast_loop()
+  scheduler table - all regular tasks should be listed here, along
-  should be listed here, along with how often they should be called
+  with how often they should be called (in 20ms units) and the maximum
-  (in 20ms units) and the maximum time they are expected to take (in
+  time they are expected to take (in microseconds)
-  microseconds)
  */
 static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
+	{ read_radio,             1,   1000 },
+    { ahrs_update,            1,   6400 },
+    { read_sonars,            1,   2000 },
+    { update_current_mode,    1,   1000 },
+    { set_servos,             1,   1000 },
     { update_GPS,             5,   2500 },
     { navigate,               5,   1600 },
     { update_compass,         5,   2000 },
     { update_commands,        5,   1000 },
     { update_logging,         5,   1000 },
+    { gcs_retry_deferred,     1,   1000 },
+    { gcs_update,             1,   1700 },
+    { gcs_data_stream_send,   1,   3000 },
+    { read_control_switch,   15,   1000 },
+    { read_trim_switch,       5,   1000 },
     { read_battery,           5,   1000 },
     { read_receiver_rssi,     5,   1000 },
-    { read_trim_switch,       5,   1000 },
-    { read_control_switch,   15,   1000 },
     { update_events,         15,   1000 },
     { check_usb_mux,         15,   1000 },
-    { mount_update,           1,    500 },
+    { mount_update,           1,    600 },
-    { gcs_failsafe_check,     5,    500 },
+    { gcs_failsafe_check,     5,    600 },
     { compass_accumulate,     1,    900 },
-    { update_notify,          1,    100 },
+    { update_notify,          1,    300 },
     { one_second_loop,       50,   3000 }
 };
 
@@ -610,66 +614,44 @@ void loop()
     if (!ins.wait_for_sample(1000)) {
         return;
     }
-    uint32_t timer = millis();
+    uint32_t timer = hal.scheduler->micros();
 
-    delta_ms_fast_loop	= timer - fast_loopTimer;
+    delta_us_fast_loop	= timer - fast_loopTimer_us;
-    G_Dt                = (float)delta_ms_fast_loop / 1000.f;
+    G_Dt                = delta_us_fast_loop * 1.0e-6f;
-    fast_loopTimer      = timer;
+    fast_loopTimer_us   = timer;
+
+	if (delta_us_fast_loop > G_Dt_max)
+		G_Dt_max = delta_us_fast_loop;
 
     mainLoop_count++;
 
-    // Execute the fast loop
-    // ---------------------
-    fast_loop();
-
     // tell the scheduler one tick has passed
     scheduler.tick();
-    fast_loopTimeStamp = millis();
 
-    scheduler.run(19000U);
+    scheduler.run(19500U);
 }
 
-// Main loop 50Hz
+// update AHRS system
-static void fast_loop()
+static void ahrs_update()
 {
-	// This is the fast loop - we want it to execute at 50Hz if possible
+#if HIL_MODE != HIL_MODE_DISABLED
-	// -----------------------------------------------------------------
+    // update hil before AHRS update
-	if (delta_ms_fast_loop > G_Dt_max)
-		G_Dt_max = delta_ms_fast_loop;
-
-	// Read radio
-	// ----------
-	read_radio();
-
-    // try to send any deferred messages if the serial port now has
-    // some space available
-    gcs_send_message(MSG_RETRY_DEFERRED);
-
-	#if HIL_MODE != HIL_MODE_DISABLED
-		// update hil before dcm update
     gcs_update();
-	#endif
+#endif
+
+    // when in reverse we need to tell AHRS not to assume we are a
+    // 'fly forward' vehicle, otherwise it will see a large
+    // discrepancy between the mag and the GPS heading and will try to
+    // correct for it, leading to a large yaw error
+    ahrs.set_fly_forward(!in_reverse);
 
     ahrs.update();
 
-    read_sonars();
-
     if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST)
         Log_Write_Attitude();
 
     if (g.log_bitmask & MASK_LOG_IMU)
-        DataFlash.Log_Write_IMU(&ins);
+        DataFlash.Log_Write_IMU(ins);
-
-	// custom code/exceptions for flight modes
-	// ---------------------------------------
-	update_current_mode();
-
-	// write out the servo PWM values
-	// ------------------------------
-	set_servos();
-
-    gcs_update();
-    gcs_data_stream_send();
 }
 
 /*
@@ -787,9 +769,13 @@ static void one_second_loop(void)
     counter++;
 
     // write perf data every 20s
-    if (counter == 20) {
+    if (counter % 10 == 0) {
+        if (scheduler.debug() != 0) {
+            hal.console->printf_P(PSTR("G_Dt_max=%lu\n"), (unsigned long)G_Dt_max);
+        }
         if (g.log_bitmask & MASK_LOG_PM)
             Log_Write_Performance();
+        G_Dt_max = 0;
         resetPerfData();
     }
 
@@ -832,6 +818,10 @@ static void update_GPS(void)
 
 			} else {
                 init_home();
+
+                // set system clock for log timestamps
+                hal.util->set_system_clock(g_gps->time_epoch_usec());
+
 				if (g.compass_enabled) {
 					// Set compass declination automatically
 					compass.set_initial_location(g_gps->latitude, g_gps->longitude);
@@ -879,7 +869,12 @@ static void update_current_mode(void)
         // and throttle gives speed in proportion to cruise speed, up
         // to 50% throttle, then uses nudging above that.
         float target_speed = channel_throttle->pwm_to_angle() * 0.01 * 2 * g.speed_cruise;
+        in_reverse = (target_speed < 0);
+        if (in_reverse) {
+            target_speed = constrain_float(target_speed, -g.speed_cruise, 0);
+        } else {
             target_speed = constrain_float(target_speed, 0, g.speed_cruise);
+        }
         calc_throttle(target_speed);
         break;
     }
@@ -894,6 +889,10 @@ static void update_current_mode(void)
          */
         channel_throttle->servo_out = channel_throttle->control_in;
         channel_steer->servo_out = channel_steer->pwm_to_angle();
+
+        // mark us as in_reverse when using a negative throttle to
+        // stop AHRS getting off
+        in_reverse = (channel_throttle->servo_out < 0);
         break;
 
     case HOLD:

APMrover2/GCS_Mavlink.pde

@@ -9,6 +9,9 @@ static bool in_mavlink_delay;
 // true when we have received at least 1 MAVLink packet
 static bool mavlink_active;
 
+// true if we are out of time in our event timeslice
+static bool	gcs_out_of_time;
+
 // check if a message will fit in the payload space available
 #define CHECK_PAYLOAD_SIZE(id) if (payload_space < MAVLINK_MSG_ID_## id ##_LEN) return false
 
@@ -158,6 +161,9 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
     if (g_gps != NULL && g_gps->status() > GPS::NO_GPS) {
         control_sensors_health |= MAV_SYS_STATUS_SENSOR_GPS;
     }
+    if (!ins.healthy()) {
+        control_sensors_health &= ~(MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL);
+    }
 
     int16_t battery_current = -1;
     int8_t battery_remaining = -1;
@@ -246,6 +252,14 @@ static void NOINLINE send_gps_raw(mavlink_channel_t chan)
         g_gps->num_sats);
 }
 
+static void NOINLINE send_system_time(mavlink_channel_t chan)
+{
+    mavlink_msg_system_time_send(
+        chan,
+        g_gps->time_epoch_usec(),
+        hal.scheduler->millis());
+}
+
 
 #if HIL_MODE != HIL_MODE_DISABLED
 static void NOINLINE send_servo_out(mavlink_channel_t chan)
@@ -471,6 +485,14 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id,
         return false;
     }
 
+    // if we don't have at least 1ms remaining before the main loop
+    // wants to fire then don't send a mavlink message. We want to
+    // prioritise the main flight control loop over communications
+    if (!in_mavlink_delay && scheduler.time_available_usec() < 1200) {
+        gcs_out_of_time = true;
+        return false;
+    }
+
     switch (id) {
     case MSG_HEARTBEAT:
         CHECK_PAYLOAD_SIZE(HEARTBEAT);
@@ -509,6 +531,11 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id,
         send_gps_raw(chan);
         break;
 
+    case MSG_SYSTEM_TIME:
+        CHECK_PAYLOAD_SIZE(SYSTEM_TIME);
+        send_system_time(chan);
+        break;
+
     case MSG_SERVO_OUT:
 #if HIL_MODE != HIL_MODE_DISABLED
         CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED);
@@ -756,8 +783,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] PROGMEM = {
     // @Range: 0 10
     // @Increment: 1
     // @User: Advanced
-    AP_GROUPINFO("PARAMS",   8, GCS_MAVLINK, streamRates[8],  5),
+    AP_GROUPINFO("PARAMS",   8, GCS_MAVLINK, streamRates[8],  10),
-
     AP_GROUPEND
 };
 
@@ -858,7 +884,8 @@ bool GCS_MAVLINK::stream_trigger(enum streams stream_num)
 
     // send at a much lower rate while handling waypoints and
     // parameter sends
-    if (waypoint_receiving || _queued_parameter != NULL) {
+    if ((stream_num != STREAM_PARAMS) && 
+        (waypoint_receiving || _queued_parameter != NULL)) {
         rate *= 0.25;
     }
 
@@ -883,18 +910,19 @@ bool GCS_MAVLINK::stream_trigger(enum streams stream_num)
 void
 GCS_MAVLINK::data_stream_send(void)
 {
+    gcs_out_of_time = false;
+
     if (_queued_parameter != NULL) {
         if (streamRates[STREAM_PARAMS].get() <= 0) {
-            streamRates[STREAM_PARAMS].set(5);
+            streamRates[STREAM_PARAMS].set(10);
-        }
-        if (streamRates[STREAM_PARAMS].get() > 5) {
-            streamRates[STREAM_PARAMS].set(5);
         }
         if (stream_trigger(STREAM_PARAMS)) {
             send_message(MSG_NEXT_PARAM);
         }
     }
 
+    if (gcs_out_of_time) return;
+
     if (in_mavlink_delay) {
 #if HIL_MODE != HIL_MODE_DISABLED
         // in HIL we need to keep sending servo values to ensure
@@ -911,11 +939,15 @@ GCS_MAVLINK::data_stream_send(void)
         return;
     }
 
+    if (gcs_out_of_time) return;
+
     if (stream_trigger(STREAM_RAW_SENSORS)) {
         send_message(MSG_RAW_IMU1);
         send_message(MSG_RAW_IMU3);
     }
 
+    if (gcs_out_of_time) return;
+
     if (stream_trigger(STREAM_EXTENDED_STATUS)) {
         send_message(MSG_EXTENDED_STATUS1);
         send_message(MSG_EXTENDED_STATUS2);
@@ -924,33 +956,46 @@ GCS_MAVLINK::data_stream_send(void)
         send_message(MSG_NAV_CONTROLLER_OUTPUT);
     }
 
+    if (gcs_out_of_time) return;
+
     if (stream_trigger(STREAM_POSITION)) {
         // sent with GPS read
         send_message(MSG_LOCATION);
     }
 
+    if (gcs_out_of_time) return;
+
     if (stream_trigger(STREAM_RAW_CONTROLLER)) {
         send_message(MSG_SERVO_OUT);
     }
 
+    if (gcs_out_of_time) return;
+
     if (stream_trigger(STREAM_RC_CHANNELS)) {
         send_message(MSG_RADIO_OUT);
         send_message(MSG_RADIO_IN);
     }
 
+    if (gcs_out_of_time) return;
+
     if (stream_trigger(STREAM_EXTRA1)) {
         send_message(MSG_ATTITUDE);
         send_message(MSG_SIMSTATE);
     }
 
+    if (gcs_out_of_time) return;
+
     if (stream_trigger(STREAM_EXTRA2)) {
         send_message(MSG_VFR_HUD);
     }
 
+    if (gcs_out_of_time) return;
+
     if (stream_trigger(STREAM_EXTRA3)) {
         send_message(MSG_AHRS);
         send_message(MSG_HWSTATUS);
         send_message(MSG_RANGEFINDER);
+        send_message(MSG_SYSTEM_TIME);
     }
 }
 
@@ -1293,8 +1338,10 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             mavlink_msg_param_request_list_decode(msg, &packet);
             if (mavlink_check_target(packet.target_system,packet.target_component)) break;
 
-            // Start sending parameters - next call to ::update will kick the first one out
+            // mark the firmware version in the tlog
+            send_text_P(SEVERITY_LOW, PSTR(FIRMWARE_STRING));
 
+            // Start sending parameters - next call to ::update will kick the first one out
             _queued_parameter = AP_Param::first(&_queued_parameter_token, &_queued_parameter_type);
             _queued_parameter_index = 0;
             _queued_parameter_count = _count_parameters();
@@ -1713,7 +1760,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 			if(msg->sysid != g.sysid_my_gcs) break;
             last_heartbeat_ms = failsafe.rc_override_timer = millis();
             failsafe_trigger(FAILSAFE_EVENT_GCS, false);
-			pmTest1++;
             break;
         }
 
@@ -2007,3 +2053,11 @@ void gcs_send_text_fmt(const prog_char_t *fmt, ...)
     }
 }
 
+
+/**
+   retry any deferred messages
+ */
+static void gcs_retry_deferred(void)
+{
+    gcs_send_message(MSG_RETRY_DEFERRED);
+}

APMrover2/Log.pde

@@ -165,15 +165,14 @@ struct PACKED log_Performance {
     LOG_PACKET_HEADER;
     uint32_t loop_time;
     uint16_t main_loop_count;
-    int16_t  g_dt_max;
+    uint32_t g_dt_max;
     uint8_t  renorm_count;
     uint8_t  renorm_blowup;
-    uint8_t  gps_fix_count;
     int16_t  gyro_drift_x;
     int16_t  gyro_drift_y;
     int16_t  gyro_drift_z;
-    int16_t  pm_test;
     uint8_t  i2c_lockup_count;
+    uint16_t ins_error_count;
 };
 
 // Write a performance monitoring packet. Total length : 19 bytes
@@ -186,12 +185,11 @@ static void Log_Write_Performance()
         g_dt_max        : G_Dt_max,
         renorm_count    : ahrs.renorm_range_count,
         renorm_blowup   : ahrs.renorm_blowup_count,
-        gps_fix_count   : gps_fix_count,
         gyro_drift_x    : (int16_t)(ahrs.get_gyro_drift().x * 1000),
         gyro_drift_y    : (int16_t)(ahrs.get_gyro_drift().y * 1000),
         gyro_drift_z    : (int16_t)(ahrs.get_gyro_drift().z * 1000),
-        pm_test         : pmTest1,
+        i2c_lockup_count: hal.i2c->lockup_count(),
-        i2c_lockup_count: hal.i2c->lockup_count()
+        ins_error_count  : ins.error_count()
     };
     DataFlash.WriteBlock(&pkt, sizeof(pkt));
 }
@@ -228,6 +226,7 @@ static void Log_Write_Cmd(uint8_t num, const struct Location *wp)
 struct PACKED log_Camera {
     LOG_PACKET_HEADER;
     uint32_t gps_time;
+    uint16_t gps_week;
     int32_t  latitude;
     int32_t  longitude;
     int16_t  roll;
@@ -241,7 +240,8 @@ static void Log_Write_Camera()
 #if CAMERA == ENABLED
     struct log_Camera pkt = {
         LOG_PACKET_HEADER_INIT(LOG_CAMERA_MSG),
-        gps_time    : g_gps->time,
+        gps_time    : g_gps->time_week_ms,
+        gps_week    : g_gps->time_week,
         latitude    : current_loc.lat,
         longitude   : current_loc.lng,
         roll        : (int16_t)ahrs.roll_sensor,
@@ -454,11 +454,11 @@ static const struct LogStructure log_structure[] PROGMEM = {
     { LOG_ATTITUDE_MSG, sizeof(log_Attitude),       
       "ATT", "ccC",        "Roll,Pitch,Yaw" },
     { LOG_PERFORMANCE_MSG, sizeof(log_Performance), 
-      "PM",  "IHhBBBhhhhB", "LTime,MLC,gDt,RNCnt,RNBl,GPScnt,GDx,GDy,GDz,PMT,I2CErr" },
+      "PM",  "IHIBBhhhBH", "LTime,MLC,gDt,RNCnt,RNBl,GDx,GDy,GDz,I2CErr,INSErr" },
     { LOG_CMD_MSG, sizeof(log_Cmd),                 
       "CMD", "BBBBBeLL",   "CTot,CNum,CId,COpt,Prm1,Alt,Lat,Lng" },
     { LOG_CAMERA_MSG, sizeof(log_Camera),                 
-      "CAM", "ILLccC",   "GPSTime,Lat,Lng,Roll,Pitch,Yaw" },
+      "CAM", "IHLLeccC",   "GPSTime,GPSWeek,Lat,Lng,Alt,Roll,Pitch,Yaw" },
     { LOG_STARTUP_MSG, sizeof(log_Startup),         
       "STRT", "BB",         "SType,CTot" },
     { LOG_CTUN_MSG, sizeof(log_Control_Tuning),     
@@ -479,7 +479,7 @@ static const struct LogStructure log_structure[] PROGMEM = {
 // Read the DataFlash log memory : Packet Parser
 static void Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page)
 {
-    cliSerial->printf_P(PSTR("\n" THISFIRMWARE
+    cliSerial->printf_P(PSTR("\n" FIRMWARE_STRING
                              "\nFree RAM: %u\n"),
                         (unsigned) memcheck_available_memory());
 
@@ -496,6 +496,7 @@ static void Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page)
 static void start_logging() 
 {
     DataFlash.StartNewLog(sizeof(log_structure)/sizeof(log_structure[0]), log_structure);
+    DataFlash.Log_Write_Message_P(PSTR(FIRMWARE_STRING));
 }
 
 #else // LOGGING_ENABLED

APMrover2/Steering.pde

@@ -75,13 +75,8 @@ static void calc_throttle(float target_speed)
         return;
     }
 
-    if (target_speed <= 0) {
+    float throttle_base = (fabsf(target_speed) / g.speed_cruise) * g.throttle_cruise;
-        // cope with zero requested speed
+    int throttle_target = throttle_base + throttle_nudge;  
-        channel_throttle->servo_out = g.throttle_min.get();
-        return;
-    }
-
-    int throttle_target = g.throttle_cruise + throttle_nudge;  
 
     /*
       reduce target speed in proportion to turning rate, up to the
@@ -102,7 +97,7 @@ static void calc_throttle(float target_speed)
     // reduce the target speed by the reduction factor
     target_speed *= reduction;
 
-    groundspeed_error = target_speed - ground_speed; 
+    groundspeed_error = fabsf(target_speed) - ground_speed; 
     
     throttle = throttle_target + (g.pidSpeedThrottle.get_pid(groundspeed_error * 100) / 100);
 
@@ -110,7 +105,11 @@ static void calc_throttle(float target_speed)
     // much faster response in turns
     throttle *= reduction;
 
-    channel_throttle->servo_out = constrain_int16(throttle, g.throttle_min.get(), g.throttle_max.get());
+    if (in_reverse) {
+        channel_throttle->servo_out = constrain_int16(-throttle, -g.throttle_max, -g.throttle_min);
+    } else {
+        channel_throttle->servo_out = constrain_int16(throttle, g.throttle_min, g.throttle_max);
+    }
 }
 
 /*****************************************
@@ -179,9 +178,15 @@ static void set_servos(void)
         }
 	} else {       
         channel_steer->calc_pwm();
+        if (in_reverse) {
+            channel_throttle->servo_out = constrain_int16(channel_throttle->servo_out, 
+                                                          -g.throttle_max,
+                                                          -g.throttle_min);
+        } else {
             channel_throttle->servo_out = constrain_int16(channel_throttle->servo_out, 
                                                           g.throttle_min.get(), 
                                                           g.throttle_max.get());
+        }
 
         if ((failsafe.bits & FAILSAFE_EVENT_THROTTLE) && control_mode < AUTO) {
             // suppress throttle if in failsafe

APMrover2/config.h

@@ -366,3 +366,13 @@
 #ifndef SONAR_ENABLED
 # define SONAR_ENABLED       DISABLED
 #endif
+
+/*
+  build a firmware version string.
+  GIT_VERSION comes from Makefile builds
+*/
+#ifndef GIT_VERSION
+#define FIRMWARE_STRING THISFIRMWARE
+#else
+#define FIRMWARE_STRING THISFIRMWARE " (" GIT_VERSION ")"
+#endif

APMrover2/defines.h

@@ -101,6 +101,7 @@ enum ap_message {
     MSG_RAW_IMU1,
     MSG_RAW_IMU3,
     MSG_GPS_RAW,
+    MSG_SYSTEM_TIME,
     MSG_SERVO_OUT,
     MSG_NEXT_WAYPOINT,
     MSG_NEXT_PARAM,

APMrover2/system.pde

@@ -94,7 +94,7 @@ static void init_ardupilot()
     // standard gps running
     hal.uartB->begin(115200, 256, 16);
 
-	cliSerial->printf_P(PSTR("\n\nInit " THISFIRMWARE
+	cliSerial->printf_P(PSTR("\n\nInit " FIRMWARE_STRING
 						 "\n\nFree RAM: %u\n"),
                     memcheck_available_memory());
                     
@@ -252,6 +252,8 @@ static void set_mode(enum mode mode)
 	control_mode = mode;
     throttle_last = 0;
     throttle = 500;
+    in_reverse = false;
+    g.pidSpeedThrottle.reset_I();
 
     if (control_mode != AUTO) {
         auto_triggered = false;
@@ -370,7 +372,6 @@ static void resetPerfData(void) {
 	ahrs.renorm_range_count 	= 0;
 	ahrs.renorm_blowup_count = 0;
 	gps_fix_count 			= 0;
-	pmTest1					= 0;
 	perf_mon_timer 			= millis();
 }
 

APMrover2/test.pde

@@ -341,14 +341,8 @@ test_ins(uint8_t argc, const Menu::arg *argv)
     uint8_t medium_loopCounter = 0;
 
 	while(1){
-		delay(20);
+        ins.wait_for_sample(1000);
-		if (millis() - fast_loopTimer > 19) {
-			delta_ms_fast_loop 	= millis() - fast_loopTimer;
-			G_Dt 				= (float)delta_ms_fast_loop / 1000.f;		// used by DCM integrator
-			fast_loopTimer		= millis();
 
-			// INS
-			// ---
         ahrs.update();
 
         if(g.compass_enabled) {
@@ -373,7 +367,6 @@ test_ins(uint8_t argc, const Menu::arg *argv)
     if(cliSerial->available() > 0){
         return (0);
     }
-	}
 }
 
 
@@ -408,14 +401,7 @@ test_mag(uint8_t argc, const Menu::arg *argv)
     uint8_t medium_loopCounter = 0;
 
     while(1) {
-		delay(20);
+        ins.wait_for_sample(1000);
-		if (millis() - fast_loopTimer > 19) {
-			delta_ms_fast_loop 	= millis() - fast_loopTimer;
-			G_Dt 				= (float)delta_ms_fast_loop / 1000.f;		// used by DCM integrator
-			fast_loopTimer		= millis();
-
-			// IMU
-			// ---
         ahrs.update();
 
         medium_loopCounter++;
@@ -446,7 +432,6 @@ test_mag(uint8_t argc, const Menu::arg *argv)
             }
             counter=0;
         }
-		}
         if (cliSerial->available() > 0) {
             break;
         }

ArduCopter/APM_Config.h

@@ -15,14 +15,24 @@
  *  OCTA_FRAME
  *  OCTA_QUAD_FRAME
  *  HELI_FRAME
+ *  SINGLE_FRAME
  */
 
-// uncomment the lines below to save on flash space if compiling for the APM using Arduino IDE
+// uncomment the lines below to save on flash space if compiling for the APM using Arduino IDE.  Top items save the most flash space
-//#define OPTFLOW               DISABLED            // disable optical flow sensor to save 5K of flash space
-//#define LOGGING_ENABLED       DISABLED            // disable dataflash logging to save 11K of flash space
-//#define MOUNT                 DISABLED            // disable the camera gimbal to save 8K of flash space
 //#define CLI_ENABLED           DISABLED            // disable the CLI (command-line-interface) to save 21K of flash space
+//#define LOGGING_ENABLED       DISABLED            // disable dataflash logging to save 11K of flash space
+//#define GPS_PROTOCOL          GPS_PROTOCOL_UBLOX  // hard code GPS to Ublox to save 8k of flash
+//#define GPS_PROTOCOL          GPS_PROTOCOL_MTK19  // hard cdoe GPS to Mediatek to save 10k of flash
+//#define MOUNT                 DISABLED            // disable the camera gimbal to save 8K of flash space
 //#define AUTOTUNE              DISABLED            // disable the auto tune functionality to save 7k of flash
+//#define OPTFLOW               DISABLED            // disable optical flow sensor to save 5K of flash space
+//#define AC_FENCE              DISABLED            // disable fence to save 2k of flash
+//#define CAMERA                DISABLED            // disable camera trigger to save 1k of flash
+//#define COPTER_LEDS           DISABLED            // disable external navigation leds to save 1k of flash
+//#define CONFIG_SONAR          DISABLED            // disable sonar to save 1k of flash
+
+// features below are disabled by default
+//#define SPRAYER               ENABLED             // enable the crop sprayer feature (two ESC controlled pumps the speed of which depends upon the vehicle's horizontal velocity)
 
 // redefine size of throttle deadband in pwm (0 ~ 1000)
 //#define THROTTLE_IN_DEADBAND   100

ArduCopter/APM_Config_mavlink_hil.h

@@ -25,7 +25,7 @@
 // the loop port.  Alternatively, use a telemetry/HIL shim like FGShim
 // https://ardupilot-mega.googlecode.com/svn/Tools/trunk/FlightGear
 //
-// The buad rate is controlled by SERIAL3_BAUD in this mode.
+// The buad rate is controlled by SERIAL1_BAUD in this mode.
 
 #define HIL_PORT            3
 

ArduCopter/AP_State.pde

@@ -131,3 +131,10 @@ void set_pre_arm_check(bool b)
     }
 }
 
+void set_pre_arm_rc_check(bool b)
+{
+    if(ap.pre_arm_rc_check != b) {
+        ap.pre_arm_rc_check = b;
+    }
+}
+

ArduCopter/ArduCopter.pde

@@ -1,6 +1,6 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
-#define THISFIRMWARE "ArduCopter V3.1-rc5"
+#define THISFIRMWARE "ArduCopter V3.1.5"
 /*
    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
@@ -19,44 +19,55 @@
  *  ArduCopter Version 3.0
  *  Creator:        Jason Short
  *  Lead Developer: Randy Mackay
- *  Based on code and ideas from the Arducopter team: Pat Hickey, Jose Julio, Jani Hirvinen, Andrew Tridgell, Justin Beech, Adam Rivera, Jean-Louis Naudin, Roberto Navoni
+ *  Lead Tester:    Marco Robustini 
- *  Thanks to:	Chris Anderson, Mike Smith, Jordi Munoz, Doug Weibel, James Goppert, Benjamin Pelletier, Robert Lefebvre, Marco Robustini
+ *  Based on code and ideas from the Arducopter team: Leonard Hall, Andrew Tridgell, Robert Lefebvre, Pat Hickey, Michael Oborne, Jani Hirvinen, 
+                                                      Olivier Adler, Kevin Hester, Arthur Benemann, Jonathan Challinger, John Arne Birkeland,
+                                                      Jean-Louis Naudin, Mike Smith, and more
+ *  Thanks to:	Chris Anderson, Jordi Munoz, Jason Short, Doug Weibel, Jose Julio
  *
- *  Special Thanks for Contributors (in alphabetical order by first name):
+ *  Special Thanks to contributors (in alphabetical order by first name):
  *
  *  Adam M Rivera		:Auto Compass Declination
  *  Amilcar Lucas		:Camera mount library
  *  Andrew Tridgell		:General development, Mavlink Support
  *  Angel Fernandez		:Alpha testing
- *  Doug Weibel			:Libraries
+ *  AndreasAntonopoulous:GeoFence
+ *  Arthur Benemann     :DroidPlanner GCS
+ *  Benjamin Pelletier  :Libraries
+ *  Bill King           :Single Copter
  *  Christof Schmid		:Alpha testing
+ *  Craig Elder         :Release Management, Support
  *  Dani Saez           :V Octo Support
+ *  Doug Weibel			:DCM, Libraries, Control law advice
  *  Gregory Fletcher	:Camera mount orientation math
  *  Guntars				:Arming safety suggestion
  *  HappyKillmore		:Mavlink GCS
- *  Hein Hollander      :Octo Support
+ *  Hein Hollander      :Octo Support, Heli Testing
  *  Igor van Airde      :Control Law optimization
- *  Leonard Hall 		:Flight Dynamics, Throttle, Loiter and Navigation Controllers
- *  Jonathan Challinger :Inertial Navigation
- *  Jean-Louis Naudin   :Auto Landing
- *  Max Levine			:Tri Support, Graphics
  *  Jack Dunkle			:Alpha testing
  *  James Goppert		:Mavlink Support
  *  Jani Hiriven		:Testing feedback
+ *  Jean-Louis Naudin   :Auto Landing
  *  John Arne Birkeland	:PPM Encoder
- *  Jose Julio			:Stabilization Control laws
+ *  Jose Julio			:Stabilization Control laws, MPU6k driver
+ *  Julian Oes          :Pixhawk
+ *  Jonathan Challinger :Inertial Navigation, CompassMot, Spin-When-Armed
+ *  Kevin Hester        :Andropilot GCS
+ *  Max Levine			:Tri Support, Graphics
+ *  Leonard Hall 		:Flight Dynamics, Throttle, Loiter and Navigation Controllers
  *  Marco Robustini		:Lead tester
  *  Michael Oborne		:Mission Planner GCS
- *  Mike Smith			:Libraries, Coding support
+ *  Mike Smith			:Pixhawk driver, coding support
- *  Oliver				:Piezo support
+ *  Olivier Adler       :PPM Encoder, piezo buzzer
- *  Olivier Adler       :PPM Encoder
+ *  Pat Hickey          :Hardware Abstraaction Layer (HAL)
- *  Robert Lefebvre		:Heli Support & LEDs
+ *  Robert Lefebvre		:Heli Support, Copter LEDs
- *  Sandro Benigno      :Camera support
+ *  Roberto Navoni      :Library testing, Porting to VRBrain
+ *  Sandro Benigno      :Camera support, MinimOSD
+ *  ..and many more.
  *
- *  And much more so PLEASE PM me on DIYDRONES to add your contribution to the List
+ *  Code commit statistics can be found here: https://github.com/diydrones/ardupilot/graphs/contributors
- *
+ *  Wiki: http://copter.ardupilot.com/
- *  Requires modified "mrelax" version of Arduino, which can be found here:
+ *  Requires modified version of Arduino, which can be found here: http://ardupilot.com/downloads/?category=6
- *  http://code.google.com/p/ardupilot-mega/downloads/list
  *
  */
 
@@ -289,7 +300,7 @@ AP_GPS_None     g_gps_driver;
   #error Unrecognised GPS_PROTOCOL setting.
  #endif // GPS PROTOCOL
 
-static AP_AHRS_DCM ahrs(&ins, g_gps);
+static AP_AHRS_DCM ahrs(ins, g_gps);
 
 #elif HIL_MODE == HIL_MODE_SENSORS
 // sensor emulators
@@ -298,7 +309,7 @@ static AP_Baro_HIL      barometer;
 static AP_Compass_HIL          compass;
 static AP_GPS_HIL              g_gps_driver;
 static AP_InertialSensor_HIL   ins;
-static AP_AHRS_DCM             ahrs(&ins, g_gps);
+static AP_AHRS_DCM             ahrs(ins, g_gps);
 
 
  #if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
@@ -309,7 +320,7 @@ static SITL sitl;
 #elif HIL_MODE == HIL_MODE_ATTITUDE
 static AP_ADC_HIL              adc;
 static AP_InertialSensor_HIL   ins;
-static AP_AHRS_HIL             ahrs(&ins, g_gps);
+static AP_AHRS_HIL             ahrs(ins, g_gps);
 static AP_GPS_HIL              g_gps_driver;
 static AP_Compass_HIL          compass;                  // never used
 static AP_Baro_HIL      barometer;
@@ -335,8 +346,8 @@ static AP_OpticalFlow optflow;
 ////////////////////////////////////////////////////////////////////////////////
 // GCS selection
 ////////////////////////////////////////////////////////////////////////////////
-static GCS_MAVLINK gcs0;
+static const uint8_t num_gcs = MAVLINK_COMM_NUM_BUFFERS;
-static GCS_MAVLINK gcs3;
+static GCS_MAVLINK gcs[MAVLINK_COMM_NUM_BUFFERS];
 
 ////////////////////////////////////////////////////////////////////////////////
 // SONAR selection
@@ -395,6 +406,8 @@ static union {
         uint8_t yaw_stopped         : 1; // 16      // Used to manage the Yaw hold capabilities
 
         uint8_t disable_stab_rate_limit : 1; // 17  // disables limits rate request from the stability controller
+
+        uint8_t rc_receiver_present : 1; // 18  // true if we have an rc receiver present (i.e. if we've ever received an update
     };
     uint32_t value;
 } ap;
@@ -445,14 +458,18 @@ static struct {
  #define MOTOR_CLASS AP_MotorsOctaQuad
 #elif FRAME_CONFIG == HELI_FRAME
  #define MOTOR_CLASS AP_MotorsHeli
+#elif FRAME_CONFIG == SINGLE_FRAME
+ #define MOTOR_CLASS AP_MotorsSingle
 #else
  #error Unrecognised frame type
 #endif
 
 #if FRAME_CONFIG == HELI_FRAME  // helicopter constructor requires more arguments
-static MOTOR_CLASS motors(&g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.rc_8, &g.heli_servo_1, &g.heli_servo_2, &g.heli_servo_3, &g.heli_servo_4);
+static MOTOR_CLASS motors(&g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.rc_7, &g.rc_8, &g.heli_servo_1, &g.heli_servo_2, &g.heli_servo_3, &g.heli_servo_4);
 #elif FRAME_CONFIG == TRI_FRAME  // tri constructor requires additional rc_7 argument to allow tail servo reversing
 static MOTOR_CLASS motors(&g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.rc_7);
+#elif FRAME_CONFIG == SINGLE_FRAME  // single constructor requires extra servos for flaps
+static MOTOR_CLASS motors(&g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.single_servo_1, &g.single_servo_2, &g.single_servo_3, &g.single_servo_4);
 #else
 static MOTOR_CLASS motors(&g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4);
 #endif
@@ -642,8 +659,6 @@ static float target_sonar_alt;      // desired altitude in cm above the ground
 // The altitude as reported by Baro in cm – Values can be quite high
 static int32_t baro_alt;
 
-static int16_t saved_toy_throttle;
-
 
 ////////////////////////////////////////////////////////////////////////////////
 // flight modes
@@ -652,11 +667,11 @@ static int16_t saved_toy_throttle;
 // Each Flight mode is a unique combination of these modes
 //
 // The current desired control scheme for Yaw
-static uint8_t yaw_mode;
+static uint8_t yaw_mode = STABILIZE_YAW;
 // The current desired control scheme for roll and pitch / navigation
-static uint8_t roll_pitch_mode;
+static uint8_t roll_pitch_mode = STABILIZE_RP;
 // The current desired control scheme for altitude hold
-static uint8_t throttle_mode;
+static uint8_t throttle_mode = STABILIZE_THR;
 
 
 ////////////////////////////////////////////////////////////////////////////////
@@ -712,7 +727,6 @@ static uint32_t throttle_integrator;
 ////////////////////////////////////////////////////////////////////////////////
 // The Commanded Yaw from the autopilot.
 static int32_t nav_yaw;
-static uint8_t yaw_timer;
 // Yaw will point at this location if yaw_mode is set to YAW_LOOK_AT_LOCATION
 static Vector3f yaw_look_at_WP;
 // bearing from current location to the yaw_look_at_WP
@@ -750,14 +764,14 @@ static uint32_t condition_start;
 ////////////////////////////////////////////////////////////////////////////////
 // IMU variables
 ////////////////////////////////////////////////////////////////////////////////
-// Integration time for the gyros (DCM algorithm)
+// Integration time (in seconds) for the gyros (DCM algorithm)
 // Updated with the fast loop
 static float G_Dt = 0.02;
 
 ////////////////////////////////////////////////////////////////////////////////
 // Inertial Navigation
 ////////////////////////////////////////////////////////////////////////////////
-static AP_InertialNav inertial_nav(&ahrs, &ins, &barometer, g_gps, gps_glitch);
+static AP_InertialNav inertial_nav(&ahrs, &barometer, g_gps, gps_glitch);
 
 ////////////////////////////////////////////////////////////////////////////////
 // Waypoint navigation object
@@ -768,8 +782,6 @@ static AC_WPNav wp_nav(&inertial_nav, &ahrs, &g.pi_loiter_lat, &g.pi_loiter_lon,
 ////////////////////////////////////////////////////////////////////////////////
 // Performance monitoring
 ////////////////////////////////////////////////////////////////////////////////
-// The number of GPS fixes we have had
-static uint8_t gps_fix_count;
 static int16_t pmTest1;
 
 // System Timers
@@ -780,8 +792,6 @@ static uint32_t fast_loopTimer;
 static uint16_t mainLoop_count;
 // Loiter timer - Records how long we have been in loiter
 static uint32_t rtl_loiter_start_time;
-// prevents duplicate GPS messages from entering system
-static uint32_t last_gps_time;
 
 // Used to exit the roll and pitch auto trim function
 static uint8_t auto_trim_counter;
@@ -861,14 +871,17 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
     { read_aux_switches,    10,      50 },
     { arm_motors_check,     10,      10 },
     { auto_trim,            10,     140 },
-    { update_toy_throttle,  10,      50 },
     { update_altitude,      10,    1000 },
     { run_nav_updates,      10,     800 },
     { three_hz_loop,        33,      90 },
     { compass_accumulate,    2,     420 },
     { barometer_accumulate,  2,     250 },
+#if FRAME_CONFIG == HELI_FRAME
+    { check_dynamic_flight,  2,     100 },
+#endif
     { update_notify,         2,     100 },
     { one_hz_loop,         100,     420 },
+    { crash_check,          10,      20 },
     { gcs_check_input,	     2,     550 },
     { gcs_send_heartbeat,  100,     150 },
     { gcs_send_deferred,     2,     720 },
@@ -966,7 +979,6 @@ static void perf_update(void)
                             (unsigned long)perf_info_get_max_time());
     }
     perf_info_reset();
-    gps_fix_count = 0;
     pmTest1 = 0;
 }
 
@@ -974,7 +986,7 @@ void loop()
 {
     // wait for an INS sample
     if (!ins.wait_for_sample(1000)) {
-        Log_Write_Error(ERROR_SUBSYSTEM_MAIN, ERROR_CODE_INS_DELAY);
+        Log_Write_Error(ERROR_SUBSYSTEM_MAIN, ERROR_CODE_MAIN_INS_DELAY);
         return;
     }
     uint32_t timer = micros();
@@ -1076,12 +1088,16 @@ static void throttle_loop()
     // check if we've landed
     update_land_detector();
 
-#if TOY_EDF == ENABLED
-    edf_toy();
-#endif
-
     // check auto_armed status
     update_auto_armed();
+
+#if FRAME_CONFIG == HELI_FRAME
+    // update rotor speed
+    heli_update_rotor_speed_targets();
+
+    // update trad heli swash plate movement
+    heli_update_landing_swash();
+#endif
 }
 
 // update_mount - update camera mount position
@@ -1155,7 +1171,7 @@ static void fifty_hz_logging_loop()
     }
 
     if (g.log_bitmask & MASK_LOG_IMU && motors.armed()) {
-        DataFlash.Log_Write_IMU(&ins);
+        DataFlash.Log_Write_IMU(ins);
     }
 #endif
 }
@@ -1198,8 +1214,15 @@ static void one_hz_loop()
     if ((g.log_bitmask & MASK_LOG_CURRENT) && motors.armed())
         Log_Write_Current();
 
-    // perform pre-arm checks
+    // perform pre-arm checks & display failures every 30 seconds
+    static uint8_t pre_arm_display_counter = 15;
+    pre_arm_display_counter++;
+    if (pre_arm_display_counter >= 30) {
+        pre_arm_checks(true);
+        pre_arm_display_counter = 0;
+    }else{
         pre_arm_checks(false);
+    }
 
     // auto disarm checks
     auto_disarm_check();
@@ -1212,12 +1235,8 @@ static void one_hz_loop()
         motors.set_frame_orientation(g.frame_orientation);
     }
 
-#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+    // update assigned functions and enable auxiliar servos
-    update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8, &g.rc_9, &g.rc_10, &g.rc_11, &g.rc_12);
+    aux_servos_update_fn();
-#elif MOUNT == ENABLED
-    update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8, &g.rc_10, &g.rc_11);
-#endif
-
     enable_aux_servos();
 
 #if MOUNT == ENABLED
@@ -1258,27 +1277,22 @@ static void update_optical_flow(void)
 // called at 50hz
 static void update_GPS(void)
 {
-    // A counter that is used to grab at least 10 reads before commiting the Home location
+    static uint32_t last_gps_reading;           // time of last gps message
-    static uint8_t ground_start_count  = 10;
+    static uint8_t ground_start_count = 10;     // counter used to grab at least 10 reads before commiting the Home location
 
     g_gps->update();
 
-    if (g_gps->new_data && last_gps_time != g_gps->time && g_gps->status() >= GPS::GPS_OK_FIX_2D) {
+    // logging and glitch protection run after every gps message
-        // clear new data flag
+    if (g_gps->last_message_time_ms() != last_gps_reading) {
-        g_gps->new_data = false;
+        last_gps_reading = g_gps->last_message_time_ms();
 
-        // save GPS time so we don't get duplicate reads
+        // log GPS message
-        last_gps_time = g_gps->time;
+        if ((g.log_bitmask & MASK_LOG_GPS) && motors.armed()) {
-
-        // log location if we have at least a 2D fix
-        if (g.log_bitmask & MASK_LOG_GPS && motors.armed()) {
             DataFlash.Log_Write_GPS(g_gps, current_loc.alt);
         }
 
-        // for performance monitoring
+        // run glitch protection and update AP_Notify if home has been initialised
-        gps_fix_count++;
+        if (ap.home_is_set) {
-
-        // run glitch protection and update AP_Notify
             gps_glitch.check_position();
             if (AP_Notify::flags.gps_glitching != gps_glitch.glitching()) {
                 if (gps_glitch.glitching()) {
@@ -1288,6 +1302,13 @@ static void update_GPS(void)
                 }
                 AP_Notify::flags.gps_glitching = gps_glitch.glitching();
             }
+        }
+    }
+
+    // checks to initialise home and take location based pictures
+    if (g_gps->new_data && g_gps->status() >= GPS::GPS_OK_FIX_3D) {
+        // clear new data flag
+        g_gps->new_data = false;
 
         // check if we can initialise home yet
         if (!ap.home_is_set) {
@@ -1300,6 +1321,10 @@ static void update_GPS(void)
                     // ap.home_is_set will be true so this will only happen once
                     ground_start_count = 0;
                     init_home();
+
+                    // set system clock for log timestamps
+                    hal.util->set_system_clock(g_gps->time_epoch_usec());
+
                     if (g.compass_enabled) {
                         // Set compass declination automatically
                         compass.set_initial_location(g_gps->latitude, g_gps->longitude);
@@ -1375,7 +1400,7 @@ bool set_yaw_mode(uint8_t new_yaw_mode)
                 yaw_initialised = true;
             }
             break;
-        case YAW_TOY:
+        case YAW_DRIFT:
             yaw_initialised = true;
             break;
         case YAW_RESETTOARMEDYAW:
@@ -1397,6 +1422,13 @@ bool set_yaw_mode(uint8_t new_yaw_mode)
 // 100hz update rate
 void update_yaw_mode(void)
 {
+    int16_t pilot_yaw = g.rc_4.control_in;
+
+    // do not process pilot's yaw input during radio failsafe
+    if (failsafe.radio) {
+        pilot_yaw = 0;
+    }
+
     switch(yaw_mode) {
 
     case YAW_HOLD:
@@ -1405,12 +1437,12 @@ void update_yaw_mode(void)
             nav_yaw = ahrs.yaw_sensor;
         }
         // heading hold at heading held in nav_yaw but allow input from pilot
-        get_yaw_rate_stabilized_ef(g.rc_4.control_in);
+        get_yaw_rate_stabilized_ef(pilot_yaw);
         break;
 
     case YAW_ACRO:
         // pilot controlled yaw using rate controller
-        get_yaw_rate_stabilized_bf(g.rc_4.control_in);
+        get_yaw_rate_stabilized_bf(pilot_yaw);
         break;
 
     case YAW_LOOK_AT_NEXT_WP:
@@ -1425,7 +1457,7 @@ void update_yaw_mode(void)
         get_stabilize_yaw(nav_yaw);
 
         // if there is any pilot input, switch to YAW_HOLD mode for the next iteration
-        if( g.rc_4.control_in != 0 ) {
+        if (pilot_yaw != 0) {
             set_yaw_mode(YAW_HOLD);
         }
         break;
@@ -1439,7 +1471,7 @@ void update_yaw_mode(void)
         get_look_at_yaw();
 
         // if there is any pilot input, switch to YAW_HOLD mode for the next iteration
-        if( g.rc_4.control_in != 0 ) {
+        if (pilot_yaw != 0) {
             set_yaw_mode(YAW_HOLD);
         }
         break;
@@ -1453,7 +1485,7 @@ void update_yaw_mode(void)
         get_circle_yaw();
 
         // if there is any pilot input, switch to YAW_HOLD mode for the next iteration
-        if( g.rc_4.control_in != 0 ) {
+        if (pilot_yaw != 0) {
             set_yaw_mode(YAW_HOLD);
         }
         break;
@@ -1469,7 +1501,7 @@ void update_yaw_mode(void)
         get_stabilize_yaw(nav_yaw);
 
         // if there is any pilot input, switch to YAW_HOLD mode for the next iteration
-        if( g.rc_4.control_in != 0 ) {
+        if (pilot_yaw != 0) {
             set_yaw_mode(YAW_HOLD);
         }
         break;
@@ -1491,22 +1523,16 @@ void update_yaw_mode(void)
             nav_yaw = ahrs.yaw_sensor;
         }
 		// Commanded Yaw to automatically look ahead.
-        get_look_ahead_yaw(g.rc_4.control_in);
+        get_look_ahead_yaw(pilot_yaw);
         break;
 
-#if TOY_LOOKUP == TOY_EXTERNAL_MIXER
+    case YAW_DRIFT:
-    case YAW_TOY:
+        // if we have landed reset yaw target to current heading
-        // if we are landed reset yaw target to current heading
         if (ap.land_complete) {
             nav_yaw = ahrs.yaw_sensor;
-        }else{
-            // update to allow external roll/yaw mixing
-            // keep heading always pointing at home with no pilot input allowed
-            nav_yaw = get_yaw_slew(nav_yaw, home_bearing, AUTO_YAW_SLEW_RATE);
         }
-        get_stabilize_yaw(nav_yaw);
+        get_yaw_drift();
         break;
-#endif
 
     case YAW_RESETTOARMEDYAW:
         // if we are landed reset yaw target to current heading
@@ -1519,7 +1545,7 @@ void update_yaw_mode(void)
         get_stabilize_yaw(nav_yaw);
 
         // if there is any pilot input, switch to YAW_HOLD mode for the next iteration
-        if( g.rc_4.control_in != 0 ) {
+        if (pilot_yaw != 0) {
             set_yaw_mode(YAW_HOLD);
         }
 
@@ -1577,18 +1603,12 @@ bool set_roll_pitch_mode(uint8_t new_roll_pitch_mode)
             break;
         case ROLL_PITCH_AUTO:
         case ROLL_PITCH_STABLE_OF:
-        case ROLL_PITCH_TOY:
+        case ROLL_PITCH_DRIFT:
+        case ROLL_PITCH_LOITER:
         case ROLL_PITCH_SPORT:
             roll_pitch_initialised = true;
             break;
 
-        case ROLL_PITCH_LOITER:
-            // require gps lock
-            if( ap.home_is_set ) {
-                roll_pitch_initialised = true;
-            }
-            break;
-
 #if AUTOTUNE == ENABLED
         case ROLL_PITCH_AUTOTUNE:
             // only enter autotune mode from stabilized roll-pitch mode when armed and flying
@@ -1633,7 +1653,7 @@ void update_roll_pitch_mode(void)
 
 #if FRAME_CONFIG == HELI_FRAME
         // ACRO does not get SIMPLE mode ability
-        if (motors.flybar_mode == 1) {
+        if (motors.has_flybar()) {
             g.rc_1.servo_out = g.rc_1.control_in;
             g.rc_2.servo_out = g.rc_2.control_in;
         }else{
@@ -1654,8 +1674,13 @@ void update_roll_pitch_mode(void)
         // apply SIMPLE mode transform
         update_simple_mode();
 
+        if(failsafe.radio) {
+            // don't allow copter to fly away during failsafe
+            get_pilot_desired_lean_angles(0.0f, 0.0f, control_roll, control_pitch);
+        } else {
             // convert pilot input to lean angles
             get_pilot_desired_lean_angles(g.rc_1.control_in, g.rc_2.control_in, control_roll, control_pitch);
+        }
 
         // pass desired roll, pitch to stabilize attitude controllers
         get_stabilize_roll(control_roll);
@@ -1679,18 +1704,21 @@ void update_roll_pitch_mode(void)
         // apply SIMPLE mode transform
         update_simple_mode();
 
+        if(failsafe.radio) {
+            // don't allow copter to fly away during failsafe
+            get_pilot_desired_lean_angles(0.0f, 0.0f, control_roll, control_pitch);
+        } else {
             // convert pilot input to lean angles
             get_pilot_desired_lean_angles(g.rc_1.control_in, g.rc_2.control_in, control_roll, control_pitch);
+        }
 
         // mix in user control with optical flow
         get_stabilize_roll(get_of_roll(control_roll));
         get_stabilize_pitch(get_of_pitch(control_pitch));
         break;
 
-    // THOR
+    case ROLL_PITCH_DRIFT:
-    // a call out to the main toy logic
+        get_roll_pitch_drift();
-    case ROLL_PITCH_TOY:
-        roll_pitch_toy();
         break;
 
     case ROLL_PITCH_LOITER:
@@ -1701,8 +1729,13 @@ void update_roll_pitch_mode(void)
         control_roll            = g.rc_1.control_in;
         control_pitch           = g.rc_2.control_in;
 
+        if(failsafe.radio) {
+            // don't allow loiter target to move during failsafe
+            wp_nav.move_loiter_target(0.0f, 0.0f, 0.01f);
+        } else {
             // update loiter target from user controls
-        wp_nav.move_loiter_target(control_roll, control_pitch,0.01f);
+            wp_nav.move_loiter_target(g.rc_1.control_in, g.rc_2.control_in, 0.01f);
+        }
 
         // copy latest output from nav controller to stabilize controller
         nav_roll = wp_nav.get_desired_roll();
@@ -1738,7 +1771,7 @@ void update_roll_pitch_mode(void)
         get_stabilize_pitch(control_pitch);
 
         // copy user input for reporting purposes
-        get_autotune_roll_pitch_controller(g.rc_1.control_in, g.rc_2.control_in);
+        get_autotune_roll_pitch_controller(g.rc_1.control_in, g.rc_2.control_in, g.rc_4.control_in);
         break;
 #endif
     }
@@ -1814,6 +1847,12 @@ void update_super_simple_bearing(bool force_update)
     }
 }
 
+// throttle_mode_manual - returns true if the throttle is directly controlled by the pilot
+bool throttle_mode_manual(uint8_t thr_mode)
+{
+    return (thr_mode == THROTTLE_MANUAL || thr_mode == THROTTLE_MANUAL_TILT_COMPENSATED || thr_mode == THROTTLE_MANUAL_HELI);
+}
+
 // set_throttle_mode - sets the throttle mode and initialises any variables as required
 bool set_throttle_mode( uint8_t new_throttle_mode )
 {
@@ -1838,7 +1877,7 @@ bool set_throttle_mode( uint8_t new_throttle_mode )
         case THROTTLE_AUTO:
             controller_desired_alt = get_initial_alt_hold(current_loc.alt, climb_rate);     // reset controller desired altitude to current altitude
             wp_nav.set_desired_alt(controller_desired_alt);                                 // same as above but for loiter controller
-            if ( throttle_mode <= THROTTLE_MANUAL_TILT_COMPENSATED ) {      // reset the alt hold I terms if previous throttle mode was manual
+            if (throttle_mode_manual(throttle_mode)) {  // reset the alt hold I terms if previous throttle mode was manual
                 reset_throttle_I();
                 set_accel_throttle_I_from_pilot_throttle(get_pilot_desired_throttle(g.rc_3.control_in));
             }
@@ -1850,6 +1889,14 @@ bool set_throttle_mode( uint8_t new_throttle_mode )
             controller_desired_alt = get_initial_alt_hold(current_loc.alt, climb_rate);   // reset controller desired altitude to current altitude
             throttle_initialised = true;
             break;
+
+#if FRAME_CONFIG == HELI_FRAME
+        case THROTTLE_MANUAL_HELI:
+            throttle_accel_deactivate();                // this controller does not use accel based throttle controller
+            altitude_error = 0;                         // clear altitude error reported to GCS
+            throttle_initialised = true;
+            break;
+#endif
     }
 
     // update the throttle mode
@@ -1875,33 +1922,15 @@ void update_throttle_mode(void)
     if(ap.do_flip)     // this is pretty bad but needed to flip in AP modes.
         return;
 
-#if FRAME_CONFIG == HELI_FRAME
+#if FRAME_CONFIG != HELI_FRAME
-
+    // do not run throttle controllers if motors disarmed
-	if (control_mode == STABILIZE){
-		motors.stab_throttle = true;
-	} else {
-		motors.stab_throttle = false;
-	}
-    
-    // allow swash collective to move if we are in manual throttle modes, even if disarmed
-    if( !motors.armed() ) {
-        if ( !(throttle_mode == THROTTLE_MANUAL) && !(throttle_mode == THROTTLE_MANUAL_TILT_COMPENSATED)){
-            throttle_accel_deactivate();    // do not allow the accel based throttle to override our command
-            return;
-        }
-    }
-
-#else // HELI_FRAME
-
-// do not run throttle controllers if motors disarmed
     if( !motors.armed() ) {
         set_throttle_out(0, false);
         throttle_accel_deactivate();    // do not allow the accel based throttle to override our command
         set_target_alt_for_reporting(0);
         return;
     }
-    
+#endif // FRAME_CONFIG != HELI_FRAME
-#endif // HELI_FRAME
 
     switch(throttle_mode) {
 
@@ -1916,12 +1945,11 @@ void update_throttle_mode(void)
 
             // update estimate of throttle cruise
 			#if FRAME_CONFIG == HELI_FRAME
-            update_throttle_cruise(motors.coll_out);
+            update_throttle_cruise(motors.get_collective_out());
 			#else
 			update_throttle_cruise(pilot_throttle_scaled);
             #endif  //HELI_FRAME
 
-
             // check if we've taken off yet
             if (!ap.takeoff_complete && motors.armed()) {
                 if (pilot_throttle_scaled > g.throttle_cruise) {
@@ -1943,7 +1971,7 @@ void update_throttle_mode(void)
 
             // update estimate of throttle cruise
             #if FRAME_CONFIG == HELI_FRAME
-            update_throttle_cruise(motors.coll_out);
+            update_throttle_cruise(motors.get_collective_out());
 			#else
 			update_throttle_cruise(pilot_throttle_scaled);
             #endif  //HELI_FRAME
@@ -2020,6 +2048,20 @@ void update_throttle_mode(void)
         get_throttle_land();
         set_target_alt_for_reporting(0);
         break;
+
+#if FRAME_CONFIG == HELI_FRAME
+    case THROTTLE_MANUAL_HELI:
+        // trad heli manual throttle controller
+        // send pilot's output directly to swash plate
+        pilot_throttle_scaled = get_pilot_desired_collective(g.rc_3.control_in);
+        set_throttle_out(pilot_throttle_scaled, false);
+
+        // update estimate of throttle cruise
+        update_throttle_cruise(motors.get_collective_out());
+        set_target_alt_for_reporting(0);
+        break;
+#endif // HELI_FRAME
+
     }
 }
 
@@ -2219,7 +2261,7 @@ static void tuning(){
 
 #if FRAME_CONFIG == HELI_FRAME
     case CH6_HELI_EXTERNAL_GYRO:
-        motors.ext_gyro_gain = tuning_value;
+        motors.ext_gyro_gain(g.rc_6.control_in);
         break;
 #endif
 

ArduCopter/Attitude.pde

@@ -7,6 +7,10 @@ static void get_pilot_desired_lean_angles(int16_t roll_in, int16_t pitch_in, int
     static float _scaler = 1.0;
     static int16_t _angle_max = 0;
 
+    // range check the input
+    roll_in = constrain_int16(roll_in, -ROLL_PITCH_INPUT_MAX, ROLL_PITCH_INPUT_MAX);
+    pitch_in = constrain_int16(pitch_in, -ROLL_PITCH_INPUT_MAX, ROLL_PITCH_INPUT_MAX);
+
     // return immediately if no scaling required
     if (g.angle_max == ROLL_PITCH_INPUT_MAX) {
         roll_out = roll_in;
@@ -36,7 +40,7 @@ get_stabilize_roll(int32_t target_angle)
 
     // constrain the target rate
     if (!ap.disable_stab_rate_limit) {
-        target_rate = constrain_int32(target_rate, -STABILIZE_RATE_LIMIT, STABILIZE_RATE_LIMIT);
+        target_rate = constrain_int32(target_rate, -g.angle_rate_max, g.angle_rate_max);
     }
 
     // set targets for rate controller
@@ -54,7 +58,7 @@ get_stabilize_pitch(int32_t target_angle)
 
     // constrain the target rate
     if (!ap.disable_stab_rate_limit) {
-        target_rate = constrain_int32(target_rate, -STABILIZE_RATE_LIMIT, STABILIZE_RATE_LIMIT);
+        target_rate = constrain_int32(target_rate, -g.angle_rate_max, g.angle_rate_max);
     }
 
     // set targets for rate controller
@@ -79,7 +83,7 @@ get_stabilize_yaw(int32_t target_angle)
 
     // do not use rate controllers for helicotpers with external gyros
 #if FRAME_CONFIG == HELI_FRAME
-    if(motors.ext_gyro_enabled) {
+    if(motors.tail_type() == AP_MOTORS_HELI_TAILTYPE_SERVO_EXTGYRO) {
         g.rc_4.servo_out = constrain_int32(target_rate, -4500, 4500);
     }
 #endif
@@ -99,15 +103,6 @@ get_stabilize_yaw(int32_t target_angle)
     set_yaw_rate_target(target_rate, EARTH_FRAME);
 }
 
-static void
-get_acro_yaw(int32_t target_rate)
-{
-    target_rate = target_rate * g.acro_yaw_p;
-
-    // set targets for rate controller
-    set_yaw_rate_target(target_rate, BODY_FRAME);
-}
-
 // get_acro_level_rates - calculate earth frame rate corrections to pull the copter back to level while in ACRO mode
 static void
 get_acro_level_rates()
@@ -192,9 +187,15 @@ get_roll_rate_stabilized_bf(int32_t stick_angle)
     // don't let angle error grow too large
     angle_error = constrain_float(angle_error, -MAX_ROLL_OVERSHOOT, MAX_ROLL_OVERSHOOT);
 
+#if FRAME_CONFIG == HELI_FRAME
+    if (!motors.motor_runup_complete()) {
+           angle_error = 0;
+    }
+#else
     if (!motors.armed() || g.rc_3.servo_out == 0) {
         angle_error = 0;
     }
+#endif // HELI_FRAME
 }
 
 // Pitch with rate input and stabilized in the body frame
@@ -231,9 +232,15 @@ get_pitch_rate_stabilized_bf(int32_t stick_angle)
     // don't let angle error grow too large
     angle_error = constrain_float(angle_error, -MAX_PITCH_OVERSHOOT, MAX_PITCH_OVERSHOOT);
 
+#if FRAME_CONFIG == HELI_FRAME
+    if (!motors.motor_runup_complete()) {
+           angle_error = 0;
+    }
+#else
     if (!motors.armed() || g.rc_3.servo_out == 0) {
         angle_error = 0;
     }
+#endif // HELI_FRAME
 }
 
 // Yaw with rate input and stabilized in the body frame
@@ -270,9 +277,15 @@ get_yaw_rate_stabilized_bf(int32_t stick_angle)
     // don't let angle error grow too large
     angle_error = constrain_float(angle_error, -MAX_YAW_OVERSHOOT, MAX_YAW_OVERSHOOT);
 
+#if FRAME_CONFIG == HELI_FRAME
+    if (!motors.motor_runup_complete()) {
+           angle_error = 0;
+    }
+#else
     if (!motors.armed() || g.rc_3.servo_out == 0) {
         angle_error = 0;
     }
+#endif // HELI_FRAME
 }
 
 // Roll with rate input and stabilized in the earth frame
@@ -299,7 +312,7 @@ get_roll_rate_stabilized_ef(int32_t stick_angle)
     }
 
 #if FRAME_CONFIG == HELI_FRAME
-    if (!motors.motor_runup_complete) {
+    if (!motors.motor_runup_complete()) {
         angle_error = 0;
     }
 #else      
@@ -340,7 +353,7 @@ get_pitch_rate_stabilized_ef(int32_t stick_angle)
     }
 
 #if FRAME_CONFIG == HELI_FRAME
-    if (!motors.motor_runup_complete) {
+    if (!motors.motor_runup_complete()) {
         angle_error = 0;
     }
 #else       
@@ -378,7 +391,7 @@ get_yaw_rate_stabilized_ef(int32_t stick_angle)
     angle_error	= constrain_int32(angle_error, -MAX_YAW_OVERSHOOT, MAX_YAW_OVERSHOOT);
 
 #if FRAME_CONFIG == HELI_FRAME
-    if (!motors.motor_runup_complete) {
+    if (!motors.motor_runup_complete()) {
     	angle_error = 0;
     }
 #else   
@@ -447,10 +460,15 @@ update_rate_contoller_targets()
 void
 run_rate_controllers()
 {
-#if FRAME_CONFIG == HELI_FRAME          // helicopters only use rate controllers for yaw and only when not using an external gyro
+#if FRAME_CONFIG == HELI_FRAME
-    if(!motors.ext_gyro_enabled) {
+    // convert desired roll and pitch rate to roll and pitch swash angles
     heli_integrated_swash_controller(roll_rate_target_bf, pitch_rate_target_bf);
+    // helicopters only use rate controllers for yaw and only when not using an external gyro
+    if(motors.tail_type() != AP_MOTORS_HELI_TAILTYPE_SERVO_EXTGYRO) {
         g.rc_4.servo_out = get_heli_rate_yaw(yaw_rate_target_bf);
+    }else{
+        // do not use rate controllers for helicotpers with external gyros
+        g.rc_4.servo_out = constrain_int32(yaw_rate_target_bf, -4500, 4500);
     }
 #else
     // call rate controllers
@@ -465,143 +483,6 @@ run_rate_controllers()
     }
 }
 
-#if FRAME_CONFIG == HELI_FRAME
-// init_rate_controllers - set-up filters for rate controller inputs
-void init_rate_controllers()
-{
-   // initalise low pass filters on rate controller inputs
-   // 1st parameter is time_step, 2nd parameter is time_constant
-   // rate_roll_filter.set_cutoff_frequency(0.01f, 0.1f);
-   // rate_pitch_filter.set_cutoff_frequency(0.01f, 0.1f);
-}
-
-static void heli_integrated_swash_controller(int32_t target_roll_rate, int32_t target_pitch_rate)
-{
-    int32_t         roll_p, roll_i, roll_d, roll_ff;            // used to capture pid values for logging
-    int32_t         pitch_p, pitch_i, pitch_d, pitch_ff;
-	int32_t         current_roll_rate, current_pitch_rate;	    // this iteration's rate
-    int32_t         roll_rate_error, pitch_rate_error;          // simply target_rate - current_rate
-    int32_t         roll_output, pitch_output;                  // output from pid controller
-    static bool     roll_pid_saturated, pitch_pid_saturated;    // tracker from last loop if the PID was saturated
-    
-    current_roll_rate = (omega.x * DEGX100);                    // get current roll rate
-    current_pitch_rate = (omega.y * DEGX100);                   // get current pitch rate
-	
-    roll_rate_error = target_roll_rate - current_roll_rate;
-    pitch_rate_error = target_pitch_rate - current_pitch_rate;
-    
-    roll_p = g.pid_rate_roll.get_p(roll_rate_error);
-    pitch_p = g.pid_rate_pitch.get_p(pitch_rate_error);
-
-    if (roll_pid_saturated){
-        roll_i = g.pid_rate_roll.get_integrator();                                                      // Locked Integrator due to PID saturation on previous cycle
-    } else {
-        if (motors.flybar_mode == 1) {												                    // Mechanical Flybars get regular integral for rate auto trim
-            if (target_roll_rate > -50 && target_roll_rate < 50){								        // Frozen at high rates
-                roll_i = g.pid_rate_roll.get_i(roll_rate_error, G_Dt);
-            } else {
-                roll_i = g.pid_rate_roll.get_integrator();
-            }
-        } else {
-            roll_i = g.pid_rate_roll.get_leaky_i(roll_rate_error, G_Dt, RATE_INTEGRATOR_LEAK_RATE);	    // Flybarless Helis get huge I-terms. I-term controls much of the rate
-        }
-    }
-    
-    if (pitch_pid_saturated){
-        pitch_i = g.pid_rate_pitch.get_integrator();                                                    // Locked Integrator due to PID saturation on previous cycle
-    } else {
-        if (motors.flybar_mode == 1) {												                    // Mechanical Flybars get regular integral for rate auto trim
-            if (target_pitch_rate > -50 && target_pitch_rate < 50){								        // Frozen at high rates
-                pitch_i = g.pid_rate_pitch.get_i(pitch_rate_error, G_Dt);
-            } else {
-                pitch_i = g.pid_rate_pitch.get_integrator();
-            }
-        } else {
-            pitch_i = g.pid_rate_pitch.get_leaky_i(pitch_rate_error, G_Dt, RATE_INTEGRATOR_LEAK_RATE);	// Flybarless Helis get huge I-terms. I-term controls much of the rate
-        }
-    }
-	
-	roll_d = g.pid_rate_roll.get_d(target_roll_rate, G_Dt);
-	pitch_d = g.pid_rate_pitch.get_d(target_pitch_rate, G_Dt);
-    
-	roll_ff = g.heli_roll_ff * target_roll_rate;
-    pitch_ff = g.heli_pitch_ff * target_pitch_rate;
-
-    // Joly, I think your PC and CC code goes here
-    
-    roll_output = roll_p + roll_i + roll_d + roll_ff;
-    pitch_output = pitch_p + pitch_i + pitch_d + pitch_ff;
-
-    if (labs(roll_output) > 4500){
-        roll_output = constrain_int32(roll_output, -4500, 4500);         // constrain output
-        roll_pid_saturated = true;                                       // freeze integrator next cycle
-    } else {
-        roll_pid_saturated = false;                                      // unfreeze integrator
-    }
-    
-    if (labs(pitch_output) > 4500){
-        pitch_output = constrain_int32(pitch_output, -4500, 4500);        // constrain output
-        pitch_pid_saturated = true;                                       // freeze integrator next cycle
-    } else {
-        pitch_pid_saturated = false;                                      // unfreeze integrator
-    }
-
-    g.rc_1.servo_out = roll_output;
-	g.rc_2.servo_out = pitch_output;
-}
-
-static int16_t
-get_heli_rate_yaw(int32_t target_rate)
-{
-    int32_t         p,i,d,ff;               // used to capture pid values for logging
-	int32_t         current_rate;           // this iteration's rate
-    int32_t         rate_error;
-    int32_t         output;
-    static bool     pid_saturated;          // tracker from last loop if the PID was saturated
-
-    current_rate = (omega.z * DEGX100);                         // get current rate
-	
-    // rate control
-    rate_error = target_rate - current_rate;
-
-    // separately calculate p, i, d values for logging
-    p = g.pid_rate_yaw.get_p(rate_error);
-    
-    if (pid_saturated){
-        i = g.pid_rate_yaw.get_integrator();                    // Locked Integrator due to PID saturation on previous cycle
-    } else {
-        i = g.pid_rate_yaw.get_i(rate_error, G_Dt);
-    }
-
-    d = g.pid_rate_yaw.get_d(rate_error, G_Dt);
-	
-	ff = g.heli_yaw_ff*target_rate;
-
-    output = p + i + d + ff;
-    
-    if (labs(output) > 4500){
-        output = constrain_int32(output, -4500, 4500);          // constrain output
-        pid_saturated = true;                                   // freeze integrator next cycle
-    } else {
-        pid_saturated = false;                                  // unfreeze integrator
-    }
-
-#if LOGGING_ENABLED == ENABLED
-    // log output if PID loggins is on and we are tuning the yaw
-    if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_YAW_RATE_KP || g.radio_tuning == CH6_YAW_RATE_KD) ) {
-        pid_log_counter++;
-        if( pid_log_counter >= 10 ) {               // (update rate / desired output rate) = (100hz / 10hz) = 10
-            pid_log_counter = 0;
-            Log_Write_PID(CH6_YAW_RATE_KP, rate_error, p, i, d, output, tuning_value);
-        }
-    }
-	
-#endif
-
-	return output;                                              // output control
-}
-#endif // HELI_FRAME
-
 #if FRAME_CONFIG != HELI_FRAME
 static int16_t
 get_rate_roll(int32_t target_rate)
@@ -922,7 +803,7 @@ static int16_t get_angle_boost(int16_t throttle)
 {
     float angle_boost_factor = cos_pitch_x * cos_roll_x;
     angle_boost_factor = 1.0f - constrain_float(angle_boost_factor, .5f, 1.0f);
-    int16_t throttle_above_mid = max(throttle - motors.throttle_mid,0);
+    int16_t throttle_above_mid = max(throttle - motors.get_collective_mid(),0);
 
     // to allow logging of angle boost
     angle_boost = throttle_above_mid*angle_boost_factor;
@@ -988,9 +869,8 @@ static void
 set_throttle_takeoff()
 {
     // set alt target
-    if (controller_desired_alt < current_loc.alt) {
     controller_desired_alt = current_loc.alt + ALT_HOLD_TAKEOFF_JUMP;
-    }
+
     // clear i term from acceleration controller
     if (g.pid_throttle_accel.get_integrator() < 0) {
         g.pid_throttle_accel.reset_I();
@@ -1037,8 +917,6 @@ get_throttle_accel(int16_t z_target_accel)
 
     d = g.pid_throttle_accel.get_d(z_accel_error, .01f);
 
-    //
-    // limit the rate
     output =  constrain_float(p+i+d+g.throttle_cruise, g.throttle_min, g.throttle_max);
 
 #if LOGGING_ENABLED == ENABLED

ArduCopter/GCS.h

@@ -8,7 +8,9 @@
 #define __GCS_H
 
 #include <AP_HAL.h>
+#include <AP_Common.h>
 #include <GPS.h>
+#include <stdint.h>
 
 ///
 /// @class	GCS
@@ -39,7 +41,6 @@ public:
     ///
     void        init(AP_HAL::UARTDriver *port) {
         _port = port;
-        initialised = true;
     }
 
     /// Update GCS state.
@@ -60,6 +61,14 @@ public:
     void        send_message(enum ap_message id) {
     }
 
+    /// Send a text message.
+    ///
+    /// @param	severity	A value describing the importance of the message.
+    /// @param	str			The text to be sent.
+    ///
+    void        send_text(gcs_severity severity, const char *str) {
+    }
+
     /// Send a text message with a PSTR()
     ///
     /// @param	severity	A value describing the importance of the message.
@@ -122,6 +131,10 @@ public:
     // see if we should send a stream now. Called at 50Hz
     bool        stream_trigger(enum streams stream_num);
 
+	// this costs us 51 bytes per instance, but means that low priority
+	// messages don't block the CPU
+    mavlink_statustext_t pending_status;
+
     // call to reset the timeout window for entering the cli
     void reset_cli_timeout();
 private:
@@ -140,6 +153,7 @@ private:
     uint16_t                    _queued_parameter_count; ///< saved count of
                                                          // parameters for
                                                          // queued send
+    uint32_t                    _queued_parameter_send_time_ms;
 
     /// Count the number of reportable parameters.
     ///
@@ -179,16 +193,8 @@ private:
     uint16_t        waypoint_send_timeout; // milliseconds
     uint16_t        waypoint_receive_timeout; // milliseconds
 
-    // data stream rates
+    // saveable rate of each stream
-    AP_Int16        streamRateRawSensors;
+    AP_Int16        streamRates[NUM_STREAMS];
-    AP_Int16        streamRateExtendedStatus;
-    AP_Int16        streamRateRCChannels;
-    AP_Int16        streamRateRawController;
-    AP_Int16        streamRatePosition;
-    AP_Int16        streamRateExtra1;
-    AP_Int16        streamRateExtra2;
-    AP_Int16        streamRateExtra3;
-    AP_Int16        streamRateParams;
 
     // number of 50Hz ticks until we next send this stream
     uint8_t         stream_ticks[NUM_STREAMS];

ArduCopter/GCS_Mavlink.pde

@@ -6,11 +6,6 @@
 // use this to prevent recursion during sensor init
 static bool in_mavlink_delay;
 
-
-// this costs us 51 bytes, but means that low priority
-// messages don't block the CPU
-static mavlink_statustext_t pending_status;
-
 // true when we have received at least 1 MAVLink packet
 static bool mavlink_active;
 
@@ -105,6 +100,8 @@ static NOINLINE void send_heartbeat(mavlink_channel_t chan)
         MAV_TYPE_OCTOROTOR,
 #elif (FRAME_CONFIG == HELI_FRAME)
         MAV_TYPE_HELICOPTER,
+#elif (FRAME_CONFIG == SINGLE_FRAME)  //because mavlink did not define a singlecopter, we use a rocket
+        MAV_TYPE_ROCKET,
 #else
   #error Unrecognised frame type
 #endif
@@ -156,6 +153,9 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
         control_sensors_present |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW;
     }
 #endif
+    if (ap.rc_receiver_present) {
+        control_sensors_present |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER;
+    }
 
     // all present sensors enabled by default except altitude and position control which we will set individually
     control_sensors_enabled = control_sensors_present & (~MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL & ~MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL);
@@ -180,14 +180,20 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
         break;
     }
 
-    // default to all healthy except compass and gps which we set individually
+    // default to all healthy except compass, gps and receiver which we set individually
-    control_sensors_health = control_sensors_present & (~MAV_SYS_STATUS_SENSOR_3D_MAG & ~MAV_SYS_STATUS_SENSOR_GPS);
+    control_sensors_health = control_sensors_present & (~MAV_SYS_STATUS_SENSOR_3D_MAG & ~MAV_SYS_STATUS_SENSOR_GPS & ~MAV_SYS_STATUS_SENSOR_RC_RECEIVER);
     if (g.compass_enabled && compass.healthy && ahrs.use_compass()) {
         control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG;
     }
     if (g_gps != NULL && g_gps->status() > GPS::NO_GPS && !gps_glitch.glitching()) {
         control_sensors_health |= MAV_SYS_STATUS_SENSOR_GPS;
     }
+    if (ap.rc_receiver_present && !failsafe.radio) {
+        control_sensors_health |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER;
+    }
+    if (!ins.healthy()) {
+        control_sensors_health &= ~(MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL);
+    }
 
     int16_t battery_current = -1;
     int8_t battery_remaining = -1;
@@ -307,6 +313,14 @@ static void NOINLINE send_gps_raw(mavlink_channel_t chan)
 
 }
 
+static void NOINLINE send_system_time(mavlink_channel_t chan)
+{
+    mavlink_msg_system_time_send(
+        chan,
+        g_gps->time_epoch_usec(),
+        hal.scheduler->millis());
+}
+
 #if HIL_MODE != HIL_MODE_DISABLED
 static void NOINLINE send_servo_out(mavlink_channel_t chan)
 {
@@ -455,9 +469,9 @@ static void NOINLINE send_raw_imu2(mavlink_channel_t chan)
 
 static void NOINLINE send_raw_imu3(mavlink_channel_t chan)
 {
-    Vector3f mag_offsets = compass.get_offsets();
+    const Vector3f &mag_offsets = compass.get_offsets();
-    Vector3f accel_offsets = ins.get_accel_offsets();
+    const Vector3f &accel_offsets = ins.get_accel_offsets();
-    Vector3f gyro_offsets = ins.get_gyro_offsets();
+    const Vector3f &gyro_offsets = ins.get_gyro_offsets();
 
     mavlink_msg_sensor_offsets_send(chan,
                                     mag_offsets.x,
@@ -483,10 +497,11 @@ static void NOINLINE send_current_waypoint(mavlink_channel_t chan)
 
 static void NOINLINE send_statustext(mavlink_channel_t chan)
 {
+    mavlink_statustext_t *s = &gcs[chan-MAVLINK_COMM_0].pending_status;
     mavlink_msg_statustext_send(
         chan,
-        pending_status.severity,
+        s->severity,
-        pending_status.text);
+        s->text);
 }
 
 // are we still delaying telemetry to try to avoid Xbee bricking?
@@ -561,6 +576,11 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id,
         send_gps_raw(chan);
         break;
 
+    case MSG_SYSTEM_TIME:
+        CHECK_PAYLOAD_SIZE(SYSTEM_TIME);
+        send_system_time(chan);
+        break;
+
     case MSG_SERVO_OUT:
 #if HIL_MODE != HIL_MODE_DISABLED
         CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED);
@@ -605,20 +625,12 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id,
 
     case MSG_NEXT_PARAM:
         CHECK_PAYLOAD_SIZE(PARAM_VALUE);
-        if (chan == MAVLINK_COMM_0) {
+        gcs[chan-MAVLINK_COMM_0].queued_param_send();
-            gcs0.queued_param_send();
-        } else if (gcs3.initialised) {
-            gcs3.queued_param_send();
-        }
         break;
 
     case MSG_NEXT_WAYPOINT:
         CHECK_PAYLOAD_SIZE(MISSION_REQUEST);
-        if (chan == MAVLINK_COMM_0) {
+        gcs[chan-MAVLINK_COMM_0].queued_waypoint_send();
-            gcs0.queued_waypoint_send();
-        } else if (gcs3.initialised) {
-            gcs3.queued_waypoint_send();
-        }
         break;
 
     case MSG_STATUSTEXT:
@@ -663,7 +675,7 @@ static struct mavlink_queue {
     enum ap_message deferred_messages[MAX_DEFERRED_MESSAGES];
     uint8_t next_deferred_message;
     uint8_t num_deferred_messages;
-} mavlink_queue[2];
+} mavlink_queue[MAVLINK_COMM_NUM_BUFFERS];
 
 // send a message using mavlink
 static void mavlink_send_message(mavlink_channel_t chan, enum ap_message id, uint16_t packet_drops)
@@ -725,15 +737,13 @@ void mavlink_send_text(mavlink_channel_t chan, gcs_severity severity, const char
 
     if (severity == SEVERITY_LOW) {
         // send via the deferred queuing system
-        pending_status.severity = (uint8_t)severity;
+        mavlink_statustext_t *s = &gcs[chan-MAVLINK_COMM_0].pending_status;
-        strncpy((char *)pending_status.text, str, sizeof(pending_status.text));
+        s->severity = (uint8_t)severity;
+        strncpy((char *)s->text, str, sizeof(s->text));
         mavlink_send_message(chan, MSG_STATUSTEXT, 0);
     } else {
         // send immediately
-        mavlink_msg_statustext_send(
+        mavlink_msg_statustext_send(chan, severity, str);
-            chan,
-            severity,
-            str);
     }
 }
 
@@ -745,7 +755,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] PROGMEM = {
     // @Range: 0 10
     // @Increment: 1
     // @User: Advanced
-    AP_GROUPINFO("RAW_SENS", 0, GCS_MAVLINK, streamRateRawSensors,      0),
+    AP_GROUPINFO("RAW_SENS", 0, GCS_MAVLINK, streamRates[0],  0),
 
     // @Param: EXT_STAT
     // @DisplayName: Extended status stream rate to ground station
@@ -754,7 +764,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] PROGMEM = {
     // @Range: 0 10
     // @Increment: 1
     // @User: Advanced
-    AP_GROUPINFO("EXT_STAT", 1, GCS_MAVLINK, streamRateExtendedStatus,  0),
+    AP_GROUPINFO("EXT_STAT", 1, GCS_MAVLINK, streamRates[1],  0),
 
     // @Param: RC_CHAN
     // @DisplayName: RC Channel stream rate to ground station
@@ -763,7 +773,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] PROGMEM = {
     // @Range: 0 10
     // @Increment: 1
     // @User: Advanced
-    AP_GROUPINFO("RC_CHAN",  2, GCS_MAVLINK, streamRateRCChannels,      0),
+    AP_GROUPINFO("RC_CHAN",  2, GCS_MAVLINK, streamRates[2],  0),
 
     // @Param: RAW_CTRL
     // @DisplayName: Raw Control stream rate to ground station
@@ -772,7 +782,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] PROGMEM = {
     // @Range: 0 10
     // @Increment: 1
     // @User: Advanced
-    AP_GROUPINFO("RAW_CTRL", 3, GCS_MAVLINK, streamRateRawController,   0),
+    AP_GROUPINFO("RAW_CTRL", 3, GCS_MAVLINK, streamRates[3],  0),
 
     // @Param: POSITION
     // @DisplayName: Position stream rate to ground station
@@ -781,7 +791,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] PROGMEM = {
     // @Range: 0 10
     // @Increment: 1
     // @User: Advanced
-    AP_GROUPINFO("POSITION", 4, GCS_MAVLINK, streamRatePosition,        0),
+    AP_GROUPINFO("POSITION", 4, GCS_MAVLINK, streamRates[4],  0),
 
     // @Param: EXTRA1
     // @DisplayName: Extra data type 1 stream rate to ground station
@@ -790,7 +800,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] PROGMEM = {
     // @Range: 0 10
     // @Increment: 1
     // @User: Advanced
-    AP_GROUPINFO("EXTRA1",   5, GCS_MAVLINK, streamRateExtra1,          0),
+    AP_GROUPINFO("EXTRA1",   5, GCS_MAVLINK, streamRates[5],  0),
 
     // @Param: EXTRA2
     // @DisplayName: Extra data type 2 stream rate to ground station
@@ -799,7 +809,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] PROGMEM = {
     // @Range: 0 10
     // @Increment: 1
     // @User: Advanced
-    AP_GROUPINFO("EXTRA2",   6, GCS_MAVLINK, streamRateExtra2,          0),
+    AP_GROUPINFO("EXTRA2",   6, GCS_MAVLINK, streamRates[6],  0),
 
     // @Param: EXTRA3
     // @DisplayName: Extra data type 3 stream rate to ground station
@@ -808,7 +818,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] PROGMEM = {
     // @Range: 0 10
     // @Increment: 1
     // @User: Advanced
-    AP_GROUPINFO("EXTRA3",   7, GCS_MAVLINK, streamRateExtra3,          0),
+    AP_GROUPINFO("EXTRA3",   7, GCS_MAVLINK, streamRates[7],  0),
 
     // @Param: PARAMS
     // @DisplayName: Parameter stream rate to ground station
@@ -817,7 +827,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] PROGMEM = {
     // @Range: 0 10
     // @Increment: 1
     // @User: Advanced
-    AP_GROUPINFO("PARAMS",   8, GCS_MAVLINK, streamRateParams,          0),
+    AP_GROUPINFO("PARAMS",   8, GCS_MAVLINK, streamRates[8],  0),
     AP_GROUPEND
 };
 
@@ -834,12 +844,20 @@ void
 GCS_MAVLINK::init(AP_HAL::UARTDriver* port)
 {
     GCS_Class::init(port);
-    if (port == hal.uartA) {
+    if (port == (AP_HAL::BetterStream*)hal.uartA) {
         mavlink_comm_0_port = port;
         chan = MAVLINK_COMM_0;
-    }else{
+        initialised = true;
+    } else if (port == (AP_HAL::BetterStream*)hal.uartC) {
         mavlink_comm_1_port = port;
         chan = MAVLINK_COMM_1;
+        initialised = true;
+#if MAVLINK_COMM_NUM_BUFFERS > 2
+    } else if (port == (AP_HAL::BetterStream*)hal.uartD) {
+        mavlink_comm_2_port = port;
+        chan = MAVLINK_COMM_2;
+        initialised = true;
+#endif
     }
     _queued_parameter = NULL;
     reset_cli_timeout();
@@ -855,7 +873,8 @@ GCS_MAVLINK::update(void)
 
     // process received bytes
     uint16_t nbytes = comm_get_available(chan);
-    for (uint16_t i=0; i<nbytes; i++) {
+    for (uint16_t i=0; i<nbytes; i++)
+    {
         uint8_t c = comm_receive_ch(chan);
 
 #if CLI_ENABLED == ENABLED
@@ -915,40 +934,19 @@ GCS_MAVLINK::update(void)
 // see if we should send a stream now. Called at 50Hz
 bool GCS_MAVLINK::stream_trigger(enum streams stream_num)
 {
-    uint8_t rate;
+    if (stream_num >= NUM_STREAMS) {
-    switch (stream_num) {
+        return false;
-        case STREAM_RAW_SENSORS:
+    }
-            rate = streamRateRawSensors.get();
+    float rate = (uint8_t)streamRates[stream_num].get();
-            break;
+
-        case STREAM_EXTENDED_STATUS:
+    // send at a much lower rate while handling waypoints and
-            rate = streamRateExtendedStatus.get();
+    // parameter sends
-            break;
+    if ((stream_num != STREAM_PARAMS) && 
-        case STREAM_RC_CHANNELS:
+        (waypoint_receiving || _queued_parameter != NULL)) {
-            rate = streamRateRCChannels.get();
+        rate *= 0.25;
-            break;
-        case STREAM_RAW_CONTROLLER:
-            rate = streamRateRawController.get();
-            break;
-        case STREAM_POSITION:
-            rate = streamRatePosition.get();
-            break;
-        case STREAM_EXTRA1:
-            rate = streamRateExtra1.get();
-            break;
-        case STREAM_EXTRA2:
-            rate = streamRateExtra2.get();
-            break;
-        case STREAM_EXTRA3:
-            rate = streamRateExtra3.get();
-            break;
-        case STREAM_PARAMS:
-            rate = streamRateParams.get();
-            break;
-        default:
-            rate = 0;
     }
 
-    if (rate == 0) {
+    if (rate <= 0) {
         return false;
     }
 
@@ -977,8 +975,8 @@ GCS_MAVLINK::data_stream_send(void)
     gcs_out_of_time = false;
 
     if (_queued_parameter != NULL) {
-        if (streamRateParams.get() <= 0) {
+        if (streamRates[STREAM_PARAMS].get() <= 0) {
-            streamRateParams.set(50);
+            streamRates[STREAM_PARAMS].set(10);
         }
         if (stream_trigger(STREAM_PARAMS)) {
             send_message(MSG_NEXT_PARAM);
@@ -1048,6 +1046,7 @@ GCS_MAVLINK::data_stream_send(void)
     if (stream_trigger(STREAM_EXTRA3)) {
         send_message(MSG_AHRS);
         send_message(MSG_HWSTATUS);
+        send_message(MSG_SYSTEM_TIME);
     }
 }
 
@@ -1077,9 +1076,10 @@ GCS_MAVLINK::send_text_P(gcs_severity severity, const prog_char_t *str)
 void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 {
     struct Location tell_command = {};                                  // command for telemetry
+
     switch (msg->msgid) {
 
-    case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:     //66
+    case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
     {
         // decode
         mavlink_request_data_stream_t packet;
@@ -1100,32 +1100,22 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
         switch(packet.req_stream_id) {
 
         case MAV_DATA_STREAM_ALL:
-            streamRateRawSensors            = freq;
+            // note that we don't set STREAM_PARAMS - that is internal only
-            streamRateExtendedStatus        = freq;
+            for (uint8_t i=0; i<STREAM_PARAMS; i++) {
-            streamRateRCChannels            = freq;
+                streamRates[i].set(freq);
-            streamRateRawController         = freq;
+            }
-            streamRatePosition                      = freq;
-            streamRateExtra1                        = freq;
-            streamRateExtra2                        = freq;
-            //streamRateExtra3.set_and_save(freq);	// We just do set and save on the last as it takes care of the whole group.
-            streamRateExtra3                        = freq;                             // Don't save!!
             break;
-
         case MAV_DATA_STREAM_RAW_SENSORS:
-            streamRateRawSensors = freq;                                        // We do not set and save this one so that if HIL is shut down incorrectly
+            streamRates[STREAM_RAW_SENSORS].set(freq);
-            // we will not continue to broadcast raw sensor data at 50Hz.
             break;
         case MAV_DATA_STREAM_EXTENDED_STATUS:
-            //streamRateExtendedStatus.set_and_save(freq);
+            streamRates[STREAM_EXTENDED_STATUS].set(freq);
-            streamRateExtendedStatus = freq;
             break;
-
         case MAV_DATA_STREAM_RC_CHANNELS:
-            streamRateRCChannels  = freq;
+            streamRates[STREAM_RC_CHANNELS].set(freq);
             break;
-
         case MAV_DATA_STREAM_RAW_CONTROLLER:
-            streamRateRawController = freq;
+            streamRates[STREAM_RAW_CONTROLLER].set(freq);
             break;
 
         //case MAV_DATA_STREAM_RAW_SENSOR_FUSION:
@@ -1133,22 +1123,16 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
         //	break;
 
         case MAV_DATA_STREAM_POSITION:
-            streamRatePosition = freq;
+            streamRates[STREAM_POSITION].set(freq);
             break;
-
         case MAV_DATA_STREAM_EXTRA1:
-            streamRateExtra1 = freq;
+            streamRates[STREAM_EXTRA1].set(freq);
             break;
-
         case MAV_DATA_STREAM_EXTRA2:
-            streamRateExtra2 = freq;
+            streamRates[STREAM_EXTRA2].set(freq);
             break;
-
         case MAV_DATA_STREAM_EXTRA3:
-            streamRateExtra3 = freq;
+            streamRates[STREAM_EXTRA3].set(freq);
-            break;
-
-        default:
             break;
         }
         break;
@@ -1190,11 +1174,15 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 
         case MAV_CMD_PREFLIGHT_CALIBRATION:
             if (packet.param1 == 1 ||
-                packet.param2 == 1 ||
+                packet.param2 == 1) {
-                packet.param3 == 1) {
                 ins.init_accel();
                 ahrs.set_trim(Vector3f(0,0,0));             // clear out saved trim
             } 
+            if (packet.param3 == 1) {
+#if HIL_MODE != HIL_MODE_ATTITUDE
+                init_barometer();
+#endif
+            }
             if (packet.param4 == 1) {
                 trim_radio();
             }
@@ -1252,7 +1240,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
         break;
     }
 
-    case MAVLINK_MSG_ID_SET_MODE:      //11
+
+    case MAVLINK_MSG_ID_SET_MODE:
     {
         // decode
         mavlink_set_mode_t packet;
@@ -1280,8 +1269,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
      */
     case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:     //43
     {
-        //send_text_P(SEVERITY_LOW,PSTR("waypoint request list"));
-
         // decode
         mavlink_mission_request_list_t packet;
         mavlink_msg_mission_request_list_decode(msg, &packet);
@@ -1305,8 +1292,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
     // XXX read a WP from EEPROM and send it to the GCS
     case MAVLINK_MSG_ID_MISSION_REQUEST:     // 40
     {
-        //send_text_P(SEVERITY_LOW,PSTR("waypoint request"));
-
         // Check if sending waypiont
         //if (!waypoint_sending) break;
         // 5/10/11 - We are trying out relaxing the requirement that we be in waypoint sending mode to respond to a waypoint request.  DEW
@@ -1362,6 +1347,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
         case MAV_CMD_NAV_ROI:
         case MAV_CMD_DO_SET_ROI:
             param1 = tell_command.p1;                        // MAV_ROI (aka roi mode) is held in wp's parameter but we actually do nothing with it because we only support pointing at a specific location provided by x,y and z parameters
+            x = tell_command.lat/1.0e7f;                     // local (x), global (latitude)
+            y = tell_command.lng/1.0e7f;                     // local (y), global (longitude)
+            z = tell_command.alt/1.0e2f;                     // local (z), global/relative (altitude)
             break;
 
         case MAV_CMD_CONDITION_YAW:
@@ -1434,10 +1422,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
         break;
     }
 
-    case MAVLINK_MSG_ID_MISSION_ACK:     //47
-    {
-        //send_text_P(SEVERITY_LOW,PSTR("waypoint ack"));
 
+    case MAVLINK_MSG_ID_MISSION_ACK:
+    {
         // decode
         mavlink_mission_ack_t packet;
         mavlink_msg_mission_ack_decode(msg, &packet);
@@ -1448,17 +1435,17 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
         break;
     }
 
-    case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:     // 21
+    case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
     {
-        // gcs_send_text_P(SEVERITY_LOW,PSTR("param request list"));
-
         // decode
         mavlink_param_request_list_t packet;
         mavlink_msg_param_request_list_decode(msg, &packet);
         if (mavlink_check_target(packet.target_system,packet.target_component)) break;
 
-        // Start sending parameters - next call to ::update will kick the first one out
+        // mark the firmware version in the tlog
+        send_text_P(SEVERITY_LOW, PSTR(FIRMWARE_STRING));
 
+        // Start sending parameters - next call to ::update will kick the first one out
         _queued_parameter = AP_Param::first(&_queued_parameter_token, &_queued_parameter_type);
         _queued_parameter_index = 0;
         _queued_parameter_count = _count_parameters();
@@ -1504,10 +1491,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
         break;
     }
 
-    case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:     // 45
+    case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:
     {
-        //send_text_P(SEVERITY_LOW,PSTR("waypoint clear all"));
-
         // decode
         mavlink_mission_clear_all_t packet;
         mavlink_msg_mission_clear_all_decode(msg, &packet);
@@ -1524,10 +1509,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
         break;
     }
 
-    case MAVLINK_MSG_ID_MISSION_SET_CURRENT:     // 41
+    case MAVLINK_MSG_ID_MISSION_SET_CURRENT:
     {
-        //send_text_P(SEVERITY_LOW,PSTR("waypoint set current"));
-
         // decode
         mavlink_mission_set_current_t packet;
         mavlink_msg_mission_set_current_decode(msg, &packet);
@@ -1540,10 +1523,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
         break;
     }
 
-    case MAVLINK_MSG_ID_MISSION_COUNT:     // 44
+    case MAVLINK_MSG_ID_MISSION_COUNT:
     {
-        //send_text_P(SEVERITY_LOW,PSTR("waypoint count"));
-
         // decode
         mavlink_mission_count_t packet;
         mavlink_msg_mission_count_decode(msg, &packet);
@@ -1603,7 +1584,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 #endif
 
     // XXX receive a WP from GCS and store in EEPROM
-    case MAVLINK_MSG_ID_MISSION_ITEM:     //39
+    case MAVLINK_MSG_ID_MISSION_ITEM:
     {
         // decode
         uint8_t result = MAV_MISSION_ACCEPTED;
@@ -1863,7 +1844,7 @@ mission_failed:
         break;
     }             // end case
 
-    case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: //70
+    case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE:
     {
         // allow override of RC channel values for HIL
         // or for complete GCS control of switch position
@@ -2077,9 +2058,23 @@ GCS_MAVLINK::_count_parameters()
 void
 GCS_MAVLINK::queued_param_send()
 {
-    // Check to see if we are sending parameters
+    if (!initialised || _queued_parameter == NULL) {
-    if (NULL == _queued_parameter) return;
+        return;
+    }
 
+    uint16_t bytes_allowed;
+    uint8_t count;
+    uint32_t tnow = millis();
+
+    // use at most 30% of bandwidth on parameters. The constant 26 is
+    // 1/(1000 * 1/8 * 0.001 * 0.3)
+    bytes_allowed = g.serial1_baud * (tnow - _queued_parameter_send_time_ms) * 26;
+    if (bytes_allowed > comm_get_txspace(chan)) {
+        bytes_allowed = comm_get_txspace(chan);
+    }
+    count = bytes_allowed / (MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES);
+
+    while (_queued_parameter != NULL && count--) {
     AP_Param      *vp;
     float value;
 
@@ -2103,6 +2098,8 @@ GCS_MAVLINK::queued_param_send()
     _queued_parameter = AP_Param::next_scalar(&_queued_parameter_token, &_queued_parameter_type);
     _queued_parameter_index++;
 }
+    _queued_parameter_send_time_ms = tnow;
+}
 
 /**
  * queued_waypoint_send - Send the next pending waypoint, called from deferred message
@@ -2134,8 +2131,7 @@ void GCS_MAVLINK::reset_cli_timeout() {
 static void mavlink_delay_cb()
 {
     static uint32_t last_1hz, last_50hz, last_5s;
-
+    if (!gcs[0].initialised) return;
-    if (!gcs0.initialised) return;
 
     in_mavlink_delay = true;
 
@@ -2166,9 +2162,10 @@ static void mavlink_delay_cb()
  */
 static void gcs_send_message(enum ap_message id)
 {
-    gcs0.send_message(id);
+    for (uint8_t i=0; i<num_gcs; i++) {
-    if (gcs3.initialised) {
+        if (gcs[i].initialised) {
-        gcs3.send_message(id);
+            gcs[i].send_message(id);
+        }
     }
 }
 
@@ -2177,9 +2174,10 @@ static void gcs_send_message(enum ap_message id)
  */
 static void gcs_data_stream_send(void)
 {
-    gcs0.data_stream_send();
+    for (uint8_t i=0; i<num_gcs; i++) {
-    if (gcs3.initialised) {
+        if (gcs[i].initialised) {
-        gcs3.data_stream_send();
+            gcs[i].data_stream_send();
+        }
     }
 }
 
@@ -2188,17 +2186,19 @@ static void gcs_data_stream_send(void)
  */
 static void gcs_check_input(void)
 {
-    gcs0.update();
+    for (uint8_t i=0; i<num_gcs; i++) {
-    if (gcs3.initialised) {
+        if (gcs[i].initialised) {
-        gcs3.update();
+            gcs[i].update();
+        }
     }
 }
 
 static void gcs_send_text_P(gcs_severity severity, const prog_char_t *str)
 {
-    gcs0.send_text_P(severity, str);
+    for (uint8_t i=0; i<num_gcs; i++) {
-    if (gcs3.initialised) {
+        if (gcs[i].initialised) {
-        gcs3.send_text_P(severity, str);
+            gcs[i].send_text_P(severity, str);
+        }
     }
 }
 
@@ -2207,17 +2207,19 @@ static void gcs_send_text_P(gcs_severity severity, const prog_char_t *str)
  *  only one fits in the queue, so if you send more than one before the
  *  last one gets into the serial buffer then the old one will be lost
  */
-static void gcs_send_text_fmt(const prog_char_t *fmt, ...)
+void gcs_send_text_fmt(const prog_char_t *fmt, ...)
 {
     va_list arg_list;
-    pending_status.severity = (uint8_t)SEVERITY_LOW;
+    gcs[0].pending_status.severity = (uint8_t)SEVERITY_LOW;
     va_start(arg_list, fmt);
-    hal.util->vsnprintf_P((char *)pending_status.text,
+    hal.util->vsnprintf_P((char *)gcs[0].pending_status.text,
-            sizeof(pending_status.text), fmt, arg_list);
+            sizeof(gcs[0].pending_status.text), fmt, arg_list);
     va_end(arg_list);
     mavlink_send_message(MAVLINK_COMM_0, MSG_STATUSTEXT, 0);
-    if (gcs3.initialised) {
+    for (uint8_t i=1; i<num_gcs; i++) {
-        mavlink_send_message(MAVLINK_COMM_1, MSG_STATUSTEXT, 0);
+        if (gcs[i].initialised) {
+            gcs[i].pending_status = gcs[0].pending_status;
+            mavlink_send_message((mavlink_channel_t)i, MSG_STATUSTEXT, 0);
+        }
     }
 }
-

ArduCopter/Log.pde

@@ -208,7 +208,7 @@ static void Log_Write_AutoTuneDetails(int16_t angle_cd, float rate_cds)
 
 struct PACKED log_Current {
     LOG_PACKET_HEADER;
-    int16_t throttle_in;
+    int16_t throttle_out;
     uint32_t throttle_integrator;
     int16_t battery_voltage;
     int16_t current_amps;
@@ -221,7 +221,7 @@ static void Log_Write_Current()
 {
     struct log_Current pkt = {
         LOG_PACKET_HEADER_INIT(LOG_CURRENT_MSG),
-        throttle_in         : g.rc_3.control_in,
+        throttle_out        : g.rc_3.servo_out,
         throttle_integrator : throttle_integrator,
         battery_voltage     : (int16_t) (battery.voltage() * 100.0f),
         current_amps        : (int16_t) (battery.current_amps() * 100.0f),
@@ -238,8 +238,7 @@ struct PACKED log_Motors {
 #elif FRAME_CONFIG == HEXA_FRAME || FRAME_CONFIG == Y6_FRAME
     int16_t motor_out[6];
 #elif FRAME_CONFIG == HELI_FRAME
-    int16_t motor_out[4];
+    int16_t motor_out[6];
-    int16_t ext_gyro_gain;
 #else        // quads & TRI_FRAME
     int16_t motor_out[4];
 #endif
@@ -270,13 +269,14 @@ static void Log_Write_Motors()
         motor_out   :   {motors.motor_out[AP_MOTORS_MOT_1],
                          motors.motor_out[AP_MOTORS_MOT_2],
                          motors.motor_out[AP_MOTORS_MOT_3],
-                         motors.motor_out[AP_MOTORS_MOT_4]},
+                         motors.motor_out[AP_MOTORS_MOT_4],
-        ext_gyro_gain   : motors.ext_gyro_gain
+                         motors.motor_out[AP_MOTORS_MOT_7],
+                         motors.motor_out[AP_MOTORS_MOT_8]}
 #elif FRAME_CONFIG == TRI_FRAME
         motor_out   :   {motors.motor_out[AP_MOTORS_MOT_1],
                          motors.motor_out[AP_MOTORS_MOT_2],
                          motors.motor_out[AP_MOTORS_MOT_4],
-                         motors.motor_out[g.rc_4.radio_out]}
+                         g.rc_4.radio_out}
 #else // QUAD frame
         motor_out   :   {motors.motor_out[AP_MOTORS_MOT_1],
                          motors.motor_out[AP_MOTORS_MOT_2],
@@ -307,7 +307,7 @@ static void Log_Write_Optflow()
     struct log_Optflow pkt = {
         LOG_PACKET_HEADER_INIT(LOG_OPTFLOW_MSG),
         dx              : optflow.dx,
-        dy              : optflow.dx,
+        dy              : optflow.dy,
         surface_quality : optflow.surface_quality,
         x_cm            : (int16_t) optflow.x_cm,
         y_cm            : (int16_t) optflow.y_cm,
@@ -339,7 +339,7 @@ struct PACKED log_Nav_Tuning {
 // Write an Nav Tuning packet
 static void Log_Write_Nav_Tuning()
 {
-    Vector3f velocity = inertial_nav.get_velocity();
+    const Vector3f &velocity = inertial_nav.get_velocity();
 
     struct log_Nav_Tuning pkt = {
         LOG_PACKET_HEADER_INIT(LOG_NAV_TUNING_MSG),
@@ -427,12 +427,13 @@ struct PACKED log_Performance {
     LOG_PACKET_HEADER;
     uint8_t renorm_count;
     uint8_t renorm_blowup;
-    uint8_t gps_fix_count;
     uint16_t num_long_running;
     uint16_t num_loops;
     uint32_t max_time;
     int16_t  pm_test;
     uint8_t i2c_lockup_count;
+    uint16_t ins_error_count;
+    uint8_t inav_error_count;
 };
 
 // Write a performance monitoring packet
@@ -442,12 +443,13 @@ static void Log_Write_Performance()
         LOG_PACKET_HEADER_INIT(LOG_PERFORMANCE_MSG),
         renorm_count     : ahrs.renorm_range_count,
         renorm_blowup    : ahrs.renorm_blowup_count,
-        gps_fix_count    : gps_fix_count,
         num_long_running : perf_info_get_num_long_running(),
         num_loops        : perf_info_get_num_loops(),
         max_time         : perf_info_get_max_time(),
         pm_test          : pmTest1,
-        i2c_lockup_count : hal.i2c->lockup_count()
+        i2c_lockup_count : hal.i2c->lockup_count(),
+        ins_error_count  : ins.error_count(),
+        inav_error_count : inertial_nav.error_count()
     };
     DataFlash.WriteBlock(&pkt, sizeof(pkt));
 }
@@ -525,7 +527,7 @@ struct PACKED log_INAV {
 // Write an INAV packet
 static void Log_Write_INAV()
 {
-    Vector3f accel_corr = inertial_nav.accel_correction_ef;
+    const Vector3f &accel_corr = inertial_nav.accel_correction_ef;
 
     struct log_INAV pkt = {
         LOG_PACKET_HEADER_INIT(LOG_INAV_MSG),
@@ -715,6 +717,7 @@ static void Log_Write_PID(uint8_t pid_id, int32_t error, int32_t p, int32_t i, i
 struct PACKED log_Camera {
     LOG_PACKET_HEADER;
     uint32_t gps_time;
+    uint16_t gps_week;
     int32_t  latitude;
     int32_t  longitude;
     int32_t  altitude;
@@ -729,7 +732,8 @@ static void Log_Write_Camera()
 #if CAMERA == ENABLED
     struct log_Camera pkt = {
         LOG_PACKET_HEADER_INIT(LOG_CAMERA_MSG),
-        gps_time    : g_gps->time,
+        gps_time    : g_gps->time_week_ms,
+        gps_week    : g_gps->time_week,
         latitude    : current_loc.lat,
         longitude   : current_loc.lng,
         altitude    : current_loc.alt,
@@ -767,7 +771,7 @@ static const struct LogStructure log_structure[] PROGMEM = {
       "ATDE", "cf",          "Angle,Rate" },
 #endif
     { LOG_CURRENT_MSG, sizeof(log_Current),             
-      "CURR", "hIhhhf",      "Thr,ThrInt,Volt,Curr,Vcc,CurrTot" },
+      "CURR", "hIhhhf",      "ThrOut,ThrInt,Volt,Curr,Vcc,CurrTot" },
 
 #if FRAME_CONFIG == OCTA_FRAME || FRAME_CONFIG == OCTA_QUAD_FRAME
     { LOG_MOTORS_MSG, sizeof(log_Motors),       
@@ -792,7 +796,7 @@ static const struct LogStructure log_structure[] PROGMEM = {
     { LOG_COMPASS_MSG, sizeof(log_Compass),             
       "MAG", "hhhhhhhhh",    "MagX,MagY,MagZ,OfsX,OfsY,OfsZ,MOfsX,MOfsY,MOfsZ" },
     { LOG_PERFORMANCE_MSG, sizeof(log_Performance), 
-      "PM",  "BBBHHIhB",       "RenCnt,RenBlw,FixCnt,NLon,NLoop,MaxT,PMT,I2CErr" },
+      "PM",  "BBHHIhBHB",    "RenCnt,RenBlw,NLon,NLoop,MaxT,PMT,I2CErr,INSErr,INAVErr" },
     { LOG_CMD_MSG, sizeof(log_Cmd),                 
       "CMD", "BBBBBeLL",     "CTot,CNum,CId,COpt,Prm1,Alt,Lat,Lng" },
     { LOG_ATTITUDE_MSG, sizeof(log_Attitude),       
@@ -818,7 +822,7 @@ static const struct LogStructure log_structure[] PROGMEM = {
     { LOG_PID_MSG, sizeof(log_PID),         
       "PID",   "Biiiiif",    "Id,Error,P,I,D,Out,Gain" },
     { LOG_CAMERA_MSG, sizeof(log_Camera),                 
-      "CAM",   "ILLeccC",    "GPSTime,Lat,Lng,Alt,Roll,Pitch,Yaw" },
+      "CAM",   "IHLLeccC",   "GPSTime,GPSWeek,Lat,Lng,Alt,Roll,Pitch,Yaw" },
     { LOG_ERROR_MSG, sizeof(log_Error),         
       "ERR",   "BB",         "Subsys,ECode" },
 };
@@ -830,7 +834,7 @@ static void Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page)
     cliSerial->printf_P(PSTR((AIRFRAME_NAME)));
  #endif
 
-    cliSerial->printf_P(PSTR("\n" THISFIRMWARE
+    cliSerial->printf_P(PSTR("\n" FIRMWARE_STRING
                              "\nFree RAM: %u\n"),
                         (unsigned) memcheck_available_memory());
 
@@ -849,6 +853,10 @@ static void start_logging()
     if (g.log_bitmask != 0 && !ap.logging_started) {
         ap.logging_started = true;
         DataFlash.StartNewLog(sizeof(log_structure)/sizeof(log_structure[0]), log_structure);
+        DataFlash.Log_Write_Message_P(PSTR(FIRMWARE_STRING));
+
+        // log the flight mode
+        Log_Write_Mode(control_mode);
     }
 }
 

ArduCopter/Parameters.h

@@ -73,11 +73,7 @@ public:
         k_param_log_last_filenumber,            // *** Deprecated - remove
                                                 // with next eeprom number
                                                 // change
-        k_param_toy_yaw_rate,                           // THOR The memory
+        k_param_toy_yaw_rate,                   // deprecated - remove
-                                                        // location for the
-                                                        // Yaw Rate 1 = fast,
-                                                        // 2 = med, 3 = slow
-
         k_param_crosstrack_min_distance,	// deprecated - remove with next eeprom number change
         k_param_rssi_pin,
         k_param_throttle_accel_enabled,     // deprecated - remove
@@ -92,7 +88,8 @@ public:
         k_param_angle_max,
         k_param_gps_hdop_good,
         k_param_battery,
-        k_param_fs_batt_mah,             // 37
+        k_param_fs_batt_mah,
+        k_param_angle_rate_max,         // 38
 
         // 65: AP_Limits Library
         k_param_limits = 65,            // deprecated - remove
@@ -102,6 +99,14 @@ public:
         k_param_fence,
         k_param_gps_glitch,             // 70
 
+        //
+        // 75: Singlecopter
+        //
+        k_param_single_servo_1 = 75,
+        k_param_single_servo_2,
+        k_param_single_servo_3,
+        k_param_single_servo_4, // 78
+
         //
         // 80: Heli
         //
@@ -112,6 +117,8 @@ public:
         k_param_heli_pitch_ff,
         k_param_heli_roll_ff,
         k_param_heli_yaw_ff,
+        k_param_heli_stab_col_min,
+        k_param_heli_stab_col_max,  // 88
 
         //
         // 90: Motors
@@ -127,11 +134,13 @@ public:
         // 110: Telemetry control
         //
         k_param_gcs0 = 110,
-        k_param_gcs3,
+        k_param_gcs1,
         k_param_sysid_this_mav,
         k_param_sysid_my_gcs,
-        k_param_serial3_baud,
+        k_param_serial1_baud,
         k_param_telem_delay,
+        k_param_gcs2,
+        k_param_serial2_baud,
 
         //
         // 140: Sensor parameters
@@ -274,7 +283,10 @@ public:
     //
     AP_Int16        sysid_this_mav;
     AP_Int16        sysid_my_gcs;
-    AP_Int8         serial3_baud;
+    AP_Int8         serial1_baud;
+#if MAVLINK_COMM_NUM_BUFFERS > 2
+    AP_Int8         serial2_baud;
+#endif
     AP_Int8         telem_delay;
 
     AP_Int16        rtl_altitude;
@@ -302,6 +314,7 @@ public:
     AP_Int8         rssi_pin;
     AP_Int8         wp_yaw_behavior;            // controls how the autopilot controls yaw during missions
     AP_Int16        angle_max;                  // maximum lean angle of the copter in centi-degrees
+    AP_Int32        angle_rate_max;             // maximum rotation rate in roll/pitch axis requested by angle controller used in stabilize, loiter, rtl, auto flight modes
 
     // Waypoints
     //
@@ -336,12 +349,6 @@ public:
     // Misc
     //
     AP_Int16        log_bitmask;
-
-    AP_Int8         toy_yaw_rate;                               // THOR The
-                                                                // Yaw Rate 1
-                                                                // = fast, 2 =
-                                                                // med, 3 =
-                                                                // slow
     AP_Int8         esc_calibrate;
     AP_Int8         radio_tuning;
     AP_Int16        radio_tuning_high;
@@ -357,6 +364,12 @@ public:
     AP_Float        heli_pitch_ff;												// pitch rate feed-forward
     AP_Float        heli_roll_ff;												// roll rate feed-forward
     AP_Float        heli_yaw_ff;												// yaw rate feed-forward
+    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
+#endif
+#if FRAME_CONFIG ==     SINGLE_FRAME
+    // Single
+    RC_Channel      single_servo_1, single_servo_2, single_servo_3, single_servo_4;     // servos for four flaps
 #endif
 
     // RC channels
@@ -368,11 +381,8 @@ public:
     RC_Channel_aux          rc_6;
     RC_Channel_aux          rc_7;
     RC_Channel_aux          rc_8;
-
-#if MOUNT == ENABLED
     RC_Channel_aux          rc_10;
     RC_Channel_aux          rc_11;
-#endif
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4
     RC_Channel_aux          rc_9;
@@ -417,6 +427,12 @@ public:
         heli_servo_3        (CH_3),
         heli_servo_4        (CH_4),
 #endif
+#if FRAME_CONFIG ==     SINGLE_FRAME
+        single_servo_1        (CH_1),
+        single_servo_2        (CH_2),
+        single_servo_3        (CH_3),
+        single_servo_4        (CH_4),
+#endif
 
         rc_1                (CH_1),
         rc_2                (CH_2),
@@ -428,12 +444,11 @@ public:
         rc_8                (CH_8),
 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4
         rc_9                (CH_9),
+#endif
         rc_10               (CH_10),
         rc_11               (CH_11),
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
         rc_12               (CH_12),
-#elif MOUNT == ENABLED
-        rc_10               (CH_10),
-        rc_11               (CH_11),
 #endif
 
         // PID controller	initial P	        initial I		    initial D

ArduCopter/Parameters.pde

@@ -22,6 +22,7 @@
 #define GSCALAR(v, name, def) { g.v.vtype, name, Parameters::k_param_ ## v, &g.v, {def_value : def} }
 #define GGROUP(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &g.v, {group_info : class::var_info} }
 #define GOBJECT(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &v, {group_info : class::var_info} }
+#define GOBJECTN(v, pname, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## pname, &v, {group_info : class::var_info} }
 
 const AP_Param::Info var_info[] PROGMEM = {
     // @Param: SYSID_SW_MREV
@@ -48,12 +49,21 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @User: Advanced
     GSCALAR(sysid_my_gcs,   "SYSID_MYGCS",     255),
 
-    // @Param: SERIAL3_BAUD
+    // @Param: SERIAL1_BAUD
     // @DisplayName: Telemetry Baud Rate
-    // @Description: The baud rate used on the telemetry port
+    // @Description: The baud rate used on the first telemetry port
     // @Values: 1:1200,2:2400,4:4800,9:9600,19:19200,38:38400,57:57600,111:111100,115:115200
     // @User: Standard
-    GSCALAR(serial3_baud,   "SERIAL3_BAUD",     SERIAL3_BAUD/1000),
+    GSCALAR(serial1_baud,   "SERIAL1_BAUD",     SERIAL1_BAUD/1000),
+
+#if MAVLINK_COMM_NUM_BUFFERS > 2
+    // @Param: SERIAL2_BAUD
+    // @DisplayName: Telemetry Baud Rate
+    // @Description: The baud rate used on the seconds telemetry port
+    // @Values: 1:1200,2:2400,4:4800,9:9600,19:19200,38:38400,57:57600,111:111100,115:115200
+    // @User: Standard
+    GSCALAR(serial2_baud,   "SERIAL2_BAUD",     SERIAL2_BAUD/1000),
+#endif
 
     // @Param: TELEM_DELAY
     // @DisplayName: Telemetry startup delay
@@ -98,14 +108,15 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @Param: FS_BATT_ENABLE
     // @DisplayName: Battery Failsafe Enable
     // @Description: Controls whether failsafe will be invoked when battery voltage or current runs low
-    // @Values: 0:Disabled,1:Enabled
+    // @Values: 0:Disabled,1:Land,2:RTL
     // @User: Standard
-    GSCALAR(failsafe_battery_enabled, "FS_BATT_ENABLE", FS_BATTERY),
+    GSCALAR(failsafe_battery_enabled, "FS_BATT_ENABLE", FS_BATT_DISABLED),
 
     // @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 then the copter will RTL
     // @Units: Volts
+    // @Increment: 0.1
     // @User: Standard
     GSCALAR(fs_batt_voltage,        "FS_BATT_VOLTAGE", FS_BATT_VOLTAGE_DEFAULT),
 
@@ -113,15 +124,16 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @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 copter will RTL
     // @Units: mAh
+    // @Increment: 50
     // @User: Standard
     GSCALAR(fs_batt_mah,            "FS_BATT_MAH", FS_BATT_MAH_DEFAULT),
 
     // @Param: FS_GPS_ENABLE
     // @DisplayName: GPS Failsafe Enable
-    // @Description: Controls whether failsafe will be invoked when gps signal is lost
+    // @Description: Controls what action will be taken if GPS signal is lost for at least 5 seconds
-    // @Values: 0:Disabled,1:Enabled
+    // @Values: 0:Disabled,1:Land,2:AltHold,3:Land even from Stabilize
     // @User: Standard
-    GSCALAR(failsafe_gps_enabled, "FS_GPS_ENABLE", FS_GPS),
+    GSCALAR(failsafe_gps_enabled, "FS_GPS_ENABLE", FS_GPS_LAND),
 
     // @Param: FS_GCS_ENABLE
     // @DisplayName: Ground Station Failsafe Enable
@@ -224,7 +236,7 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @DisplayName: Land speed
     // @Description: The descent speed for the final stage of landing in cm/s
     // @Units: cm/s
-    // @Range: 20 200
+    // @Range: 30 200
     // @Increment: 10
     // @User: Standard
     GSCALAR(land_speed,             "LAND_SPEED",   LAND_SPEED),
@@ -292,42 +304,42 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @Param: FLTMODE1
     // @DisplayName: Flight Mode 1
     // @Description: Flight mode when Channel 5 pwm is <= 1230
-    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:ToyA,12:ToyM,13:Sport
+    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:Drift,13:Sport
     // @User: Standard
     GSCALAR(flight_mode1, "FLTMODE1",               FLIGHT_MODE_1),
 
     // @Param: FLTMODE2
     // @DisplayName: Flight Mode 2
     // @Description: Flight mode when Channel 5 pwm is >1230, <= 1360
-    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:ToyA,12:ToyM,13:Sport
+    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:Drift,13:Sport
     // @User: Standard
     GSCALAR(flight_mode2, "FLTMODE2",               FLIGHT_MODE_2),
 
     // @Param: FLTMODE3
     // @DisplayName: Flight Mode 3
     // @Description: Flight mode when Channel 5 pwm is >1360, <= 1490
-    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:ToyA,12:ToyM,13:Sport
+    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:Drift,13:Sport
     // @User: Standard
     GSCALAR(flight_mode3, "FLTMODE3",               FLIGHT_MODE_3),
 
     // @Param: FLTMODE4
     // @DisplayName: Flight Mode 4
     // @Description: Flight mode when Channel 5 pwm is >1490, <= 1620
-    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:ToyA,12:ToyM,13:Sport
+    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:Drift,13:Sport
     // @User: Standard
     GSCALAR(flight_mode4, "FLTMODE4",               FLIGHT_MODE_4),
 
     // @Param: FLTMODE5
     // @DisplayName: Flight Mode 5
     // @Description: Flight mode when Channel 5 pwm is >1620, <= 1749
-    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:ToyA,12:ToyM,13:Sport
+    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:Drift,13:Sport
     // @User: Standard
     GSCALAR(flight_mode5, "FLTMODE5",               FLIGHT_MODE_5),
 
     // @Param: FLTMODE6
     // @DisplayName: Flight Mode 6
     // @Description: Flight mode when Channel 5 pwm is >=1750
-    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:ToyA,12:ToyM,13:Sport
+    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:Drift,13:Sport
     // @User: Standard
     GSCALAR(flight_mode6, "FLTMODE6",               FLIGHT_MODE_6),
 
@@ -344,13 +356,6 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @User: Advanced
     GSCALAR(log_bitmask,    "LOG_BITMASK",          DEFAULT_LOG_BITMASK),
 
-    // @Param: TOY_RATE
-    // @DisplayName: Toy Yaw Rate
-    // @Description: Controls yaw rate in Toy mode.  Higher values will cause a slower yaw rate.  Do not set to zero!
-    // @User: Advanced
-    // @Range: 1 10
-    GSCALAR(toy_yaw_rate, "TOY_RATE",               1),
-
     // @Param: ESC
     // @DisplayName: ESC Calibration
     // @Description: Controls whether ArduCopter will enter ESC calibration on the next restart.  Do not adjust this parameter manually.
@@ -402,10 +407,10 @@ const AP_Param::Info var_info[] PROGMEM = {
 
     // @Param: ARMING_CHECK
     // @DisplayName: Arming check
-    // @Description: Allows enabling or disabling of pre-arming checks of receiver, accelerometer, barometer and compass
+    // @Description: Allows enabling or disabling of pre-arming checks of receiver, accelerometer, barometer, compass and GPS
-    // @Values: 0:Disabled, 1:Enabled
+    // @Values: 0:Disabled, 1:Enabled, -3:Skip Baro, -5:Skip Compass, -9:Skip GPS, -17:Skip INS, -33:Skip Parameters, -65:Skip RC, 127:Skip Voltage
     // @User: Standard
-    GSCALAR(arming_check_enabled, "ARMING_CHECK",   1),
+    GSCALAR(arming_check_enabled, "ARMING_CHECK",   ARMING_CHECK_ALL),
 
     // @Param: ANGLE_MAX
     // @DisplayName: Angle Max
@@ -414,6 +419,13 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @User: Advanced
     GSCALAR(angle_max, "ANGLE_MAX",                 DEFAULT_ANGLE_MAX),
 
+    // @Param: ANGLE_RATE_MAX
+    // @DisplayName: Angle Rate max
+    // @Description: maximum rotation rate in roll/pitch axis requested by angle controller used in stabilize, loiter, rtl, auto flight modes
+    // @Range 90000 250000
+    // @User: Advanced
+    GSCALAR(angle_rate_max, "ANGLE_RATE_MAX",  ANGLE_RATE_MAX),
+
 #if FRAME_CONFIG ==     HELI_FRAME
     // @Group: HS1_
     // @Path: ../libraries/RC_Channel/RC_Channel.cpp
@@ -432,6 +444,7 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @DisplayName: Rate Pitch Feed Forward
     // @Description: Rate Pitch Feed Forward (for TradHeli Only)
     // @Range: 0 10
+    // @Increment: 0.01
     // @User: Standard
 	GSCALAR(heli_pitch_ff, "RATE_PIT_FF",            HELI_PITCH_FF),
 
@@ -439,6 +452,7 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @DisplayName: Rate Roll Feed Forward
     // @Description: Rate Roll Feed Forward (for TradHeli Only)
     // @Range: 0 10
+    // @Increment: 0.01
     // @User: Standard
 	GSCALAR(heli_roll_ff, "RATE_RLL_FF",            HELI_ROLL_FF),
 
@@ -446,10 +460,45 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @DisplayName: Rate Yaw Feed Forward
     // @Description: Rate Yaw Feed Forward (for TradHeli Only)
     // @Range: 0 10
+    // @Increment: 0.01
     // @User: Standard
 	GSCALAR(heli_yaw_ff, "RATE_YAW_FF",            HELI_YAW_FF),
+
+    // @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: pwm
+    // @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: pwm
+    // @Increment: 1
+    // @User: Standard
+    GSCALAR(heli_stab_col_max, "H_STAB_COL_MAX", HELI_STAB_COLLECTIVE_MAX_DEFAULT),
 #endif
 
+#if FRAME_CONFIG ==     SINGLE_FRAME
+    // @Group: SS1_
+    // @Path: ../libraries/RC_Channel/RC_Channel.cpp
+    GGROUP(single_servo_1,    "SS1_", RC_Channel),
+    // @Group: SS2_
+    // @Path: ../libraries/RC_Channel/RC_Channel.cpp
+    GGROUP(single_servo_2,    "SS2_", RC_Channel),
+    // @Group: SS3_
+    // @Path: ../libraries/RC_Channel/RC_Channel.cpp
+    GGROUP(single_servo_3,    "SS3_", RC_Channel),
+    // @Group: SS4_
+    // @Path: ../libraries/RC_Channel/RC_Channel.cpp
+    GGROUP(single_servo_4,    "SS4_", RC_Channel),
+#endif
+
+
     // RC channel
     //-----------
     // @Group: RC1_
@@ -477,20 +526,20 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
     GGROUP(rc_8,    "RC8_", RC_Channel_aux),
 
-#if MOUNT == ENABLED
-    // @Group: RC10_
-    // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
-    GGROUP(rc_10,                    "RC10_", RC_Channel_aux),
-
-    // @Group: RC11_
-    // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
-    GGROUP(rc_11,                    "RC11_", RC_Channel_aux),
-#endif
-
 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4
     // @Group: RC9_
     // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
     GGROUP(rc_9,                    "RC9_", RC_Channel_aux),
+#endif
+
+    // @Group: RC10_
+    // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
+    GGROUP(rc_10,                    "RC10_", RC_Channel_aux),
+    // @Group: RC11_
+    // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
+    GGROUP(rc_11,                    "RC11_", RC_Channel_aux),
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
     // @Group: RC12_
     // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
     GGROUP(rc_12,                   "RC12_", RC_Channel_aux),
@@ -929,7 +978,6 @@ const AP_Param::Info var_info[] PROGMEM = {
 
     // variables not in the g class which contain EEPROM saved variables
 
-    // variables not in the g class which contain EEPROM saved variables
 #if CAMERA == ENABLED
     // @Group: CAM_
     // @Path: ../libraries/AP_Camera/AP_Camera.cpp
@@ -960,11 +1008,17 @@ const AP_Param::Info var_info[] PROGMEM = {
 
     // @Group: SR0_
     // @Path: GCS_Mavlink.pde
-    GOBJECT(gcs0,                   "SR0_",     GCS_MAVLINK),
+    GOBJECTN(gcs[0],  gcs0,       "SR0_",     GCS_MAVLINK),
 
-    // @Group: SR3_
+    // @Group: SR1_
     // @Path: GCS_Mavlink.pde
-    GOBJECT(gcs3,                   "SR3_",     GCS_MAVLINK),
+    GOBJECTN(gcs[1],  gcs1,       "SR1_",     GCS_MAVLINK),
+
+#if MAVLINK_COMM_NUM_BUFFERS > 2
+    // @Group: SR2_
+    // @Path: GCS_Mavlink.pde
+    GOBJECTN(gcs[2],  gcs2,       "SR2_",     GCS_MAVLINK),
+#endif
 
     // @Group: AHRS_
     // @Path: ../libraries/AP_AHRS/AP_AHRS.cpp
@@ -1018,6 +1072,24 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @Group: H_
     // @Path: ../libraries/AP_Motors/AP_MotorsHeli.cpp
     GOBJECT(motors, "H_",           AP_MotorsHeli),
+
+#elif FRAME_CONFIG == SINGLE_FRAME
+    // @Group: SS1_
+    // @Path: ../libraries/RC_Channel/RC_Channel.cpp
+    GGROUP(single_servo_1,    "SS1_", RC_Channel),
+    // @Group: SS2_
+    // @Path: ../libraries/RC_Channel/RC_Channel.cpp
+    GGROUP(single_servo_2,    "SS2_", RC_Channel),
+    // @Group: SS3_
+    // @Path: ../libraries/RC_Channel/RC_Channel.cpp
+    GGROUP(single_servo_3,    "SS3_", RC_Channel),
+    // @Group: SS4_
+    // @Path: ../libraries/RC_Channel/RC_Channel.cpp
+    GGROUP(single_servo_4,    "SS4_", RC_Channel),
+    // @Group: (SingleCopter)MOT_
+    // @Path: ../libraries/AP_Motors/AP_MotorsSingle.cpp
+    GOBJECT(motors, "MOT_",           AP_MotorsSingle),
+
 #else
     // @Group: MOT_
     // @Path: ../libraries/AP_Motors/AP_Motors_Class.cpp

ArduCopter/ReleaseNotes.txt

@@ -1,5 +1,117 @@
 ArduCopter Release Notes:
 ------------------------------------------------------------------
+ArduCopter 3.1.5 27-May-2014 / 3.1.5-rc2 20-May-2014
+Changes from 3.1.5-rc1
+1) Bug Fix to broken loiter (pixhawk only)
+2) Workaround to read from FRAM in 128byte chunks to resolve a few users boot issues (Pixhawk only)
+------------------------------------------------------------------
+ArduCopter 3.1.5-rc1 14-May-2014
+Changes from 3.1.4
+1) Bug Fix to ignore roll and pitch inputs to loiter controller when in radio failsafe
+2) Bug Fix to allow compassmot to work on Pixhawk
+------------------------------------------------------------------
+ArduCopter 3.1.4 8-May-2014 / 3.1.4-rc1 2-May-2014
+Changes from 3.1.3
+1) Bug Fix for Pixhawk/PX4 NuttX I2C memory corruption when errors are found on I2C bus
+------------------------------------------------------------------
+ArduCopter 3.1.3 7-Apr-2014
+Changes from 3.1.2
+1) Stability patch fix which could cause motors to go to min at full throttle and with large roll/pitch inputs
+------------------------------------------------------------------
+ArduCopter 3.1.2 13-Feb-2014 / ArduCopter 3.1.2-rc2 12-Feb-2014
+Changes from 3.1.2-rc1
+1) GPS Glitch detection disabled when connected via USB
+2) RC_FEEL_RP param added for adjusting responsiveness to pilot roll/pitch input in Stabilize, Drift, AltHold modes
+------------------------------------------------------------------
+ArduCopter 3.1.2-rc1 30-Jan-2014
+Changes from 3.1.1
+1) Pixhawk baro bug fix to SPI communication which could cause large altitude estimate jumps at high temperatures
+------------------------------------------------------------------
+ArduCopter 3.1.1 26-Jan-2014 / ArduCopter 3.1.1-rc2 21-Jan-2014
+Changes from 3.1.1-rc1
+1) Pixhawk improvements (available for APM2 when AC3.2 is released):
+    a) Faster arming
+    b) AHRS_TRIM fix - reduces movement in loiter when yawing
+    c) AUX Out 5 & 6 turned into general purpose I/O pins
+    d) Three more relays added (relays are pins that can be set to 0V or 5V)
+    e) do-set-servo fix to allow servos to be controlled from ground station
+    f) Motorsync CLI test
+    g) PX4 parameters moved from SD card to eeprom
+    h) additional pre-arm checks for baro & inertial nav altitude and lean angle
+------------------------------------------------------------------
+ArduCopter 3.1.1-rc1 14-Jan-2014
+Changes from 3.1
+1) Pixhawk improvements:
+    a) Telemetry port 2 enabled (for MinimOSD)
+    b) SD card reliability improvements
+    c) parameters moved to FRAM
+    d) faster parameter loading via USB
+    e) Futaba SBUS receiver support
+2) Bug fixes:
+    a) Loiter initialisation fix (Loiter would act like AltHold until flight mode switch changed position)
+    b) ROI commands were not returning Lat, Lon, Alt to mission planner when mission was loaded from APM
+3) TradHeli only fixes:
+    a) Drift now uses same (reduced) collective range as stabilize mode
+    b) AutoTune disabled (for tradheli only)
+    c) Landing collective (smaller than normal collective) used whenever copter is not moving
+------------------------------------------------------------------
+ArduCopter 3.1 14-Dec-2013
+Changes from 3.1-rc8
+1) Pixhawk improvements:
+    a) switch to use MPU6k as main accel/gyro
+    b) auto loading of IO-board firmware on startup
+2) RTL fixes:
+    a) initialise waypoint leash length (first RTL stop would be more aggressive than 2nd)
+    b) reduce projected stopping distance for higher speed stops
+------------------------------------------------------------------
+ArduCopter 3.1-rc8 9-Dec-2013
+Changes from 3.1-rc7
+1) add Y6 motor mapping with all top props CW, bottom pros CCW (set FRAME = 10)
+2) Safety Changes:
+    a) ignore yaw input during radio failsafe (previously the copter could return home spinning if yaw was full over at time of failsafe)
+    b) Reduce GPSGLITCH_RADIUS to 2m (was 5m) to catch glitches faster
+3) Bug fixes:
+    a) Optical flow SPI bus rates
+    b) TradHeli main rotor ramp up speed fix
+------------------------------------------------------------------
+ArduCopter 3.1-rc7 22-Nov-2013
+Changes from 3.1-rc6
+1) MOT_SPIN_ARMED default to 70
+2) Smoother inertial nav response to missed GPS messages
+3) Safety related changes
+    a) radio and battery failsafe disarm copter if landed in Loiter or AltHold (previously they would RTL)
+    b) Pre-Arm check failure warning output to ground station every 30 seconds until they pass
+    c) INS and inertial nav errors logged to dataflash's PM message
+    d) pre-arm check for ACRO_BAL_ROLL, ACRO_BAL_PITCH
+------------------------------------------------------------------
+ArduCopter 3.1-rc6 16-Nov-2013
+Improvements over 3.1-rc5
+1) Heli improvements:
+    a) support for direct drive tails (uses TAIL_TYPE and TAIL_SPEED parameters)
+    b) smooth main rotor ramp-up for those without external govenor (RSC_RAMP_TIME)
+    c) internal estimate of rotor speed configurable with RSC_RUNUP_TIME parameter to ensure rotor at top speed before starting missions
+    d) LAND_COL_MIN collective position used when landed (reduces chance copter will push too hard into the ground when landing or before starting missions)
+    e) reduced collective while in stabilize mode (STAB_COL_MIN, STAB_COL_MAX) for more precise throttle control
+    f) external gyro parameter range changed from 1000~2000 to 0~1000 (more consistent with other parameters)
+    g) dynamic flight detector switches on/off leaky-i term depending on copter speed
+2) SingleCopter airframe support (contribution from Bill King)
+3) Drift mode replaces TOY
+4) MPU6k SPI bus speed decreased to 500khz after 4 errors
+5) Safety related changes:
+    a) crash detector cuts motors if copter upside down for more than 2 seconds
+    b) INS (accel and gyro) health check in pre-arm checks
+    c) ARMING_CHECK allows turning on/off individual checks for baro, GPS, compass, parameters, board voltage, radio
+    d) detect Ublox GPS running at less than 5hz and resend configuration
+    e) GPSGlitch acceptable radius reduced to 5m (stricter detection of glitches)
+    f) range check roll, pitch input to ensure crazy radio values don't get through to stabilize controller
+    g) GPS failsafe options to trigger AltHold instead of LAND or to trigger LAND even if in flight mode that does not require GPS
+    h) Battery failsafe option to trigger RTL instead of LAND
+    i) MOT_SPIN_ARMED set to zero by default
+6) Bug fixes:
+    a) missing throttle controller initialisation would mean Stabilize mode's throttle could be non-tilt-compensated
+    b) inertial nav baro and gps delay compensation fix (contribution from Neurocopter)
+    c) GPS failsafe was invoking LAND mode which still used GPS for horizontal control
+------------------------------------------------------------------
 ArduCopter 3.1-rc5 22-Oct-2013
 Improvements over 3.1-rc4
 1) Pixhawk USB reliability improvements

ArduCopter/auto_tune.pde

@@ -35,7 +35,7 @@
 #define AUTO_TUNE_RP_RATIO_FINAL            1.0f    // I is set 1x P after testing
 #define AUTO_TUNE_RD_MIN                  0.004f    // minimum Rate D value
 #define AUTO_TUNE_RD_MAX                  0.015f    // maximum Rate D value
-#define AUTO_TUNE_RP_MIN                   0.02f    // minimum Rate P value
+#define AUTO_TUNE_RP_MIN                   0.01f    // minimum Rate P value
 #define AUTO_TUNE_RP_MAX                   0.25f    // maximum Rate P value
 #define AUTO_TUNE_SP_MAX                   15.0f    // maximum Stab P value
 #define AUTO_TUNE_SUCCESS_COUNT                4    // how many successful iterations we need to freeze at current gains
@@ -279,7 +279,7 @@ void auto_tune_update_gcs(uint8_t message_id)
 
 // Auto tuning roll-pitch controller
 static void
-get_autotune_roll_pitch_controller(int32_t pilot_roll_angle, int32_t pilot_pitch_angle)
+get_autotune_roll_pitch_controller(int16_t pilot_roll_angle, int16_t pilot_pitch_angle, int16_t pilot_yaw_command)
 {
     int32_t target_roll_rate, target_pitch_rate;
     float rotation_rate;        // rotation rate in radians/second
@@ -291,7 +291,7 @@ get_autotune_roll_pitch_controller(int32_t pilot_roll_angle, int32_t pilot_pitch
     }
 
     // check for pilot override
-    if (pilot_roll_angle != 0 || pilot_pitch_angle != 0 ) {
+    if (pilot_roll_angle != 0 || pilot_pitch_angle != 0 || pilot_yaw_command != 0) {
         if (!auto_tune_state.pilot_override) {
             // restore pids to their original values
             auto_tune_restore_orig_gains();

ArduCopter/commands_logic.pde

@@ -178,7 +178,6 @@ static bool verify_nav_command()
         break;
 
     default:
-        //gcs_send_text_P(SEVERITY_HIGH,PSTR("<verify_must: default> No current Must commands"));
         return false;
         break;
     }
@@ -207,7 +206,6 @@ static bool verify_cond_command()
         break;
 
     default:
-        //gcs_send_text_P(SEVERITY_HIGH,PSTR("<verify_must: default> No current May commands"));
         return false;
         break;
     }
@@ -464,6 +462,7 @@ static void do_loiter_time()
 static bool verify_takeoff()
 {
     // have we reached our target altitude?
+    set_takeoff_complete(wp_nav.reached_destination());
     return wp_nav.reached_destination();
 }
 

ArduCopter/commands_process.pde

@@ -4,6 +4,11 @@
 //----------------------------------------
 static void change_command(uint8_t cmd_index)
 {
+    // check we are in AUTO mode
+    if (control_mode != AUTO) {
+        return;
+    }
+
     // limit range
     cmd_index = min(g.command_total - 1, cmd_index);
 

ArduCopter/config.h

@@ -99,26 +99,33 @@
 #ifndef FRAME_ORIENTATION
  # define FRAME_ORIENTATION      X_FRAME
 #endif
-#ifndef TOY_EDF
- # define TOY_EDF        DISABLED
-#endif
-#ifndef TOY_MIXER
- # define TOY_MIXER      TOY_LINEAR_MIXER
-#endif
 
 /////////////////////////////////////////////////////////////////////////////////
 // TradHeli defaults
 #if FRAME_CONFIG == HELI_FRAME
   # define RC_FAST_SPEED                        125
-  # define WP_YAW_BEHAVIOR_DEFAULT      YAW_LOOK_AT_HOME
+  # define WP_YAW_BEHAVIOR_DEFAULT              WP_YAW_BEHAVIOR_LOOK_AHEAD
   # define RATE_INTEGRATOR_LEAK_RATE            0.02f
   # define RATE_ROLL_D                          0
   # define RATE_PITCH_D                         0
   # define HELI_PITCH_FF                        0
   # define HELI_ROLL_FF                         0
   # define HELI_YAW_FF                          0  
-  # define STABILIZE_THROTTLE			THROTTLE_MANUAL
+  # define STABILIZE_THR                        THROTTLE_MANUAL_HELI
+  # define DRIFT_THR                            THROTTLE_MANUAL_HELI
   # define MPU6K_FILTER                         10
+  # define HELI_STAB_COLLECTIVE_MIN_DEFAULT     0
+  # define HELI_STAB_COLLECTIVE_MAX_DEFAULT     1000
+  # define THR_MIN_DEFAULT                      0
+  # define AUTOTUNE                             DISABLED
+  
+  # ifndef HELI_CC_COMP
+    #define HELI_CC_COMP DISABLED
+  #endif
+  # ifndef HELI_PIRO_COMP
+    #define HELI_PIRO_COMP DISABLED
+  #endif
+  
 #endif
 
 /////////////////////////////////////////////////////////////////////////////////
@@ -173,31 +180,19 @@
 #if CONFIG_HAL_BOARD == HAL_BOARD_APM1
  # define LED_ON           HIGH
  # define LED_OFF          LOW
- # define BATTERY_VOLT_PIN      0      // Battery voltage on A0
- # define BATTERY_CURR_PIN      1      // Battery current on A1
 #elif CONFIG_HAL_BOARD == HAL_BOARD_APM2
  # define LED_ON           LOW
  # define LED_OFF          HIGH
- # define BATTERY_VOLT_PIN      1      // Battery voltage on A1
- # define BATTERY_CURR_PIN      2      // Battery current on A2
 #elif CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
  # define LED_ON           LOW
  # define LED_OFF          HIGH
- # define BATTERY_VOLT_PIN 1      // Battery voltage on A1
- # define BATTERY_CURR_PIN 2      // Battery current on A2
 #elif CONFIG_HAL_BOARD == HAL_BOARD_PX4
  # define LED_ON           LOW
  # define LED_OFF          HIGH
- # define BATTERY_VOLT_PIN -1
- # define BATTERY_CURR_PIN -1
 #elif CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE
- # define BATTERY_VOLT_PIN      20
- # define BATTERY_CURR_PIN      19
  # define LED_ON           LOW
  # define LED_OFF          HIGH
 #elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX
- # define BATTERY_VOLT_PIN      -1
- # define BATTERY_CURR_PIN      -1
  # define LED_ON           LOW
  # define LED_OFF          HIGH
 #endif
@@ -286,7 +281,7 @@
 #endif
 
 #ifndef SONAR_GAIN_DEFAULT
- # define SONAR_GAIN_DEFAULT 2.0            // gain for controlling how quickly sonar range adjusts target altitude (lower means slower reaction)
+ # define SONAR_GAIN_DEFAULT 0.8            // gain for controlling how quickly sonar range adjusts target altitude (lower means slower reaction)
 #endif
 
 #ifndef THR_SURFACE_TRACKING_VELZ_MAX
@@ -298,7 +293,7 @@
 //
 
 #ifndef CH7_OPTION
- # define CH7_OPTION            AUX_SWITCH_SAVE_WP
+ # define CH7_OPTION            AUX_SWITCH_DO_NOTHING
 #endif
 
 #ifndef CH8_OPTION
@@ -341,8 +336,11 @@
 #ifndef SERIAL0_BAUD
  # define SERIAL0_BAUD                   115200
 #endif
-#ifndef SERIAL3_BAUD
+#ifndef SERIAL1_BAUD
- # define SERIAL3_BAUD                    57600
+ # define SERIAL1_BAUD                    57600
+#endif
+#ifndef SERIAL2_BAUD
+ # define SERIAL2_BAUD                    57600
 #endif
 
 
@@ -365,15 +363,7 @@
  # define BOARD_VOLTAGE_MAX             5800        // max board voltage in milli volts for pre-arm checks
 #endif
 
-// Battery failsafe
-#ifndef FS_BATTERY
- # define FS_BATTERY              DISABLED
-#endif
-
 // GPS failsafe
-#ifndef FS_GPS
- # define FS_GPS                        ENABLED
-#endif
 #ifndef FAILSAFE_GPS_TIMEOUT_MS
  # define FAILSAFE_GPS_TIMEOUT_MS       5000    // gps failsafe triggers after 5 seconds with no GPS
 #endif
@@ -415,10 +405,21 @@
  #endif
 #endif
 
+// max compass offset length (i.e. sqrt(offs_x^2+offs_y^2+offs_Z^2))
+#ifndef CONFIG_ARCH_BOARD_PX4FMU_V1
+ #ifndef COMPASS_OFFSETS_MAX
+  # define COMPASS_OFFSETS_MAX          600         // PX4 onboard compass has high offsets
+ #endif
+#else   // APM1, APM2, SITL, FLYMAPLE, etc
+ #ifndef COMPASS_OFFSETS_MAX
+  # define COMPASS_OFFSETS_MAX          500
+ #endif
+#endif
+
 //////////////////////////////////////////////////////////////////////////////
 //  OPTICAL_FLOW
 #ifndef OPTFLOW                         // sets global enabled/disabled flag for optflow (as seen in CLI)
- # define OPTFLOW                       DISABLED
+ # define OPTFLOW                       ENABLED
 #endif
 #ifndef OPTFLOW_ORIENTATION
  # define OPTFLOW_ORIENTATION    AP_OPTICALFLOW_ADNS3080_PINS_FORWARD
@@ -497,12 +498,6 @@
 //////////////////////////////////////////////////////////////////////////////
 // Throttle Failsafe
 //
-// possible values for FS_THR parameter
-#define FS_THR_DISABLED                    0
-#define FS_THR_ENABLED_ALWAYS_RTL          1
-#define FS_THR_ENABLED_CONTINUE_MISSION    2
-#define FS_THR_ENABLED_ALWAYS_LAND         3
-
 #ifndef FS_THR_VALUE_DEFAULT
  # define FS_THR_VALUE_DEFAULT             975
 #endif
@@ -517,20 +512,6 @@
  # define LAND_DETECTOR_TRIGGER 50    // number of 50hz iterations with near zero climb rate and low throttle that triggers landing complete.
 #endif
 
-//////////////////////////////////////////////////////////////////////////////
-//////////////////////////////////////////////////////////////////////////////
-// STARTUP BEHAVIOUR
-//////////////////////////////////////////////////////////////////////////////
-//////////////////////////////////////////////////////////////////////////////
-
-//////////////////////////////////////////////////////////////////////////////
-// GROUND_START_DELAY
-//
-#ifndef GROUND_START_DELAY
- # define GROUND_START_DELAY             3
-#endif
-
-
 //////////////////////////////////////////////////////////////////////////////
 // CAMERA TRIGGER AND CONTROL
 //
@@ -563,6 +544,17 @@
 
 // Flight mode roll, pitch, yaw, throttle and navigation definitions
 
+// Stabilize Mode
+#ifndef STABILIZE_YAW
+ # define STABILIZE_YAW           	YAW_HOLD
+#endif
+#ifndef STABILIZE_RP
+ # define STABILIZE_RP           	ROLL_PITCH_STABLE
+#endif
+#ifndef STABILIZE_THR
+ # define STABILIZE_THR           	THROTTLE_MANUAL_TILT_COMPENSATED
+#endif
+
 // Acro Mode
 #ifndef ACRO_YAW
  # define ACRO_YAW           	    YAW_ACRO
@@ -580,6 +572,11 @@
  # define ACRO_LEVEL_MAX_ANGLE      3000
 #endif
 
+// Drift Mode
+#ifndef DRIFT_THR
+ # define DRIFT_THR                 THROTTLE_MANUAL_TILT_COMPENSATED
+#endif
+
 // Sport Mode
 #ifndef SPORT_YAW
  # define SPORT_YAW           	    YAW_HOLD
@@ -782,10 +779,6 @@
  # define STABILIZE_PITCH_IMAX   	0
 #endif
 
-#ifndef STABILIZE_RATE_LIMIT
- # define STABILIZE_RATE_LIMIT      18000
-#endif
-
 #ifndef  STABILIZE_YAW_P
  # define STABILIZE_YAW_P           4.5f            // increase for more aggressive Yaw Hold, decrease if it's bouncy
 #endif
@@ -810,6 +803,9 @@
 #ifndef DEFAULT_ANGLE_MAX
  # define DEFAULT_ANGLE_MAX         4500            // ANGLE_MAX parameters default value
 #endif
+#ifndef ANGLE_RATE_MAX
+ # define ANGLE_RATE_MAX            18000           // default maximum rotation rate in roll/pitch axis requested by angle controller used in stabilize, loiter, rtl, auto flight modes
+#endif
 #ifndef RATE_ROLL_P
  # define RATE_ROLL_P        		0.150f
 #endif
@@ -1086,4 +1082,14 @@
   #  define CLI_ENABLED           ENABLED
 #endif
 
+/*
+  build a firmware version string.
+  GIT_VERSION comes from Makefile builds
+*/
+#ifndef GIT_VERSION
+#define FIRMWARE_STRING THISFIRMWARE
+#else
+#define FIRMWARE_STRING THISFIRMWARE " (" GIT_VERSION ")"
+#endif
+
 #endif // __ARDUCOPTER_CONFIG_H__

ArduCopter/control_modes.pde

@@ -178,34 +178,50 @@ static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
             break;
 
         case AUX_SWITCH_SAVE_WP:
-            // save waypoint when switch is switched off
+            // save waypoint when switch is brought high
-            if (ch_flag == AUX_SWITCH_LOW) {
+            if (ch_flag == AUX_SWITCH_HIGH) {
 
                 // if in auto mode, reset the mission
                 if(control_mode == AUTO) {
                     aux_switch_wp_index = 0;
                     g.command_total.set_and_save(1);
                     set_mode(RTL);  // if by chance we are unable to switch to RTL we just stay in AUTO and hope the GPS failsafe will take-over
+                    Log_Write_Event(DATA_SAVEWP_CLEAR_MISSION_RTL);
                     return;
                 }
 
+				// we're on the ground
+				if((g.rc_3.control_in == 0) && (aux_switch_wp_index == 0)){
+					// nothing to do
+					return;
+				}
+
+                // initialise new waypoint to current location
+                Location new_wp;
+
                 if(aux_switch_wp_index == 0) {
                     // this is our first WP, let's save WP 1 as a takeoff
                     // increment index to WP index of 1 (home is stored at 0)
                     aux_switch_wp_index = 1;
 
-                    Location temp   = home;
                     // set our location ID to 16, MAV_CMD_NAV_WAYPOINT
-                    temp.id = MAV_CMD_NAV_TAKEOFF;
+                    new_wp.id = MAV_CMD_NAV_TAKEOFF;
-                    temp.alt = current_loc.alt;
+                    new_wp.options = 0;
+                    new_wp.p1 = 0;
+                    new_wp.lat = 0;
+                    new_wp.lng = 0;
+                    new_wp.alt = max(current_loc.alt,100);
 
                     // save command:
                     // we use the current altitude to be the target for takeoff.
                     // only altitude will matter to the AP mission script for takeoff.
                     // If we are above the altitude, we will skip the command.
-                    set_cmd_with_index(temp, aux_switch_wp_index);
+                    set_cmd_with_index(new_wp, aux_switch_wp_index);
                 }
 
+                // initialise new waypoint to current location
+                new_wp = current_loc;
+
                 // increment index
                 aux_switch_wp_index++;
 
@@ -215,14 +231,17 @@ static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
 
                 if(g.rc_3.control_in > 0) {
                     // set our location ID to 16, MAV_CMD_NAV_WAYPOINT
-                    current_loc.id = MAV_CMD_NAV_WAYPOINT;
+                    new_wp.id = MAV_CMD_NAV_WAYPOINT;
                 }else{
 					// set our location ID to 21, MAV_CMD_NAV_LAND
-                    current_loc.id = MAV_CMD_NAV_LAND;
+					new_wp.id = MAV_CMD_NAV_LAND;
                 }
 
                 // save command
-                set_cmd_with_index(current_loc, aux_switch_wp_index);
+                set_cmd_with_index(new_wp, aux_switch_wp_index);
+
+                // log event
+                Log_Write_Event(DATA_SAVEWP_ADD_WP);
 
                 // Cause the CopterLEDs to blink twice to indicate saved waypoint
                 copter_leds_nav_blink = 10;
@@ -251,8 +270,10 @@ static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
             // enable or disable the fence
             if (ch_flag == AUX_SWITCH_HIGH) {
                 fence.enable(true);
+                Log_Write_Event(DATA_FENCE_ENABLE);
             }else{
                 fence.enable(false);
+                Log_Write_Event(DATA_FENCE_DISABLE);
             }
             break;
 #endif
@@ -268,12 +289,15 @@ static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
             switch(ch_flag) {
                 case AUX_SWITCH_LOW:
                     g.acro_trainer = ACRO_TRAINER_DISABLED;
+                    Log_Write_Event(DATA_ACRO_TRAINER_DISABLED);
                     break;
                 case AUX_SWITCH_MIDDLE:
                     g.acro_trainer = ACRO_TRAINER_LEVELING;
+                    Log_Write_Event(DATA_ACRO_TRAINER_LEVELING);
                     break;
                 case AUX_SWITCH_HIGH:
                     g.acro_trainer = ACRO_TRAINER_LIMITED;
+                    Log_Write_Event(DATA_ACRO_TRAINER_LIMITED);
                     break;
             }
             break;
@@ -337,6 +361,7 @@ static void save_trim()
     float roll_trim = ToRad((float)g.rc_1.control_in/100.0f);
     float pitch_trim = ToRad((float)g.rc_2.control_in/100.0f);
     ahrs.add_trim(roll_trim, pitch_trim);
+    Log_Write_Event(DATA_SAVE_TRIM);
 }
 
 // auto_trim - slightly adjusts the ahrs.roll_trim and ahrs.pitch_trim towards the current stick positions

ArduCopter/crash_check.pde

@@ -0,0 +1,61 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+// Code to detect a crash main ArduCopter code
+#ifndef CRASH_CHECK_ITERATIONS_MAX
+ # define CRASH_CHECK_ITERATIONS_MAX        20      // 2 second (ie. 10 iterations at 10hz) inverted indicates a crash
+#endif
+#ifndef CRASH_CHECK_ANGLE_DEVIATION_CD
+ # define CRASH_CHECK_ANGLE_DEVIATION_CD    2000    // 20 degrees beyond angle max is signal we are inverted
+#endif
+#ifndef CRASH_CHECK_ALT_CHANGE_LIMIT_CM
+ # define CRASH_CHECK_ALT_CHANGE_LIMIT_CM   50      // baro altitude must not change by more than 50cm
+#endif
+
+// crash_check - disarms motors if a crash has been detected
+// crashes are detected by the vehicle being more than 20 degrees beyond it's angle limits continuously for more than 1 second
+// should be called at 10hz
+void crash_check()
+{
+    static uint8_t inverted_count;  // number of iterations we have been inverted
+    static int32_t baro_alt_prev;
+
+    // return immediately if motors are not armed or pilot's throttle is above zero
+    if (!motors.armed() || (g.rc_3.control_in != 0 && !failsafe.radio)) {
+        inverted_count = 0;
+        return;
+    }
+
+    // return immediately if we are not in an angle stabilize flight mode or we are flipping
+    if (control_mode == ACRO || ap.do_flip) {
+        inverted_count = 0;
+        return;
+    }
+
+    // check angles
+    int32_t lean_max = g.angle_max + CRASH_CHECK_ANGLE_DEVIATION_CD;
+    if (labs(ahrs.roll_sensor) > lean_max || labs(ahrs.pitch_sensor) > lean_max) {
+        inverted_count++;
+
+        // if we have just become inverted record the baro altitude
+        if (inverted_count == 1) {
+            baro_alt_prev = baro_alt;
+
+        // exit if baro altitude change indicates we are moving (probably falling)
+        }else if (labs(baro_alt - baro_alt_prev) > CRASH_CHECK_ALT_CHANGE_LIMIT_CM) {
+            inverted_count = 0;
+            return;
+
+        // check if inverted for 2 seconds
+        }else if (inverted_count >= CRASH_CHECK_ITERATIONS_MAX) {
+            // log an error in the dataflash
+            Log_Write_Error(ERROR_SUBSYSTEM_CRASH_CHECK, ERROR_CODE_CRASH_CHECK_CRASH);
+            // send message to gcs
+            gcs_send_text_P(SEVERITY_HIGH,PSTR("Crash: Disarming"));
+            // disarm motors
+            init_disarm_motors();
+        }
+    }else{
+        // we are not inverted so reset counter
+        inverted_count = 0;
+    }
+}

ArduCopter/defines.h

@@ -23,14 +23,14 @@
 #define YAW_LOOK_AT_HOME    		    5       // point towards home (no pilot input accepted)
 #define YAW_LOOK_AT_HEADING    		    6       // point towards a particular angle (not pilot input accepted)
 #define YAW_LOOK_AHEAD					7		// WARNING!  CODE IN DEVELOPMENT NOT PROVEN
-#define YAW_TOY                         8       // THOR This is the Yaw mode
+#define YAW_DRIFT                       8       //
 #define YAW_RESETTOARMEDYAW				9       // point towards heading at time motors were armed
 
 #define ROLL_PITCH_STABLE           0       // pilot input roll, pitch angles
 #define ROLL_PITCH_ACRO             1       // pilot inputs roll, pitch rotation rates in body frame
 #define ROLL_PITCH_AUTO             2       // no pilot input.  autopilot roll, pitch is sent to stabilize controller inputs
 #define ROLL_PITCH_STABLE_OF        3       // pilot inputs roll, pitch angles which are mixed with optical flow based position controller lean anbles
-#define ROLL_PITCH_TOY              4       // THOR This is the Roll and Pitch mode
+#define ROLL_PITCH_DRIFT            4       //
 #define ROLL_PITCH_LOITER           5       // pilot inputs the desired horizontal velocities
 #define ROLL_PITCH_SPORT            6       // pilot inputs roll, pitch rotation rates in earth frame
 #define ROLL_PITCH_AUTOTUNE         7       // description of new roll-pitch mode
@@ -40,6 +40,7 @@
 #define THROTTLE_HOLD                       2   // alt hold plus pilot input of climb rate
 #define THROTTLE_AUTO                       3   // auto pilot altitude controller with target altitude held in next_WP.alt
 #define THROTTLE_LAND                       4   // landing throttle controller
+#define THROTTLE_MANUAL_HELI                5   // pilot manually controlled throttle for traditional helicopters
 
 
 // sonar - for use with CONFIG_SONAR_SOURCE
@@ -86,6 +87,7 @@
 #define OCTA_FRAME 5
 #define HELI_FRAME 6
 #define OCTA_QUAD_FRAME 7
+#define SINGLE_FRAME 8
 
 #define PLUS_FRAME 0
 #define X_FRAME 1
@@ -144,11 +146,11 @@
 #define POSITION 8                      // AUTO control
 #define LAND 9                          // AUTO control
 #define OF_LOITER 10                    // Hold a single location using optical flow sensor
-#define TOY_A 11                        // THOR Enum for Toy mode
+#define DRIFT 11                        // DRIFT mode (Note: 12 is no longer used)
-#define TOY_M 12                        // THOR Enum for Toy mode
 #define SPORT 13                        // earth frame rate control
 #define NUM_MODES 14
 
+
 // CH_6 Tuning
 // -----------
 #define CH6_NONE                        0   // no tuning performed
@@ -208,11 +210,6 @@
 #define WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL    2   // auto pilot will face next waypoint except when doing RTL at which time it will stay in it's last
 #define WP_YAW_BEHAVIOR_LOOK_AHEAD                    3   // auto pilot will look ahead during missions and rtl (primarily meant for traditional helicotpers)
 
-// TOY mixing options
-#define TOY_LOOKUP_TABLE 0
-#define TOY_LINEAR_MIXER 1
-#define TOY_EXTERNAL_MIXER 2
-
 
 // Waypoint options
 #define MASK_OPTIONS_RELATIVE_ALT               1
@@ -259,6 +256,7 @@ enum ap_message {
     MSG_RAW_IMU2,
     MSG_RAW_IMU3,
     MSG_GPS_RAW,
+    MSG_SYSTEM_TIME,
     MSG_SERVO_OUT,
     MSG_NEXT_WAYPOINT,
     MSG_NEXT_PARAM,
@@ -345,6 +343,14 @@ enum ap_message {
 #define DATA_AUTOTUNE_REACHED_LIMIT     35
 #define DATA_AUTOTUNE_TESTING           36
 #define DATA_AUTOTUNE_SAVEDGAINS        37
+#define DATA_SAVE_TRIM                  38
+#define DATA_SAVEWP_ADD_WP              39
+#define DATA_SAVEWP_CLEAR_MISSION_RTL   40
+#define DATA_FENCE_ENABLE               41
+#define DATA_FENCE_DISABLE              42
+#define DATA_ACRO_TRAINER_DISABLED      43
+#define DATA_ACRO_TRAINER_LEVELING      44
+#define DATA_ACRO_TRAINER_LIMITED       45
 
 
 
@@ -444,6 +450,7 @@ enum ap_message {
 #define ERROR_SUBSYSTEM_FAILSAFE_FENCE      9
 #define ERROR_SUBSYSTEM_FLIGHT_MODE         10
 #define ERROR_SUBSYSTEM_GPS                 11
+#define ERROR_SUBSYSTEM_CRASH_CHECK         12
 // general error codes
 #define ERROR_CODE_ERROR_RESOLVED           0
 #define ERROR_CODE_FAILED_TO_INITIALISE     1
@@ -457,8 +464,36 @@ enum ap_message {
 // subsystem specific error codes -- gps
 #define ERROR_CODE_GPS_GLITCH               2
 // subsystem specific error codes -- main
-#define ERROR_CODE_INS_DELAY                1
+#define ERROR_CODE_MAIN_INS_DELAY           1
+// subsystem specific error codes -- crash checker
+#define ERROR_CODE_CRASH_CHECK_CRASH        1
 
+// Arming Check Enable/Disable bits
+#define ARMING_CHECK_NONE                   0x00
+#define ARMING_CHECK_ALL                    0x01
+#define ARMING_CHECK_BARO                   0x02
+#define ARMING_CHECK_COMPASS                0x04
+#define ARMING_CHECK_GPS                    0x08
+#define ARMING_CHECK_INS                    0x10
+#define ARMING_CHECK_PARAMETERS             0x20
+#define ARMING_CHECK_RC                     0x40
+#define ARMING_CHECK_VOLTAGE                0x80
 
+// Radio failsafe definitions (FS_THR parameter)
+#define FS_THR_DISABLED                    0
+#define FS_THR_ENABLED_ALWAYS_RTL          1
+#define FS_THR_ENABLED_CONTINUE_MISSION    2
+#define FS_THR_ENABLED_ALWAYS_LAND         3
+
+// Battery failsafe definitions (FS_BATT_ENABLE parameter)
+#define FS_BATT_DISABLED                    0       // battery failsafe disabled
+#define FS_BATT_LAND                        1       // switch to LAND mode on battery failsafe
+#define FS_BATT_RTL                         2       // switch to RTL mode on battery failsafe
+
+// GPS Failsafe definitions (FS_GPS_ENABLE parameter)
+#define FS_GPS_DISABLED                     0       // GPS failsafe disabled
+#define FS_GPS_LAND                         1       // switch to LAND mode on GPS Failsafe
+#define FS_GPS_ALTHOLD                      2       // switch to ALTHOLD mode on GPS failsafe
+#define FS_GPS_LAND_EVEN_STABILIZE          3       // switch to LAND mode on GPS failsafe even if in a manual flight mode like Stabilize
 
 #endif // _DEFINES_H

ArduCopter/drift.pde

@@ -0,0 +1,52 @@
+////////////////////////////////////////////////////////////////////////////////
+// Drift Mode
+////////////////////////////////////////////////////////////////////////////////
+
+#define SPEEDGAIN 14.0
+
+
+// The function call for managing the flight mode drift
+static void
+get_roll_pitch_drift()
+{
+}
+
+
+
+static void
+get_yaw_drift()
+{
+	static float breaker = 0.0;
+	// convert pilot input to lean angles
+	// moved to Yaw since it is called before get_roll_pitch_drift();
+	get_pilot_desired_lean_angles(g.rc_1.control_in, g.rc_2.control_in, control_roll, control_pitch);
+
+	// Grab inertial velocity
+	Vector3f vel = inertial_nav.get_velocity();
+
+	// rotate roll, pitch input from north facing to vehicle's perspective
+	float roll_vel =  vel.y * cos_yaw - vel.x * sin_yaw; // body roll vel
+	float pitch_vel = vel.y * sin_yaw + vel.x * cos_yaw; // body pitch vel
+
+	float pitch_vel2 = min(fabs(pitch_vel), 800);
+	// simple gain scheduling for yaw input
+	get_yaw_rate_stabilized_ef((float)(control_roll/2) * (1.0 - (pitch_vel2 / 2400.0)));
+
+	roll_vel = constrain_float(roll_vel, -322, 322);
+	pitch_vel = constrain_float(pitch_vel, -322, 322);
+
+	// always limit roll
+	get_stabilize_roll(roll_vel * -SPEEDGAIN);
+
+	if(control_pitch == 0){
+		// .14/ (.03 * 100) = 4.6 seconds till full breaking
+		breaker+= .03;
+		breaker = min(breaker, SPEEDGAIN);
+		// If we let go of sticks, bring us to a stop
+		get_stabilize_pitch(pitch_vel * breaker);
+	}else{
+		breaker = 0.0;
+		get_stabilize_pitch(control_pitch);
+	}
+}
+

ArduCopter/events.pde

@@ -22,7 +22,7 @@ static void failsafe_radio_on_event()
             }else if(g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND) {
                 // if failsafe_throttle is 3 (i.e. FS_THR_ENABLED_ALWAYS_LAND) land immediately
                 set_mode(LAND);
-            }else if(GPS_ok() && home_distance > wp_nav.get_waypoint_radius()) {
+            }else if(home_distance > wp_nav.get_waypoint_radius()) {
                 if (!set_mode(RTL)) {
                     set_mode(LAND);
                 }
@@ -34,7 +34,7 @@ static void failsafe_radio_on_event()
         case AUTO:
             // failsafe_throttle is 1 do RTL, 2 means continue with the mission
             if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_RTL) {
-                if(GPS_ok() && home_distance > wp_nav.get_waypoint_radius()) {
+                if(home_distance > wp_nav.get_waypoint_radius()) {
                     if (!set_mode(RTL)) {
                         set_mode(LAND);
                     }
@@ -48,16 +48,33 @@ static void failsafe_radio_on_event()
             }
             // if failsafe_throttle is 2 (i.e. FS_THR_ENABLED_CONTINUE_MISSION) no need to do anything
             break;
+        case LOITER:
+        case ALT_HOLD:
+            // if landed with throttle at zero disarm, otherwise do the regular thing
+            if (g.rc_3.control_in == 0 && ap.land_complete) {
+                init_disarm_motors();
+            }else if(g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND) {
+                // if failsafe_throttle is 3 (i.e. FS_THR_ENABLED_ALWAYS_LAND) land immediately
+                set_mode(LAND);
+            }else if(home_distance > wp_nav.get_waypoint_radius()) {
+                if (!set_mode(RTL)) {
+                    set_mode(LAND);
+                }
+            }else{
+                // We have no GPS or are very close to home so we will land
+                set_mode(LAND);
+            }
+            break;
         case LAND:
             // continue to land if battery failsafe is also active otherwise fall through to default handling
-            if (g.failsafe_battery_enabled && failsafe.battery) {
+            if (g.failsafe_battery_enabled == FS_BATT_LAND && failsafe.battery) {
                 break;
             }
         default:
             if(g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND) {
                 // if failsafe_throttle is 3 (i.e. FS_THR_ENABLED_ALWAYS_LAND) land immediately
                 set_mode(LAND);
-            }else if(GPS_ok() && home_distance > wp_nav.get_waypoint_radius()) {
+            }else if(home_distance > wp_nav.get_waypoint_radius()) {
                 if (!set_mode(RTL)){
                     set_mode(LAND);
                 }
@@ -91,7 +108,7 @@ static void failsafe_battery_event(void)
     }
 
     // failsafe check
-    if (g.failsafe_battery_enabled && motors.armed()) {
+    if (g.failsafe_battery_enabled != FS_BATT_DISABLED && motors.armed()) {
         switch(control_mode) {
             case STABILIZE:
             case ACRO:
@@ -100,22 +117,42 @@ static void failsafe_battery_event(void)
                 if (g.rc_3.control_in == 0) {
                     init_disarm_motors();
                 }else{
-                    set_mode(LAND);
+                    // set mode to RTL or LAND
-                }
+                    if (g.failsafe_battery_enabled == FS_BATT_RTL && home_distance > wp_nav.get_waypoint_radius()) {
-                break;
+                        if (!set_mode(RTL)) {
-            case AUTO:
+                            set_mode(LAND);
-                // set_mode to RTL or LAND
+                        }
-                if (GPS_ok() && home_distance > wp_nav.get_waypoint_radius()) {
+                    }else{
+                        set_mode(LAND);
+                    }
+                }
+                break;
+            case AUTO:
+                // set mode to RTL or LAND
+                if (home_distance > wp_nav.get_waypoint_radius()) {
                     if (!set_mode(RTL)) {
                         set_mode(LAND);
                     }
                 }else{
-                    // We have no GPS or are very close to home so we will land
                     set_mode(LAND);
                 }
                 break;
+            case LOITER:
+            case ALT_HOLD:
+                // if landed with throttle at zero disarm, otherwise fall through to default handling
+                if (g.rc_3.control_in == 0 && ap.land_complete) {
+                    init_disarm_motors();
+                    break;
+                }
             default:
+                // set mode to RTL or LAND
+                if (g.failsafe_battery_enabled == FS_BATT_RTL && home_distance > wp_nav.get_waypoint_radius()) {
+                    if (!set_mode(RTL)) {
                         set_mode(LAND);
+                    }
+                }else{
+                    set_mode(LAND);
+                }
                 break;
         }
     }
@@ -137,8 +174,13 @@ static void failsafe_gps_check()
 {
     uint32_t last_gps_update_ms;
 
-    // return immediately if gps failsafe is disabled
+    // return immediately if gps failsafe is disabled or we have never had GPS lock
-    if( !g.failsafe_gps_enabled ) {
+    if (g.failsafe_gps_enabled == FS_GPS_DISABLED || !ap.home_is_set) {
+        // if we have just disabled the gps failsafe, ensure the gps failsafe event is cleared
+        if (failsafe.gps) {
+            failsafe_gps_off_event();
+            set_failsafe_gps(false);
+        }
         return;
     }
 
@@ -166,16 +208,14 @@ static void failsafe_gps_check()
     gcs_send_text_P(SEVERITY_LOW,PSTR("Lost GPS!"));
     Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_GPS, ERROR_CODE_FAILSAFE_OCCURRED);
 
-    // take action based on flight mode
+    // take action based on flight mode and FS_GPS_ENABLED parameter
-    if(mode_requires_GPS(control_mode))
+    if (mode_requires_GPS(control_mode) || g.failsafe_gps_enabled == FS_GPS_LAND_EVEN_STABILIZE) {
-        set_mode(LAND);
+        if (g.failsafe_gps_enabled == FS_GPS_ALTHOLD && !failsafe.radio) {
-
+            set_mode(ALT_HOLD);
-    // land if circular fence is enabled
+        }else{
-#if AC_FENCE == ENABLED
-    if((fence.get_enabled_fences() & AC_FENCE_TYPE_CIRCLE) != 0) {
             set_mode(LAND);
         }
-#endif
+    }
 }
 
 // failsafe_gps_off_event - actions to take when GPS contact is restored
@@ -227,7 +267,7 @@ static void failsafe_gcs_check()
             // if throttle is zero disarm motors
             if (g.rc_3.control_in == 0) {
                 init_disarm_motors();
-            }else if(GPS_ok() && home_distance > wp_nav.get_waypoint_radius()) {
+            }else if(home_distance > wp_nav.get_waypoint_radius()) {
                 if (!set_mode(RTL)) {
                     set_mode(LAND);
                 }
@@ -239,7 +279,7 @@ static void failsafe_gcs_check()
         case AUTO:
             // if g.failsafe_gcs is 1 do RTL, 2 means continue with the mission
             if (g.failsafe_gcs == FS_GCS_ENABLED_ALWAYS_RTL) {
-                if (GPS_ok() && home_distance > wp_nav.get_waypoint_radius()) {
+                if (home_distance > wp_nav.get_waypoint_radius()) {
                     if (!set_mode(RTL)) {
                         set_mode(LAND);
                     }
@@ -251,7 +291,7 @@ static void failsafe_gcs_check()
             // if failsafe_throttle is 2 (i.e. FS_THR_ENABLED_CONTINUE_MISSION) no need to do anything
             break;
         default:
-            if(GPS_ok() && home_distance > wp_nav.get_waypoint_radius()) {
+            if(home_distance > wp_nav.get_waypoint_radius()) {
                 if (!set_mode(RTL)) {
                     set_mode(LAND);
                 }

ArduCopter/fence.pde

@@ -35,26 +35,16 @@ void fence_check()
             if(manual_flight_mode(control_mode) && g.rc_3.control_in == 0 && !failsafe.radio && ((fence.get_breaches() & AC_FENCE_TYPE_ALT_MAX)== 0)){
                 init_disarm_motors();
             }else{
-                // if we have a GPS
-                if (GPS_ok()) {
                 // if we are within 100m of the fence, RTL
-                    if( fence.get_breach_distance(new_breaches) <= AC_FENCE_GIVE_UP_DISTANCE) {
+                if (fence.get_breach_distance(new_breaches) <= AC_FENCE_GIVE_UP_DISTANCE) {
-                        if(control_mode != RTL) {
+                    if (!set_mode(RTL)) {
-                            set_mode(RTL);
+                        set_mode(LAND);
                     }
                 }else{
                     // if more than 100m outside the fence just force a land
-                        if(control_mode != LAND) {
                     set_mode(LAND);
                 }
             }
-                }else{
-                    // we have no GPS so LAND
-                    if(control_mode != LAND) {
-                        set_mode(LAND);
-                    }
-                }
-            }
         }
 
         // log an error in the dataflash

ArduCopter/flip.pde

@@ -35,7 +35,7 @@ void roll_flip()
         if (roll < 4500) {
             // Roll control
 			roll_rate_target_bf     = 40000 * flip_dir;
-		    if(throttle_mode == THROTTLE_MANUAL || throttle_mode == THROTTLE_MANUAL_TILT_COMPENSATED){
+		    if (throttle_mode_manual(throttle_mode)){
     		    // increase throttle right before flip
                 set_throttle_out(g.rc_3.control_in + AAP_THR_INC, false);
             }
@@ -52,7 +52,7 @@ void roll_flip()
 			    roll_rate_target_bf = 40000 * flip_dir;
 		    #endif
 		    // decrease throttle while inverted
-		    if(throttle_mode == THROTTLE_MANUAL || throttle_mode == THROTTLE_MANUAL_TILT_COMPENSATED){
+		    if (throttle_mode_manual(throttle_mode)){
                 set_throttle_out(g.rc_3.control_in - AAP_THR_DEC, false);
             }
         }else{
@@ -68,7 +68,7 @@ void roll_flip()
             // It will be handled by normal flight control loops
 
             // increase throttle to gain any lost alitude
-            if(throttle_mode == THROTTLE_MANUAL || throttle_mode == THROTTLE_MANUAL_TILT_COMPENSATED){
+            if (throttle_mode_manual(throttle_mode)){
                 set_throttle_out(g.rc_3.control_in + AAP_THR_INC, false);
             }
             flip_timer++;

ArduCopter/heli.pde

@@ -0,0 +1,347 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+// Traditional helicopter variables and functions
+
+#if FRAME_CONFIG == HELI_FRAME
+
+#ifndef HELI_DYNAMIC_FLIGHT_SPEED_MIN
+ #define HELI_DYNAMIC_FLIGHT_SPEED_MIN      500     // we are in "dynamic flight" when the speed is over 1m/s for 2 seconds
+#endif
+
+// counter to control dynamic flight profile
+static int8_t heli_dynamic_flight_counter;
+
+// Tradheli flags
+static struct {
+    uint8_t dynamic_flight  : 1;    // 0   // true if we are moving at a significant speed (used to turn on/off leaky I terms)
+} heli_flags;
+
+#if HELI_CC_COMP == ENABLED
+static LowPassFilterFloat rate_dynamics_filter;     // Rate Dynamics filter
+#endif
+
+// heli_init - perform any special initialisation required for the tradheli
+static void heli_init()
+{
+#if HELI_CC_COMP == ENABLED
+    rate_dynamics_filter.set_cutoff_frequency(0.01f, 4.0f);
+#endif
+}
+
+// get_pilot_desired_collective - converts pilot input (from 0 ~ 1000) to a value that can be fed into the g.rc_3.servo_out function
+static int16_t 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;
+    }
+
+    // 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;
+}
+
+// heli_check_dynamic_flight - updates the dynamic_flight flag based on our horizontal velocity
+// should be called at 50hz
+static void check_dynamic_flight(void)
+{
+    if (!motors.armed() || throttle_mode == THROTTLE_LAND || !motors.motor_runup_complete()) {
+        heli_dynamic_flight_counter = 0;
+        heli_flags.dynamic_flight = false;
+        return;
+    }
+
+    bool moving = false;
+
+    // with GPS lock use inertial nav to determine if we are moving
+    if (GPS_ok()) {
+        // get horizontal velocity
+        float velocity = inertial_nav.get_velocity_xy();
+        moving = (velocity >= HELI_DYNAMIC_FLIGHT_SPEED_MIN);
+    }else{
+        // with no GPS lock base it on throttle and forward lean angle
+        moving = (g.rc_3.servo_out > 800 || ahrs.pitch_sensor < -1500);
+    }
+
+    if (moving) {
+        // if moving for 2 seconds, set the dynamic flight flag
+        if (!heli_flags.dynamic_flight) {
+            heli_dynamic_flight_counter++;
+            if (heli_dynamic_flight_counter >= 100) {
+                heli_flags.dynamic_flight = true;
+                heli_dynamic_flight_counter = 100;
+            }
+        }
+    }else{
+        // if not moving for 2 seconds, clear the dynamic flight flag
+        if (heli_flags.dynamic_flight) {
+            if (heli_dynamic_flight_counter > 0) {
+                heli_dynamic_flight_counter--;
+            }else{
+                heli_flags.dynamic_flight = false;
+            }
+        }
+    }
+}
+
+// heli_integrated_swash_controller - convert desired roll and pitch rate to roll and pitch swash angles
+// should be called at 100hz
+// output placed directly into g.rc_1.servo_out and g.rc_2.servo_out
+static void heli_integrated_swash_controller(int32_t target_roll_rate, int32_t target_pitch_rate)
+{
+    int32_t         roll_p, roll_i, roll_d, roll_ff;            // used to capture pid values for logging
+    int32_t         pitch_p, pitch_i, pitch_d, pitch_ff;
+	int32_t         current_roll_rate, current_pitch_rate;	    // this iteration's rate
+    int32_t         roll_rate_error, pitch_rate_error;          // simply target_rate - current_rate
+    int32_t         roll_output, pitch_output;                  // output from pid controller
+    static bool     roll_pid_saturated, pitch_pid_saturated;    // tracker from last loop if the PID was saturated
+
+    current_roll_rate = (omega.x * DEGX100);                    // get current roll rate
+    current_pitch_rate = (omega.y * DEGX100);                   // get current pitch rate
+
+    roll_rate_error = target_roll_rate - current_roll_rate;
+    pitch_rate_error = target_pitch_rate - current_pitch_rate;
+
+    roll_p = g.pid_rate_roll.get_p(roll_rate_error);
+    pitch_p = g.pid_rate_pitch.get_p(pitch_rate_error);
+
+    if (roll_pid_saturated){
+        roll_i = g.pid_rate_roll.get_integrator();                                                      // Locked Integrator due to PID saturation on previous cycle
+    } else {
+        if (motors.has_flybar()) {                                                                      // Mechanical Flybars get regular integral for rate auto trim
+            if (target_roll_rate > -50 && target_roll_rate < 50){								        // Frozen at high rates
+                roll_i = g.pid_rate_roll.get_i(roll_rate_error, G_Dt);
+            } else {
+                roll_i = g.pid_rate_roll.get_integrator();
+            }
+        } else {
+            if (heli_flags.dynamic_flight){
+                roll_i = g.pid_rate_roll.get_i(roll_rate_error, G_Dt);
+            } else {
+                roll_i = g.pid_rate_roll.get_leaky_i(roll_rate_error, G_Dt, RATE_INTEGRATOR_LEAK_RATE);
+            }
+        }
+    }
+
+    if (pitch_pid_saturated){
+        pitch_i = g.pid_rate_pitch.get_integrator();                                                    // Locked Integrator due to PID saturation on previous cycle
+    } else {
+        if (motors.has_flybar()) {                                                                      // Mechanical Flybars get regular integral for rate auto trim
+            if (target_pitch_rate > -50 && target_pitch_rate < 50){								        // Frozen at high rates
+                pitch_i = g.pid_rate_pitch.get_i(pitch_rate_error, G_Dt);
+            } else {
+                pitch_i = g.pid_rate_pitch.get_integrator();
+            }
+        } else {
+            if (heli_flags.dynamic_flight){
+                pitch_i = g.pid_rate_pitch.get_i(pitch_rate_error, G_Dt);
+            } else {
+                pitch_i = g.pid_rate_pitch.get_leaky_i(pitch_rate_error, G_Dt, RATE_INTEGRATOR_LEAK_RATE);
+            }
+        }
+    }
+
+    roll_d = g.pid_rate_roll.get_d(target_roll_rate, G_Dt);
+    pitch_d = g.pid_rate_pitch.get_d(target_pitch_rate, G_Dt);
+
+    roll_ff = g.heli_roll_ff * target_roll_rate;
+    pitch_ff = g.heli_pitch_ff * target_pitch_rate;
+    
+    roll_output = roll_p + roll_i + roll_d + roll_ff;
+    pitch_output = pitch_p + pitch_i + pitch_d + pitch_ff;
+
+#if HELI_CC_COMP == ENABLED
+// Do cross-coupling compensation for low rpm helis
+// Credit: Jolyon Saunders
+// Note: This is not widely tested at this time.  Will not be used by default yet.
+    float cc_axis_ratio = 2.0f; // Ratio of compensation on pitch vs roll axes. Number >1 means pitch is affected more than roll
+    float cc_kp = 0.0002f;      // Compensation p term. Setting this to zero gives h_phang only, while increasing it will increase the p term of correction
+    float cc_kd = 0.127f;       // Compensation d term, scaled. This accounts for flexing of the blades, dampers etc. Originally was (motors.ext_gyro_gain * 0.0001)
+    float cc_angle, cc_total_output;
+    uint32_t cc_roll_d, cc_pitch_d, cc_sum_d;
+    int32_t cc_scaled_roll;
+    int32_t cc_roll_output;     // Used to temporarily hold output while rotation is being calculated
+    int32_t cc_pitch_output;    // Used to temporarily hold output while rotation is being calculated
+    static int32_t last_roll_output = 0;
+    static int32_t last_pitch_output = 0;
+
+    cc_scaled_roll  = roll_output / cc_axis_ratio; // apply axis ratio to roll
+    cc_total_output = safe_sqrt(cc_scaled_roll * cc_scaled_roll + pitch_output * pitch_output) * cc_kp;
+    
+    // find the delta component
+    cc_roll_d  = (roll_output - last_roll_output) / cc_axis_ratio;
+    cc_pitch_d = pitch_output - last_pitch_output;
+    cc_sum_d = safe_sqrt(cc_roll_d * cc_roll_d + cc_pitch_d * cc_pitch_d);
+
+    // do the magic.
+    cc_angle = cc_kd * cc_sum_d * cc_total_output - cc_total_output * motors.get_phase_angle();
+
+    // smooth angle variations, apply constraints
+    cc_angle = rate_dynamics_filter.apply(cc_angle);
+    cc_angle = constrain_float(cc_angle, -90.0f, 0.0f);
+    cc_angle = radians(cc_angle);
+
+    // Make swash rate vector
+    Vector2f swashratevector;
+    swashratevector.x = cosf(cc_angle);
+    swashratevector.y = sinf(cc_angle);
+    swashratevector.normalize();
+
+    // rotate the output
+    cc_roll_output  = roll_output;
+    cc_pitch_output = pitch_output;
+    roll_output     = - (cc_pitch_output * swashratevector.y - cc_roll_output * swashratevector.x);
+    pitch_output    =    cc_pitch_output * swashratevector.x + cc_roll_output * swashratevector.y;
+
+    // make current outputs old, for next iteration
+    last_roll_output  = cc_roll_output;
+    last_pitch_output = cc_pitch_output;
+# endif // HELI_CC_COMP   
+    
+#if HELI_PIRO_COMP == ENABLED
+    if (control_mode <= ACRO){
+    
+        int32_t         piro_roll_i, piro_pitch_i;            // used to hold i term while doing prio comp
+    
+        piro_roll_i  = roll_i;
+        piro_pitch_i = pitch_i;
+
+        Vector2f yawratevector;
+        yawratevector.x     = cos(-omega.z/100);
+        yawratevector.y     = sin(-omega.z/100);
+        yawratevector.normalize();
+            
+        roll_i      = piro_roll_i * yawratevector.x - piro_pitch_i * yawratevector.y;
+        pitch_i     = piro_pitch_i * yawratevector.x + piro_roll_i * yawratevector.y;
+
+        g.pid_rate_pitch.set_integrator(pitch_i);
+        g.pid_rate_roll.set_integrator(roll_i); 
+    }
+#endif //HELI_PIRO_COMP   
+
+    if (labs(roll_output) > 4500){
+        roll_output = constrain_int32(roll_output, -4500, 4500);         // constrain output
+        roll_pid_saturated = true;                                       // freeze integrator next cycle
+    } else {
+        roll_pid_saturated = false;                                      // unfreeze integrator
+    }
+    
+    if (labs(pitch_output) > 4500){
+        pitch_output = constrain_int32(pitch_output, -4500, 4500);        // constrain output
+        pitch_pid_saturated = true;                                       // freeze integrator next cycle
+    } else {
+        pitch_pid_saturated = false;                                      // unfreeze integrator
+    }
+
+    g.rc_1.servo_out = roll_output;
+    g.rc_2.servo_out = pitch_output;
+}
+
+static int16_t
+get_heli_rate_yaw(int32_t target_rate)
+{
+    int32_t         p,i,d,ff;               // used to capture pid values for logging
+    int32_t         current_rate;           // this iteration's rate
+    int32_t         rate_error;
+    int32_t         output;
+    static bool     pid_saturated;          // tracker from last loop if the PID was saturated
+
+    current_rate = (omega.z * DEGX100);                         // get current rate
+
+    // rate control
+    rate_error = target_rate - current_rate;
+
+    // separately calculate p, i, d values for logging
+    p = g.pid_rate_yaw.get_p(rate_error);
+
+    if (pid_saturated){
+        i = g.pid_rate_yaw.get_integrator();                    // Locked Integrator due to PID saturation on previous cycle
+    } else {
+        if (motors.motor_runup_complete()){
+            i = g.pid_rate_yaw.get_i(rate_error, G_Dt);
+        } else {
+            i = g.pid_rate_yaw.get_leaky_i(rate_error, G_Dt, RATE_INTEGRATOR_LEAK_RATE);	// If motor is not running use leaky I-term to avoid excessive build-up
+        }
+    }
+
+    d = g.pid_rate_yaw.get_d(rate_error, G_Dt);
+
+    ff = g.heli_yaw_ff*target_rate;
+
+    output = p + i + d + ff;
+
+    if (labs(output) > 4500){
+        output = constrain_int32(output, -4500, 4500);          // constrain output
+        pid_saturated = true;                                   // freeze integrator next cycle
+    } else {
+        pid_saturated = false;                                  // unfreeze integrator
+    }
+
+#if LOGGING_ENABLED == ENABLED
+    // log output if PID loggins is on and we are tuning the yaw
+    if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_YAW_RATE_KP || g.radio_tuning == CH6_YAW_RATE_KD) ) {
+        pid_log_counter++;
+        if( pid_log_counter >= 10 ) {               // (update rate / desired output rate) = (100hz / 10hz) = 10
+            pid_log_counter = 0;
+            Log_Write_PID(CH6_YAW_RATE_KP, rate_error, p, i, d, output, tuning_value);
+        }
+    }
+#endif
+
+	return output;                                              // output control
+}
+
+// heli_update_landing_swash - sets swash plate flag so higher minimum is used when landed or landing
+// should be called soon after update_land_detector in main code
+static void heli_update_landing_swash()
+{
+    switch(throttle_mode) {
+        case THROTTLE_MANUAL:
+        case THROTTLE_MANUAL_TILT_COMPENSATED:
+        case THROTTLE_MANUAL_HELI:
+            // manual modes always uses full swash range
+            motors.set_collective_for_landing(false);
+            break;
+
+        case THROTTLE_LAND:
+            // landing always uses limit swash range
+            motors.set_collective_for_landing(true);
+            break;
+
+        case THROTTLE_HOLD:
+        case THROTTLE_AUTO:
+        default:
+            // auto and hold use limited swash when landed
+            motors.set_collective_for_landing(!heli_flags.dynamic_flight || ap.land_complete || !ap.auto_armed);
+            break;
+    }
+}
+
+// heli_update_rotor_speed_targets - reads pilot input and passes new rotor speed targets to heli motors object
+static void heli_update_rotor_speed_targets()
+{
+    // get rotor control method
+    uint8_t rsc_control_mode = motors.get_rsc_mode();
+
+    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
+            motors.set_desired_rotor_speed(g.rc_8.control_in);
+            break;
+        case AP_MOTORS_HELI_RSC_MODE_SETPOINT:
+            // pass setpoint through as desired rotor speed
+            if (g.rc_8.control_in > 0) {
+                motors.set_desired_rotor_speed(motors.get_rsc_setpoint());
+            }else{
+                motors.set_desired_rotor_speed(0);
+            }
+            break;
+    }
+}
+
+#endif  // FRAME_CONFIG == HELI_FRAME

ArduCopter/motors.pde

@@ -3,7 +3,7 @@
 #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    25  // called at 1hz so 25 seconds
+#define AUTO_DISARMING_DELAY    15  // called at 1hz so 15 seconds
 
 // arm_motors_check - checks for pilot input to arm or disarm the copter
 // called at 10hz
@@ -18,7 +18,7 @@ static void arm_motors_check()
         return;
     }
 
-    // allow arming/disarming in fully manual flight modes ACRO, STABILIZE, SPORT and TOY
+    // allow arming/disarming in fully manual flight modes ACRO, STABILIZE, SPORT and DRIFT
     if (manual_flight_mode(control_mode)) {
         allow_arming = true;
     }
@@ -35,17 +35,14 @@ static void arm_motors_check()
     }
 
     #if FRAME_CONFIG == HELI_FRAME
-	if ((motors.rsc_mode > 0) && (g.rc_8.control_in >= 10)){
+    // heli specific arming check
+    if (!motors.allow_arming()){
         arming_counter = 0;
         return;
     }
     #endif  // HELI_FRAME
 
-#if TOY_EDF == ENABLED
-    int16_t tmp = g.rc_1.control_in;
-#else
     int16_t tmp = g.rc_4.control_in;
-#endif
 
     // full right
     if (tmp > 4000) {
@@ -91,19 +88,25 @@ static void arm_motors_check()
     }
 }
 
-// auto_disarm_check - disarms the copter if it has been sitting on the ground in manual mode with throttle low for at least 25 seconds
+// auto_disarm_check - disarms the copter if it has been sitting on the ground in manual mode with throttle low for at least 15 seconds
 // called at 1hz
 static void auto_disarm_check()
 {
     static uint8_t auto_disarming_counter;
 
-    if(manual_flight_mode(control_mode) && (g.rc_3.control_in == 0) && motors.armed()) {
+    // exit immediately if we are already disarmed or throttle is not zero
+    if (!motors.armed() || g.rc_3.control_in > 0) {
+        auto_disarming_counter = 0;
+        return;
+    }
+
+    // allow auto disarm in manual flight modes or Loiter/AltHold if we're landed
+    if(manual_flight_mode(control_mode) || (ap.land_complete && (control_mode == LOITER || control_mode == ALT_HOLD))) {
         auto_disarming_counter++;
 
-        if(auto_disarming_counter == AUTO_DISARMING_DELAY) {
+        if(auto_disarming_counter >= AUTO_DISARMING_DELAY) {
             init_disarm_motors();
-        }else if (auto_disarming_counter > AUTO_DISARMING_DELAY) {
+            auto_disarming_counter = 0;
-            auto_disarming_counter = AUTO_DISARMING_DELAY + 1;
         }
     }else{
         auto_disarming_counter = 0;
@@ -122,6 +125,9 @@ static void init_arm_motors()
     // disable cpu failsafe because initialising everything takes a while
     failsafe_disable();
 
+    // disable inertial nav errors temporarily
+    inertial_nav.ignore_next_error();
+
 #if LOGGING_ENABLED == ENABLED
     // start dataflash
     start_logging();
@@ -135,8 +141,9 @@ static void init_arm_motors()
     // mid-flight, so set the serial ports non-blocking once we arm
     // the motors
     hal.uartA->set_blocking_writes(false);
-    if (gcs3.initialised) {
     hal.uartC->set_blocking_writes(false);
+    if (hal.uartD != NULL) {
+        hal.uartD->set_blocking_writes(false);
     }
 
 #if COPTER_LEDS == ENABLED
@@ -151,8 +158,10 @@ static void init_arm_motors()
 
     // Reset home position
     // -------------------
-    if(ap.home_is_set)
+    if (ap.home_is_set) {
         init_home();
+        calc_distance_and_bearing();
+    }
 
     // all I terms are invalid
     // -----------------------
@@ -160,7 +169,7 @@ static void init_arm_motors()
 
     if(did_ground_start == false) {
         did_ground_start = true;
-        startup_ground();
+        startup_ground(true);
     }
 
 #if HIL_MODE != HIL_MODE_ATTITUDE
@@ -213,13 +222,14 @@ static void init_arm_motors()
 static void pre_arm_checks(bool display_failure)
 {
     // exit immediately if we've already successfully performed the pre-arm check
-    if( ap.pre_arm_check ) {
+    if (ap.pre_arm_check) {
         return;
     }
 
     // succeed if pre arm checks are disabled
-    if(!g.arming_check_enabled) {
+    if(g.arming_check_enabled == ARMING_CHECK_NONE) {
         set_pre_arm_check(true);
+        set_pre_arm_rc_check(true);
         return;
     }
 
@@ -232,22 +242,19 @@ static void pre_arm_checks(bool display_failure)
         return;
     }
 
-    // pre-arm check to ensure ch7 and ch8 have different functions
+    // check Baro
-    if ((g.ch7_option != 0 || g.ch8_option != 0) && g.ch7_option == g.ch8_option) {
+    if ((g.arming_check_enabled == ARMING_CHECK_ALL) || (g.arming_check_enabled & ARMING_CHECK_BARO)) {
+        // barometer health check
+        if(!barometer.healthy) {
             if (display_failure) {
-            gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Ch7&Ch8 Opt cannot be same"));
+                gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Baro not healthy"));
             }
             return;
         }
-
-    // check accelerometers have been calibrated
-    if(!ins.calibrated()) {
-        if (display_failure) {
-            gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: INS not calibrated"));
-        }
-        return;
     }
 
+    // check Compass
+    if ((g.arming_check_enabled == ARMING_CHECK_ALL) || (g.arming_check_enabled & ARMING_CHECK_COMPASS)) {
         // check the compass is healthy
         if(!compass.healthy) {
             if (display_failure) {
@@ -281,35 +288,65 @@ static void pre_arm_checks(bool display_failure)
             }
             return;
         }
-
-    // barometer health check
-    if(!barometer.healthy) {
-        if (display_failure) {
-            gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Baro not healthy"));
     }
+
+    // check GPS
+    if ((g.arming_check_enabled == ARMING_CHECK_ALL) || (g.arming_check_enabled & ARMING_CHECK_GPS)) {
+        // check gps is ok if required - note this same check is repeated again in arm_checks
+        if ((mode_requires_GPS(control_mode) || g.failsafe_gps_enabled == FS_GPS_LAND_EVEN_STABILIZE) && !pre_arm_gps_checks(display_failure)) {
             return;
         }
 
 #if AC_FENCE == ENABLED
         // check fence is initialised
-    if(!fence.pre_arm_check() || (((fence.get_enabled_fences() & AC_FENCE_TYPE_CIRCLE) != 0) && !pre_arm_gps_checks())) {
+        if(!fence.pre_arm_check() || (((fence.get_enabled_fences() & AC_FENCE_TYPE_CIRCLE) != 0) && !pre_arm_gps_checks(display_failure))) {
-        if (display_failure) {
-            gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Bad GPS Pos"));
-        }
             return;
         }
 #endif
+    }
+
+    // check INS
+    if ((g.arming_check_enabled == ARMING_CHECK_ALL) || (g.arming_check_enabled & ARMING_CHECK_INS)) {
+        // check accelerometers have been calibrated
+        if(!ins.calibrated()) {
+            if (display_failure) {
+                gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: INS not calibrated"));
+            }
+            return;
+        }
+
+        // check accels and gyros are healthy
+        if(!ins.healthy()) {
+            if (display_failure) {
+                gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: INS not healthy"));
+            }
+            return;
+        }
+    }
 
 #ifndef CONFIG_ARCH_BOARD_PX4FMU_V1
     // check board voltage
+    if ((g.arming_check_enabled == ARMING_CHECK_ALL) || (g.arming_check_enabled & ARMING_CHECK_VOLTAGE)) {
         if(board_voltage() < BOARD_VOLTAGE_MIN || board_voltage() > BOARD_VOLTAGE_MAX) {
             if (display_failure) {
                 gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Check Board Voltage"));
             }
             return;
         }
+    }
 #endif
 
+    // check various parameter values
+    if ((g.arming_check_enabled == ARMING_CHECK_ALL) || (g.arming_check_enabled & ARMING_CHECK_PARAMETERS)) {
+
+        // ensure ch7 and ch8 have different functions
+        if ((g.ch7_option != 0 || g.ch8_option != 0) && g.ch7_option == g.ch8_option) {
+            if (display_failure) {
+                gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Ch7&Ch8 Opt cannot be same"));
+            }
+            return;
+        }
+
         // failsafe parameter checks
         if (g.failsafe_throttle) {
             // check throttle min is above throttle failsafe trigger and that the trigger is above ppm encoder's loss-of-signal value of 900
@@ -329,13 +366,14 @@ static void pre_arm_checks(bool display_failure)
             return;
         }
 
-    // check gps is ok if required - note this same check is repeated again in arm_checks
+        // acro balance parameter check
-    if (mode_requires_GPS(control_mode) && !pre_arm_gps_checks()) {
+        if ((g.acro_balance_roll > g.pi_stabilize_roll.kP()) || (g.acro_balance_pitch > g.pi_stabilize_pitch.kP())) {
             if (display_failure) {
-            gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Bad GPS Pos"));
+                gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: ACRO_BAL_ROLL/PITCH"));
             }
             return;
         }
+    }
 
     // if we've gotten this far then pre arm checks have completed
     set_pre_arm_check(true);
@@ -349,6 +387,12 @@ static void pre_arm_rc_checks()
         return;
     }
 
+    // set rc-checks to success if RC checks are disabled
+    if ((g.arming_check_enabled != ARMING_CHECK_ALL) && !(g.arming_check_enabled & ARMING_CHECK_RC)) {
+        set_pre_arm_rc_check(true);
+        return;
+    }
+
     // check if radio has been calibrated
     if(!g.rc_3.radio_min.load() && !g.rc_3.radio_max.load()) {
         return;
@@ -365,16 +409,19 @@ static void pre_arm_rc_checks()
     }
 
     // if we've gotten this far rc is ok
-    ap.pre_arm_rc_check = true;
+    set_pre_arm_rc_check(true);
 }
 
 // performs pre_arm gps related checks and returns true if passed
-static bool pre_arm_gps_checks()
+static bool pre_arm_gps_checks(bool display_failure)
 {
     float speed_cms = inertial_nav.get_velocity().length();     // speed according to inertial nav in cm/s
 
     // ensure GPS is ok and our speed is below 50cm/s
     if (!GPS_ok() || g_gps->hdop > g.gps_hdop_good || gps_glitch.glitching() || speed_cms == 0 || speed_cms > PREARM_MAX_VELOCITY_CMS) {
+        if (display_failure) {
+            gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Bad GPS Pos"));
+        }
         return false;
     }
 
@@ -387,17 +434,27 @@ static bool pre_arm_gps_checks()
 static bool arm_checks(bool display_failure)
 {
     // succeed if arming checks are disabled
-    if(!g.arming_check_enabled) {
+    if (g.arming_check_enabled == ARMING_CHECK_NONE) {
         return true;
     }
 
     // check gps is ok if required - note this same check is also done in pre-arm checks
-    if (mode_requires_GPS(control_mode) && !pre_arm_gps_checks()) {
+    if ((g.arming_check_enabled == ARMING_CHECK_ALL) || (g.arming_check_enabled & ARMING_CHECK_GPS)) {
+        if ((mode_requires_GPS(control_mode) || g.failsafe_gps_enabled == FS_GPS_LAND_EVEN_STABILIZE) && !pre_arm_gps_checks(display_failure)) {
+            return false;
+        }
+    }
+
+    // check parameters
+    if ((g.arming_check_enabled == ARMING_CHECK_ALL) || (g.arming_check_enabled & ARMING_CHECK_PARAMETERS)) {
+        // check throttle is above failsafe throttle
+        if (g.failsafe_throttle != FS_THR_DISABLED && g.rc_3.radio_in < g.failsafe_throttle_value) {
             if (display_failure) {
-            gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Bad GPS Pos"));
+                gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Thr below FS"));
             }
             return false;
         }
+    }
 
     // check if safety switch has been pushed
     if (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) {
@@ -411,14 +468,23 @@ static bool arm_checks(bool display_failure)
     return true;
 }
 
+// init_disarm_motors - disarm motors
 static void init_disarm_motors()
 {
+    // return immediately if we are already disarmed
+    if (!motors.armed()) {
+        return;
+    }
+
 #if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
     gcs_send_text_P(SEVERITY_HIGH, PSTR("DISARMING MOTORS"));
 #endif
 
     motors.armed(false);
 
+    // disable inertial nav errors temporarily
+    inertial_nav.ignore_next_error();
+
     compass.save_offsets();
 
     g.throttle_cruise.save();

ArduCopter/navigation.pde

@@ -42,15 +42,12 @@ static void calc_distance_and_bearing()
     }
 
     // calculate home distance and bearing
-    if( ap.home_is_set && (g_gps->status() == GPS::GPS_OK_FIX_3D || g_gps->status() == GPS::GPS_OK_FIX_2D)) {
+    if(GPS_ok()) {
         home_distance = pythagorous2(curr.x, curr.y);
         home_bearing = pv_get_bearing_cd(curr,Vector3f(0,0,0));
 
         // update super simple bearing (if required) because it relies on home_bearing
         update_super_simple_bearing(false);
-    }else{
-        home_distance = 0;
-        home_bearing = 0;
     }
 }
 
@@ -164,16 +161,6 @@ static void update_nav_mode()
         log_counter = 0;
         Log_Write_Nav_Tuning();
     }
-
-    /*
-    // To-Do: check that we haven't broken toy mode
-    case TOY_A:
-    case TOY_M:
-        set_nav_mode(NAV_NONE);
-        update_nav_wp();
-        break;
-    }
-    */
 }
 
 // Keeps old data out of our calculation / logs

ArduCopter/position_vector.pde

@@ -8,39 +8,39 @@
 //    .z = altitude above home in cm
 
 // pv_latlon_to_vector - convert lat/lon coordinates to a position vector
-const Vector3f pv_latlon_to_vector(int32_t lat, int32_t lon, int32_t alt)
+Vector3f pv_latlon_to_vector(int32_t lat, int32_t lon, int32_t alt)
 {
     Vector3f tmp((lat-home.lat) * LATLON_TO_CM, (lon-home.lng) * LATLON_TO_CM * scaleLongDown, alt);
     return tmp;
 }
 
 // pv_latlon_to_vector - convert lat/lon coordinates to a position vector
-const Vector3f pv_location_to_vector(Location loc)
+Vector3f pv_location_to_vector(Location loc)
 {
     Vector3f tmp((loc.lat-home.lat) * LATLON_TO_CM, (loc.lng-home.lng) * LATLON_TO_CM * scaleLongDown, loc.alt);
     return tmp;
 }
 
 // pv_get_lon - extract latitude from position vector
-const int32_t pv_get_lat(const Vector3f pos_vec)
+int32_t pv_get_lat(const Vector3f pos_vec)
 {
     return home.lat + (int32_t)(pos_vec.x / LATLON_TO_CM);
 }
 
 // pv_get_lon - extract longitude from position vector
-const int32_t pv_get_lon(const Vector3f &pos_vec)
+int32_t pv_get_lon(const Vector3f &pos_vec)
 {
     return home.lng + (int32_t)(pos_vec.y / LATLON_TO_CM * scaleLongUp);
 }
 
 // pv_get_horizontal_distance_cm - return distance between two positions in cm
-const float pv_get_horizontal_distance_cm(const Vector3f &origin, const Vector3f &destination)
+float pv_get_horizontal_distance_cm(const Vector3f &origin, const Vector3f &destination)
 {
     return pythagorous2(destination.x-origin.x,destination.y-origin.y);
 }
 
 // pv_get_bearing_cd - return bearing in centi-degrees between two positions
-const float pv_get_bearing_cd(const Vector3f &origin, const Vector3f &destination)
+float pv_get_bearing_cd(const Vector3f &origin, const Vector3f &destination)
 {
     float bearing = 9000 + atan2f(-(destination.x-origin.x), destination.y-origin.y) * DEGX100;
     if (bearing < 0) {

ArduCopter/radio.pde

@@ -10,6 +10,7 @@ static void default_dead_zones()
 #if FRAME_CONFIG == HELI_FRAME
     g.rc_3.set_default_dead_zone(10);
     g.rc_4.set_default_dead_zone(15);
+    g.rc_8.set_default_dead_zone(10);
 #else
     g.rc_3.set_default_dead_zone(30);
     g.rc_4.set_default_dead_zone(40);
@@ -22,29 +23,32 @@ static void init_rc_in()
     // set rc channel ranges
     g.rc_1.set_angle(ROLL_PITCH_INPUT_MAX);
     g.rc_2.set_angle(ROLL_PITCH_INPUT_MAX);
-#if FRAME_CONFIG == HELI_FRAME
-    // we do not want to limit the movment of the heli's swash plate
-    g.rc_3.set_range(0, 1000);
-#else
     g.rc_3.set_range(g.throttle_min, g.throttle_max);
-#endif
     g.rc_4.set_angle(4500);
 
     g.rc_1.set_type(RC_CHANNEL_TYPE_ANGLE_RAW);
     g.rc_2.set_type(RC_CHANNEL_TYPE_ANGLE_RAW);
     g.rc_4.set_type(RC_CHANNEL_TYPE_ANGLE_RAW);
+#if FRAME_CONFIG == SINGLE_FRAME
+    // we set four servos to angle
+    g.single_servo_1.set_type(RC_CHANNEL_TYPE_ANGLE);
+    g.single_servo_2.set_type(RC_CHANNEL_TYPE_ANGLE);
+    g.single_servo_3.set_type(RC_CHANNEL_TYPE_ANGLE);
+    g.single_servo_4.set_type(RC_CHANNEL_TYPE_ANGLE);
+    g.single_servo_1.set_angle(DEFAULT_ANGLE_MAX);
+    g.single_servo_2.set_angle(DEFAULT_ANGLE_MAX);
+    g.single_servo_3.set_angle(DEFAULT_ANGLE_MAX);
+    g.single_servo_4.set_angle(DEFAULT_ANGLE_MAX);
+#endif
 
-    //set auxiliary ranges
+    //set auxiliary servo ranges
     g.rc_5.set_range(0,1000);
     g.rc_6.set_range(0,1000);
     g.rc_7.set_range(0,1000);
     g.rc_8.set_range(0,1000);
 
-#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+    // update assigned functions for auxiliary servos
-    update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8, &g.rc_9, &g.rc_10, &g.rc_11, &g.rc_12);
+    aux_servos_update_fn();
-#elif MOUNT == ENABLED
-    update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8, &g.rc_10, &g.rc_11);
-#endif
 
     // set default dead zones
     default_dead_zones();
@@ -95,12 +99,6 @@ static void init_rc_out()
     if (ap.pre_arm_rc_check) {
         output_min();
     }
-
-#if TOY_EDF == ENABLED
-    // add access to CH8 and CH6
-    APM_RC.enable_out(CH_8);
-    APM_RC.enable_out(CH_6);
-#endif
 }
 
 // output_min - enable and output lowest possible value to motors
@@ -130,6 +128,11 @@ static void read_radio()
         g.rc_6.set_pwm(periods[5]);
         g.rc_7.set_pwm(periods[6]);
         g.rc_8.set_pwm(periods[7]);
+
+        // flag we must have an rc receiver attached
+        if (!failsafe.rc_override_active) {
+            ap.rc_receiver_present = true;
+        }
     }else{
         uint32_t elapsed = millis() - last_update;
         // turn on throttle failsafe if no update from ppm encoder for 2 seconds
@@ -183,6 +186,55 @@ static void set_throttle_and_failsafe(uint16_t throttle_pwm)
     }
 }
 
+// aux_servos_update - update auxiliary servos assigned functions in case the user has changed them
+void aux_servos_update_fn()
+{
+// Quads can use RC5 and higher as auxiliary channels
+#if (FRAME_CONFIG == QUAD_FRAME)
+ #if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+    update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8, &g.rc_9, &g.rc_10, &g.rc_11, &g.rc_12);
+ #else // APM1, APM2, SITL
+    update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8, &g.rc_10, &g.rc_11);
+ #endif
+
+// Tri's and Singles can use RC5, RC6, RC8 and higher
+#elif (FRAME_CONFIG == TRI_FRAME || FRAME_CONFIG == SINGLE_FRAME)
+ #if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+    update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_8, &g.rc_9, &g.rc_10, &g.rc_11, &g.rc_12);
+ #else // APM1, APM2, SITL
+    update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_8, &g.rc_10, &g.rc_11);
+ #endif
+
+// Hexa and Y6 can use RC7 and higher
+#elif (FRAME_CONFIG == HEXA_FRAME || FRAME_CONFIG == Y6_FRAME)
+ #if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+    update_aux_servo_function(&g.rc_7, &g.rc_8, &g.rc_9, &g.rc_10, &g.rc_11, &g.rc_12);
+ #else
+    update_aux_servo_function(&g.rc_7, &g.rc_8, &g.rc_10, &g.rc_11);
+ #endif
+
+// Octa and X8 can use RC9 and higher
+#elif (FRAME_CONFIG == OCTA_FRAME || FRAME_CONFIG == OCTA_QUAD_FRAME)
+ #if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+    update_aux_servo_function(&g.rc_9, &g.rc_10, &g.rc_11, &g.rc_12);
+ #else
+    update_aux_servo_function(&g.rc_10, &g.rc_11);
+ #endif
+
+// Heli's can use RC5, RC6, RC7, not RC8, and higher
+#elif (FRAME_CONFIG == HELI_FRAME)
+ #if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+    update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_9, &g.rc_10, &g.rc_11, &g.rc_12);
+ #else // APM1, APM2, SITL
+    update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_10, &g.rc_11);
+ #endif
+
+// throw compile error if frame type is unrecognise
+#else
+  #error Unrecognised frame type
+#endif
+}
+
 static void trim_radio()
 {
     for (uint8_t i = 0; i < 30; i++) {

ArduCopter/setup.pde

@@ -8,10 +8,6 @@ static int8_t   setup_compass           (uint8_t argc, const Menu::arg *argv);
 static int8_t   setup_compassmot        (uint8_t argc, const Menu::arg *argv);
 static int8_t   setup_erase             (uint8_t argc, const Menu::arg *argv);
 static int8_t   setup_frame             (uint8_t argc, const Menu::arg *argv);
- #if FRAME_CONFIG == HELI_FRAME
-static int8_t   setup_gyro              (uint8_t argc, const Menu::arg *argv);
-static int8_t   setup_heli              (uint8_t argc, const Menu::arg *argv);
- #endif
 static int8_t   setup_flightmodes       (uint8_t argc, const Menu::arg *argv);
 static int8_t   setup_optflow           (uint8_t argc, const Menu::arg *argv);
 static int8_t   setup_radio             (uint8_t argc, const Menu::arg *argv);
@@ -31,10 +27,6 @@ const struct Menu::command setup_menu_commands[] PROGMEM = {
     {"compassmot",                  setup_compassmot},
     {"erase",                       setup_erase},
     {"frame",                       setup_frame},
- #if FRAME_CONFIG == HELI_FRAME
-    {"gyro",                        setup_gyro},
-    {"heli",                        setup_heli},
- #endif
     {"modes",                       setup_flightmodes},
     {"optflow",                     setup_optflow},
     {"radio",                       setup_radio},
@@ -172,7 +164,7 @@ setup_compassmot(uint8_t argc, const Menu::arg *argv)
 
     // disable throttle and battery failsafe
     g.failsafe_throttle = FS_THR_DISABLED;
-    g.failsafe_battery_enabled = false;
+    g.failsafe_battery_enabled = FS_BATT_DISABLED;
 
     // read radio
     read_radio();
@@ -365,253 +357,6 @@ setup_frame(uint8_t argc, const Menu::arg *argv)
     return 0;
 }
 
-#if FRAME_CONFIG == HELI_FRAME
-// setup for external tail gyro (for heli only)
-static int8_t
-setup_gyro(uint8_t argc, const Menu::arg *argv)
-{
-    if (!strcmp_P(argv[1].str, PSTR("on"))) {
-        motors.ext_gyro_enabled.set_and_save(true);
-
-        // optionally capture the gain
-        if( argc >= 2 && argv[2].i >= 1000 && argv[2].i <= 2000 ) {
-            motors.ext_gyro_gain = argv[2].i;
-            motors.ext_gyro_gain.save();
-        }
-
-    } else if (!strcmp_P(argv[1].str, PSTR("off"))) {
-        motors.ext_gyro_enabled.set_and_save(false);
-
-        // capture gain if user simply provides a number
-    } else if( argv[1].i >= 1000 && argv[1].i <= 2000 ) {
-        motors.ext_gyro_enabled.set_and_save(true);
-        motors.ext_gyro_gain = argv[1].i;
-        motors.ext_gyro_gain.save();
-
-    }else{
-        cliSerial->printf_P(PSTR("\nOp:[on, off] gain\n"));
-    }
-
-    report_gyro();
-    return 0;
-}
-
-// Perform heli setup.
-// Called by the setup menu 'radio' command.
-static int8_t
-setup_heli(uint8_t argc, const Menu::arg *argv)
-{
-
-    uint8_t active_servo = 0;
-    int16_t value = 0;
-    int16_t temp;
-    int16_t state = 0;       // 0 = set rev+pos, 1 = capture min/max
-    int16_t max_roll=0, max_pitch=0, min_collective=0, max_collective=0, min_tail=0, max_tail=0;
-
-    // initialise swash plate
-    motors.init_swash();
-
-    // source swash plate movements directly from radio
-    motors.servo_manual = true;
-
-    // display initial settings
-    report_heli();
-
-    // display help
-    cliSerial->printf_P(PSTR("Instructions:"));
-    print_divider();
-    cliSerial->printf_P(PSTR("\td\t\tdisplay settings\n"));
-    cliSerial->printf_P(PSTR("\t1~4\t\tselect servo\n"));
-    cliSerial->printf_P(PSTR("\ta or z\t\tmove mid up/down\n"));
-    cliSerial->printf_P(PSTR("\tc\t\tset coll when blade pitch zero\n"));
-    cliSerial->printf_P(PSTR("\tm\t\tset roll, pitch, coll min/max\n"));
-    cliSerial->printf_P(PSTR("\tp<angle>\tset pos (i.e. p0 = front, p90 = right)\n"));
-    cliSerial->printf_P(PSTR("\tr\t\treverse servo\n"));
-    cliSerial->printf_P(PSTR("\tu a|d\t\tupdate rate (a=analog servo, d=digital)\n"));
-    cliSerial->printf_P(PSTR("\tt<angle>\tset trim (-500 ~ 500)\n"));
-    cliSerial->printf_P(PSTR("\tx\t\texit & save\n"));
-
-    // start capturing
-    while( value != 'x' ) {
-
-        // read radio although we don't use it yet
-        read_radio();
-
-        // allow swash plate to move
-        motors.output_armed();
-
-        // record min/max
-        if( state == 1 ) {
-            if( abs(g.rc_1.control_in) > max_roll )
-                max_roll = abs(g.rc_1.control_in);
-            if( abs(g.rc_2.control_in) > max_pitch )
-                max_pitch = abs(g.rc_2.control_in);
-            if( g.rc_3.radio_out < min_collective )
-                min_collective = g.rc_3.radio_out;
-            if( g.rc_3.radio_out > max_collective )
-                max_collective = g.rc_3.radio_out;
-            min_tail = min(g.rc_4.radio_out, min_tail);
-            max_tail = max(g.rc_4.radio_out, max_tail);
-        }
-
-        if( cliSerial->available() ) {
-            value = cliSerial->read();
-
-            // process the user's input
-            switch( value ) {
-            case '1':
-                active_servo = CH_1;
-                break;
-            case '2':
-                active_servo = CH_2;
-                break;
-            case '3':
-                active_servo = CH_3;
-                break;
-            case '4':
-                active_servo = CH_4;
-                break;
-            case 'a':
-            case 'A':
-                heli_get_servo(active_servo)->radio_trim += 10;
-                break;
-            case 'c':
-            case 'C':
-                if( g.rc_3.radio_out >= 900 && g.rc_3.radio_out <= 2100 ) {
-                    motors.collective_mid = g.rc_3.radio_out;
-                    cliSerial->printf_P(PSTR("Collective when blade pitch at zero: %d\n"),(int)motors.collective_mid);
-                }
-                break;
-            case 'd':
-            case 'D':
-                // display settings
-                report_heli();
-                break;
-            case 'm':
-            case 'M':
-                if( state == 0 ) {
-                    state = 1;                          // switch to capture min/max mode
-                    cliSerial->printf_P(PSTR("Move coll, roll, pitch and tail to extremes, press 'm' when done\n"));
-
-                    // reset servo ranges
-                    motors.roll_max = motors.pitch_max = 4500;
-                    motors.collective_min = 1000;
-                    motors.collective_max = 2000;
-                    motors._servo_4->radio_min = 1000;
-                    motors._servo_4->radio_max = 2000;
-
-                    // set sensible values in temp variables
-                    max_roll = abs(g.rc_1.control_in);
-                    max_pitch = abs(g.rc_2.control_in);
-                    min_collective = 2000;
-                    max_collective = 1000;
-                    min_tail = max_tail = abs(g.rc_4.radio_out);
-                }else{
-                    state = 0;                          // switch back to normal mode
-                    // double check values aren't totally terrible
-                    if( max_roll <= 1000 || max_pitch <= 1000 || (max_collective - min_collective < 200) || (max_tail - min_tail < 200) || min_tail < 1000 || max_tail > 2000 )
-                        cliSerial->printf_P(PSTR("Invalid min/max captured roll:%d,  pitch:%d,  collective min: %d max: %d,  tail min:%d max:%d\n"),max_roll,max_pitch,min_collective,max_collective,min_tail,max_tail);
-                    else{
-                        motors.roll_max = max_roll;
-                        motors.pitch_max = max_pitch;
-                        motors.collective_min = min_collective;
-                        motors.collective_max = max_collective;
-                        motors._servo_4->radio_min = min_tail;
-                        motors._servo_4->radio_max = max_tail;
-
-                        // reinitialise swash
-                        motors.init_swash();
-
-                        // display settings
-                        report_heli();
-                    }
-                }
-                break;
-            case 'p':
-            case 'P':
-                temp = read_num_from_serial();
-                if( temp >= -360 && temp <= 360 ) {
-                    if( active_servo == CH_1 )
-                        motors.servo1_pos = temp;
-                    if( active_servo == CH_2 )
-                        motors.servo2_pos = temp;
-                    if( active_servo == CH_3 )
-                        motors.servo3_pos = temp;
-                    motors.init_swash();
-                    cliSerial->printf_P(PSTR("Servo %d\t\tpos:%d\n"),active_servo+1, temp);
-                }
-                break;
-            case 'r':
-            case 'R':
-                heli_get_servo(active_servo)->set_reverse(!heli_get_servo(active_servo)->get_reverse());
-                break;
-            case 't':
-            case 'T':
-                temp = read_num_from_serial();
-                if( temp > 1000 )
-                    temp -= 1500;
-                if( temp > -500 && temp < 500 ) {
-                    heli_get_servo(active_servo)->radio_trim = 1500 + temp;
-                    motors.init_swash();
-                    cliSerial->printf_P(PSTR("Servo %d\t\ttrim:%d\n"),active_servo+1, 1500 + temp);
-                }
-                break;
-            case 'u':
-            case 'U':
-                temp = 0;
-                // delay up to 2 seconds for servo type from user
-                while( !cliSerial->available() && temp < 20 ) {
-                    temp++;
-                    delay(100);
-                }
-                if( cliSerial->available() ) {
-                    value = cliSerial->read();
-                    if( value == 'a' || value == 'A' ) {
-                        g.rc_speed.set_and_save(AP_MOTORS_HELI_SPEED_ANALOG_SERVOS);
-                        //motors._speed_hz = AP_MOTORS_HELI_SPEED_ANALOG_SERVOS;  // need to force this update to take effect immediately
-                        cliSerial->printf_P(PSTR("Analog Servo %dhz\n"),(int)g.rc_speed);
-                    }
-                    if( value == 'd' || value == 'D' ) {
-                        g.rc_speed.set_and_save(AP_MOTORS_HELI_SPEED_ANALOG_SERVOS);
-                        //motors._speed_hz = AP_MOTORS_HELI_SPEED_ANALOG_SERVOS;  // need to force this update to take effect immediately
-                        cliSerial->printf_P(PSTR("Digital Servo %dhz\n"),(int)g.rc_speed);
-                    }
-                }
-                break;
-            case 'z':
-            case 'Z':
-                heli_get_servo(active_servo)->radio_trim -= 10;
-                break;
-            }
-        }
-
-        delay(20);
-    }
-
-    // display final settings
-    report_heli();
-
-    // save to eeprom
-    motors._servo_1->save_eeprom();
-    motors._servo_2->save_eeprom();
-    motors._servo_3->save_eeprom();
-    motors._servo_4->save_eeprom();
-    motors.servo1_pos.save();
-    motors.servo2_pos.save();
-    motors.servo3_pos.save();
-    motors.roll_max.save();
-    motors.pitch_max.save();
-    motors.collective_min.save();
-    motors.collective_max.save();
-    motors.collective_mid.save();
-
-    // return swash plate movements to attitude controller
-    motors.servo_manual = false;
-
-    return(0);
-}
-#endif // FRAME_CONFIG == HELI
-
 static int8_t
 setup_flightmodes(uint8_t argc, const Menu::arg *argv)
 {
@@ -926,11 +671,6 @@ setup_show(uint8_t argc, const Menu::arg *argv)
     report_compass();
     report_optflow();
 
- #if FRAME_CONFIG == HELI_FRAME
-    report_heli();
-    report_gyro();
- #endif
-
     AP_Param::show_all(cliSerial);
 
     return(0);
@@ -1094,44 +834,6 @@ void report_optflow()
  #endif     // OPTFLOW == ENABLED
 }
 
- #if FRAME_CONFIG == HELI_FRAME
-static void report_heli()
-{
-    cliSerial->printf_P(PSTR("Heli\n"));
-    print_divider();
-
-    // main servo settings
-    cliSerial->printf_P(PSTR("Servo \tpos \tmin \tmax \trev\n"));
-    cliSerial->printf_P(PSTR("1:\t%d \t%d \t%d \t%d\n"),(int)motors.servo1_pos, (int)motors._servo_1->radio_min, (int)motors._servo_1->radio_max, (int)motors._servo_1->get_reverse());
-    cliSerial->printf_P(PSTR("2:\t%d \t%d \t%d \t%d\n"),(int)motors.servo2_pos, (int)motors._servo_2->radio_min, (int)motors._servo_2->radio_max, (int)motors._servo_2->get_reverse());
-    cliSerial->printf_P(PSTR("3:\t%d \t%d \t%d \t%d\n"),(int)motors.servo3_pos, (int)motors._servo_3->radio_min, (int)motors._servo_3->radio_max, (int)motors._servo_3->get_reverse());
-    cliSerial->printf_P(PSTR("tail:\t\t%d \t%d \t%d\n"), (int)motors._servo_4->radio_min, (int)motors._servo_4->radio_max, (int)motors._servo_4->get_reverse());
-
-    cliSerial->printf_P(PSTR("roll max: \t%d\n"), (int)motors.roll_max);
-    cliSerial->printf_P(PSTR("pitch max: \t%d\n"), (int)motors.pitch_max);
-    cliSerial->printf_P(PSTR("coll min:\t%d\t mid:%d\t max:%d\n"),(int)motors.collective_min, (int)motors.collective_mid, (int)motors.collective_max);
-
-    // calculate and print servo rate
-    cliSerial->printf_P(PSTR("servo rate:\t%d hz\n"),(int)g.rc_speed);
-
-    print_blanks(2);
-}
-
-static void report_gyro()
-{
-
-    cliSerial->printf_P(PSTR("Gyro:\n"));
-    print_divider();
-
-    print_enabled( motors.ext_gyro_enabled );
-    if( motors.ext_gyro_enabled )
-        cliSerial->printf_P(PSTR("gain: %d"),(int)motors.ext_gyro_gain);
-
-    print_blanks(2);
-}
-
- #endif // FRAME_CONFIG == HELI_FRAME
-
 /***************************************************************************/
 // CLI utilities
 /***************************************************************************/
@@ -1182,8 +884,8 @@ static void zero_eeprom(void)
 static void
 print_accel_offsets_and_scaling(void)
 {
-    Vector3f accel_offsets = ins.get_accel_offsets();
+    const Vector3f &accel_offsets = ins.get_accel_offsets();
-    Vector3f accel_scale = ins.get_accel_scale();
+    const Vector3f &accel_scale = ins.get_accel_scale();
     cliSerial->printf_P(PSTR("A_off: %4.2f, %4.2f, %4.2f\nA_scale: %4.2f, %4.2f, %4.2f\n"),
                     (float)accel_offsets.x,                           // Pitch
                     (float)accel_offsets.y,                           // Roll
@@ -1196,49 +898,13 @@ print_accel_offsets_and_scaling(void)
 static void
 print_gyro_offsets(void)
 {
-    Vector3f gyro_offsets = ins.get_gyro_offsets();
+    const Vector3f &gyro_offsets = ins.get_gyro_offsets();
     cliSerial->printf_P(PSTR("G_off: %4.2f, %4.2f, %4.2f\n"),
                     (float)gyro_offsets.x,
                     (float)gyro_offsets.y,
                     (float)gyro_offsets.z);
 }
 
- #if FRAME_CONFIG == HELI_FRAME
-
-static RC_Channel *
-heli_get_servo(int16_t servo_num){
-    if( servo_num == CH_1 )
-        return motors._servo_1;
-    if( servo_num == CH_2 )
-        return motors._servo_2;
-    if( servo_num == CH_3 )
-        return motors._servo_3;
-    if( servo_num == CH_4 )
-        return motors._servo_4;
-    return NULL;
-}
-
-// Used to read integer values from the serial port
-static int16_t read_num_from_serial() {
-    uint8_t index = 0;
-    uint8_t timeout = 0;
-    char data[5] = "";
-
-    do {
-        if (cliSerial->available() == 0) {
-            delay(10);
-            timeout++;
-        }else{
-            data[index] = cliSerial->read();
-            timeout = 0;
-            index++;
-        }
-    } while (timeout < 5 && index < 5);
-
-    return atoi(data);
-}
- #endif
-
 #endif // CLI_ENABLED
 
 static void

ArduCopter/system.pde

@@ -103,7 +103,7 @@ static void init_ardupilot()
     hal.uartB->begin(38400, 256, 16);
 #endif
 
-    cliSerial->printf_P(PSTR("\n\nInit " THISFIRMWARE
+    cliSerial->printf_P(PSTR("\n\nInit " FIRMWARE_STRING
                          "\n\nFree RAM: %u\n"),
                     memcheck_available_memory());
 
@@ -134,7 +134,7 @@ static void init_ardupilot()
 #endif
 
     // init the GCS
-    gcs0.init(hal.uartA);
+    gcs[0].init(hal.uartA);
 
     // Register the mavlink service callback. This will run
     // anytime there are more than 5ms remaining in a call to
@@ -150,8 +150,14 @@ static void init_ardupilot()
     // we have a 2nd serial port for telemetry on all boards except
     // APM2. We actually do have one on APM2 but it isn't necessary as
     // a MUX is used
-    hal.uartC->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128);
+    hal.uartC->begin(map_baudrate(g.serial1_baud, SERIAL1_BAUD), 128, 128);
-    gcs3.init(hal.uartC);
+    gcs[1].init(hal.uartC);
+#endif
+#if MAVLINK_COMM_NUM_BUFFERS > 2
+    if (hal.uartD != NULL) {
+        hal.uartD->begin(map_baudrate(g.serial2_baud, SERIAL2_BAUD), 128, 128);
+        gcs[2].init(hal.uartD);
+    }
 #endif
 
     // identify ourselves correctly with the ground station
@@ -166,15 +172,10 @@ static void init_ardupilot()
     } else if (DataFlash.NeedErase()) {
         gcs_send_text_P(SEVERITY_LOW, PSTR("ERASING LOGS"));
         do_erase_logs();
-        gcs0.reset_cli_timeout();
+        gcs[0].reset_cli_timeout();
     }
 #endif
 
-#if FRAME_CONFIG == HELI_FRAME
-    motors.servo_manual = false;
-    motors.init_swash();              // heli initialisation
-#endif
-
     init_rc_in();               // sets up rc channels from radio
     init_rc_out();              // sets up motors and output to escs
 
@@ -214,9 +215,12 @@ static void init_ardupilot()
 #if CLI_ENABLED == ENABLED
     const prog_char_t *msg = PSTR("\nPress ENTER 3 times to start interactive setup\n");
     cliSerial->println_P(msg);
-    if (gcs3.initialised) {
+    if (gcs[1].initialised) {
         hal.uartC->println_P(msg);
     }
+    if (num_gcs > 2 && gcs[2].initialised) {
+        hal.uartD->println_P(msg);
+    }
 #endif // CLI_ENABLED
 
 #if HIL_MODE != HIL_MODE_DISABLED
@@ -239,11 +243,6 @@ static void init_ardupilot()
     init_sonar();
 #endif
 
-#if FRAME_CONFIG == HELI_FRAME
-    // initialise controller filters
-    init_rate_controllers();
-#endif // HELI_FRAME
-
     // initialize commands
     // -------------------
     init_commands();
@@ -253,7 +252,12 @@ static void init_ardupilot()
     reset_control_switch();
     init_aux_switches();
 
-    startup_ground();
+#if FRAME_CONFIG == HELI_FRAME
+    // trad heli specific initialisation
+    heli_init();
+#endif
+
+    startup_ground(true);
 
 #if LOGGING_ENABLED == ENABLED
     Log_Write_Startup();
@@ -266,7 +270,7 @@ static void init_ardupilot()
 //******************************************************************************
 //This function does all the calibrations, etc. that we need during a ground start
 //******************************************************************************
-static void startup_ground(void)
+static void startup_ground(bool force_gyro_cal)
 {
     gcs_send_text_P(SEVERITY_LOW,PSTR("GROUND START"));
 
@@ -275,7 +279,7 @@ static void startup_ground(void)
 
     // Warm up and read Gyro offsets
     // -----------------------------
-    ins.init(AP_InertialSensor::COLD_START,
+    ins.init(force_gyro_cal?AP_InertialSensor::COLD_START:AP_InertialSensor::WARM_START,
              ins_sample_rate);
  #if CLI_ENABLED == ENABLED
     report_ins();
@@ -291,7 +295,7 @@ static void startup_ground(void)
 // returns true if the GPS is ok and home position is set
 static bool GPS_ok()
 {
-    if (g_gps != NULL && ap.home_is_set && g_gps->status() == GPS::GPS_OK_FIX_3D) {
+    if (g_gps != NULL && ap.home_is_set && g_gps->status() == GPS::GPS_OK_FIX_3D && !gps_glitch.glitching() && !failsafe.gps) {
         return true;
     }else{
         return false;
@@ -307,6 +311,7 @@ static bool mode_requires_GPS(uint8_t mode) {
         case RTL:
         case CIRCLE:
         case POSITION:
+        case DRIFT:
             return true;
         default:
             return false;
@@ -320,8 +325,7 @@ static bool manual_flight_mode(uint8_t mode) {
     switch(mode) {
         case ACRO:
         case STABILIZE:
-        case TOY_A:
+        case DRIFT:
-        case TOY_M:
         case SPORT:
             return true;
         default:
@@ -332,8 +336,9 @@ static bool manual_flight_mode(uint8_t mode) {
 }
 
 // set_mode - change flight mode and perform any necessary initialisation
+// optional force parameter used to force the flight mode change (used only first time mode is set)
 // returns true if mode was succesfully set
-// STABILIZE, ACRO, SPORT and LAND can always be set successfully but the return state of other flight modes should be checked and the caller should deal with failures appropriately
+// ACRO, STABILIZE, ALTHOLD, LAND, DRIFT and SPORT can always be set successfully but the return state of other flight modes should be checked and the caller should deal with failures appropriately
 static bool set_mode(uint8_t mode)
 {
     // boolean to record if flight mode could be set
@@ -356,9 +361,9 @@ static bool set_mode(uint8_t mode)
 
         case STABILIZE:
             success = true;
-            set_yaw_mode(YAW_HOLD);
+            set_yaw_mode(STABILIZE_YAW);
-            set_roll_pitch_mode(ROLL_PITCH_STABLE);
+            set_roll_pitch_mode(STABILIZE_RP);
-            set_throttle_mode(THROTTLE_MANUAL_TILT_COMPENSATED);
+            set_throttle_mode(STABILIZE_THR);
             set_nav_mode(NAV_NONE);
             break;
 
@@ -441,26 +446,12 @@ static bool set_mode(uint8_t mode)
             }
             break;
 
-        // THOR
+        case DRIFT:
-        // These are the flight modes for Toy mode
-        // See the defines for the enumerated values
-        case TOY_A:
             success = true;
-            set_yaw_mode(YAW_TOY);
+            set_yaw_mode(YAW_DRIFT);
-            set_roll_pitch_mode(ROLL_PITCH_TOY);
+            set_roll_pitch_mode(ROLL_PITCH_DRIFT);
-            set_throttle_mode(THROTTLE_AUTO);
             set_nav_mode(NAV_NONE);
-
+            set_throttle_mode(DRIFT_THR);
-            // save throttle for fast exit of Alt hold
-            saved_toy_throttle = g.rc_3.control_in;
-            break;
-
-        case TOY_M:
-            success = true;
-            set_yaw_mode(YAW_TOY);
-            set_roll_pitch_mode(ROLL_PITCH_TOY);
-            set_nav_mode(NAV_NONE);
-            set_throttle_mode(THROTTLE_HOLD);
             break;
 
         case SPORT:
@@ -509,10 +500,18 @@ static void update_auto_armed()
         }
     }else{
         // arm checks
+        
+#if FRAME_CONFIG == HELI_FRAME
+        // for tradheli if motors are armed and throttle is above zero and the motor is started, auto_armed should be true
+        if(motors.armed() && g.rc_3.control_in != 0 && motors.motor_runup_complete()) {
+            set_auto_armed(true);
+        }
+#else
         // if motors are armed and throttle is above zero auto_armed should be true
         if(motors.armed() && g.rc_3.control_in != 0) {
             set_auto_armed(true);
         }
+#endif // HELI_FRAME
     }
 }
 
@@ -532,7 +531,7 @@ static uint32_t map_baudrate(int8_t rate, uint32_t default_baud)
     case 111:  return 111100;
     case 115:  return 115200;
     }
-    //cliSerial->println_P(PSTR("Invalid SERIAL3_BAUD"));
+    //cliSerial->println_P(PSTR("Invalid baudrate"));
     return default_baud;
 }
 
@@ -550,11 +549,11 @@ static void check_usb_mux(void)
     // the APM2 has a MUX setup where the first serial port switches
     // between USB and a TTL serial connection. When on USB we use
     // SERIAL0_BAUD, but when connected as a TTL serial port we run it
-    // at SERIAL3_BAUD.
+    // at SERIAL1_BAUD.
     if (ap.usb_connected) {
         hal.uartA->begin(SERIAL0_BAUD);
     } else {
-        hal.uartA->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD));
+        hal.uartA->begin(map_baudrate(g.serial1_baud, SERIAL1_BAUD));
     }
 #endif
 }
@@ -607,11 +606,8 @@ print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
     case OF_LOITER:
         port->print_P(PSTR("OF_LOITER"));
         break;
-    case TOY_M:
+    case DRIFT:
-        port->print_P(PSTR("TOY_M"));
+        port->print_P(PSTR("DRIFT"));
-        break;
-    case TOY_A:
-        port->print_P(PSTR("TOY_A"));
         break;
     case SPORT:
         port->print_P(PSTR("SPORT"));

ArduCopter/test.pde

@@ -284,8 +284,7 @@ test_optflow(uint8_t argc, const Menu::arg *argv)
 
         while(1) {
             delay(200);
-            optflow.update(millis());
+            optflow.update();
-            Log_Write_Optflow();
             cliSerial->printf_P(PSTR("x/dx: %d/%d\t y/dy %d/%d\t squal:%d\n"),
                             optflow.x,
                             optflow.dx,

ArduCopter/toy.pde

@@ -1,175 +0,0 @@
-////////////////////////////////////////////////////////////////////////////////
-// Toy Mode - THOR
-////////////////////////////////////////////////////////////////////////////////
-static bool CH7_toy_flag;
-
-#if TOY_MIXER == TOY_LOOKUP_TABLE
-static const int16_t toy_lookup[] = {
-    186,    373,    558,    745,
-    372,    745,    1117,   1490,
-    558,    1118,   1675,   2235,
-    743,    1490,   2233,   2980,
-    929,    1863,   2792,   3725,
-    1115,   2235,   3350,   4470,
-    1301,   2608,   3908,   4500,
-    1487,   2980,   4467,   4500,
-    1673,   3353,   4500,   4500
-};
-#endif
-
-//called at 10hz
-void update_toy_throttle()
-{
-    if(control_mode == TOY_A) {
-        // look for a change in throttle position to exit throttle hold
-        if(abs(g.rc_3.control_in - saved_toy_throttle) > 40) {
-            throttle_mode   = THROTTLE_MANUAL;
-        }
-
-        if(throttle_mode == THROTTLE_AUTO) {
-            update_toy_altitude();
-        }
-    }
-}
-
-#define TOY_ALT_SMALL 25
-#define TOY_ALT_LARGE 100
-
-//called at 10hz
-void update_toy_altitude()
-{
-    int16_t input = g.rc_3.radio_in;     // throttle
-    float current_target_alt = wp_nav.get_desired_alt();
-    //int16_t input = g.rc_7.radio_in;
-
-    // Trigger upward alt change
-    if(false == CH7_toy_flag && input > 1666) {
-        CH7_toy_flag = true;
-        // go up
-        if(current_target_alt >= 400) {
-            wp_nav.set_desired_alt(current_target_alt + TOY_ALT_LARGE);
-        }else{
-            wp_nav.set_desired_alt(current_target_alt + TOY_ALT_SMALL);
-        }
-
-        // Trigger downward alt change
-    }else if(false == CH7_toy_flag && input < 1333) {
-        CH7_toy_flag = true;
-        // go down
-        if(current_target_alt >= (400 + TOY_ALT_LARGE)) {
-            wp_nav.set_desired_alt(current_target_alt - TOY_ALT_LARGE);
-        }else if(current_target_alt >= TOY_ALT_SMALL) {
-            wp_nav.set_desired_alt(current_target_alt - TOY_ALT_SMALL);
-        }else if(current_target_alt < TOY_ALT_SMALL) {
-            wp_nav.set_desired_alt(0); 
-        }
-
-        // clear flag
-    }else if (CH7_toy_flag && ((input < 1666) && (input > 1333))) {
-        CH7_toy_flag = false;
-    }
-}
-
-// called at 50 hz from all flight modes
-#if TOY_EDF == ENABLED
-void edf_toy()
-{
-    // EDF control:
-    g.rc_8.radio_out = 1000 + ((abs(g.rc_2.control_in) << 1) / 9);
-    if(g.rc_8.radio_out < 1050)
-        g.rc_8.radio_out = 1000;
-
-    // output throttle to EDF
-    if(motors.armed()) {
-        APM_RC.OutputCh(CH_8, g.rc_8.radio_out);
-    }else{
-        APM_RC.OutputCh(CH_8, 1000);
-    }
-
-    // output Servo direction
-    if(g.rc_2.control_in > 0) {
-        APM_RC.OutputCh(CH_6, 1000);         // 1000 : 2000
-    }else{
-        APM_RC.OutputCh(CH_6, 2000);         // less than 1450
-    }
-}
-#endif
-
-// The function call for managing the flight mode Toy
-void roll_pitch_toy()
-{
-#if TOY_MIXER == TOY_LOOKUP_TABLE || TOY_MIXER == TOY_LINEAR_MIXER
-    int16_t yaw_rate = g.rc_1.control_in / g.toy_yaw_rate;
-
-    if(g.rc_1.control_in != 0) {    // roll
-        get_acro_yaw(yaw_rate/2);
-        ap.yaw_stopped = false;
-        yaw_timer = 150;
-
-    }else if (!ap.yaw_stopped) {
-        get_acro_yaw(0);
-        yaw_timer--;
-
-        if((yaw_timer == 0) || (fabsf(omega.z) < 0.17f)) {
-            ap.yaw_stopped = true;
-            nav_yaw = ahrs.yaw_sensor;
-        }
-    }else{
-        if(motors.armed() == false || g.rc_3.control_in == 0)
-            nav_yaw = ahrs.yaw_sensor;
-
-        get_stabilize_yaw(nav_yaw);
-    }
-#endif
-
-    // roll_rate is the outcome of the linear equation or lookup table
-    // based on speed and Yaw rate
-    int16_t roll_rate = 0;
-
-#if TOY_MIXER == TOY_LOOKUP_TABLE
-    uint8_t xx, yy;
-    // Lookup value
-    //xx	= g_gps->ground_speed / 200;
-    xx      = abs(g.rc_2.control_in / 1000);
-    yy      = abs(yaw_rate / 500);
-
-    // constrain to lookup Array range
-    xx = constrain_int16(xx, 0, 3);
-    yy = constrain_int16(yy, 0, 8);
-
-    roll_rate = toy_lookup[yy * 4 + xx];
-
-    if(yaw_rate == 0) {
-        roll_rate = 0;
-    }else if(yaw_rate < 0) {
-        roll_rate = -roll_rate;
-    }
-
-    int16_t roll_limit = 4500 / g.toy_yaw_rate;
-    roll_rate = constrain_int16(roll_rate, -roll_limit, roll_limit);
-
-#elif TOY_MIXER == TOY_LINEAR_MIXER
-    roll_rate = -((int32_t)g.rc_2.control_in * (yaw_rate/100)) /30;
-    roll_rate = constrain_int32(roll_rate, -2000, 2000);
-
-#elif TOY_MIXER == TOY_EXTERNAL_MIXER
-    // JKR update to allow external roll/yaw mixing
-    roll_rate = g.rc_1.control_in;
-#endif
-
-#if TOY_EDF == ENABLED
-    // Output the attitude
-    //g.rc_1.servo_out = get_stabilize_roll(roll_rate);
-    //g.rc_2.servo_out = get_stabilize_pitch(g.rc_6.control_in);             // use CH6 to trim pitch
-    get_stabilize_roll(roll_rate);
-    get_stabilize_pitch(g.rc_6.control_in);             // use CH6 to trim pitch
-
-#else
-    // Output the attitude
-    //g.rc_1.servo_out = get_stabilize_roll(roll_rate);
-    //g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in);
-    get_stabilize_roll(roll_rate);
-    get_stabilize_pitch(g.rc_2.control_in);
-#endif
-
-}

ArduPlane/ArduPlane.pde

@@ -1,6 +1,6 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
-#define THISFIRMWARE "ArduPlane V2.75beta4"
+#define THISFIRMWARE "ArduPlane V2.76"
 /*
    Lead developer: Andrew Tridgell
  
@@ -147,11 +147,12 @@ static void print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode);
 ////////////////////////////////////////////////////////////////////////////////
 #if LOGGING_ENABLED == ENABLED
 #if CONFIG_HAL_BOARD == HAL_BOARD_APM1
-DataFlash_APM1 DataFlash;
+static DataFlash_APM1 DataFlash;
 #elif CONFIG_HAL_BOARD == HAL_BOARD_APM2
-DataFlash_APM2 DataFlash;
+static DataFlash_APM2 DataFlash;
 #elif CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
-DataFlash_SITL DataFlash;
+//static DataFlash_File DataFlash("logs");
+static DataFlash_SITL DataFlash;
 #elif CONFIG_HAL_BOARD == HAL_BOARD_PX4
 static DataFlash_File DataFlash("/fs/microsd/APM/logs");
 #elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX
@@ -162,6 +163,10 @@ DataFlash_Empty DataFlash;
 #endif
 #endif
 
+// scaled roll limit based on pitch
+static int32_t roll_limit_cd;
+static int32_t pitch_limit_min_cd;
+
 ////////////////////////////////////////////////////////////////////////////////
 // Sensors
 ////////////////////////////////////////////////////////////////////////////////
@@ -258,7 +263,7 @@ AP_InertialSensor_L3G4200D ins;
   #error Unrecognised CONFIG_INS_TYPE setting.
 #endif // CONFIG_INS_TYPE
 
-AP_AHRS_DCM ahrs(&ins, g_gps);
+AP_AHRS_DCM ahrs(ins, g_gps);
 
 static AP_L1_Control L1_controller(ahrs);
 static AP_TECS TECS_controller(ahrs, aparm);
@@ -774,6 +779,9 @@ void loop()
     G_Dt                = delta_us_fast_loop * 1.0e-6f;
     fast_loopTimer_us   = timer;
 
+    if (delta_us_fast_loop > G_Dt_max)
+        G_Dt_max = delta_us_fast_loop;
+
     mainLoop_count++;
 
     // tell the scheduler one tick has passed
@@ -794,9 +802,6 @@ void loop()
 // update AHRS system
 static void ahrs_update()
 {
-    if (delta_us_fast_loop > G_Dt_max)
-        G_Dt_max = delta_us_fast_loop;
-
 #if HIL_MODE != HIL_MODE_DISABLED
     // update hil before AHRS update
     gcs_update();
@@ -809,6 +814,10 @@ static void ahrs_update()
 
     if (g.log_bitmask & MASK_LOG_IMU)
         Log_Write_IMU();
+
+    // calculate a scaled roll limit based on current pitch
+    roll_limit_cd = g.roll_limit_cd * cosf(ahrs.pitch);
+    pitch_limit_min_cd = aparm.pitch_limit_min_cd * fabsf(cosf(ahrs.roll));
 }
 
 /*
@@ -885,6 +894,9 @@ static void update_logging(void)
     if ((g.log_bitmask & MASK_LOG_ATTITUDE_MED) && !(g.log_bitmask & MASK_LOG_ATTITUDE_FAST))
         Log_Write_Attitude();
 
+    if ((g.log_bitmask & MASK_LOG_ATTITUDE_MED) && !(g.log_bitmask & MASK_LOG_IMU))
+        Log_Write_IMU();
+    
     if (g.log_bitmask & MASK_LOG_CTUN)
         Log_Write_Control_Tuning();
     
@@ -950,9 +962,13 @@ static void one_second_loop()
     static uint8_t counter;
     counter++;
 
-    if (counter == 20) {
+    if (counter % 10 == 0) {
+        if (scheduler.debug() != 0) {
+            hal.console->printf_P(PSTR("G_Dt_max=%lu\n"), (unsigned long)G_Dt_max);
+        }
         if (g.log_bitmask & MASK_LOG_PM)
             Log_Write_Performance();
+        G_Dt_max = 0;
         resetPerfData();
     }
 
@@ -976,9 +992,9 @@ static void airspeed_ratio_update(void)
         // don't calibrate when not moving
         return;
     }
-    if (abs(ahrs.roll_sensor) > g.roll_limit_cd ||
+    if (abs(ahrs.roll_sensor) > roll_limit_cd ||
         ahrs.pitch_sensor > aparm.pitch_limit_max_cd ||
-        ahrs.pitch_sensor < aparm.pitch_limit_min_cd) {
+        ahrs.pitch_sensor < pitch_limit_min_cd) {
         // don't calibrate when going beyond normal flight envelope
         return;
     }
@@ -1027,6 +1043,9 @@ static void update_GPS(void)
             } else {
                 init_home();
 
+                // set system clock for log timestamps
+                hal.util->set_system_clock(g_gps->time_epoch_usec());
+
                 if (g.compass_enabled) {
                     // Set compass declination automatically
                     compass.set_initial_location(g_gps->latitude, g_gps->longitude);
@@ -1157,10 +1176,10 @@ static void update_flight_mode(void)
         
         // if the roll is past the set roll limit, then
         // we set target roll to the limit
-        if (ahrs.roll_sensor >= g.roll_limit_cd) {
+        if (ahrs.roll_sensor >= roll_limit_cd) {
-            nav_roll_cd = g.roll_limit_cd;
+            nav_roll_cd = roll_limit_cd;
-        } else if (ahrs.roll_sensor <= -g.roll_limit_cd) {
+        } else if (ahrs.roll_sensor <= -roll_limit_cd) {
-            nav_roll_cd = -g.roll_limit_cd;                
+            nav_roll_cd = -roll_limit_cd;                
         } else {
             training_manual_roll = true;
             nav_roll_cd = 0;
@@ -1170,8 +1189,8 @@ static void update_flight_mode(void)
         // we set target pitch to the limit
         if (ahrs.pitch_sensor >= aparm.pitch_limit_max_cd) {
             nav_pitch_cd = aparm.pitch_limit_max_cd;
-        } else if (ahrs.pitch_sensor <= aparm.pitch_limit_min_cd) {
+        } else if (ahrs.pitch_sensor <= pitch_limit_min_cd) {
-            nav_pitch_cd = aparm.pitch_limit_min_cd;
+            nav_pitch_cd = pitch_limit_min_cd;
         } else {
             training_manual_pitch = true;
             nav_pitch_cd = 0;
@@ -1199,15 +1218,15 @@ static void update_flight_mode(void)
 
     case FLY_BY_WIRE_A: {
         // set nav_roll and nav_pitch using sticks
-        nav_roll_cd  = channel_roll->norm_input() * g.roll_limit_cd;
+        nav_roll_cd  = channel_roll->norm_input() * roll_limit_cd;
-        nav_roll_cd = constrain_int32(nav_roll_cd, -g.roll_limit_cd, g.roll_limit_cd);
+        nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd);
         float pitch_input = channel_pitch->norm_input();
         if (pitch_input > 0) {
             nav_pitch_cd = pitch_input * aparm.pitch_limit_max_cd;
         } else {
-            nav_pitch_cd = -(pitch_input * aparm.pitch_limit_min_cd);
+            nav_pitch_cd = -(pitch_input * pitch_limit_min_cd);
         }
-        nav_pitch_cd = constrain_int32(nav_pitch_cd, aparm.pitch_limit_min_cd.get(), aparm.pitch_limit_max_cd.get());
+        nav_pitch_cd = constrain_int32(nav_pitch_cd, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get());
         if (inverted_flight) {
             nav_pitch_cd = -nav_pitch_cd;
         }
@@ -1221,7 +1240,7 @@ static void update_flight_mode(void)
 
     case FLY_BY_WIRE_B:
         // Thanks to Yury MonZon for the altitude limit code!
-        nav_roll_cd = channel_roll->norm_input() * g.roll_limit_cd;
+        nav_roll_cd = channel_roll->norm_input() * roll_limit_cd;
         update_fbwb_speed_height();
         break;
         
@@ -1238,7 +1257,7 @@ static void update_flight_mode(void)
         }                 
         
         if (!cruise_state.locked_heading) {
-            nav_roll_cd = channel_roll->norm_input() * g.roll_limit_cd;
+            nav_roll_cd = channel_roll->norm_input() * roll_limit_cd;
         } else {
             calc_nav_roll();
         }
@@ -1256,7 +1275,7 @@ static void update_flight_mode(void)
         // or we just want to fly around in a gentle circle w/o GPS,
         // holding altitude at the altitude we set when we
         // switched into the mode
-        nav_roll_cd  = g.roll_limit_cd / 3;
+        nav_roll_cd  = roll_limit_cd / 3;
         calc_nav_pitch();
         calc_throttle();
         break;
@@ -1290,6 +1309,12 @@ static void update_navigation()
     case LOITER:
     case RTL:
     case GUIDED:
+        // allow loiter direction to be changed in flight
+        if (g.loiter_radius < 0) {
+            loiter.direction = -1;
+        } else {
+            loiter.direction = 1;
+        }
         update_loiter();
         break;
 

ArduPlane/Attitude.pde

@@ -43,8 +43,7 @@ static bool stick_mixing_enabled(void)
         if (g.stick_mixing != STICK_MIXING_DISABLED &&
             geofence_stickmixing() &&
             failsafe.state == FAILSAFE_NONE &&
-            (g.throttle_fs_enabled == 0 || 
+            !throttle_failsafe_level()) {
-             channel_throttle->radio_in >= g.throttle_fs_value)) {
             // we're in an auto mode, and haven't triggered failsafe
             return true;
         } else {
@@ -167,8 +166,8 @@ static void stabilize_stick_mixing_fbw()
     } else if (roll_input < -0.5f) {
         roll_input = (3*roll_input + 1);
     }
-    nav_roll_cd += roll_input * g.roll_limit_cd;
+    nav_roll_cd += roll_input * roll_limit_cd;
-    nav_roll_cd = constrain_int32(nav_roll_cd, -g.roll_limit_cd.get(), g.roll_limit_cd.get());
+    nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd);
     
     float pitch_input = channel_pitch->norm_input();
     if (fabsf(pitch_input) > 0.5f) {
@@ -180,9 +179,9 @@ static void stabilize_stick_mixing_fbw()
     if (pitch_input > 0) {
         nav_pitch_cd += pitch_input * aparm.pitch_limit_max_cd;
     } else {
-        nav_pitch_cd += -(pitch_input * aparm.pitch_limit_min_cd);
+        nav_pitch_cd += -(pitch_input * pitch_limit_min_cd);
     }
-    nav_pitch_cd = constrain_int32(nav_pitch_cd, aparm.pitch_limit_min_cd.get(), aparm.pitch_limit_max_cd.get());
+    nav_pitch_cd = constrain_int32(nav_pitch_cd, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get());
 }
 
 
@@ -441,14 +440,14 @@ static void calc_nav_pitch()
     // Calculate the Pitch of the plane
     // --------------------------------
     nav_pitch_cd = SpdHgt_Controller->get_pitch_demand();
-    nav_pitch_cd = constrain_int32(nav_pitch_cd, aparm.pitch_limit_min_cd.get(), aparm.pitch_limit_max_cd.get());
+    nav_pitch_cd = constrain_int32(nav_pitch_cd, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get());
 }
 
 
 static void calc_nav_roll()
 {
     nav_roll_cd = nav_controller->nav_roll_cd();
-    nav_roll_cd = constrain_int32(nav_roll_cd, -g.roll_limit_cd.get(), g.roll_limit_cd.get());
+    nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd);
 }
 
 

ArduPlane/GCS_Mavlink.pde

@@ -142,7 +142,8 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
     if (g.compass_enabled) {
         control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG; // compass present
     }
-    if (airspeed.enabled() && airspeed.use()) {
+
+    if (airspeed.enabled()) {
         control_sensors_present |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE;
     }
     if (g_gps != NULL && g_gps->status() > GPS::NO_GPS) {
@@ -152,6 +153,10 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
     // all present sensors enabled by default except rate control, attitude stabilization, yaw, altitude, position control and motor output which we will set individually
     control_sensors_enabled = control_sensors_present & (~MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL & ~MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION & ~MAV_SYS_STATUS_SENSOR_YAW_POSITION & ~MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL & ~MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL & ~MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS);
 
+    if (airspeed.enabled() && airspeed.use()) {
+        control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE;
+    }
+
     switch (control_mode) {
     case MANUAL:
         break;
@@ -197,14 +202,22 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
         break;
     }
 
-    // default to all healthy except compass and gps which we set individually
+    // default to all healthy
-    control_sensors_health = control_sensors_present & (~MAV_SYS_STATUS_SENSOR_3D_MAG & ~MAV_SYS_STATUS_SENSOR_GPS);
+    control_sensors_health = control_sensors_present & ~(MAV_SYS_STATUS_SENSOR_3D_MAG | 
+                                                         MAV_SYS_STATUS_SENSOR_GPS |
+                                                         MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE);
     if (g.compass_enabled && compass.healthy && ahrs.use_compass()) {
         control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG;
     }
     if (g_gps != NULL && g_gps->status() > GPS::NO_GPS) {
         control_sensors_health |= MAV_SYS_STATUS_SENSOR_GPS;
     }
+    if (!ins.healthy()) {
+        control_sensors_health &= ~(MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL);
+    }
+    if (airspeed.healthy()) {
+        control_sensors_health |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE;
+    }
 
     int16_t battery_current = -1;
     int8_t battery_remaining = -1;
@@ -279,6 +292,11 @@ static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
 
 static void NOINLINE send_gps_raw(mavlink_channel_t chan)
 {
+    static uint32_t last_send_time;
+    if (last_send_time != 0 && last_send_time == g_gps->last_fix_time) {
+        return;
+    }
+    last_send_time = g_gps->last_fix_time;
     mavlink_msg_gps_raw_int_send(
         chan,
         g_gps->last_fix_time*(uint64_t)1000,
@@ -293,6 +311,14 @@ static void NOINLINE send_gps_raw(mavlink_channel_t chan)
         g_gps->num_sats);
 }
 
+static void NOINLINE send_system_time(mavlink_channel_t chan)
+{
+    mavlink_msg_system_time_send(
+        chan,
+        g_gps->time_epoch_usec(),
+        hal.scheduler->millis());
+}
+
 #if HIL_MODE != HIL_MODE_DISABLED
 static void NOINLINE send_servo_out(mavlink_channel_t chan)
 {
@@ -574,6 +600,11 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id,
         send_gps_raw(chan);
         break;
 
+    case MSG_SYSTEM_TIME:
+        CHECK_PAYLOAD_SIZE(SYSTEM_TIME);
+        send_system_time(chan);
+        break;
+
     case MSG_SERVO_OUT:
 #if HIL_MODE != HIL_MODE_DISABLED
         CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED);
@@ -934,7 +965,8 @@ bool GCS_MAVLINK::stream_trigger(enum streams stream_num)
 
     // send at a much lower rate while handling waypoints and
     // parameter sends
-    if (waypoint_receiving || _queued_parameter != NULL) {
+    if ((stream_num != STREAM_PARAMS) && 
+        (waypoint_receiving || _queued_parameter != NULL)) {
         rate *= 0.25;
     }
 
@@ -1002,7 +1034,7 @@ GCS_MAVLINK::data_stream_send(void)
         send_message(MSG_EXTENDED_STATUS1);
         send_message(MSG_EXTENDED_STATUS2);
         send_message(MSG_CURRENT_WAYPOINT);
-        send_message(MSG_GPS_RAW);            // TODO - remove this message after location message is working
+        send_message(MSG_GPS_RAW);
         send_message(MSG_NAV_CONTROLLER_OUTPUT);
         send_message(MSG_FENCE_STATUS);
     }
@@ -1046,6 +1078,7 @@ GCS_MAVLINK::data_stream_send(void)
         send_message(MSG_AHRS);
         send_message(MSG_HWSTATUS);
         send_message(MSG_WIND);
+        send_message(MSG_SYSTEM_TIME);
     }
 }
 
@@ -1428,8 +1461,10 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
         mavlink_msg_param_request_list_decode(msg, &packet);
         if (mavlink_check_target(packet.target_system,packet.target_component)) break;
 
-        // Start sending parameters - next call to ::update will kick the first one out
+        // mark the firmware version in the tlog
+        send_text_P(SEVERITY_LOW, PSTR(FIRMWARE_STRING));
 
+        // Start sending parameters - next call to ::update will kick the first one out
         _queued_parameter = AP_Param::first(&_queued_parameter_token, &_queued_parameter_type);
         _queued_parameter_index = 0;
         _queued_parameter_count = _count_parameters();

ArduPlane/Log.pde

@@ -162,9 +162,12 @@ process_logs(uint8_t argc, const Menu::arg *argv)
 
 struct PACKED log_Attitude {
     LOG_PACKET_HEADER;
+    uint32_t time_ms;
     int16_t roll;
     int16_t pitch;
     uint16_t yaw;
+    uint16_t error_rp;
+    uint16_t error_yaw;
 };
 
 // Write an attitude packet. Total length : 10 bytes
@@ -172,9 +175,12 @@ static void Log_Write_Attitude(void)
 {
     struct log_Attitude pkt = {
         LOG_PACKET_HEADER_INIT(LOG_ATTITUDE_MSG),
+        time_ms : hal.scheduler->millis(),
         roll  : (int16_t)ahrs.roll_sensor,
         pitch : (int16_t)ahrs.pitch_sensor,
-        yaw   : (uint16_t)ahrs.yaw_sensor
+        yaw   : (uint16_t)ahrs.yaw_sensor,
+        error_rp  : (uint16_t)(ahrs.get_error_rp() * 100),
+        error_yaw : (uint16_t)(ahrs.get_error_yaw() * 100)
     };
     DataFlash.WriteBlock(&pkt, sizeof(pkt));
 }
@@ -186,11 +192,11 @@ struct PACKED log_Performance {
     uint32_t g_dt_max;
     uint8_t  renorm_count;
     uint8_t  renorm_blowup;
-    uint8_t  gps_fix_count;
     int16_t  gyro_drift_x;
     int16_t  gyro_drift_y;
     int16_t  gyro_drift_z;
     uint8_t  i2c_lockup_count;
+    uint16_t ins_error_count;
 };
 
 // Write a performance monitoring packet. Total length : 19 bytes
@@ -203,11 +209,11 @@ static void Log_Write_Performance()
         g_dt_max        : G_Dt_max,
         renorm_count    : ahrs.renorm_range_count,
         renorm_blowup   : ahrs.renorm_blowup_count,
-        gps_fix_count   : gps_fix_count,
         gyro_drift_x    : (int16_t)(ahrs.get_gyro_drift().x * 1000),
         gyro_drift_y    : (int16_t)(ahrs.get_gyro_drift().y * 1000),
         gyro_drift_z    : (int16_t)(ahrs.get_gyro_drift().z * 1000),
-        i2c_lockup_count: hal.i2c->lockup_count()
+        i2c_lockup_count: hal.i2c->lockup_count(),
+        ins_error_count  : ins.error_count()
     };
     DataFlash.WriteBlock(&pkt, sizeof(pkt));
 }
@@ -244,6 +250,7 @@ static void Log_Write_Cmd(uint8_t num, const struct Location *wp)
 struct PACKED log_Camera {
     LOG_PACKET_HEADER;
     uint32_t gps_time;
+    uint16_t gps_week;
     int32_t  latitude;
     int32_t  longitude;
     int32_t  altitude;
@@ -258,7 +265,8 @@ static void Log_Write_Camera()
 #if CAMERA == ENABLED
     struct log_Camera pkt = {
         LOG_PACKET_HEADER_INIT(LOG_CAMERA_MSG),
-        gps_time    : g_gps->time,
+        gps_time    : g_gps->time_week_ms,
+        gps_week    : g_gps->time_week,
         latitude    : current_loc.lat,
         longitude   : current_loc.lng,
         altitude    : current_loc.alt,
@@ -295,6 +303,7 @@ static void Log_Write_Startup(uint8_t type)
 
 struct PACKED log_Control_Tuning {
     LOG_PACKET_HEADER;
+    uint32_t time_ms;
     int16_t nav_roll_cd;
     int16_t roll;
     int16_t nav_pitch_cd;
@@ -310,6 +319,7 @@ static void Log_Write_Control_Tuning()
     Vector3f accel = ins.get_accel();
     struct log_Control_Tuning pkt = {
         LOG_PACKET_HEADER_INIT(LOG_CTUN_MSG),
+        time_ms         : hal.scheduler->millis(),
         nav_roll_cd     : (int16_t)nav_roll_cd,
         roll            : (int16_t)ahrs.roll_sensor,
         nav_pitch_cd    : (int16_t)nav_pitch_cd,
@@ -329,31 +339,38 @@ static void Log_Write_TECS_Tuning(void)
 
 struct PACKED log_Nav_Tuning {
     LOG_PACKET_HEADER;
+    uint32_t time_ms;
     uint16_t yaw;
     uint32_t wp_distance;
     uint16_t target_bearing_cd;
     uint16_t nav_bearing_cd;
     int16_t altitude_error_cm;
     int16_t airspeed_cm;
+    float   altitude;
+    uint32_t groundspeed_cm;
 };
 
-// Write a navigation tuning packet. Total length : 18 bytes
+// Write a navigation tuning packe
 static void Log_Write_Nav_Tuning()
 {
     struct log_Nav_Tuning pkt = {
         LOG_PACKET_HEADER_INIT(LOG_NTUN_MSG),
+        time_ms             : hal.scheduler->millis(),
         yaw                 : (uint16_t)ahrs.yaw_sensor,
         wp_distance         : wp_distance,
         target_bearing_cd   : (uint16_t)nav_controller->target_bearing_cd(),
         nav_bearing_cd      : (uint16_t)nav_controller->nav_bearing_cd(),
         altitude_error_cm   : (int16_t)altitude_error_cm,
-        airspeed_cm         : (int16_t)airspeed.get_airspeed_cm()
+        airspeed_cm         : (int16_t)airspeed.get_airspeed_cm(),
+        altitude            : barometer.get_altitude(),
+        groundspeed_cm      : g_gps->ground_speed_cm
     };
     DataFlash.WriteBlock(&pkt, sizeof(pkt));
 }
 
 struct PACKED log_Mode {
     LOG_PACKET_HEADER;
+    uint32_t time_ms;
     uint8_t mode;
     uint8_t mode_num;
 };
@@ -363,6 +380,7 @@ static void Log_Write_Mode(uint8_t mode)
 {
     struct log_Mode pkt = {
         LOG_PACKET_HEADER_INIT(LOG_MODE_MSG),
+        time_ms  : hal.scheduler->millis(),
         mode     : mode,
         mode_num : mode
     };
@@ -371,6 +389,7 @@ static void Log_Write_Mode(uint8_t mode)
 
 struct PACKED log_Current {
     LOG_PACKET_HEADER;
+    uint32_t time_ms;
     int16_t throttle_in;
     int16_t battery_voltage;
     int16_t current_amps;
@@ -382,6 +401,7 @@ static void Log_Write_Current()
 {
     struct log_Current pkt = {
         LOG_PACKET_HEADER_INIT(LOG_CURRENT_MSG),
+        time_ms                 : hal.scheduler->millis(),
         throttle_in             : channel_throttle->control_in,
         battery_voltage         : (int16_t)(battery.voltage() * 100.0),
         current_amps            : (int16_t)(battery.current_amps() * 100.0),
@@ -394,33 +414,28 @@ static void Log_Write_Current()
 
 struct PACKED log_Compass {
     LOG_PACKET_HEADER;
+    uint32_t time_ms;
     int16_t mag_x;
     int16_t mag_y;
     int16_t mag_z;
     int16_t offset_x;
     int16_t offset_y;
     int16_t offset_z;
-    int16_t motor_offset_x;
-    int16_t motor_offset_y;
-    int16_t motor_offset_z;
 };
 
 // Write a Compass packet. Total length : 15 bytes
 static void Log_Write_Compass()
 {
     Vector3f mag_offsets = compass.get_offsets();
-    Vector3f mag_motor_offsets = compass.get_motor_offsets();
     struct log_Compass pkt = {
         LOG_PACKET_HEADER_INIT(LOG_COMPASS_MSG),
+        time_ms         : hal.scheduler->millis(),
         mag_x           : compass.mag_x,
         mag_y           : compass.mag_y,
         mag_z           : compass.mag_z,
         offset_x        : (int16_t)mag_offsets.x,
         offset_y        : (int16_t)mag_offsets.y,
-        offset_z        : (int16_t)mag_offsets.z,
+        offset_z        : (int16_t)mag_offsets.z
-        motor_offset_x  : (int16_t)mag_motor_offsets.x,
-        motor_offset_y  : (int16_t)mag_motor_offsets.y,
-        motor_offset_z  : (int16_t)mag_motor_offsets.z
     };
     DataFlash.WriteBlock(&pkt, sizeof(pkt));
 }
@@ -432,38 +447,38 @@ static void Log_Write_GPS(void)
 
 static void Log_Write_IMU() 
 {
-    DataFlash.Log_Write_IMU(&ins);
+    DataFlash.Log_Write_IMU(ins);
 }
 
 static const struct LogStructure log_structure[] PROGMEM = {
     LOG_COMMON_STRUCTURES,
     { LOG_ATTITUDE_MSG, sizeof(log_Attitude),       
-      "ATT", "ccC",        "Roll,Pitch,Yaw" },
+      "ATT", "IccCCC",        "TimeMS,Roll,Pitch,Yaw,ErrorRP,ErrorYaw" },
     { LOG_PERFORMANCE_MSG, sizeof(log_Performance), 
-      "PM",  "IHIBBBhhhhB", "LTime,MLC,gDt,RNCnt,RNBl,GPScnt,GDx,GDy,GDz,I2CErr" },
+      "PM",  "IHIBBhhhBH", "LTime,MLC,gDt,RNCnt,RNBl,GDx,GDy,GDz,I2CErr,INSErr" },
     { LOG_CMD_MSG, sizeof(log_Cmd),                 
       "CMD", "BBBBBeLL",   "CTot,CNum,CId,COpt,Prm1,Alt,Lat,Lng" },
     { LOG_CAMERA_MSG, sizeof(log_Camera),                 
-      "CAM", "ILLeccC",   "GPSTime,Lat,Lng,Alt,Roll,Pitch,Yaw" },
+      "CAM", "IHLLeccC",   "GPSTime,GPSWeek,Lat,Lng,Alt,Roll,Pitch,Yaw" },
     { LOG_STARTUP_MSG, sizeof(log_Startup),         
       "STRT", "BB",         "SType,CTot" },
     { LOG_CTUN_MSG, sizeof(log_Control_Tuning),     
-      "CTUN", "cccchhf",    "NavRoll,Roll,NavPitch,Pitch,ThrOut,RdrOut,AccY" },
+      "CTUN", "Icccchhf",    "TimeMS,NavRoll,Roll,NavPitch,Pitch,ThrOut,RdrOut,AccY" },
     { LOG_NTUN_MSG, sizeof(log_Nav_Tuning),         
-      "NTUN", "CICCcc",     "Yaw,WpDist,TargBrg,NavBrg,AltErr,Arspd" },
+      "NTUN", "ICICCccfI",   "TimeMS,Yaw,WpDist,TargBrg,NavBrg,AltErr,Arspd,Alt,GSpdCM" },
     { LOG_MODE_MSG, sizeof(log_Mode),             
-      "MODE", "MB",         "Mode,ModeNum" },
+      "MODE", "IMB",         "TimeMS,Mode,ModeNum" },
     { LOG_CURRENT_MSG, sizeof(log_Current),             
-      "CURR", "hhhHf",      "Thr,Volt,Curr,Vcc,CurrTot" },
+      "CURR", "IhhhHf",      "TimeMS,Thr,Volt,Curr,Vcc,CurrTot" },
     { LOG_COMPASS_MSG, sizeof(log_Compass),             
-      "MAG", "hhhhhhhhh",   "MagX,MagY,MagZ,OfsX,OfsY,OfsZ,MOfsX,MOfsY,MOfsZ" },
+      "MAG", "Ihhhhhh",   "TimeMS,MagX,MagY,MagZ,OfsX,OfsY,OfsZ" },
     TECS_LOG_FORMAT(LOG_TECS_MSG),
 };
 
 // Read the DataFlash.log memory : Packet Parser
 static void Log_Read(uint16_t log_num, int16_t start_page, int16_t end_page)
 {
-    cliSerial->printf_P(PSTR("\n" THISFIRMWARE
+    cliSerial->printf_P(PSTR("\n" FIRMWARE_STRING
                              "\nFree RAM: %u\n"),
                         (unsigned) memcheck_available_memory());
 
@@ -480,6 +495,7 @@ static void Log_Read(uint16_t log_num, int16_t start_page, int16_t end_page)
 static void start_logging() 
 {
     DataFlash.StartNewLog(sizeof(log_structure)/sizeof(log_structure[0]), log_structure);
+    DataFlash.Log_Write_Message_P(PSTR(FIRMWARE_STRING));
 }
 
 #else // LOGGING_ENABLED

ArduPlane/Parameters.pde

@@ -149,8 +149,8 @@ const AP_Param::Info var_info[] PROGMEM = {
 
 	// @Param: NAV_CONTROLLER
 	// @DisplayName: Navigation controller selection
-	// @Description: Which navigation controller to enable
+	// @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.
-	// @Values: 0:Legacy,1:L1Controller
+	// @Values: 0:Default,1:L1Controller
 	// @User: Standard
 	GSCALAR(nav_controller,          "NAV_CONTROLLER",   AP_Navigation::CONTROLLER_L1),
 
@@ -165,8 +165,8 @@ const AP_Param::Info 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). If you set it to 1 then you will get the old (deprecated) non-airspeed based algorithm. If you set it to 3 then you will get the old (deprecated) airspeed based algorithm. Setting it to 2 selects the new 'TECS' (total energy control system) altitude control, which currently is equivalent to setting 0. Note that TECS is able to handle aircraft both with and without an airspeed sensor.
+    // @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.
-    // @Values: 0:Automatic,1:non-airspeed(deprecated),2:TECS,3:airspeed(deprecated)
+    // @Values: 0:Automatic
     // @User: Advanced
     GSCALAR(alt_control_algorithm, "ALT_CTRL_ALG",    ALT_CONTROL_DEFAULT),
 
@@ -414,15 +414,17 @@ const AP_Param::Info 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 then the plane will RTL
+    // @Description: Battery voltage to trigger failsafe. Set to 0 to disable battery voltage failsafe. If the battery voltage drops below this voltage continuously for 10 seconds then the plane will switch to RTL mode
     // @Units: Volts
+    // @Increment: 0.1
     // @User: Standard
     GSCALAR(fs_batt_voltage,        "FS_BATT_VOLTAGE", 0),
 
     // @Param: FS_BATT_MAH
     // @DisplayName: Failsafe battery milliAmpHours
-    // @Description: Battery capacity remaining to trigger failsafe. Set to 0 to disable battery remaining failsafe. If the battery remaining drops below this level then the plane will RTL
+    // @Description: Battery capacity remaining to trigger failsafe. Set to 0 to disable battery remaining failsafe. If the battery remaining drops below this level then the plane will switch to RTL mode immediately
     // @Units: mAh
+    // @Increment: 50
     // @User: Standard
     GSCALAR(fs_batt_mah,            "FS_BATT_MAH", 0),
 

ArduPlane/config.h

@@ -529,3 +529,12 @@
  # define SERIAL2_BUFSIZE 256
 #endif
 
+/*
+  build a firmware version string.
+  GIT_VERSION comes from Makefile builds
+*/
+#ifndef GIT_VERSION
+#define FIRMWARE_STRING THISFIRMWARE
+#else
+#define FIRMWARE_STRING THISFIRMWARE " (" GIT_VERSION ")"
+#endif

ArduPlane/defines.h

@@ -130,6 +130,7 @@ enum ap_message {
     MSG_RAW_IMU2,
     MSG_RAW_IMU3,
     MSG_GPS_RAW,
+    MSG_SYSTEM_TIME,
     MSG_SERVO_OUT,
     MSG_NEXT_WAYPOINT,
     MSG_NEXT_PARAM,

ArduPlane/radio.pde

@@ -129,7 +129,7 @@ static void control_failsafe(uint16_t pwm)
 
         //Check for failsafe and debounce funky reads
     } else if (g.throttle_fs_enabled) {
-        if (pwm < (unsigned)g.throttle_fs_value) {
+        if (throttle_failsafe_level()) {
             // we detect a failsafe from radio
             // throttle has dropped below the mark
             failsafe.ch3_counter++;
@@ -221,3 +221,17 @@ static void trim_radio()
 
     trim_control_surfaces();
 }
+
+/*
+  return true if throttle level is below throttle failsafe threshold
+ */
+static bool throttle_failsafe_level(void)
+{
+    if (!g.throttle_fs_enabled) {
+        return false;
+    }
+    if (channel_throttle->get_reverse()) {
+        return channel_throttle->radio_in >= g.throttle_fs_value;
+    }
+    return channel_throttle->radio_in <= g.throttle_fs_value;
+}

ArduPlane/system.pde

@@ -82,7 +82,7 @@ static void init_ardupilot()
     // standard gps running
     hal.uartB->begin(38400, 256, 16);
 
-    cliSerial->printf_P(PSTR("\n\nInit " THISFIRMWARE
+    cliSerial->printf_P(PSTR("\n\nInit " FIRMWARE_STRING
                          "\n\nFree RAM: %u\n"),
                     memcheck_available_memory());
 
@@ -419,15 +419,8 @@ static void startup_INS_ground(bool do_accel_init)
     }
 
     if (style == AP_InertialSensor::COLD_START) {
-        gcs_send_text_P(SEVERITY_MEDIUM, PSTR("Warming up ADC..."));
-        mavlink_delay(500);
-
-        // Makes the servos wiggle twice - about to begin INS calibration - HOLD LEVEL AND STILL!!
-        // -----------------------
-        demo_servos(2);
-
         gcs_send_text_P(SEVERITY_MEDIUM, PSTR("Beginning INS calibration; do not move plane"));
-        mavlink_delay(1000);
+        mavlink_delay(100);
     }
 
     ahrs.init();

README.md

@@ -4,7 +4,7 @@ You can find lots of development information at the [ArduPilot development site]
 
 #### To compile APM2.x Ardupilot after version 3.1 please follow the instructions found at 
 
-[Arduino Tools] (http://firmware.diydrones.com/Tools/Arduino/) 
+[Dev.Ardupilot] (http://dev.ardupilot.com/wiki/building-ardupilot-with-arduino-windows/) 
 
 
 ## Getting the source

Tools/AntennaTracker/APM_Config.h

@@ -0,0 +1,5 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+// This file is just a placeholder for your configuration file.  If
+// you wish to change any of the setup parameters from their default
+// values, place the appropriate #define statements here.

Tools/AntennaTracker/AntennaTracker.pde

@@ -0,0 +1,271 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#define THISFIRMWARE "AntennaTracker V0.1"
+/*
+   Lead developers: Matthew Ridley and Andrew Tridgell
+ 
+   Please contribute your ideas! See http://dev.ardupilot.com for details
+
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+////////////////////////////////////////////////////////////////////////////////
+// Header includes
+////////////////////////////////////////////////////////////////////////////////
+
+#include <math.h>
+#include <stdarg.h>
+#include <stdio.h>
+
+#include <AP_Common.h>
+#include <AP_Progmem.h>
+#include <AP_HAL.h>
+#include <AP_Param.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 <GCS_MAVLink.h>    // MAVLink GCS definitions
+#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_Vehicle.h>
+#include <AP_Notify.h>      // Notify library
+#include <AP_BattMonitor.h> // Battery monitor library
+#include <AP_Airspeed.h>
+#include <RC_Channel.h>
+
+// Configuration
+#include "config.h"
+
+// Local modules
+#include "defines.h"
+
+#include "Parameters.h"
+#include "GCS.h"
+
+#include <AP_HAL_AVR.h>
+#include <AP_HAL_AVR_SITL.h>
+#include <AP_HAL_PX4.h>
+#include <AP_HAL_FLYMAPLE.h>
+#include <AP_HAL_Linux.h>
+#include <AP_HAL_Empty.h>
+
+AP_HAL::BetterStream* cliSerial;
+
+const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
+
+////////////////////////////////////////////////////////////////////////////////
+// the rate we run the main loop at
+////////////////////////////////////////////////////////////////////////////////
+static const AP_InertialSensor::Sample_rate ins_sample_rate = AP_InertialSensor::RATE_50HZ;
+
+////////////////////////////////////////////////////////////////////////////////
+// Parameters
+////////////////////////////////////////////////////////////////////////////////
+//
+// Global parameters are all contained within the 'g' class.
+//
+static Parameters g;
+
+// main loop scheduler
+static AP_Scheduler scheduler;
+ 
+// notification object for LEDs, buzzers etc
+static AP_Notify notify;
+
+// tracking status for MAVLink
+static struct {
+    float bearing;
+    float distance;
+    float pitch;
+} nav_status;
+
+
+////////////////////////////////////////////////////////////////////////////////
+// prototypes
+void gcs_send_text_fmt(const prog_char_t *fmt, ...);
+
+
+////////////////////////////////////////////////////////////////////////////////
+// Sensors
+////////////////////////////////////////////////////////////////////////////////
+// All GPS access should be through this pointer.
+static GPS         *g_gps;
+
+#if CONFIG_BARO == AP_BARO_BMP085
+static AP_Baro_BMP085 barometer;
+#elif CONFIG_BARO == AP_BARO_PX4
+static AP_Baro_PX4 barometer;
+#elif CONFIG_BARO == AP_BARO_HIL
+static AP_Baro_HIL barometer;
+#elif CONFIG_BARO == AP_BARO_MS5611
+ #if CONFIG_MS5611_SERIAL == AP_BARO_MS5611_SPI
+ static AP_Baro_MS5611 barometer(&AP_Baro_MS5611::spi);
+ #elif CONFIG_MS5611_SERIAL == AP_BARO_MS5611_I2C
+ static AP_Baro_MS5611 barometer(&AP_Baro_MS5611::i2c);
+ #else
+ #error Unrecognized CONFIG_MS5611_SERIAL setting.
+ #endif
+#else
+ #error Unrecognized CONFIG_BARO setting
+#endif
+
+#if CONFIG_COMPASS == AP_COMPASS_PX4
+static AP_Compass_PX4 compass;
+#elif CONFIG_COMPASS == AP_COMPASS_HMC5843
+static AP_Compass_HMC5843 compass;
+#elif CONFIG_COMPASS == AP_COMPASS_HIL
+static AP_Compass_HIL compass;
+#else
+ #error Unrecognized CONFIG_COMPASS setting
+#endif
+
+// GPS selection
+AP_GPS_Auto     g_gps_driver(&g_gps);
+
+#if CONFIG_INS_TYPE == CONFIG_INS_MPU6000
+AP_InertialSensor_MPU6000 ins;
+#elif CONFIG_INS_TYPE == CONFIG_INS_PX4
+AP_InertialSensor_PX4 ins;
+#elif CONFIG_INS_TYPE == CONFIG_INS_HIL
+AP_InertialSensor_HIL ins;
+#elif CONFIG_INS_TYPE == CONFIG_INS_OILPAN
+AP_InertialSensor_Oilpan ins( &apm1_adc );
+#elif CONFIG_INS_TYPE == CONFIG_INS_FLYMAPLE
+AP_InertialSensor_Flymaple ins;
+#elif CONFIG_INS_TYPE == CONFIG_INS_L3G4200D
+AP_InertialSensor_L3G4200D ins;
+#else
+  #error Unrecognised CONFIG_INS_TYPE setting.
+#endif // CONFIG_INS_TYPE
+
+AP_AHRS_DCM ahrs(&ins, g_gps);
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
+SITL sitl;
+#endif
+
+/**
+   antenna control channels
+ */
+static RC_Channel channel_yaw(CH_1);
+static RC_Channel channel_pitch(CH_2);
+
+////////////////////////////////////////////////////////////////////////////////
+// GCS selection
+////////////////////////////////////////////////////////////////////////////////
+static GCS_MAVLINK gcs0;
+static GCS_MAVLINK gcs3;
+
+/*
+  scheduler table - all regular tasks apart from the fast_loop()
+  should be listed here, along with how often they should be called
+  (in 20ms units) and the maximum time they are expected to take (in
+  microseconds)
+ */
+static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
+    { update_ahrs,            1,   1000 },
+    { update_tracking,        1,   1000 },
+    { update_GPS,             5,   4000 },
+    { update_compass,         5,   1500 },
+    { update_barometer,       5,   1500 },
+    { update_tracking,        1,   1000 },
+    { gcs_update,             1,   1700 },
+    { gcs_data_stream_send,   1,   3000 },
+    { compass_accumulate,     1,   1500 },
+    { barometer_accumulate,   1,    900 },
+    { update_notify,          1,    100 },
+    { one_second_loop,       50,   3900 }
+};
+
+// setup the var_info table
+AP_Param param_loader(var_info, EEPROM_MAX_ADDR);
+
+/**
+  setup the sketch - called once on startup
+ */
+void setup() 
+{
+    // this needs to be the first call, as it fills memory with
+    // sentinel values
+    memcheck_init();
+
+    cliSerial = hal.console;
+
+    // load the default values of variables listed in var_info[]
+    AP_Param::setup_sketch_defaults();
+
+    // arduplane does not use arming nor pre-arm checks
+    AP_Notify::flags.armed = true;
+    AP_Notify::flags.pre_arm_check = true;
+    AP_Notify::flags.failsafe_battery = false;
+
+    notify.init();
+    init_tracker();
+
+    // initialise the main loop scheduler
+    scheduler.init(&scheduler_tasks[0], sizeof(scheduler_tasks)/sizeof(scheduler_tasks[0]));
+}
+
+/**
+   loop() is called continuously 
+ */
+void loop()
+{
+    // wait for an INS sample
+    if (!ins.wait_for_sample(1000)) {
+        return;
+    }
+
+    // tell the scheduler one tick has passed
+    scheduler.tick();
+
+    scheduler.run(19900UL);
+}
+
+static void one_second_loop()
+{
+    // send a heartbeat
+    gcs_send_message(MSG_HEARTBEAT);
+
+    // make it possible to change orientation at runtime
+    ahrs.set_orientation();
+
+    // sync MAVLink system ID
+    mavlink_system.sysid = g.sysid_this_mav;
+
+    static uint8_t counter;
+    counter++;
+
+    if (counter >= 60) {                                               
+        if(g.compass_enabled) {
+            compass.save_offsets();
+        }
+        counter = 0;
+    }
+}
+
+AP_HAL_MAIN();

Tools/AntennaTracker/GCS.h

@@ -0,0 +1,187 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/// @file	GCS.h
+/// @brief	Interface definition for the various Ground Control System
+// protocols.
+
+#ifndef __GCS_H
+#define __GCS_H
+
+#include <AP_HAL.h>
+#include <AP_Common.h>
+#include <GPS.h>
+#include <stdint.h>
+
+///
+/// @class	GCS
+/// @brief	Class describing the interface between the APM code
+///			proper and the GCS implementation.
+///
+/// GCS' are currently implemented inside the sketch and as such have
+/// access to all global state.  The sketch should not, however, call GCS
+/// internal functions - all calls to the GCS should be routed through
+/// this interface (or functions explicitly exposed by a subclass).
+///
+class GCS_Class
+{
+public:
+
+    /// Startup initialisation.
+    ///
+    /// This routine performs any one-off initialisation required before
+    /// GCS messages are exchanged.
+    ///
+    /// @note The stream is expected to be set up and configured for the
+    ///       correct bitrate before ::init is called.
+    ///
+    /// @note The stream is currently BetterStream so that we can use the _P
+    ///	      methods; this may change if Arduino adds them to Print.
+    ///
+    /// @param	port		The stream over which messages are exchanged.
+    ///
+    void init(AP_HAL::UARTDriver *port) {
+        _port = port;
+        initialised = true;
+    }
+
+    /// Update GCS state.
+    ///
+    /// This may involve checking for received bytes on the stream,
+    /// or sending additional periodic messages.
+    void        update(void) {
+    }
+
+    /// Send a message with a single numeric parameter.
+    ///
+    /// This may be a standalone message, or the GCS driver may
+    /// have its own way of locating additional parameters to send.
+    ///
+    /// @param	id			ID of the message to send.
+    /// @param	param		Explicit message parameter.
+    ///
+    void        send_message(enum ap_message id) {
+    }
+
+    /// Send a text message.
+    ///
+    /// @param	severity	A value describing the importance of the message.
+    /// @param	str			The text to be sent.
+    ///
+    void        send_text(gcs_severity severity, const char *str) {
+    }
+
+    /// Send a text message with a PSTR()
+    ///
+    /// @param	severity	A value describing the importance of the message.
+    /// @param	str			The text to be sent.
+    ///
+    void        send_text_P(gcs_severity severity, const prog_char_t *str) {
+    }
+
+    // send streams which match frequency range
+    void            data_stream_send(void);
+
+    // set to true if this GCS link is active
+    bool            initialised;
+
+protected:
+    /// The stream we are communicating over
+    AP_HAL::UARTDriver *_port;
+};
+
+//
+// GCS class definitions.
+//
+// These are here so that we can declare the GCS object early in the sketch
+// and then reference it statically rather than via a pointer.
+//
+
+///
+/// @class	GCS_MAVLINK
+/// @brief	The mavlink protocol for qgroundcontrol
+///
+class GCS_MAVLINK : public GCS_Class
+{
+public:
+    GCS_MAVLINK();
+    void        update(void);
+    void        init(AP_HAL::UARTDriver *port);
+    void        send_message(enum ap_message id);
+    void        send_text(gcs_severity severity, const char *str);
+    void        send_text_P(gcs_severity severity, const prog_char_t *str);
+    void        data_stream_send(void);
+    void        queued_param_send();
+
+    static const struct AP_Param::GroupInfo        var_info[];
+
+    // NOTE! The streams enum below and the
+    // set of AP_Int16 stream rates _must_ be
+    // kept in the same order
+    enum streams {STREAM_RAW_SENSORS,
+                  STREAM_EXTENDED_STATUS,
+                  STREAM_RC_CHANNELS,
+                  STREAM_RAW_CONTROLLER,
+                  STREAM_POSITION,
+                  STREAM_EXTRA1,
+                  STREAM_EXTRA2,
+                  STREAM_EXTRA3,
+                  STREAM_PARAMS,
+                  NUM_STREAMS};
+
+    // see if we should send a stream now. Called at 50Hz
+    bool        stream_trigger(enum streams stream_num);
+
+	// this costs us 51 bytes per instance, but means that low priority
+	// messages don't block the CPU
+    mavlink_statustext_t pending_status;
+
+    // call to reset the timeout window for entering the cli
+    void reset_cli_timeout();
+private:
+    void        handleMessage(mavlink_message_t * msg);
+
+    /// Perform queued sending operations
+    ///
+    AP_Param *                  _queued_parameter;      ///< next parameter to
+                                                        // be sent in queue
+    enum ap_var_type            _queued_parameter_type; ///< type of the next
+                                                        // parameter
+    AP_Param::ParamToken        _queued_parameter_token; ///AP_Param token for
+                                                         // next() call
+    uint16_t                    _queued_parameter_index; ///< next queued
+                                                         // parameter's index
+    uint16_t                    _queued_parameter_count; ///< saved count of
+                                                         // parameters for
+                                                         // queued send
+    uint32_t                    _queued_parameter_send_time_ms;
+
+    /// Count the number of reportable parameters.
+    ///
+    /// Not all parameters can be reported via MAVlink.  We count the number
+    // that are
+    /// so that we can report to a GCS the number of parameters it should
+    // expect when it
+    /// requests the full set.
+    ///
+    /// @return         The number of reportable parameters.
+    ///
+    uint16_t                    _count_parameters(); ///< count reportable
+                                                     // parameters
+
+    uint16_t                    _parameter_count;   ///< cache of reportable
+                                                    // parameters
+
+    mavlink_channel_t           chan;
+    uint16_t                    packet_drops;
+
+    // saveable rate of each stream
+    AP_Int16        streamRates[NUM_STREAMS];
+
+    // number of 50Hz ticks until we next send this stream
+    uint8_t         stream_ticks[NUM_STREAMS];
+
+    // number of extra ticks to add to slow things down for the radio
+    uint8_t         stream_slowdown;
+};
+
+#endif // __GCS_H

Tools/AntennaTracker/GCS_Mavlink.pde

@@ -0,0 +1,981 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+// 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)
+
+// use this to prevent recursion during sensor init
+static bool in_mavlink_delay;
+
+// true when we have received at least 1 MAVLink packet
+static bool mavlink_active;
+
+// check if a message will fit in the payload space available
+#define CHECK_PAYLOAD_SIZE(id) if (payload_space < MAVLINK_MSG_ID_ ## id ## _LEN) return false
+
+/*
+ *  !!NOTE!!
+ *
+ *  the use of NOINLINE separate functions for each message type avoids
+ *  a compiler bug in gcc that would cause it to use far more stack
+ *  space than is needed. Without the NOINLINE we use the sum of the
+ *  stack needed for each message type. Please be careful to follow the
+ *  pattern below when adding any new messages
+ */
+
+static NOINLINE void send_heartbeat(mavlink_channel_t chan)
+{
+    mavlink_msg_heartbeat_send(
+        chan,
+        MAV_TYPE_ANTENNA_TRACKER,
+        MAV_AUTOPILOT_ARDUPILOTMEGA,
+        0,
+        0,
+        0);
+}
+
+static NOINLINE void send_attitude(mavlink_channel_t chan)
+{
+    Vector3f omega = ahrs.get_gyro();
+    mavlink_msg_attitude_send(
+        chan,
+        hal.scheduler->millis(),
+        ahrs.roll,
+        ahrs.pitch,
+        ahrs.yaw,
+        omega.x,
+        omega.y,
+        omega.z);
+}
+
+
+static void NOINLINE send_location(mavlink_channel_t chan)
+{
+    uint32_t fix_time;
+    if (g_gps->status() >= GPS::GPS_OK_FIX_2D) {
+        fix_time = g_gps->last_fix_time;
+    } else {
+        fix_time = hal.scheduler->millis();
+    }
+
+    Location loc;
+    ahrs.get_position(loc);
+
+    mavlink_msg_global_position_int_send(
+        chan,
+        fix_time,
+        loc.lat,                // in 1E7 degrees
+        loc.lng,                // in 1E7 degrees
+        g_gps->altitude_cm * 10,        // millimeters above sea level
+        0,
+        g_gps->velocity_north() * 100,  // X speed cm/s (+ve North)
+        g_gps->velocity_east()  * 100,  // Y speed cm/s (+ve East)
+        g_gps->velocity_down()  * -100, // Z speed cm/s (+ve up)
+        ahrs.yaw_sensor);
+}
+
+static void NOINLINE send_gps_raw(mavlink_channel_t chan)
+{
+    mavlink_msg_gps_raw_int_send(
+        chan,
+        g_gps->last_fix_time*(uint64_t)1000,
+        g_gps->status(),
+        g_gps->latitude,      // in 1E7 degrees
+        g_gps->longitude,     // in 1E7 degrees
+        g_gps->altitude_cm * 10, // in mm
+        g_gps->hdop,
+        65535,
+        g_gps->ground_speed_cm,  // cm/s
+        g_gps->ground_course_cd, // 1/100 degrees,
+        g_gps->num_sats);
+}
+
+static void NOINLINE send_radio_out(mavlink_channel_t chan)
+{
+    mavlink_msg_servo_output_raw_send(
+        chan,
+        hal.scheduler->micros(),
+        0,     // port
+        hal.rcout->read(0),
+        hal.rcout->read(1),
+        hal.rcout->read(2),
+        hal.rcout->read(3),
+        hal.rcout->read(4),
+        hal.rcout->read(5),
+        hal.rcout->read(6),
+        hal.rcout->read(7));
+}
+
+static void NOINLINE send_raw_imu1(mavlink_channel_t chan)
+{
+    Vector3f accel = ins.get_accel();
+    Vector3f gyro = ins.get_gyro();
+
+    mavlink_msg_raw_imu_send(
+        chan,
+        hal.scheduler->micros(),
+        accel.x * 1000.0 / GRAVITY_MSS,
+        accel.y * 1000.0 / GRAVITY_MSS,
+        accel.z * 1000.0 / GRAVITY_MSS,
+        gyro.x * 1000.0,
+        gyro.y * 1000.0,
+        gyro.z * 1000.0,
+        compass.mag_x,
+        compass.mag_y,
+        compass.mag_z);
+}
+
+static void NOINLINE send_raw_imu2(mavlink_channel_t chan)
+{
+    float pressure = barometer.get_pressure();
+    mavlink_msg_scaled_pressure_send(
+        chan,
+        hal.scheduler->millis(),
+        pressure*0.01f, // hectopascal
+        (pressure - barometer.get_ground_pressure())*0.01f, // hectopascal
+        barometer.get_temperature()*100); // 0.01 degrees C
+}
+
+static void NOINLINE send_raw_imu3(mavlink_channel_t chan)
+{
+    // run this message at a much lower rate - otherwise it
+    // pointlessly wastes quite a lot of bandwidth
+    static uint8_t counter;
+    if (counter++ < 10) {
+        return;
+    }
+    counter = 0;
+
+    Vector3f mag_offsets = compass.get_offsets();
+    Vector3f accel_offsets = ins.get_accel_offsets();
+    Vector3f gyro_offsets = ins.get_gyro_offsets();
+
+    mavlink_msg_sensor_offsets_send(chan,
+                                    mag_offsets.x,
+                                    mag_offsets.y,
+                                    mag_offsets.z,
+                                    compass.get_declination(),
+                                    barometer.get_pressure(),
+                                    barometer.get_temperature()*100,
+                                    gyro_offsets.x,
+                                    gyro_offsets.y,
+                                    gyro_offsets.z,
+                                    accel_offsets.x,
+                                    accel_offsets.y,
+                                    accel_offsets.z);
+}
+
+static void NOINLINE send_ahrs(mavlink_channel_t chan)
+{
+    Vector3f omega_I = ahrs.get_gyro_drift();
+    mavlink_msg_ahrs_send(
+        chan,
+        omega_I.x,
+        omega_I.y,
+        omega_I.z,
+        0,
+        0,
+        ahrs.get_error_rp(),
+        ahrs.get_error_yaw());
+}
+
+static void NOINLINE send_hwstatus(mavlink_channel_t chan)
+{
+    mavlink_msg_hwstatus_send(
+        chan,
+        0,
+        hal.i2c->lockup_count());
+}
+
+static void NOINLINE send_statustext(mavlink_channel_t chan)
+{
+    mavlink_statustext_t *s = (chan == MAVLINK_COMM_0?&gcs0.pending_status:&gcs3.pending_status);
+    mavlink_msg_statustext_send(
+        chan,
+        s->severity,
+        s->text);
+}
+
+static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
+{
+    mavlink_msg_nav_controller_output_send(
+        chan,
+        0,
+        nav_status.pitch,
+        nav_status.bearing,
+        nav_status.bearing,
+        nav_status.distance,
+        0,
+        0,
+        0);
+}
+
+
+// try to send a message, return false if it won't fit in the serial tx buffer
+static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, uint16_t packet_drops)
+{
+    int16_t payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES;
+
+    switch (id) {
+    case MSG_HEARTBEAT:
+        CHECK_PAYLOAD_SIZE(HEARTBEAT);
+        send_heartbeat(chan);
+        return true;
+
+    case MSG_ATTITUDE:
+        CHECK_PAYLOAD_SIZE(ATTITUDE);
+        send_attitude(chan);
+        break;
+
+    case MSG_LOCATION:
+        CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT);
+        send_location(chan);
+        break;
+
+    case MSG_NAV_CONTROLLER_OUTPUT:
+        CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
+        send_nav_controller_output(chan);
+        break;
+
+    case MSG_GPS_RAW:
+        CHECK_PAYLOAD_SIZE(GPS_RAW_INT);
+        send_gps_raw(chan);
+        break;
+
+    case MSG_RADIO_OUT:
+        CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW);
+        send_radio_out(chan);
+        break;
+
+    case MSG_RAW_IMU1:
+        CHECK_PAYLOAD_SIZE(RAW_IMU);
+        send_raw_imu1(chan);
+        break;
+
+    case MSG_RAW_IMU2:
+        CHECK_PAYLOAD_SIZE(SCALED_PRESSURE);
+        send_raw_imu2(chan);
+        break;
+
+    case MSG_RAW_IMU3:
+        CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS);
+        send_raw_imu3(chan);
+        break;
+
+    case MSG_NEXT_PARAM:
+        CHECK_PAYLOAD_SIZE(PARAM_VALUE);
+        if (chan == MAVLINK_COMM_0) {
+            gcs0.queued_param_send();
+        } else if (gcs3.initialised) {
+            gcs3.queued_param_send();
+        }
+        break;
+
+    case MSG_STATUSTEXT:
+        CHECK_PAYLOAD_SIZE(STATUSTEXT);
+        send_statustext(chan);
+        break;
+
+    case MSG_AHRS:
+        CHECK_PAYLOAD_SIZE(AHRS);
+        send_ahrs(chan);
+        break;
+
+    case MSG_HWSTATUS:
+        CHECK_PAYLOAD_SIZE(HWSTATUS);
+        send_hwstatus(chan);
+        break;
+
+    case MSG_SERVO_OUT:
+    case MSG_EXTENDED_STATUS1:
+    case MSG_EXTENDED_STATUS2:
+    case MSG_RETRY_DEFERRED:
+        break; // just here to prevent a warning
+    }
+    return true;
+}
+
+
+#define MAX_DEFERRED_MESSAGES MSG_RETRY_DEFERRED
+static struct mavlink_queue {
+    enum ap_message deferred_messages[MAX_DEFERRED_MESSAGES];
+    uint8_t next_deferred_message;
+    uint8_t num_deferred_messages;
+} mavlink_queue[2];
+
+// send a message using mavlink
+static void mavlink_send_message(mavlink_channel_t chan, enum ap_message id, uint16_t packet_drops)
+{
+    uint8_t i, nextid;
+    struct mavlink_queue *q = &mavlink_queue[(uint8_t)chan];
+
+    // see if we can send the deferred messages, if any
+    while (q->num_deferred_messages != 0) {
+        if (!mavlink_try_send_message(chan,
+                                      q->deferred_messages[q->next_deferred_message],
+                                      packet_drops)) {
+            break;
+        }
+        q->next_deferred_message++;
+        if (q->next_deferred_message == MAX_DEFERRED_MESSAGES) {
+            q->next_deferred_message = 0;
+        }
+        q->num_deferred_messages--;
+    }
+
+    if (id == MSG_RETRY_DEFERRED) {
+        return;
+    }
+
+    // this message id might already be deferred
+    for (i=0, nextid = q->next_deferred_message; i < q->num_deferred_messages; i++) {
+        if (q->deferred_messages[nextid] == id) {
+            // its already deferred, discard
+            return;
+        }
+        nextid++;
+        if (nextid == MAX_DEFERRED_MESSAGES) {
+            nextid = 0;
+        }
+    }
+
+    if (q->num_deferred_messages != 0 ||
+        !mavlink_try_send_message(chan, id, packet_drops)) {
+        // can't send it now, so defer it
+        if (q->num_deferred_messages == MAX_DEFERRED_MESSAGES) {
+            // the defer buffer is full, discard
+            return;
+        }
+        nextid = q->next_deferred_message + q->num_deferred_messages;
+        if (nextid >= MAX_DEFERRED_MESSAGES) {
+            nextid -= MAX_DEFERRED_MESSAGES;
+        }
+        q->deferred_messages[nextid] = id;
+        q->num_deferred_messages++;
+    }
+}
+
+void mavlink_send_text(mavlink_channel_t chan, gcs_severity severity, const char *str)
+{
+    if (severity == SEVERITY_LOW) {
+        // send via the deferred queuing system
+        mavlink_statustext_t *s = (chan == MAVLINK_COMM_0?&gcs0.pending_status:&gcs3.pending_status);
+        s->severity = (uint8_t)severity;
+        strncpy((char *)s->text, str, sizeof(s->text));
+        mavlink_send_message(chan, MSG_STATUSTEXT, 0);
+    } else {
+        // send immediately
+        mavlink_msg_statustext_send(chan, severity, str);
+    }
+}
+
+/*
+  default stream rates to 1Hz
+ */
+const AP_Param::GroupInfo GCS_MAVLINK::var_info[] PROGMEM = {
+    // @Param: RAW_SENS
+    // @DisplayName: Raw sensor stream rate
+    // @Description: Raw sensor stream rate to ground station
+    // @Units: Hz
+    // @Range: 0 10
+    // @Increment: 1
+    // @User: Advanced
+    AP_GROUPINFO("RAW_SENS", 0, GCS_MAVLINK, streamRates[0],  1),
+
+    // @Param: EXT_STAT
+    // @DisplayName: Extended status stream rate to ground station
+    // @Description: Extended status stream rate to ground station
+    // @Units: Hz
+    // @Range: 0 10
+    // @Increment: 1
+    // @User: Advanced
+    AP_GROUPINFO("EXT_STAT", 1, GCS_MAVLINK, streamRates[1],  1),
+
+    // @Param: RC_CHAN
+    // @DisplayName: RC Channel stream rate to ground station
+    // @Description: RC Channel stream rate to ground station
+    // @Units: Hz
+    // @Range: 0 10
+    // @Increment: 1
+    // @User: Advanced
+    AP_GROUPINFO("RC_CHAN",  2, GCS_MAVLINK, streamRates[2],  1),
+
+    // @Param: RAW_CTRL
+    // @DisplayName: Raw Control stream rate to ground station
+    // @Description: Raw Control stream rate to ground station
+    // @Units: Hz
+    // @Range: 0 10
+    // @Increment: 1
+    // @User: Advanced
+    AP_GROUPINFO("RAW_CTRL", 3, GCS_MAVLINK, streamRates[3],  1),
+
+    // @Param: POSITION
+    // @DisplayName: Position stream rate to ground station
+    // @Description: Position stream rate to ground station
+    // @Units: Hz
+    // @Range: 0 10
+    // @Increment: 1
+    // @User: Advanced
+    AP_GROUPINFO("POSITION", 4, GCS_MAVLINK, streamRates[4],  1),
+
+    // @Param: EXTRA1
+    // @DisplayName: Extra data type 1 stream rate to ground station
+    // @Description: Extra data type 1 stream rate to ground station
+    // @Units: Hz
+    // @Range: 0 10
+    // @Increment: 1
+    // @User: Advanced
+    AP_GROUPINFO("EXTRA1",   5, GCS_MAVLINK, streamRates[5],  1),
+
+    // @Param: EXTRA2
+    // @DisplayName: Extra data type 2 stream rate to ground station
+    // @Description: Extra data type 2 stream rate to ground station
+    // @Units: Hz
+    // @Range: 0 10
+    // @Increment: 1
+    // @User: Advanced
+    AP_GROUPINFO("EXTRA2",   6, GCS_MAVLINK, streamRates[6],  1),
+
+    // @Param: EXTRA3
+    // @DisplayName: Extra data type 3 stream rate to ground station
+    // @Description: Extra data type 3 stream rate to ground station
+    // @Units: Hz
+    // @Range: 0 10
+    // @Increment: 1
+    // @User: Advanced
+    AP_GROUPINFO("EXTRA3",   7, GCS_MAVLINK, streamRates[7],  1),
+
+    // @Param: PARAMS
+    // @DisplayName: Parameter stream rate to ground station
+    // @Description: Parameter stream rate to ground station
+    // @Units: Hz
+    // @Range: 0 10
+    // @Increment: 1
+    // @User: Advanced
+    AP_GROUPINFO("PARAMS",   8, GCS_MAVLINK, streamRates[8],  10),
+    AP_GROUPEND
+};
+
+
+GCS_MAVLINK::GCS_MAVLINK() :
+    packet_drops(0)
+{
+    AP_Param::setup_object_defaults(this, var_info);
+}
+
+void
+GCS_MAVLINK::init(AP_HAL::UARTDriver *port)
+{
+    GCS_Class::init(port);
+    if (port == (AP_HAL::BetterStream*)hal.uartA) {
+        mavlink_comm_0_port = port;
+        chan = MAVLINK_COMM_0;
+    }else{
+        mavlink_comm_1_port = port;
+        chan = MAVLINK_COMM_1;
+    }
+    _queued_parameter = NULL;
+}
+
+void
+GCS_MAVLINK::update(void)
+{
+    // receive new packets
+    mavlink_message_t msg;
+    mavlink_status_t status;
+    status.packet_rx_drop_count = 0;
+
+    // process received bytes
+    uint16_t nbytes = comm_get_available(chan);
+    for (uint16_t i=0; i<nbytes; i++)
+    {
+        uint8_t c = comm_receive_ch(chan);
+
+        // Try to get a new message
+        if (mavlink_parse_char(chan, c, &msg, &status)) {
+            // we exclude radio packets to make it possible to use the
+            // CLI over the radio
+            if (msg.msgid != MAVLINK_MSG_ID_RADIO && msg.msgid != MAVLINK_MSG_ID_RADIO_STATUS) {
+                mavlink_active = true;
+            }
+            handleMessage(&msg);
+        }
+    }
+
+    // Update packet drops counter
+    packet_drops += status.packet_rx_drop_count;
+}
+
+// see if we should send a stream now. Called at 50Hz
+bool GCS_MAVLINK::stream_trigger(enum streams stream_num)
+{
+    if (stream_num >= NUM_STREAMS) {
+        return false;
+    }
+    float rate = (uint8_t)streamRates[stream_num].get();
+
+    // send at a much lower rate during parameter sends
+    if (_queued_parameter != NULL) {
+        rate *= 0.25;
+    }
+
+    if (rate <= 0) {
+        return false;
+    }
+
+    if (stream_ticks[stream_num] == 0) {
+        // we're triggering now, setup the next trigger point
+        if (rate > 50) {
+            rate = 50;
+        }
+        stream_ticks[stream_num] = (50 / rate) + stream_slowdown;
+        return true;
+    }
+
+    // count down at 50Hz
+    stream_ticks[stream_num]--;
+    return false;
+}
+
+void
+GCS_MAVLINK::data_stream_send(void)
+{
+    if (_queued_parameter != NULL) {
+        if (streamRates[STREAM_PARAMS].get() <= 0) {
+            streamRates[STREAM_PARAMS].set(10);
+        }
+        if (stream_trigger(STREAM_PARAMS)) {
+            send_message(MSG_NEXT_PARAM);
+        }
+    }
+
+    if (in_mavlink_delay) {
+        // don't send any other stream types while in the delay callback
+        return;
+    }
+
+    if (stream_trigger(STREAM_RAW_SENSORS)) {
+        send_message(MSG_RAW_IMU1);
+        send_message(MSG_RAW_IMU2);
+        send_message(MSG_RAW_IMU3);
+    }
+
+    if (stream_trigger(STREAM_EXTENDED_STATUS)) {
+        send_message(MSG_EXTENDED_STATUS1);
+        send_message(MSG_EXTENDED_STATUS2);
+        send_message(MSG_NAV_CONTROLLER_OUTPUT);
+        send_message(MSG_GPS_RAW);
+    }
+
+    if (stream_trigger(STREAM_POSITION)) {
+        send_message(MSG_LOCATION);
+    }
+
+    if (stream_trigger(STREAM_RAW_CONTROLLER)) {
+        send_message(MSG_SERVO_OUT);
+    }
+
+    if (stream_trigger(STREAM_RC_CHANNELS)) {
+        send_message(MSG_RADIO_OUT);
+    }
+
+    if (stream_trigger(STREAM_EXTRA1)) {
+        send_message(MSG_ATTITUDE);
+    }
+
+    if (stream_trigger(STREAM_EXTRA3)) {
+        send_message(MSG_AHRS);
+        send_message(MSG_HWSTATUS);
+    }
+}
+
+
+
+void
+GCS_MAVLINK::send_message(enum ap_message id)
+{
+    mavlink_send_message(chan,id, packet_drops);
+}
+
+void
+GCS_MAVLINK::send_text_P(gcs_severity severity, const prog_char_t *str)
+{
+    mavlink_statustext_t m;
+    uint8_t i;
+    for (i=0; i<sizeof(m.text); i++) {
+        m.text[i] = pgm_read_byte((const prog_char *)(str++));
+        if (m.text[i] == '\0') {
+            break;
+        }
+    }
+    if (i < sizeof(m.text)) m.text[i] = 0;
+    mavlink_send_text(chan, severity, (const char *)m.text);
+}
+
+void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
+{
+    switch (msg->msgid) {
+
+    case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
+    {
+        // decode
+        mavlink_request_data_stream_t packet;
+        mavlink_msg_request_data_stream_decode(msg, &packet);
+
+        if (mavlink_check_target(packet.target_system, packet.target_component))
+            break;
+
+        int16_t freq = 0;     // packet frequency
+
+        if (packet.start_stop == 0)
+            freq = 0;                     // stop sending
+        else if (packet.start_stop == 1)
+            freq = packet.req_message_rate;                     // start sending
+        else
+            break;
+
+        switch (packet.req_stream_id) {
+        case MAV_DATA_STREAM_ALL:
+            // note that we don't set STREAM_PARAMS - that is internal only
+            for (uint8_t i=0; i<STREAM_PARAMS; i++) {
+                streamRates[i].set_and_save_ifchanged(freq);
+            }
+            break;
+        case MAV_DATA_STREAM_RAW_SENSORS:
+            streamRates[STREAM_RAW_SENSORS].set_and_save_ifchanged(freq);
+            break;
+        case MAV_DATA_STREAM_EXTENDED_STATUS:
+            streamRates[STREAM_EXTENDED_STATUS].set_and_save_ifchanged(freq);
+            break;
+        case MAV_DATA_STREAM_RC_CHANNELS:
+            streamRates[STREAM_RC_CHANNELS].set_and_save_ifchanged(freq);
+            break;
+        case MAV_DATA_STREAM_RAW_CONTROLLER:
+            streamRates[STREAM_RAW_CONTROLLER].set_and_save_ifchanged(freq);
+            break;
+        case MAV_DATA_STREAM_POSITION:
+            streamRates[STREAM_POSITION].set_and_save_ifchanged(freq);
+            break;
+        case MAV_DATA_STREAM_EXTRA1:
+            streamRates[STREAM_EXTRA1].set_and_save_ifchanged(freq);
+            break;
+        case MAV_DATA_STREAM_EXTRA2:
+            streamRates[STREAM_EXTRA2].set_and_save_ifchanged(freq);
+            break;
+        case MAV_DATA_STREAM_EXTRA3:
+            streamRates[STREAM_EXTRA3].set_and_save_ifchanged(freq);
+            break;
+        }
+        break;
+    }
+
+
+    case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
+    {
+        // decode
+        mavlink_param_request_list_t packet;
+        mavlink_msg_param_request_list_decode(msg, &packet);
+        if (mavlink_check_target(packet.target_system,packet.target_component)) break;
+
+        // Start sending parameters - next call to ::update will kick the first one out
+
+        _queued_parameter = AP_Param::first(&_queued_parameter_token, &_queued_parameter_type);
+        _queued_parameter_index = 0;
+        _queued_parameter_count = _count_parameters();
+        break;
+    }
+
+    case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
+    {
+        // decode
+        mavlink_param_request_read_t packet;
+        mavlink_msg_param_request_read_decode(msg, &packet);
+        if (mavlink_check_target(packet.target_system,packet.target_component)) break;
+        enum ap_var_type p_type;
+        AP_Param *vp;
+        char param_name[AP_MAX_NAME_SIZE+1];
+        if (packet.param_index != -1) {
+            AP_Param::ParamToken token;
+            vp = AP_Param::find_by_index(packet.param_index, &p_type, &token);
+            if (vp == NULL) {
+                gcs_send_text_fmt(PSTR("Unknown parameter index %d"), packet.param_index);
+                break;
+            }
+            vp->copy_name_token(token, param_name, AP_MAX_NAME_SIZE, true);
+            param_name[AP_MAX_NAME_SIZE] = 0;
+        } else {
+            strncpy(param_name, packet.param_id, AP_MAX_NAME_SIZE);
+            param_name[AP_MAX_NAME_SIZE] = 0;
+            vp = AP_Param::find(param_name, &p_type);
+            if (vp == NULL) {
+                gcs_send_text_fmt(PSTR("Unknown parameter %.16s"), packet.param_id);
+                break;
+            }
+        }
+
+        float value = vp->cast_to_float(p_type);
+        mavlink_msg_param_value_send(
+            chan,
+            param_name,
+            value,
+            mav_var_type(p_type),
+            _count_parameters(),
+            packet.param_index);
+        break;
+    }
+
+    case MAVLINK_MSG_ID_PARAM_SET:
+    {
+        AP_Param                  *vp;
+        enum ap_var_type var_type;
+
+        // decode
+        mavlink_param_set_t packet;
+        mavlink_msg_param_set_decode(msg, &packet);
+
+        if (mavlink_check_target(packet.target_system, packet.target_component))
+            break;
+
+        // set parameter
+
+        char key[AP_MAX_NAME_SIZE+1];
+        strncpy(key, (char *)packet.param_id, AP_MAX_NAME_SIZE);
+        key[AP_MAX_NAME_SIZE] = 0;
+
+        // find the requested parameter
+        vp = AP_Param::find(key, &var_type);
+        if ((NULL != vp) &&                                 // exists
+            !isnan(packet.param_value) &&                       // not nan
+            !isinf(packet.param_value)) {                       // not inf
+
+            // add a small amount before casting parameter values
+            // from float to integer to avoid truncating to the
+            // next lower integer value.
+            float rounding_addition = 0.01;
+
+            // handle variables with standard type IDs
+            if (var_type == AP_PARAM_FLOAT) {
+                ((AP_Float *)vp)->set_and_save(packet.param_value);
+            } else if (var_type == AP_PARAM_INT32) {
+                if (packet.param_value < 0) rounding_addition = -rounding_addition;
+                float v = packet.param_value+rounding_addition;
+                v = constrain_float(v, -2147483648.0, 2147483647.0);
+                ((AP_Int32 *)vp)->set_and_save(v);
+            } else if (var_type == AP_PARAM_INT16) {
+                if (packet.param_value < 0) rounding_addition = -rounding_addition;
+                float v = packet.param_value+rounding_addition;
+                v = constrain_float(v, -32768, 32767);
+                ((AP_Int16 *)vp)->set_and_save(v);
+            } else if (var_type == AP_PARAM_INT8) {
+                if (packet.param_value < 0) rounding_addition = -rounding_addition;
+                float v = packet.param_value+rounding_addition;
+                v = constrain_float(v, -128, 127);
+                ((AP_Int8 *)vp)->set_and_save(v);
+            } else {
+                // we don't support mavlink set on this parameter
+                break;
+            }
+
+            // Report back the new value if we accepted the change
+            // we send the value we actually set, which could be
+            // different from the value sent, in case someone sent
+            // a fractional value to an integer type
+            mavlink_msg_param_value_send(
+                chan,
+                key,
+                vp->cast_to_float(var_type),
+                mav_var_type(var_type),
+                _count_parameters(),
+                -1);     // XXX we don't actually know what its index is...
+#if LOGGING_ENABLED == ENABLED
+            DataFlash.Log_Write_Parameter(key, vp->cast_to_float(var_type));
+#endif
+        }
+
+        break;
+    }     // end case
+
+    case MAVLINK_MSG_ID_HEARTBEAT:
+    {
+        if (msg->sysid != g.sysid_my_gcs) break;
+        break;
+    }
+
+    case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: {
+        // decode
+        mavlink_global_position_int_t packet;
+        mavlink_msg_global_position_int_decode(msg, &packet);
+        tracking_update_position(packet);
+        break;
+    }
+
+    default:
+        break;
+
+    } // end switch
+} // end handle mavlink
+
+uint16_t
+GCS_MAVLINK::_count_parameters()
+{
+    // if we haven't cached the parameter count yet...
+    if (0 == _parameter_count) {
+        AP_Param  *vp;
+        AP_Param::ParamToken token;
+
+        vp = AP_Param::first(&token, NULL);
+        do {
+            _parameter_count++;
+        } while (NULL != (vp = AP_Param::next_scalar(&token, NULL)));
+    }
+    return _parameter_count;
+}
+
+/**
+ * @brief Send the next pending parameter, called from deferred message
+ * handling code
+ */
+void
+GCS_MAVLINK::queued_param_send()
+{
+    if (_queued_parameter == NULL) {
+        return;
+    }
+
+    uint16_t bytes_allowed;
+    uint8_t count;
+    uint32_t tnow = hal.scheduler->millis();
+
+    // use at most 30% of bandwidth on parameters. The constant 26 is
+    // 1/(1000 * 1/8 * 0.001 * 0.3)
+    bytes_allowed = g.serial3_baud * (tnow - _queued_parameter_send_time_ms) * 26;
+    if (bytes_allowed > comm_get_txspace(chan)) {
+        bytes_allowed = comm_get_txspace(chan);
+    }
+    count = bytes_allowed / (MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES);
+
+    while (_queued_parameter != NULL && count--) {
+        AP_Param      *vp;
+        float value;
+
+        // copy the current parameter and prepare to move to the next
+        vp = _queued_parameter;
+
+        // if the parameter can be cast to float, report it here and break out of the loop
+        value = vp->cast_to_float(_queued_parameter_type);
+
+        char param_name[AP_MAX_NAME_SIZE];
+        vp->copy_name_token(_queued_parameter_token, param_name, sizeof(param_name), true);
+
+        mavlink_msg_param_value_send(
+            chan,
+            param_name,
+            value,
+            mav_var_type(_queued_parameter_type),
+            _queued_parameter_count,
+            _queued_parameter_index);
+
+        _queued_parameter = AP_Param::next_scalar(&_queued_parameter_token, &_queued_parameter_type);
+        _queued_parameter_index++;
+    }
+    _queued_parameter_send_time_ms = tnow;
+}
+
+/*
+ *  a delay() callback that processes MAVLink packets. We set this as the
+ *  callback in long running library initialisation routines to allow
+ *  MAVLink to process packets while waiting for the initialisation to
+ *  complete
+ */
+static void mavlink_delay_cb()
+{
+    static uint32_t last_1hz, last_50hz, last_5s;
+    if (!gcs0.initialised) return;
+
+    in_mavlink_delay = true;
+
+    uint32_t tnow = hal.scheduler->millis();
+    if (tnow - last_1hz > 1000) {
+        last_1hz = tnow;
+        gcs_send_message(MSG_HEARTBEAT);
+        gcs_send_message(MSG_EXTENDED_STATUS1);
+    }
+    if (tnow - last_50hz > 20) {
+        last_50hz = tnow;
+        gcs_update();
+        gcs_data_stream_send();
+        notify.update();
+    }
+    if (tnow - last_5s > 5000) {
+        last_5s = tnow;
+        gcs_send_text_P(SEVERITY_LOW, PSTR("Initialising APM..."));
+    }
+    in_mavlink_delay = false;
+}
+
+/*
+ *  send a message on both GCS links
+ */
+static void gcs_send_message(enum ap_message id)
+{
+    gcs0.send_message(id);
+    if (gcs3.initialised) {
+        gcs3.send_message(id);
+    }
+}
+
+/*
+ *  send data streams in the given rate range on both links
+ */
+static void gcs_data_stream_send(void)
+{
+    gcs0.data_stream_send();
+    if (gcs3.initialised) {
+        gcs3.data_stream_send();
+    }
+}
+
+/*
+ *  look for incoming commands on the GCS links
+ */
+static void gcs_update(void)
+{
+    gcs0.update();
+    if (gcs3.initialised) {
+        gcs3.update();
+    }
+}
+
+static void gcs_send_text_P(gcs_severity severity, const prog_char_t *str)
+{
+    gcs0.send_text_P(severity, str);
+    if (gcs3.initialised) {
+        gcs3.send_text_P(severity, str);
+    }
+#if LOGGING_ENABLED == ENABLED
+    DataFlash.Log_Write_Message_P(str);
+#endif
+}
+
+/*
+ *  send a low priority formatted message to the GCS
+ *  only one fits in the queue, so if you send more than one before the
+ *  last one gets into the serial buffer then the old one will be lost
+ */
+void gcs_send_text_fmt(const prog_char_t *fmt, ...)
+{
+    va_list arg_list;
+    gcs0.pending_status.severity = (uint8_t)SEVERITY_LOW;
+    va_start(arg_list, fmt);
+    hal.util->vsnprintf_P((char *)gcs0.pending_status.text,
+            sizeof(gcs0.pending_status.text), fmt, arg_list);
+    va_end(arg_list);
+#if LOGGING_ENABLED == ENABLED
+    DataFlash.Log_Write_Message(gcs0.pending_status.text);
+#endif
+    gcs3.pending_status = gcs0.pending_status;
+    mavlink_send_message(MAVLINK_COMM_0, MSG_STATUSTEXT, 0);
+    if (gcs3.initialised) {
+        mavlink_send_message(MAVLINK_COMM_1, MSG_STATUSTEXT, 0);
+    }
+}
+

Tools/AntennaTracker/Makefile

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

Tools/AntennaTracker/Parameters.h

@@ -0,0 +1,101 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#ifndef PARAMETERS_H
+#define PARAMETERS_H
+
+#include <AP_Common.h>
+
+// Global parameter class.
+//
+class Parameters {
+public:
+
+    /*
+     *  The value of k_format_version determines whether the existing
+     *  eeprom data is considered valid. You should only change this
+     *  value under the following circumstances:
+     *
+     *  1) the meaning of an existing eeprom parameter changes
+     *
+     *  2) the value of an existing k_param_* enum value changes
+     *
+     *  Adding a new parameter should _not_ require a change to
+     *  k_format_version except under special circumstances. If you
+     *  change it anyway then all ArduPlane users will need to reload all
+     *  their parameters. We want that to be an extremely rare
+     *  thing. Please do not just change it "just in case".
+     *
+     *  To determine if a k_param_* value has changed, use the rules of
+     *  C++ enums to work out the value of the neighboring enum
+     *  values. If you don't know the C++ enum rules then please ask for
+     *  help.
+     */
+
+    //////////////////////////////////////////////////////////////////
+    // STOP!!! DO NOT CHANGE THIS VALUE UNTIL YOU FULLY UNDERSTAND THE
+    // COMMENTS ABOVE. IF UNSURE, ASK ANOTHER DEVELOPER!!!
+    static const uint16_t k_format_version = 1;
+    //////////////////////////////////////////////////////////////////
+
+
+    // The parameter software_type is set up solely for ground station use
+    // and identifies the software type (eg ArduPilotMega versus ArduCopterMega)
+    // GCS will interpret values 0-9 as ArduPilotMega.  Developers may use
+    // values within that range to identify different branches.
+    //
+    static const uint16_t k_software_type = 4;
+
+    enum {
+        // Layout version number, always key zero.
+        //
+        k_param_format_version = 0,
+        k_param_software_type,
+
+        k_param_gcs0 = 100,         // stream rates for port0
+        k_param_gcs3,               // stream rates for port3
+        k_param_sysid_this_mav,
+        k_param_sysid_my_gcs,
+        k_param_serial0_baud,
+        k_param_serial3_baud,
+        k_param_imu,
+        k_param_compass_enabled,
+        k_param_compass,
+        k_param_ahrs,  // AHRS group
+        k_param_barometer,
+        k_param_scheduler,
+        k_param_ins,
+        k_param_sitl,
+        k_param_pidPitch2Srv,
+        k_param_pidYaw2Srv,
+
+        k_param_channel_yaw = 200,
+        k_param_channel_pitch
+
+        // 254,255: reserved
+    };
+
+    AP_Int16 format_version;
+    AP_Int8 software_type;
+
+    // Telemetry control
+    //
+    AP_Int16 sysid_this_mav;
+    AP_Int16 sysid_my_gcs;
+    AP_Int8 serial0_baud;
+    AP_Int8 serial3_baud;
+
+    AP_Int8 compass_enabled;
+
+    // PID controllers
+    PID         pidPitch2Srv;
+    PID         pidYaw2Srv;
+
+    Parameters() :
+        pidPitch2Srv(1.0f, 0.2f, 0.05f, 4000.0f),
+        pidYaw2Srv(1.0f, 0.2f, 0.05f, 4000.0f)
+        {}
+};
+
+extern const AP_Param::Info var_info[];
+
+#endif // PARAMETERS_H

Tools/AntennaTracker/Parameters.pde

@@ -0,0 +1,125 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/*
+ *  AntennaTracker parameter definitions
+ *
+ */
+
+#define GSCALAR(v, name, def) { g.v.vtype, name, Parameters::k_param_ ## v, &g.v, {def_value : def} }
+#define ASCALAR(v, name, def) { aparm.v.vtype, name, Parameters::k_param_ ## v, &aparm.v, {def_value : def} }
+#define GGROUP(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &g.v, {group_info : class::var_info} }
+#define GOBJECT(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &v, {group_info : class::var_info} }
+
+const AP_Param::Info var_info[] PROGMEM = {
+    GSCALAR(format_version,         "FORMAT_VERSION", 0),
+    GSCALAR(software_type,          "SYSID_SW_TYPE",  Parameters::k_software_type),
+
+    // @Param: SYSID_THISMAV
+    // @DisplayName: MAVLink system ID
+    // @Description: The identifier of this device in the MAVLink protocol
+    // @Range: 1 255
+    // @User: Advanced
+    GSCALAR(sysid_this_mav,         "SYSID_THISMAV",  MAV_SYSTEM_ID),
+
+    // @Param: SYSID_MYGCS
+    // @DisplayName: Ground station MAVLink system ID
+    // @Description: The identifier of the ground station in the MAVLink protocol. Don't change this unless you also modify the ground station to match.
+    // @Range: 1 255
+    // @User: Advanced
+    GSCALAR(sysid_my_gcs,           "SYSID_MYGCS",    255),
+
+    // @Param: SERIAL0_BAUD
+    // @DisplayName: USB Console Baud Rate
+    // @Description: The baud rate used on the main uart
+    // @Values: 1:1200,2:2400,4:4800,9:9600,19:19200,38:38400,57:57600,111:111100,115:115200
+    // @User: Standard
+    GSCALAR(serial0_baud,           "SERIAL0_BAUD",   SERIAL0_BAUD/1000),
+
+    // @Param: SERIAL3_BAUD
+    // @DisplayName: Telemetry Baud Rate
+    // @Description: The baud rate used on the telemetry port
+    // @Values: 1:1200,2:2400,4:4800,9:9600,19:19200,38:38400,57:57600,111:111100,115:115200
+    // @User: Standard
+    GSCALAR(serial3_baud,           "SERIAL3_BAUD",   SERIAL3_BAUD/1000),
+
+
+
+    // @Param: MAG_ENABLE
+    // @DisplayName: Enable Compass
+    // @Description: Setting this to Enabled(1) will enable the compass. Setting this to Disabled(0) will disable the compass. Note that this is separate from COMPASS_USE. This will enable the low level senor, and will enable logging of magnetometer data. To use the compass for navigation you must also set COMPASS_USE to 1.
+    // @Values: 0:Disabled,1:Enabled
+    // @User: Standard
+    GSCALAR(compass_enabled,        "MAG_ENABLE",     1),
+
+    // barometer ground calibration. The GND_ prefix is chosen for
+    // compatibility with previous releases of ArduPlane
+    // @Group: GND_
+    // @Path: ../libraries/AP_Baro/AP_Baro.cpp
+    GOBJECT(barometer, "GND_", AP_Baro),
+
+    // @Group: COMPASS_
+    // @Path: ../libraries/AP_Compass/Compass.cpp
+    GOBJECT(compass,                "COMPASS_",     Compass),
+
+    // @Group: SCHED_
+    // @Path: ../libraries/AP_Scheduler/AP_Scheduler.cpp
+    GOBJECT(scheduler, "SCHED_", AP_Scheduler),
+
+    // @Group: SR0_
+    // @Path: GCS_Mavlink.pde
+    GOBJECT(gcs0,                                   "SR0_",     GCS_MAVLINK),
+
+    // @Group: SR3_
+    // @Path: GCS_Mavlink.pde
+    GOBJECT(gcs3,                                   "SR3_",     GCS_MAVLINK),
+
+    // @Group: INS_
+    // @Path: ../libraries/AP_InertialSensor/AP_InertialSensor.cpp
+    GOBJECT(ins,                    "INS_", AP_InertialSensor),
+
+    // @Group: AHRS_
+    // @Path: ../libraries/AP_AHRS/AP_AHRS.cpp
+    GOBJECT(ahrs,                   "AHRS_",    AP_AHRS),
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
+    // @Group: SIM_
+    // @Path: ../libraries/SITL/SITL.cpp
+    GOBJECT(sitl, "SIM_", SITL),
+#endif
+
+    // RC channel
+    //-----------
+    // @Group: RC1_
+    // @Path: ../libraries/RC_Channel/RC_Channel.cpp
+    GOBJECT(channel_yaw,       "RC1_", RC_Channel),
+
+    // @Group: RC2_
+    // @Path: ../libraries/RC_Channel/RC_Channel.cpp
+    GOBJECT(channel_pitch,     "RC2_", RC_Channel),
+
+	GGROUP(pidPitch2Srv,       "PITCH2SRV_", PID),
+	GGROUP(pidYaw2Srv,         "YAW2SRV_", PID),
+
+    AP_VAREND
+};
+
+
+static void load_parameters(void)
+{
+    if (!g.format_version.load() ||
+        g.format_version != Parameters::k_format_version) {
+
+        // erase all parameters
+        cliSerial->printf_P(PSTR("Firmware change: erasing EEPROM...\n"));
+        AP_Param::erase_all();
+
+        // save the current format version
+        g.format_version.set_and_save(Parameters::k_format_version);
+        cliSerial->println_P(PSTR("done."));
+    } else {
+        uint32_t before = hal.scheduler->micros();
+        // Load all auto-loaded EEPROM variables
+        AP_Param::load_all();
+        cliSerial->printf_P(PSTR("load_all took %luus\n"), hal.scheduler->micros() - before);
+    }
+}

Tools/AntennaTracker/config.h

@@ -0,0 +1,83 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+//
+#include "defines.h"
+
+#include "APM_Config.h" // <== THIS INCLUDE, DO NOT EDIT IT. EVER.
+
+///
+/// DO NOT EDIT THIS INCLUDE - if you want to make a local change, make that
+/// change in your local copy of APM_Config.h.
+///
+
+// Just so that it's completely clear...
+#define ENABLED                 1
+#define DISABLED                0
+
+// this avoids a very common config error
+#define ENABLE ENABLED
+#define DISABLE DISABLED
+
+//////////////////////////////////////////////////////////////////////////////
+// main board differences
+//
+#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
+ # define CONFIG_INS_TYPE CONFIG_INS_OILPAN
+ # define CONFIG_BARO     AP_BARO_BMP085
+ # define CONFIG_COMPASS  AP_COMPASS_HMC5843
+#elif CONFIG_HAL_BOARD == HAL_BOARD_APM2
+ # define CONFIG_INS_TYPE CONFIG_INS_MPU6000
+ # define CONFIG_BARO          AP_BARO_MS5611
+ # define CONFIG_MS5611_SERIAL AP_BARO_MS5611_SPI
+ # define CONFIG_COMPASS  AP_COMPASS_HMC5843
+#elif CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
+ # define CONFIG_INS_TYPE CONFIG_INS_HIL
+ # define CONFIG_BARO     AP_BARO_HIL
+ # define CONFIG_COMPASS  AP_COMPASS_HIL
+#elif CONFIG_HAL_BOARD == HAL_BOARD_PX4
+ # define CONFIG_INS_TYPE CONFIG_INS_PX4
+ # define CONFIG_BARO AP_BARO_PX4
+ # define CONFIG_COMPASS  AP_COMPASS_PX4
+ # define SERIAL0_BAUD 115200
+#elif CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE
+ # define CONFIG_INS_TYPE CONFIG_INS_FLYMAPLE
+ # define CONFIG_BARO AP_BARO_BMP085
+ # define CONFIG_COMPASS  AP_COMPASS_HMC5843
+ # define SERIAL0_BAUD 115200
+#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX
+ # define CONFIG_INS_TYPE CONFIG_INS_L3G4200D
+ # define CONFIG_BARO     AP_BARO_BMP085
+ # define CONFIG_COMPASS  AP_COMPASS_HMC5843
+#endif
+
+
+#ifndef CONFIG_BARO
+ # error "CONFIG_BARO not set"
+#endif
+
+#ifndef CONFIG_COMPASS
+ # error "CONFIG_COMPASS not set"
+#endif
+
+#ifndef MAV_SYSTEM_ID
+ // use 2 for antenna tracker by default
+ # define MAV_SYSTEM_ID          2
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// Serial port speeds.
+//
+#ifndef SERIAL0_BAUD
+ # define SERIAL0_BAUD                   115200
+#endif
+#ifndef SERIAL3_BAUD
+ # define SERIAL3_BAUD                    57600
+#endif
+
+
+#ifndef SERIAL_BUFSIZE
+ # define SERIAL_BUFSIZE 512
+#endif
+
+#ifndef SERIAL2_BUFSIZE
+ # define SERIAL2_BUFSIZE 256
+#endif

Tools/AntennaTracker/defines.h

@@ -0,0 +1,56 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#ifndef _DEFINES_H
+#define _DEFINES_H
+
+#define ToRad(x) radians(x)	// *pi/180
+#define ToDeg(x) degrees(x)	// *180/pi
+
+/// please keep each MSG_ to a single MAVLink message. If need be
+/// create new MSG_ IDs for additional messages on the same
+/// stream
+enum ap_message {
+    MSG_HEARTBEAT,
+    MSG_ATTITUDE,
+    MSG_LOCATION,
+    MSG_AHRS,
+    MSG_HWSTATUS,
+    MSG_GPS_RAW,
+    MSG_SERVO_OUT,
+    MSG_RADIO_OUT,
+    MSG_RAW_IMU1,
+    MSG_RAW_IMU2,
+    MSG_RAW_IMU3,
+    MSG_NEXT_PARAM,
+    MSG_STATUSTEXT,
+    MSG_EXTENDED_STATUS1,
+    MSG_EXTENDED_STATUS2,
+    MSG_NAV_CONTROLLER_OUTPUT,
+    MSG_RETRY_DEFERRED // this must be last
+};
+
+#define EEPROM_MAX_ADDR         4096
+
+// mark a function as not to be inlined
+#define NOINLINE __attribute__((noinline))
+
+// InertialSensor driver types
+#define CONFIG_INS_OILPAN  1
+#define CONFIG_INS_MPU6000 2
+#define CONFIG_INS_HIL     3
+#define CONFIG_INS_PX4     4
+#define CONFIG_INS_FLYMAPLE 5
+#define CONFIG_INS_L3G4200D 6
+
+// barometer driver types
+#define AP_BARO_BMP085   1
+#define AP_BARO_MS5611   2
+#define AP_BARO_PX4      3
+#define AP_BARO_HIL      4
+
+// compass driver types
+#define AP_COMPASS_HMC5843   1
+#define AP_COMPASS_PX4       2
+#define AP_COMPASS_HIL       3
+
+#endif // _DEFINES_H

Tools/AntennaTracker/sensors.pde

@@ -0,0 +1,65 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+static void init_barometer(void)
+{
+    gcs_send_text_P(SEVERITY_LOW, PSTR("Calibrating barometer"));    
+    barometer.calibrate();
+    gcs_send_text_P(SEVERITY_LOW, PSTR("barometer calibration complete"));
+}
+
+// read the barometer and return the updated altitude in meters
+static void update_barometer(void)
+{
+    barometer.read();
+}
+
+
+/*
+  update INS and attitude
+ */
+static void update_ahrs()
+{
+    ahrs.update();
+}
+
+
+/*
+  read and update compass
+ */
+static void update_compass(void)
+{
+    if (g.compass_enabled && compass.read()) {
+        ahrs.set_compass(&compass);
+        compass.null_offsets();
+    } else {
+        ahrs.set_compass(NULL);
+    }
+}
+
+/*
+  if the compass is enabled then try to accumulate a reading
+ */
+static void compass_accumulate(void)
+{
+    if (g.compass_enabled) {
+        compass.accumulate();
+    }    
+}
+
+/*
+  try to accumulate a baro reading
+ */
+static void barometer_accumulate(void)
+{
+    barometer.accumulate();
+}
+
+
+/*
+  read the GPS
+ */
+static void update_GPS(void)
+{
+    g_gps->update();
+}
+

Tools/AntennaTracker/system.pde

@@ -0,0 +1,109 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+static void init_tracker()
+{
+    hal.uartA->begin(SERIAL0_BAUD, 128, SERIAL_BUFSIZE);
+
+    // gps port
+    hal.uartB->begin(38400, 256, 16);
+
+    cliSerial->printf_P(PSTR("\n\nInit " THISFIRMWARE
+                         "\n\nFree RAM: %u\n"),
+                    memcheck_available_memory());
+
+    // Check the EEPROM format version before loading any parameters from EEPROM
+    load_parameters();
+
+    // reset the uartA baud rate after parameter load
+    hal.uartA->begin(map_baudrate(g.serial0_baud, SERIAL0_BAUD));
+
+    // init baro before we start the GCS, so that the CLI baro test works
+    barometer.init();
+
+    // init the GCS
+    gcs0.init(hal.uartA);
+    // Register mavlink_delay_cb, which will run anytime you have
+    // more than 5ms remaining in your call to hal.scheduler->delay
+    hal.scheduler->register_delay_callback(mavlink_delay_cb, 5);
+
+    // we have a 2nd serial port for telemetry
+    hal.uartC->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD),
+                     128, SERIAL2_BUFSIZE);
+    gcs3.init(hal.uartC);
+
+    mavlink_system.sysid = g.sysid_this_mav;
+
+    if (g.compass_enabled==true) {
+        if (!compass.init() || !compass.read()) {
+            cliSerial->println_P(PSTR("Compass initialisation failed!"));
+            g.compass_enabled = false;
+        } else {
+            ahrs.set_compass(&compass);
+        }
+    }
+
+	// Do GPS init
+	g_gps = &g_gps_driver;
+
+    // GPS Initialization
+    g_gps->init(hal.uartB, GPS::GPS_ENGINE_STATIONARY);
+
+    mavlink_system.compid = 4;
+    mavlink_system.type = MAV_TYPE_ANTENNA_TRACKER;
+
+    ahrs.init();
+    ahrs.set_fly_forward(false);
+
+    ins.init(AP_InertialSensor::WARM_START, ins_sample_rate);
+    ahrs.reset();
+
+    init_barometer();
+
+    hal.uartA->set_blocking_writes(false);
+    hal.uartC->set_blocking_writes(false);
+
+    // setup antenna control PWM channels
+    channel_yaw.set_angle(4500);
+    channel_pitch.set_angle(4500);
+
+    channel_yaw.output_trim();
+    channel_pitch.output_trim();
+
+    channel_yaw.calc_pwm();
+    channel_pitch.calc_pwm();
+
+    channel_yaw.enable_out();
+    channel_pitch.enable_out();
+
+    gcs_send_text_P(SEVERITY_LOW,PSTR("\nReady to track."));
+    hal.scheduler->delay(1000);
+}
+
+// updates the status of the notify objects
+// should be called at 50hz
+static void update_notify()
+{
+    notify.update();
+}
+
+/*
+ *  map from a 8 bit EEPROM baud rate to a real baud rate
+ */
+static uint32_t map_baudrate(int8_t rate, uint32_t default_baud)
+{
+    switch (rate) {
+    case 1:    return 1200;
+    case 2:    return 2400;
+    case 4:    return 4800;
+    case 9:    return 9600;
+    case 19:   return 19200;
+    case 38:   return 38400;
+    case 57:   return 57600;
+    case 111:  return 111100;
+    case 115:  return 115200;
+    }
+    cliSerial->println_P(PSTR("Invalid SERIAL3_BAUD"));
+    return default_baud;
+}
+
+

Tools/AntennaTracker/tracking.pde

@@ -0,0 +1,81 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/**
+   state of the vehicle we are tracking
+ */
+static struct {
+    Location location;
+    uint32_t last_update_us;
+    float heading; // degrees
+    float ground_speed; // m/s
+} vehicle;
+
+static Location our_location;
+
+/**
+   update the pitch servo. The aim is to drive the boards pitch to the
+   requested pitch
+ */
+static void update_pitch_servo(float pitch)
+{
+    channel_pitch.servo_out = g.pidPitch2Srv.get_pid_4500(degrees(ahrs.pitch) - pitch);
+    channel_pitch.calc_pwm();
+    channel_pitch.output();
+}
+
+/**
+   update the yaw servo
+ */
+static void update_yaw_servo(float yaw)
+{
+    channel_yaw.servo_out = g.pidYaw2Srv.get_pid_4500(degrees(ahrs.yaw) - yaw);    
+    channel_yaw.calc_pwm();
+    channel_yaw.output();
+}
+
+
+/**
+  main antenna tracking code, called at 50Hz
+ */
+static void update_tracking(void)
+{
+    // project the vehicle position to take account of lost radio packets
+    Location vpos = vehicle.location;
+    float dt = (hal.scheduler->micros() - vehicle.last_update_us) * 1.0e-6f;
+    location_update(vpos, vehicle.heading, vehicle.ground_speed * dt);
+
+    // update our position if we have at least a 2D fix
+    if (g_gps->status() >= GPS::GPS_OK_FIX_2D) {
+        our_location.lat = g_gps->latitude;
+        our_location.lng = g_gps->longitude;
+        our_location.alt = 0; // assume ground level for now
+    }
+
+    // calculate the bearing to the vehicle
+    float bearing  = get_bearing_cd(our_location, vehicle.location) * 0.01f;
+    float distance = get_distance(our_location, vehicle.location);
+    float pitch    = degrees(atan2(vehicle.location.alt - our_location.alt, distance));
+    
+    // update the servos
+    update_pitch_servo(pitch);
+    update_yaw_servo(bearing);
+
+    // update nav_status for NAV_CONTROLLER_OUTPUT
+    nav_status.bearing  = bearing;
+    nav_status.pitch    = pitch;
+    nav_status.distance = distance;
+}
+
+
+/**
+   handle an updated position from the aircraft
+ */
+static void tracking_update_position(const mavlink_global_position_int_t &msg)
+{
+    vehicle.location.lat = msg.lat;
+    vehicle.location.lng = msg.lon;
+    vehicle.location.alt = msg.relative_alt/10;
+    vehicle.heading      = msg.hdg * 0.01f;
+    vehicle.ground_speed = pythagorous2(msg.vx, msg.vy) * 0.01f;
+    vehicle.last_update_us = hal.scheduler->micros();    
+}

Tools/Frame_params/Advanced.param

@@ -0,0 +1,24 @@
+#NOTE: Advanced
+ACRO_BAL_PITCH,0.25
+ACRO_BAL_ROLL,0.25
+ACRO_RP_P,8
+ACRO_TRAINER,0
+ACRO_YAW_P,4.5
+ANGLE_MAX,4500
+HLD_LAT_P,1
+HLD_LON_P,1
+LAND_SPEED,200
+LOITER_LAT_I,2
+LOITER_LAT_P,1
+LOITER_LON_I,2
+LOITER_LON_P,1
+PILOT_VELZ_MAX,1000
+THR_ACCEL_I,1.5
+THR_ACCEL_P,0.75
+THR_ALT_P,1
+WPNAV_ACCEL,500
+WPNAV_LOIT_SPEED,1500
+WPNAV_RADIUS,200
+WPNAV_SPEED,1500
+WPNAV_SPEED_DN,200
+WPNAV_SPEED_UP,500

Tools/Frame_params/Beginner.param

@@ -0,0 +1,24 @@
+#NOTE: Beginner
+ACRO_BAL_PITCH,0.5
+ACRO_BAL_ROLL,0.5
+ACRO_RP_P,4
+ACRO_TRAINER,2
+ACRO_YAW_P,4.5
+ANGLE_MAX,3000
+HLD_LAT_P,0.5
+HLD_LON_P,0.5
+LAND_SPEED,50
+LOITER_LAT_I,1
+LOITER_LAT_P,0.5
+LOITER_LON_I,1
+LOITER_LON_P,0.5
+PILOT_VELZ_MAX,250
+THR_ACCEL_I,1.5
+THR_ACCEL_P,0.75
+THR_ALT_P,1
+WPNAV_ACCEL,100
+WPNAV_LOIT_SPEED,500
+WPNAV_RADIUS,200
+WPNAV_SPEED,500
+WPNAV_SPEED_DN,200
+WPNAV_SPEED_UP,500

Tools/Frame_params/CameraPlatform.param

@@ -0,0 +1,24 @@
+#NOTE: Camera
+ACRO_BAL_PITCH,1
+ACRO_BAL_ROLL,1
+ACRO_RP_P,8
+ACRO_TRAINER,0
+ACRO_YAW_P,4.5
+ANGLE_MAX,3000
+HLD_LAT_P,1
+HLD_LON_P,1
+LAND_SPEED,50
+LOITER_LAT_I,2
+LOITER_LAT_P,1
+LOITER_LON_I,2
+LOITER_LON_P,1
+PILOT_VELZ_MAX,500
+THR_ACCEL_I,1.5
+THR_ACCEL_P,0.75
+THR_ALT_P,1
+WPNAV_ACCEL,100
+WPNAV_LOIT_SPEED,1000
+WPNAV_RADIUS,200
+WPNAV_SPEED,500
+WPNAV_SPEED_DN,200
+WPNAV_SPEED_UP,500

Tools/Frame_params/Intermediate.param

@@ -0,0 +1,24 @@
+#NOTE: Intermediate
+ACRO_BAL_PITCH,0.25
+ACRO_BAL_ROLL,0.25
+ACRO_RP_P,6
+ACRO_TRAINER,1
+ACRO_YAW_P,4.5
+ANGLE_MAX,4500
+HLD_LAT_P,1
+HLD_LON_P,1
+LAND_SPEED,100
+LOITER_LAT_I,2
+LOITER_LAT_P,1
+LOITER_LON_I,2
+LOITER_LON_P,1
+PILOT_VELZ_MAX,500
+THR_ACCEL_I,1.5
+THR_ACCEL_P,0.75
+THR_ALT_P,1
+WPNAV_ACCEL,200
+WPNAV_LOIT_SPEED,1000
+WPNAV_RADIUS,200
+WPNAV_SPEED,1000
+WPNAV_SPEED_DN,200
+WPNAV_SPEED_UP,500

Tools/Frame_params/Iris.param

@@ -19,7 +19,7 @@ FRAME,2
 FS_BATT_ENABLE,1
 FS_BATT_VOLTAGE,10.5
 FS_THR_ENABLE,1
-LOG_BITMASK, 958
+LOG_BITMASK, 26622
 MOT_SPIN_ARMED,90
 RATE_PIT_D,0.006
 RATE_PIT_I,0.25

Tools/VARTest/VARTest.pde

@@ -66,7 +66,7 @@ static Parameters g;
 static GPS         *g_gps;
 AP_GPS_Auto     g_gps_driver(&g_gps);
 AP_InertialSensor_MPU6000 ins;
-AP_AHRS_DCM  ahrs(&ins, g_gps);
+AP_AHRS_DCM  ahrs(ins, g_gps);
 
 static AP_Compass_HIL compass;
 AP_Baro_HIL      barometer;

Tools/autotest/ArduCopter.parm

@@ -2,6 +2,7 @@ FRAME		0
 MAG_ENABLE	1
 FS_THR_ENABLE   1
 BATT_MONITOR    4
+CH7_OPT         7
 COMPASS_LEARN   0
 COMPASS_OFS_X   5
 COMPASS_OFS_Y   13

Tools/autotest/arducopter.py

@@ -910,7 +910,6 @@ def fly_ArduCopter(viewerip=None, map=False):
 
         print("Save landing WP")
         save_wp(mavproxy, mav)
-        mav.recv_match(condition='RC_CHANNELS_RAW.chan7_raw==1000', blocking=True)
 
         # save the stored mission to file
         print("# Save out the CH7 mission to file")

Tools/autotest/autotest.py

@@ -121,7 +121,7 @@ def convert_gpx():
         kml = m + '.kml'
         util.run_cmd('gpsbabel -i gpx -f %s -o kml,units=m,floating=1,extrude=1 -F %s' % (gpx, kml), checkfail=False)
         util.run_cmd('zip %s.kmz %s.kml' % (m, m), checkfail=False)
-        util.run_cmd(util.reltopdir("../MAVProxy/tools/mavflightview.py") + " --imagefile=%s.png %s" % (m,m))
+        util.run_cmd("mavflightview.py --imagefile=%s.png %s" % (m,m))
     return True
 
 

Tools/autotest/common.py

@@ -49,6 +49,12 @@ def get_bearing(loc1, loc2):
         bearing += 360.00
     return bearing;
 
+def wait_seconds(seconds_to_wait):
+    tstart = time.time();
+    tnow = tstart
+    while tstart + seconds_to_wait > tnow:
+        tnow = time.time()
+
 def wait_altitude(mav, alt_min, alt_max, timeout=30):
     climb_rate = 0
     previous_alt = 0
@@ -208,10 +214,15 @@ def wait_waypoint(mav, wpnum_start, wpnum_end, allow_skip=True, max_dist=2, time
     return False
 
 def save_wp(mavproxy, mav):
-    mavproxy.send('rc 7 2000\n')
-    mav.recv_match(condition='RC_CHANNELS_RAW.chan7_raw==2000', blocking=True)
     mavproxy.send('rc 7 1000\n')
     mav.recv_match(condition='RC_CHANNELS_RAW.chan7_raw==1000', blocking=True)
+    wait_seconds(1)
+    mavproxy.send('rc 7 2000\n')
+    mav.recv_match(condition='RC_CHANNELS_RAW.chan7_raw==2000', blocking=True)
+    wait_seconds(1)
+    mavproxy.send('rc 7 1000\n')
+    mav.recv_match(condition='RC_CHANNELS_RAW.chan7_raw==1000', blocking=True)
+    wait_seconds(1)
 
 def wait_mode(mav, mode, timeout=None):
     print("Waiting for mode %s" % mode)

Tools/autotest/pysim/util.py

@@ -60,7 +60,10 @@ def deltree(path):
 
 def build_SIL(atype, target='sitl'):
     '''build desktop SIL'''
-    run_cmd("make clean %s" % target,
+    run_cmd("make clean",
+            dir=reltopdir(atype),
+            checkfail=True)
+    run_cmd("make %s" % target,
             dir=reltopdir(atype),
             checkfail=True)
     return True

Tools/scripts/build_all.sh

@@ -19,7 +19,7 @@ popd
 
 echo "Testing ArduCopter build"
 pushd ArduCopter
-for b in all apm2 apm1-hil apm2-hil sitl heli dmp linux; do
+for b in all apm2 apm1-hil apm2-hil sitl apm2-heli linux; do
     pwd
     make clean
     make $b -j4

Tools/scripts/build_autotest.sh

@@ -139,6 +139,6 @@ echo $githash > "buildlogs/history/$hdate/githash.txt"
 
 killall -9 JSBSim || /bin/true
 
-timelimit 5200 APM/Tools/autotest/autotest.py --timeout=5000 > buildlogs/autotest-output.txt 2>&1
+timelimit 6500 APM/Tools/autotest/autotest.py --timeout=6000 > buildlogs/autotest-output.txt 2>&1
 
 ) >> build.log 2>&1

Tools/scripts/build_binaries.sh

@@ -118,8 +118,9 @@ build_arducopter() {
     checkout ArduCopter $tag || return
     echo "Building ArduCopter $tag binaries"
     pushd ArduCopter
+    frames="quad tri hexa y6 octa octa-quad heli"
     for b in apm1 apm2; do
-	for f in quad tri hexa y6 octa octa-quad heli quad-hil heli-hil; do
+	for f in $frames quad-hil heli-hil; do
 	    echo "Building ArduCopter $b-$f binaries"
 	    ddir="$binaries/Copter/$hdate/$b-$f"
 	    skip_build $tag $ddir && continue
@@ -131,7 +132,7 @@ build_arducopter() {
     done
     test -n "$PX4_ROOT" && {
 	make px4-clean || return
-	for f in quad tri hexa y6 octa octa-quad heli quad-hil heli-hil; do
+	for f in $frames quad-hil heli-hil; do
 	    echo "Building ArduCopter PX4-$f binaries"
 	    ddir="$binaries/Copter/$hdate/PX4-$f"
 	    skip_build $tag $ddir && continue

libraries/AC_Fence/AC_Fence.cpp

@@ -55,7 +55,7 @@ const AP_Param::GroupInfo AC_Fence::var_info[] PROGMEM = {
 };
 
 /// Default constructor.
-AC_Fence::AC_Fence(AP_InertialNav* inav) :
+AC_Fence::AC_Fence(const AP_InertialNav* inav) :
     _inav(inav),
     _alt_max_backup(0),
     _circle_radius_backup(0),
@@ -120,7 +120,7 @@ uint8_t AC_Fence::check_fence()
     }
 
     // get current altitude in meters
-    float curr_alt = _inav->get_position().z * 0.01f;
+    float curr_alt = _inav->get_altitude() * 0.01f;
 
     // altitude fence check
     if ((_enabled_fences & AC_FENCE_TYPE_ALT_MAX) != 0) {

libraries/AC_Fence/AC_Fence.h

@@ -32,7 +32,7 @@ class AC_Fence
 public:
 
     /// Constructor
-    AC_Fence(AP_InertialNav* inav);
+    AC_Fence(const AP_InertialNav* inav);
 
     /// enable - allows fence to be enabled/disabled.  Note: this does not update the eeprom saved value
     void enable(bool true_false) { _enabled = true_false; }
@@ -89,7 +89,7 @@ private:
     void clear_breach(uint8_t fence_type);
 
     // pointers to other objects we depend upon
-    AP_InertialNav* _inav;
+    const AP_InertialNav *const _inav;
 
     // parameters
     AP_Int8         _enabled;               // top level enable/disable control

libraries/AC_Fence/examples/AC_Fence_test/AC_Fence_test.pde

@@ -52,10 +52,10 @@ AP_GPS_Auto auto_gps(&gps);
 GPS_Glitch gps_glitch(gps);
 
 AP_Compass_HMC5843 compass;
-AP_AHRS_DCM ahrs(&ins, gps);
+AP_AHRS_DCM ahrs(ins, gps);
 
 // Inertial Nav declaration
-AP_InertialNav inertial_nav(&ahrs, &ins, &baro, gps, gps_glitch);
+AP_InertialNav inertial_nav(&ahrs, &baro, gps, gps_glitch);
 
 // Fence
 AC_Fence fence(&inertial_nav);

libraries/AC_Sprayer/AC_Sprayer.cpp

@@ -50,8 +50,7 @@ const AP_Param::GroupInfo AC_Sprayer::var_info[] PROGMEM = {
     AP_GROUPEND
 };
 
-/// Default constructor.
+AC_Sprayer::AC_Sprayer(const AP_InertialNav* inav) :
-AC_Sprayer::AC_Sprayer(AP_InertialNav* inav) :
     _inav(inav),
     _speed_over_min_time(0),
     _speed_under_min_time(0)
@@ -69,7 +68,6 @@ AC_Sprayer::AC_Sprayer(AP_InertialNav* inav) :
     // To-Do: ensure that the pump and spinner servo channels are enabled
 }
 
-/// enable - allows fence to be enabled/disabled.  Note: this does not update the eeprom saved value
 void AC_Sprayer::enable(bool true_false)
 {
     // return immediately if no change
@@ -97,7 +95,6 @@ void
 AC_Sprayer::update()
 {
     uint32_t now;
-    Vector3f velocity;
     float ground_speed;
 
     // exit immediately if we are disabled (perhaps set pwm values back to defaults)
@@ -111,7 +108,7 @@ AC_Sprayer::update()
     }
 
     // get horizontal velocity
-    velocity = _inav->get_velocity();
+    const Vector3f &velocity = _inav->get_velocity();
     ground_speed = pythagorous2(velocity.x,velocity.y);
 
     // get the current time

libraries/AC_Sprayer/AC_Sprayer.h

@@ -34,19 +34,19 @@
 #define AC_SPRAYER_DEFAULT_TURN_ON_DELAY    100     // delay between when we reach the minimum speed and we begin spraying.  This reduces the likelihood of constantly turning on/off the pump
 #define AC_SPRAYER_DEFAULT_SHUT_OFF_DELAY   1000    // shut-off delay in milli seconds.  This reduces the likelihood of constantly turning on/off the pump
 
-/// @class	Camera
+/// @class  AC_Sprayer
-/// @brief	Object managing a Photo or video camera
+/// @brief  Object managing a crop sprayer comprised of a spinner and a pump both controlled by pwm
 class AC_Sprayer {
 
 public:
 
     /// Constructor
-    AC_Sprayer(AP_InertialNav* inav);
+    AC_Sprayer(const AP_InertialNav* inav);
 
     /// enable - allows sprayer to be enabled/disabled.  Note: this does not update the eeprom saved value
     void enable(bool true_false);
 
-    /// enabled - returns true if fence is enabled
+    /// enabled - returns true if sprayer is enabled
     bool enabled() const { return _enabled; }
 
     /// test_pump - set to true to turn on pump as if travelling at 1m/s as a test
@@ -64,7 +64,7 @@ public:
 
 private:
     // pointers to other objects we depend upon
-    AP_InertialNav* _inav;
+    const AP_InertialNav* const _inav;
 
     // parameters
     AP_Int8         _enabled;               // top level enable/disable control

libraries/AC_WPNav/AC_WPNav.cpp

@@ -68,7 +68,7 @@ const AP_Param::GroupInfo AC_WPNav::var_info[] PROGMEM = {
 // Note that the Vector/Matrix constructors already implicitly zero
 // their values.
 //
-AC_WPNav::AC_WPNav(AP_InertialNav* inav, AP_AHRS* ahrs, APM_PI* pid_pos_lat, APM_PI* pid_pos_lon, AC_PID* pid_rate_lat, AC_PID* pid_rate_lon) :
+AC_WPNav::AC_WPNav(const AP_InertialNav* inav, const AP_AHRS* ahrs, APM_PI* pid_pos_lat, APM_PI* pid_pos_lon, AC_PID* pid_rate_lat, AC_PID* pid_rate_lon) :
     _inav(inav),
     _ahrs(ahrs),
     _pid_pos_lat(pid_pos_lat),
@@ -102,7 +102,8 @@ AC_WPNav::AC_WPNav(AP_InertialNav* inav, AP_AHRS* ahrs, APM_PI* pid_pos_lat, APM
 {
     AP_Param::setup_object_defaults(this, var_info);
 
-    // calculate loiter leash
+    // initialise leash lengths
+    calculate_wp_leash_length(true);
     calculate_loiter_leash_length();
 }
 
@@ -138,7 +139,7 @@ void AC_WPNav::get_stopping_point(const Vector3f& position, const Vector3f& velo
         linear_distance = _wp_accel_cms/(2.0f*kP*kP);
         target_dist = linear_distance + (vel_total*vel_total)/(2.0f*_wp_accel_cms);
     }
-    target_dist = constrain_float(target_dist, 0, _wp_leash_xy*2.0f);
+    target_dist = constrain_float(target_dist, 0, _wp_leash_xy);
 
     target.x = position.x + (target_dist * velocity.x / vel_total);
     target.y = position.y + (target_dist * velocity.y / vel_total);
@@ -386,7 +387,7 @@ void AC_WPNav::set_origin_and_destination(const Vector3f& origin, const Vector3f
     _flags.reached_destination = false;
 
     // initialise the limited speed to current speed along the track
-    Vector3f curr_vel = _inav->get_velocity();
+    const Vector3f &curr_vel = _inav->get_velocity();
     // get speed along track (note: we convert vertical speed into horizontal speed equivalent)
     float speed_along_track = curr_vel.x * _pos_delta_unit.x + curr_vel.y * _pos_delta_unit.y + curr_vel.z * _pos_delta_unit.z;
     _limited_speed_xy_cms = constrain_float(speed_along_track,0,_wp_speed_cms);
@@ -437,7 +438,7 @@ void AC_WPNav::advance_target_along_track(float dt)
     }
 
     // get current velocity
-    Vector3f curr_vel = _inav->get_velocity();
+    const Vector3f &curr_vel = _inav->get_velocity();
     // get speed along track
     float speed_along_track = curr_vel.x * _pos_delta_unit.x + curr_vel.y * _pos_delta_unit.y + curr_vel.z * _pos_delta_unit.z;
 
@@ -619,7 +620,7 @@ void AC_WPNav::get_loiter_position_to_velocity(float dt, float max_speed_cms)
 ///    converts desired velocities in lat/lon directions to accelerations in lat/lon frame
 void AC_WPNav::get_loiter_velocity_to_acceleration(float vel_lat, float vel_lon, float dt)
 {
-    Vector3f vel_curr = _inav->get_velocity();  // current velocity in cm/s
+    const Vector3f &vel_curr = _inav->get_velocity();  // current velocity in cm/s
     Vector3f vel_error;                         // The velocity error in cm/s.
     float accel_total;                          // total acceleration in cm/s/s
 

libraries/AC_WPNav/AC_WPNav.h

@@ -40,7 +40,7 @@ class AC_WPNav
 public:
 
     /// Constructor
-    AC_WPNav(AP_InertialNav* inav, AP_AHRS* ahrs, APM_PI* pid_pos_lat, APM_PI* pid_pos_lon, AC_PID* pid_rate_lat, AC_PID* pid_rate_lon);
+    AC_WPNav(const AP_InertialNav* inav, const AP_AHRS* ahrs, APM_PI* pid_pos_lat, APM_PI* pid_pos_lon, AC_PID* pid_rate_lat, AC_PID* pid_rate_lon);
 
     ///
     /// simple loiter controller
@@ -183,15 +183,15 @@ protected:
     ///    set climb param to true if track climbs vertically, false if descending
     void calculate_wp_leash_length(bool climb);
 
-    // pointers to inertial nav and ahrs libraries
+    // references to inertial nav and ahrs libraries
-    AP_InertialNav*	_inav;
+    const AP_InertialNav* const _inav;
-    AP_AHRS*        _ahrs;
+    const AP_AHRS*        const _ahrs;
 
     // pointers to pid controllers
-    APM_PI*		_pid_pos_lat;
+    APM_PI*		const _pid_pos_lat;
-    APM_PI*		_pid_pos_lon;
+    APM_PI*		const _pid_pos_lon;
-    AC_PID*		_pid_rate_lat;
+    AC_PID*		const _pid_rate_lat;
-    AC_PID*		_pid_rate_lon;
+    AC_PID*		const _pid_rate_lon;
 
     // parameters
     AP_Float    _loiter_speed_cms;      // maximum horizontal speed in cm/s while in loiter

libraries/APM_Control/AP_PitchController.cpp

@@ -125,12 +125,14 @@ int32_t AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool
 	float rate_error = (desired_rate - ToDeg(omega_y)) * scaler;
 	
 	// Multiply pitch rate error by _ki_rate and integrate
+	// Scaler is applied before integrator so that integrator state relates directly to elevator deflection
+	// This means elevator trim offset doesn't change as the value of scaler changes with airspeed
 	// Don't integrate if in stabilise mode as the integrator will wind up against the pilots inputs
 	if (!disable_integrator && _K_I > 0) {
         float ki_rate = _K_I * _tau;
 		//only integrate if gain and time step are positive and airspeed above min value.
 		if (dt > 0 && aspeed > 0.5f*float(aparm.airspeed_min)) {
-		    float integrator_delta = rate_error * ki_rate * delta_time;
+		    float integrator_delta = rate_error * ki_rate * delta_time * scaler;
 			if (_last_out < -45) {
 				// prevent the integrator from increasing if surface defln demand is above the upper limit
 				integrator_delta = max(integrator_delta , 0);
@@ -145,7 +147,7 @@ int32_t AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool
 	}
 
     // Scale the integration limit
-    float intLimScaled = _imax * 0.01f / scaler;
+    float intLimScaled = _imax * 0.01f;
 
     // Constrain the integrator state
     _integrator = constrain_float(_integrator, -intLimScaled, intLimScaled);
@@ -158,7 +160,7 @@ int32_t AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool
 	// Note the scaler is applied again. We want a 1/speed scaler applied to the feed-forward
 	// path, but want a 1/speed^2 scaler applied to the rate error path. 
 	// This is because acceleration scales with speed^2, but rate scales with speed.
-	_last_out = ( (rate_error * _K_D) + _integrator + (desired_rate * kp_ff) ) * scaler;
+	_last_out = ( (rate_error * _K_D) + (desired_rate * kp_ff) ) * scaler + _integrator;
 	
 	// Convert to centi-degrees and constrain
 	return constrain_float(_last_out * 100, -4500, 4500);
@@ -216,7 +218,7 @@ float AP_PitchController::_get_coordination_rate_offset(float &aspeed, bool &inv
         // don't do turn coordination handling when at very high pitch angles
         rate_offset = 0;
     } else {
-        rate_offset = fabsf(ToDeg((GRAVITY_MSS / max(aspeed , float(aparm.airspeed_min))) * tanf(bank_angle) * sinf(bank_angle))) * _roll_ff;
+        rate_offset = cosf(_ahrs.pitch)*fabsf(ToDeg((GRAVITY_MSS / max(aspeed , float(aparm.airspeed_min))) * tanf(bank_angle) * sinf(bank_angle))) * _roll_ff;
     }
 	if (inverted) {
 		rate_offset = -rate_offset;

libraries/APM_Control/AP_RollController.cpp

@@ -117,12 +117,14 @@ int32_t AP_RollController::_get_rate_out(float desired_rate, float scaler, bool
         aspeed = 0.0f;
     }
 
-	// Multiply roll rate error by _ki_rate and integrate
+	// Multiply roll rate error by _ki_rate, apply scaler and integrate
+	// Scaler is applied before integrator so that integrator state relates directly to aileron deflection
+	// This means aileron trim offset doesn't change as the value of scaler changes with airspeed
 	// Don't integrate if in stabilise mode as the integrator will wind up against the pilots inputs
 	if (!disable_integrator && ki_rate > 0) {
 		//only integrate if gain and time step are positive and airspeed above min value.
 		if (dt > 0 && aspeed > float(aparm.airspeed_min)) {
-		    float integrator_delta = rate_error * ki_rate * delta_time;
+		    float integrator_delta = rate_error * ki_rate * delta_time * scaler;
 			// prevent the integrator from increasing if surface defln demand is above the upper limit
 			if (_last_out < -45) {
                 integrator_delta = max(integrator_delta , 0);
@@ -137,7 +139,7 @@ int32_t AP_RollController::_get_rate_out(float desired_rate, float scaler, bool
 	}
 	
     // Scale the integration limit
-    float intLimScaled = _imax * 0.01f / scaler;
+    float intLimScaled = _imax * 0.01f;
 
     // Constrain the integrator state
     _integrator = constrain_float(_integrator, -intLimScaled, intLimScaled);
@@ -146,7 +148,7 @@ int32_t AP_RollController::_get_rate_out(float desired_rate, float scaler, bool
 	// Note the scaler is applied again. We want a 1/speed scaler applied to the feed-forward
 	// path, but want a 1/speed^2 scaler applied to the rate error path. 
 	// This is because acceleration scales with speed^2, but rate scales with speed.
-	_last_out = ( (rate_error * _K_D) + _integrator + (desired_rate * kp_ff) ) * scaler;
+	_last_out = ( (rate_error * _K_D) + (desired_rate * kp_ff) ) * scaler + _integrator;
 	
 	// Convert to centi-degrees and constrain
 	return constrain_float(_last_out * 100, -4500, 4500);

libraries/APM_Control/AP_YawController.cpp

@@ -106,7 +106,7 @@ int32_t AP_YawController::get_servo_out(float scaler, bool disable_integrator)
 	float omega_z = _ahrs.get_gyro().z;
 	
 	// Get the accln vector (m/s^2)
-	float accel_y = _ahrs.get_ins()->get_accel().y;
+	float accel_y = _ahrs.get_ins().get_accel().y;
 
 	// Subtract the steady turn component of rate from the measured rate
 	// to calculate the rate relative to the turn requirement in degrees/sec

libraries/AP_AHRS/AP_AHRS.cpp

@@ -41,14 +41,14 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] PROGMEM = {
     // @Description: This controls the weight the compass or GPS has on the heading. A higher value means the heading will track the yaw source (GPS or compass) more rapidly.
     // @Range: 0.1 0.4
     // @Increment: .01
-    AP_GROUPINFO("YAW_P", 4,    AP_AHRS, _kp_yaw, 0.3f),
+    AP_GROUPINFO("YAW_P", 4,    AP_AHRS, _kp_yaw, 0.2f),
 
     // @Param: RP_P
     // @DisplayName: AHRS RP_P
     // @Description: This controls how fast the accelerometers correct the attitude
     // @Range: 0.1 0.4
     // @Increment: .01
-    AP_GROUPINFO("RP_P",  5,    AP_AHRS, _kp, 0.3f),
+    AP_GROUPINFO("RP_P",  5,    AP_AHRS, _kp, 0.2f),
 
     // @Param: WIND_MAX
     // @DisplayName: Maximum wind
@@ -105,6 +105,14 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] PROGMEM = {
     // @User: Advanced
     AP_GROUPINFO("GPS_MINSATS", 11, AP_AHRS, _gps_minsats, 6),
 
+    // @Param: GPS_DELAY
+    // @DisplayName: AHRS GPS delay steps
+    // @Description: number of GPS samples to delay accels for synchronisation with the GPS velocity data
+    // @Range: 0 5
+    // @Increment: 1
+    // @User: Advanced
+    AP_GROUPINFO("GPS_DELAY", 12, AP_AHRS, _gps_delay, 2),
+
     AP_GROUPEND
 };
 

libraries/AP_AHRS/AP_AHRS.h

@@ -37,7 +37,7 @@ class AP_AHRS
 {
 public:
     // Constructor
-    AP_AHRS(AP_InertialSensor *ins, GPS *&gps) :
+    AP_AHRS(AP_InertialSensor &ins, GPS *&gps) :
         _ins(ins),
         _gps(gps)
     {
@@ -49,7 +49,7 @@ public:
         // prone than the APM1, so we should have a lower ki,
         // which will make us less prone to increasing omegaI
         // incorrectly due to sensor noise
-        _gyro_drift_limit = ins->get_gyro_drift_rate();
+        _gyro_drift_limit = ins.get_gyro_drift_rate();
 
         // enable centrifugal correction by default
         _flags.correct_centrifugal = true;
@@ -78,7 +78,7 @@ public:
     // allow for runtime change of orientation
     // this makes initial config easier
     void set_orientation() {
-        _ins->set_board_orientation((enum Rotation)_board_orientation.get());
+        _ins.set_board_orientation((enum Rotation)_board_orientation.get());
         if (_compass != NULL) {
             _compass->set_board_orientation((enum Rotation)_board_orientation.get());
         }
@@ -88,7 +88,7 @@ public:
         _airspeed = airspeed;
     }
 
-    AP_InertialSensor* get_ins() const {
+    const AP_InertialSensor &get_ins() const {
 	    return _ins;
     }
 
@@ -242,6 +242,7 @@ protected:
     AP_Int8 _wind_max;
     AP_Int8 _board_orientation;
     AP_Int8 _gps_minsats;
+    AP_Int8 _gps_delay;
 
     // flags structure
     struct ahrs_flags {
@@ -263,7 +264,7 @@ protected:
 
     // note: we use ref-to-pointer here so that our caller can change the GPS without our noticing
     //       IMU under us without our noticing.
-    AP_InertialSensor   *_ins;
+    AP_InertialSensor   &_ins;
     GPS                 *&_gps;
 
     // a vector to capture the difference between the controller and body frames

libraries/AP_AHRS/AP_AHRS_DCM.cpp

@@ -48,10 +48,10 @@ AP_AHRS_DCM::update(void)
     float delta_t;
 
     // tell the IMU to grab some data
-    _ins->update();
+    _ins.update();
 
     // ask the IMU how much time this sensor reading represents
-    delta_t = _ins->get_delta_time();
+    delta_t = _ins.get_delta_time();
 
     // if the update call took more than 0.2 seconds then discard it,
     // otherwise we may move too far. This happens when arming motors 
@@ -62,10 +62,6 @@ AP_AHRS_DCM::update(void)
         return;
     }
 
-    // Get current values for gyros
-    _gyro_vector  = _ins->get_gyro();
-    _accel_vector = _ins->get_accel();
-
     // Integrate the DCM matrix using gyro inputs
     matrix_update(delta_t);
 
@@ -91,7 +87,7 @@ AP_AHRS_DCM::matrix_update(float _G_Dt)
     // and including the P terms would give positive feedback into
     // the _P_gain() calculation, which can lead to a very large P
     // value
-    _omega = _gyro_vector + _omega_I;
+    _omega = _ins.get_gyro() + _omega_I;
 
     _dcm_matrix.rotate((_omega + _omega_P + _omega_yaw_P) * _G_Dt);
 }
@@ -351,6 +347,11 @@ AP_AHRS_DCM::drift_correction_yaw(void)
             }
             new_value = true;
             yaw_error = yaw_error_compass();
+
+            // also update the _gps_last_update, so if we later
+            // disable the compass due to significant yaw error we
+            // don't suddenly change yaw with a reset
+            _gps_last_update = _gps->last_fix_time;
         }
     } else if (_flags.fly_forward && have_gps()) {
         /*
@@ -362,7 +363,7 @@ AP_AHRS_DCM::drift_correction_yaw(void)
             _gps_last_update = _gps->last_fix_time;
             new_value = true;
             float gps_course_rad = ToRad(_gps->ground_course_cd * 0.01f);
-            float yaw_error_rad = gps_course_rad - yaw;
+            float yaw_error_rad = wrap_PI(gps_course_rad - yaw);
             yaw_error = sinf(yaw_error_rad);
 
             /* reset yaw to match GPS heading under any of the
@@ -430,7 +431,55 @@ AP_AHRS_DCM::drift_correction_yaw(void)
 }
 
 
+/**
+   return an accel vector delayed by AHRS_ACCEL_DELAY samples
+ */
+Vector3f AP_AHRS_DCM::ra_delayed(const Vector3f &ra)
+{
+    if (_ra_delay_length != _gps_delay.get()) {
+        // the AHRS_GPS_DELAY setting has changed
 
+        // constrain it between 0 and 5
+        if (_gps_delay.get() > 5) {
+            _gps_delay.set(5);
+        }
+        if (_gps_delay.get() < 0) {
+            _gps_delay.set(0);
+        }
+        if (_ra_delay_buffer != NULL) {
+            delete[] _ra_delay_buffer;
+            _ra_delay_buffer = NULL;
+        }
+
+        // allocate the new buffer
+        _ra_delay_length = _gps_delay.get();
+        if (_ra_delay_length != 0) {
+            _ra_delay_buffer = new Vector3f[_ra_delay_length];
+        }
+        _ra_delay_next = 0;
+        if (_ra_delay_buffer != NULL) {
+            // on size change prefill the buffer with the current value
+            for (uint8_t i=0; i<_ra_delay_length; i++) {
+                _ra_delay_buffer[i] = ra;
+            }
+        }
+    }
+    if (_ra_delay_buffer == NULL) {
+        // we're not doing any delay
+        return ra;
+    }
+
+    // get the old element, and then fill it with the new element
+    Vector3f ret = _ra_delay_buffer[_ra_delay_next];
+    _ra_delay_buffer[_ra_delay_next] = ra;
+
+    // move to the next element
+    _ra_delay_next++;
+    if (_ra_delay_next == _ra_delay_length) {
+        _ra_delay_next = 0;
+    }
+    return ret;
+}
 
 // perform drift correction. This function aims to update _omega_P and
 // _omega_I with our best estimate of the short term and long term
@@ -456,7 +505,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
     temp_dcm.rotateXY(_trim);
 
     // rotate accelerometer values into the earth frame
-    _accel_ef = temp_dcm * _accel_vector;
+    _accel_ef = temp_dcm * _ins.get_accel();
 
     // integrate the accel vector in the earth frame between GPS readings
     _ra_sum += _accel_ef * deltat;
@@ -543,30 +592,35 @@ AP_AHRS_DCM::drift_correction(float deltat)
     Vector3f GA_e;
     GA_e = Vector3f(0, 0, -1.0f);
 
+    bool using_gps_corrections = false;
     if (_flags.correct_centrifugal && (_have_gps_lock || _flags.fly_forward)) {
         float v_scale = gps_gain.get()/(_ra_deltat*GRAVITY_MSS);
         Vector3f vdelta = (velocity - _last_velocity) * v_scale;
-        // limit vertical acceleration correction to 0.5 gravities. The
-        // barometer sometimes gives crazy acceleration changes. 
-        vdelta.z = constrain_float(vdelta.z, -0.5f, 0.5f);
         GA_e += vdelta;
         GA_e.normalize();
         if (GA_e.is_inf()) {
             // wait for some non-zero acceleration information
             return;
         }
+        using_gps_corrections = true;
     }
 
     // calculate the error term in earth frame.
-    Vector3f GA_b = _ra_sum / (_ra_deltat * GRAVITY_MSS);
+    _ra_sum /= (_ra_deltat * GRAVITY_MSS);
-    float length = GA_b.length();
+
-    if (length > 1.0f) {
+    // get the delayed ra_sum to match the GPS lag
-        GA_b /= length;
+    Vector3f GA_b;
+    if (using_gps_corrections) {
+        GA_b = ra_delayed(_ra_sum);
+    } else {
+        GA_b = _ra_sum;
+    }
+    GA_b.normalize();
     if (GA_b.is_inf()) {
         // wait for some non-zero acceleration information
         return;
     }
-    }
+
     Vector3f error = GA_b % GA_e;
 
 #define YAW_INDEPENDENT_DRIFT_CORRECTION 0
@@ -600,8 +654,15 @@ AP_AHRS_DCM::drift_correction(float deltat)
         }
     }
 
+    // if ins is unhealthy then stop attitude drift correction and
+    // hope the gyros are OK for a while. Just slowly reduce _omega_P
+    // to prevent previous bad accels from throwing us off
+    if (!_ins.healthy()) {
+        error.zero();
+    } else {
         // convert the error term to body frame
         error = _dcm_matrix.mul_transpose(error);
+    }
 
     if (error.is_nan() || error.is_inf()) {
         // don't allow bad values
@@ -625,7 +686,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
 
     if (_flags.fly_forward && _gps && _gps->status() >= GPS::GPS_OK_FIX_2D && 
         _gps->ground_speed_cm < GPS_SPEED_MIN && 
-        _accel_vector.x >= 7 &&
+        _ins.get_accel().x >= 7 &&
 	    pitch_sensor > -3000 && pitch_sensor < 3000) {
             // assume we are in a launch acceleration, and reduce the
             // rp gain by 50% to reduce the impact of GPS lag on

libraries/AP_AHRS/AP_AHRS_DCM.h

@@ -25,7 +25,7 @@ class AP_AHRS_DCM : public AP_AHRS
 {
 public:
     // Constructors
-    AP_AHRS_DCM(AP_InertialSensor *ins, GPS *&gps) :
+    AP_AHRS_DCM(AP_InertialSensor &ins, GPS *&gps) :
         AP_AHRS(ins, gps),
         _last_declination(0),
         _mag_earth(1,0)
@@ -92,9 +92,6 @@ private:
     // primary representation of attitude
     Matrix3f _dcm_matrix;
 
-    Vector3f _gyro_vector;                      // Store the gyros turn rate in a vector
-    Vector3f _accel_vector;                     // current accel vector
-
     Vector3f _omega_P;                          // accel Omega proportional correction
     Vector3f _omega_yaw_P;                      // proportional yaw correction
     Vector3f _omega_I;                          // Omega Integrator correction
@@ -102,6 +99,12 @@ private:
     float _omega_I_sum_time;
     Vector3f _omega;                            // Corrected Gyro_Vector data
 
+    // variables to cope with delaying the GA sum to match GPS lag
+    Vector3f ra_delayed(const Vector3f &ra);
+    uint8_t   _ra_delay_length;
+    uint8_t   _ra_delay_next;
+    Vector3f *_ra_delay_buffer;
+
     // P term gain based on spin rate
     float           _P_gain(float spin_rate);
 

libraries/AP_AHRS/AP_AHRS_HIL.h

@@ -5,7 +5,7 @@ class AP_AHRS_HIL : public AP_AHRS
 {
 public:
     // Constructors
-    AP_AHRS_HIL(AP_InertialSensor *ins, GPS *&gps) : 
+    AP_AHRS_HIL(AP_InertialSensor &ins, GPS *&gps) : 
 	    AP_AHRS(ins, gps),
 	    _drift()
 		{}
@@ -21,7 +21,7 @@ public:
 
     // Methods
     void update(void) {
-        _ins->update();
+        _ins.update();
     }
     
     void setHil(float roll, float pitch, float yaw,

libraries/AP_AHRS/examples/AHRS_Test/AHRS_Test.pde

@@ -48,7 +48,7 @@ GPS *g_gps;
 AP_GPS_Auto g_gps_driver(&g_gps);
 
 // choose which AHRS system to use
-AP_AHRS_DCM  ahrs(&ins, g_gps);
+AP_AHRS_DCM  ahrs(ins, g_gps);
 
 AP_Baro_HIL barometer;
 

libraries/AP_Airspeed/AP_Airspeed.cpp

@@ -170,6 +170,7 @@ void AP_Airspeed::read(void)
     }
     airspeed_pressure       = get_pressure();
     airspeed_pressure       = max(airspeed_pressure - _offset, 0);
+    _last_pressure          = airspeed_pressure;
     _raw_airspeed           = sqrtf(airspeed_pressure * _ratio);
     _airspeed               = 0.7f * _airspeed  +  0.3f * _raw_airspeed;
 }

libraries/AP_Airspeed/AP_Airspeed.h

@@ -106,7 +106,7 @@ public:
     // return the differential pressure in Pascal for the last
     // airspeed reading. Used by the calibration code
     float get_differential_pressure(void) const {
-        return max(_last_pressure - _offset, 0);
+        return max(_last_pressure, 0);
     }
 
     // set the apparent to true airspeed ratio
@@ -125,6 +125,9 @@ public:
 	// log data to MAVLink
 	void log_mavlink_send(mavlink_channel_t chan, const Vector3f &vground);
 
+    // return health status of sensor
+    bool healthy(void) const { return _healthy; }
+
     static const struct AP_Param::GroupInfo var_info[];
 
 

libraries/AP_Airspeed/AP_Airspeed_I2C.cpp

@@ -31,9 +31,17 @@ extern const AP_HAL::HAL& hal;
 // probe and initialise the sensor
 bool AP_Airspeed_I2C::init(void)
 {
+    // get pointer to i2c bus semaphore
+    AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore();
+
+    // take i2c bus sempahore
+    if (!i2c_sem->take(200))
+        return false;
+
     _measure();
     hal.scheduler->delay(10);
     _collect();
+    i2c_sem->give();
     if (_last_sample_time_ms != 0) {
         hal.scheduler->register_timer_process(AP_HAL_MEMBERPROC(&AP_Airspeed_I2C::_timer));
         return true;
@@ -45,16 +53,16 @@ bool AP_Airspeed_I2C::init(void)
 void AP_Airspeed_I2C::_measure(void)
 {
     _measurement_started_ms = 0;
-    if (hal.i2c->writeRegisters(I2C_ADDRESS_MS4525DO, 0, 0, NULL) != 0) {
+    if (hal.i2c->writeRegisters(I2C_ADDRESS_MS4525DO, 0, 0, NULL) == 0) {
-        return;
-    }
         _measurement_started_ms = hal.scheduler->millis();
+    }
 }
 
 // read the values from the sensor
 void AP_Airspeed_I2C::_collect(void)
 {
     uint8_t data[4];
+
     _measurement_started_ms = 0;
 
     if (hal.i2c->read(I2C_ADDRESS_MS4525DO, 4, data) != 0) {
@@ -82,8 +90,14 @@ void AP_Airspeed_I2C::_collect(void)
 // 1kHz timer
 void AP_Airspeed_I2C::_timer(void)
 {
+    AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore();
+
+    if (!i2c_sem->take_nonblocking())
+        return;
+
     if (_measurement_started_ms == 0) {
         _measure();
+        i2c_sem->give();
         return;
     }
     if ((hal.scheduler->millis() - _measurement_started_ms) > 10) {
@@ -91,6 +105,7 @@ void AP_Airspeed_I2C::_timer(void)
         // start a new measurement
         _measure();
     }
+    i2c_sem->give();
 }
 
 // return the current differential_pressure in Pascal

libraries/AP_Baro/AP_Baro.h

@@ -66,13 +66,13 @@ public:
         return _ground_pressure.get();
     }
 
-    // get last time sample was taken
+    // get last time sample was taken (in ms)
     uint32_t        get_last_update() { return _last_update; };
 
     static const struct AP_Param::GroupInfo        var_info[];
 
 protected:
-    uint32_t                            _last_update;
+    uint32_t                            _last_update; // in ms
     uint8_t                             _pressure_samples;
 
 private:

libraries/AP_Baro/AP_Baro_MS5611.cpp

@@ -88,6 +88,10 @@ void AP_Baro_MS5611_SPI::init()
         return; /* never reached */
         
     }
+
+    // now that we have initialised, we set the SPI bus speed to high
+    // (8MHz on APM2)
+    _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH);
 }
 
 uint16_t AP_Baro_MS5611_SPI::read_16bits(uint8_t reg)

libraries/AP_BattMonitor/AP_BattMonitor.cpp

@@ -49,6 +49,7 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] PROGMEM = {
     // @DisplayName: Battery capacity
     // @Description: Capacity of the battery in mAh when full
     // @Units: mAh
+    // @Increment: 50
     // @User: Standard
     AP_GROUPINFO("CAPACITY", 6, AP_BattMonitor, _pack_capacity, AP_BATT_CAPACITY_DEFAULT),
 

libraries/AP_Buffer/AP_Buffer.cpp

@@ -1,104 +0,0 @@
-// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
-
-/// @file	AP_Buffer.h
-/// @brief	fifo buffer template class
-
-#include <AP_Buffer.h>
-#include <stdint.h>
-
-// Constructor
-template <class T, uint8_t SIZE>
-AP_Buffer<T,SIZE>::AP_Buffer() :
-	_num_items(0)
-{
-	// clear the buffer
-	clear();
-}
-
-// clear - removes all points from the curve
-template <class T, uint8_t SIZE>
-void AP_Buffer<T,SIZE>::clear() {
-	// clear the curve
-	_num_items = 0;
-    _head = 0;
-}
-
-// add - adds an item to the buffer.  returns TRUE if successfully added
-template <class T, uint8_t SIZE>
-void AP_Buffer<T,SIZE>::add( T item )
-{
-    // determine position of new item
-    uint8_t tail = _head + _num_items;
-    if( tail >= SIZE ) {
-        tail -= SIZE;
-    }
-
-    // add item to buffer
-    _buff[tail] = item;
-
-    // increment number of items
-    if( _num_items < SIZE ) {
-        _num_items++;
-    }else{
-        // no room for new items so drop oldest item
-        _head++;
-        if( _head >= SIZE ) {
-            _head = 0;
-        }
-    }
-}
-
-// get - returns the next value in the buffer
-template <class T, uint8_t SIZE>
-T AP_Buffer<T,SIZE>::get()
-{
-	T result;
-
-	// return zero if buffer is empty
-	if( _num_items == 0 ) {
-		return 0;
-	}
-
-	// get next value in buffer
-    result = _buff[_head];
-
-    // increment to next point
-    _head++;
-    if( _head >= SIZE )
-        _head = 0;
-
-    // reduce number of items
-    _num_items--;
-
-    // return item
-    return result;
-}
-
-// peek - check what the next value in the buffer is but don't pull it off
-template <class T, uint8_t SIZE>
-T AP_Buffer<T,SIZE>::peek(uint8_t position) const
-{
-    uint8_t j = _head+position;
-
-    // return zero if position is out of range
-    if( position >= _num_items ) {
-        return 0;
-    }
-
-    // wrap around if necessary
-    if( j >= SIZE )
-        j -= SIZE;
-
-    // return desired value
-    return _buff[j];
-}
-
-template float AP_Buffer<float,5>::peek(uint8_t position) const;
-template AP_Buffer<float, 5>::AP_Buffer();
-template void AP_Buffer<float, 5>::clear();
-template void AP_Buffer<float, 5>::add(float);
-
-template float AP_Buffer<float,15>::peek(uint8_t position) const;
-template AP_Buffer<float, 15>::AP_Buffer();
-template void AP_Buffer<float, 15>::clear();
-template void AP_Buffer<float, 15>::add(float);