AutoTest: Add ability to run AC3.2.1 in screen

No changes from AC3.2.1-rc2

.travis.yml

@@ -4,7 +4,12 @@ before_install:
   - cd .. && ./ardupilot/Tools/scripts/install-prereqs-ubuntu.sh -y && . ~/.profile
 
 script: 
-  - cd ./ardupilot/ArduCopter && make configure && make && make px4-v2
-  - cd ../ArduPlane  && make configure && make && make px4-v2
-  - cd ../APMrover2  && make configure && make && make px4-v2
-  - cd ../ArduCopter && make configure && make && make vrubrain-v51
+  - cd ./ardupilot && Tools/scripts/build_all_travis.sh
+
+notifications:
+  webhooks:
+    urls:
+      - https://webhooks.gitter.im/e/e5e0b55e353e53945b4b
+    on_success: change  # options: [always|never|change] default: always
+    on_failure: always  # options: [always|never|change] default: always
+    on_start: false     # default: false

APMrover2/GCS_Mavlink.pde

@@ -958,6 +958,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 
     case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
         {
+            // mark the firmware version in the tlog
+            send_text_P(SEVERITY_LOW, PSTR(FIRMWARE_STRING));
+
+#if defined(PX4_GIT_VERSION) && defined(NUTTX_GIT_VERSION)
+            send_text_P(SEVERITY_LOW, PSTR("PX4: " PX4_GIT_VERSION " NuttX: " NUTTX_GIT_VERSION));
+#endif
             handle_param_request_list(msg);
             break;
         }

ArduCopter/APM_Config.h

@@ -19,10 +19,6 @@
  */
 
 // uncomment the lines below to save on flash space if compiling for the APM using Arduino IDE.  Top items save the most flash space
-#if (CONFIG_HAL_BOARD == HAL_BOARD_APM2 || CONFIG_HAL_BOARD == HAL_BOARD_APM1)
- # define CLI_ENABLED           DISABLED            // disable the CLI (command-line-interface) to save 21K of flash space
- # define FRSKY_TELEM_ENABLED           DISABLED            // disable the FRSKY TELEM
-#endif
 //#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 AUTOTUNE_ENABLED      DISABLED            // disable the auto tune functionality to save 7k of flash
@@ -34,6 +30,7 @@
 // features below are disabled by default on APM (but enabled on Pixhawk)
 //#define AC_RALLY              ENABLED             // disable rally points to save 2k of flash, and also frees rally point EEPROM for more mission commands
 //#define PARACHUTE             ENABLED             // enable parachute release at a cost of 1k of flash
+//#define CLI_ENABLED           ENABLED             // enable the CLI (command-line-interface) at a cost of 21K of flash space
 
 // features below are disabled by default on all boards
 //#define OPTFLOW               ENABLED             // enable optical flow sensor and OF_LOITER flight mode at a cost of 5K of flash space

ArduCopter/AP_State.pde

@@ -110,6 +110,21 @@ void set_land_complete(bool b)
 
 // ---------------------------------------------
 
+// set land complete maybe flag
+void set_land_complete_maybe(bool b)
+{
+    // if no change, exit immediately
+    if (ap.land_complete_maybe == b)
+        return;
+
+    if (b) {
+        Log_Write_Event(DATA_LAND_COMPLETE_MAYBE);
+    }
+    ap.land_complete_maybe = b;
+}
+
+// ---------------------------------------------
+
 void set_pre_arm_check(bool b)
 {
     if(ap.pre_arm_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.2-rc5"
+#define THISFIRMWARE "ArduCopter V3.2.1"
 /*
    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
@@ -206,6 +206,9 @@ static AP_Notify notify;
 // used to detect MAVLink acks from GCS to stop compassmot
 static uint8_t command_ack_counter;
 
+// has a log download started?
+static bool in_log_download;
+
 ////////////////////////////////////////////////////////////////////////////////
 // prototypes
 ////////////////////////////////////////////////////////////////////////////////
@@ -288,25 +291,7 @@ static AP_Compass_HIL compass;
 AP_ADC_ADS7844 apm1_adc;
 #endif
 
-#if CONFIG_INS_TYPE == HAL_INS_MPU6000
-AP_InertialSensor_MPU6000 ins;
-#elif CONFIG_INS_TYPE == HAL_INS_PX4
-AP_InertialSensor_PX4 ins;
-#elif CONFIG_INS_TYPE == HAL_INS_VRBRAIN
-AP_InertialSensor_VRBRAIN ins;
-#elif CONFIG_INS_TYPE == HAL_INS_HIL
-AP_InertialSensor_HIL ins;
-#elif CONFIG_INS_TYPE == HAL_INS_OILPAN
-AP_InertialSensor_Oilpan ins( &apm1_adc );
-#elif CONFIG_INS_TYPE == HAL_INS_FLYMAPLE
-AP_InertialSensor_Flymaple ins;
-#elif CONFIG_INS_TYPE == HAL_INS_L3G4200D
-AP_InertialSensor_L3G4200D ins;
-#elif CONFIG_INS_TYPE == HAL_INS_MPU9250
-AP_InertialSensor_MPU9250 ins;
-#else
-  #error Unrecognised CONFIG_INS_TYPE setting.
-#endif // CONFIG_INS_TYPE
+AP_InertialSensor ins;
 
 // Inertial Navigation EKF
 #if AP_AHRS_NAVEKF_AVAILABLE
@@ -388,6 +373,9 @@ static union {
         uint8_t rc_receiver_present : 1; // 14  // true if we have an rc receiver present (i.e. if we've ever received an update
         uint8_t compass_mot         : 1; // 15  // true if we are currently performing compassmot calibration
         uint8_t motor_test          : 1; // 16  // true if we are currently performing the motors test
+        uint8_t initialised         : 1; // 17  // true once the init_ardupilot function has completed.  Extended status to GCS is not sent until this completes
+        uint8_t land_complete_maybe : 1; // 18  // true if we may have landed (less strict version of land_complete)
+        uint8_t throttle_zero       : 1; // 19  // true if the throttle stick is at zero, debounced
     };
     uint32_t value;
 } ap;
@@ -571,8 +559,8 @@ static int16_t climb_rate;
 static int16_t sonar_alt;
 static uint8_t sonar_alt_health;   // true if we can trust the altitude from the sonar
 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 int32_t baro_alt;            // barometer altitude in cm above home
+static float baro_climbrate;        // barometer climbrate in cm/s
 
 
 ////////////////////////////////////////////////////////////////////////////////
@@ -795,7 +783,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
     { arm_motors_check,     40,      1 },
     { auto_trim,            40,     14 },
     { update_altitude,      40,    100 },
-    { run_nav_updates,      40,     80 },
+    { run_nav_updates,       8,     80 },
     { update_thr_cruise,    40,     10 },
     { three_hz_loop,       133,      9 },
     { compass_accumulate,    8,     42 },
@@ -805,7 +793,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
 #endif
     { update_notify,         8,     10 },
     { one_hz_loop,         400,     42 },
-    { ekf_check,            40,      2 },
+    { ekf_dcm_check,        40,      2 },
     { crash_check,          40,      2 },
     { gcs_check_input,	     8,    550 },
     { gcs_send_heartbeat,  400,    150 },
@@ -863,7 +851,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
     { arm_motors_check,     10,      10 },
     { auto_trim,            10,     140 },
     { update_altitude,      10,    1000 },
-    { run_nav_updates,      10,     800 },
+    { run_nav_updates,       4,     800 },
     { update_thr_cruise,     1,      50 },
     { three_hz_loop,        33,      90 },
     { compass_accumulate,    2,     420 },
@@ -873,7 +861,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
 #endif
     { update_notify,         2,     100 },
     { one_hz_loop,         100,     420 },
-    { ekf_check,            10,      20 },
+    { ekf_dcm_check,        10,      20 },
     { crash_check,          10,      20 },
     { gcs_check_input,	     2,     550 },
     { gcs_send_heartbeat,  100,     150 },
@@ -942,7 +930,7 @@ static void barometer_accumulate(void)
 
 static void perf_update(void)
 {
-    if (g.log_bitmask & MASK_LOG_PM)
+    if (should_log(MASK_LOG_PM))
         Log_Write_Performance();
     if (scheduler.debug()) {
         cliSerial->printf_P(PSTR("PERF: %u/%u %lu\n"),
@@ -957,10 +945,7 @@ static void perf_update(void)
 void loop()
 {
     // wait for an INS sample
-    if (!ins.wait_for_sample(1000)) {
-        Log_Write_Error(ERROR_SUBSYSTEM_MAIN, ERROR_CODE_MAIN_INS_DELAY);
-        return;
-    }
+    ins.wait_for_sample();
     uint32_t timer = micros();
 
     // check loop time
@@ -1089,7 +1074,7 @@ static void update_batt_compass(void)
         compass.set_throttle((float)g.rc_3.servo_out/1000.0f);
         compass.read();
         // log compass information
-        if (g.log_bitmask & MASK_LOG_COMPASS) {
+        if (should_log(MASK_LOG_COMPASS)) {
             Log_Write_Compass();
         }
     }
@@ -1102,16 +1087,16 @@ static void update_batt_compass(void)
 // should be run at 10hz
 static void ten_hz_logging_loop()
 {
-    if (g.log_bitmask & MASK_LOG_ATTITUDE_MED) {
+    if (should_log(MASK_LOG_ATTITUDE_MED)) {
         Log_Write_Attitude();
     }
-    if (g.log_bitmask & MASK_LOG_RCIN) {
+    if (should_log(MASK_LOG_RCIN)) {
         DataFlash.Log_Write_RCIN();
     }
-    if (g.log_bitmask & MASK_LOG_RCOUT) {
+    if (should_log(MASK_LOG_RCOUT)) {
         DataFlash.Log_Write_RCOUT();
     }
-    if ((g.log_bitmask & MASK_LOG_NTUN) && mode_requires_GPS(control_mode)) {
+    if (should_log(MASK_LOG_NTUN) && (mode_requires_GPS(control_mode) || landing_with_GPS())) {
         Log_Write_Nav_Tuning();
     }
 }
@@ -1126,11 +1111,11 @@ static void fifty_hz_logging_loop()
 #endif
 
 #if HIL_MODE == HIL_MODE_DISABLED
-    if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST) {
+    if (should_log(MASK_LOG_ATTITUDE_FAST)) {
         Log_Write_Attitude();
     }
 
-    if (g.log_bitmask & MASK_LOG_IMU) {
+    if (should_log(MASK_LOG_IMU)) {
         DataFlash.Log_Write_IMU(ins);
     }
 #endif
@@ -1160,12 +1145,12 @@ static void three_hz_loop()
 // one_hz_loop - runs at 1Hz
 static void one_hz_loop()
 {
-    if (g.log_bitmask != 0) {
+    if (should_log(MASK_LOG_ANY)) {
         Log_Write_Data(DATA_AP_STATE, ap.value);
     }
 
     // log battery info to the dataflash
-    if (g.log_bitmask & MASK_LOG_CURRENT) {
+    if (should_log(MASK_LOG_CURRENT)) {
         Log_Write_Current();
     }
 
@@ -1206,6 +1191,13 @@ static void one_hz_loop()
 #if AP_TERRAIN_AVAILABLE
     terrain.update();
 #endif
+
+#if AC_FENCE == ENABLED
+    // set fence altitude limit in position controller
+    if ((fence.get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) != 0) {
+        pos_control.set_alt_max(fence.get_safe_alt()*100.0f);
+    }
+#endif
 }
 
 // called at 100hz but data from sensor only arrives at 20 Hz
@@ -1224,7 +1216,7 @@ static void update_optical_flow(void)
         of_log_counter++;
         if( of_log_counter >= 4 ) {
             of_log_counter = 0;
-            if (g.log_bitmask & MASK_LOG_OPTFLOW) {
+            if (should_log(MASK_LOG_OPTFLOW)) {
                 Log_Write_Optflow();
             }
         }
@@ -1248,7 +1240,7 @@ static void update_GPS(void)
             last_gps_reading[i] = gps.last_message_time_ms(i);
 
             // log GPS message
-            if (g.log_bitmask & MASK_LOG_GPS) {
+            if (should_log(MASK_LOG_GPS)) {
                 DataFlash.Log_Write_GPS(gps, i, current_loc.alt);
             }
 
@@ -1334,7 +1326,7 @@ init_simple_bearing()
     super_simple_sin_yaw = simple_sin_yaw;
 
     // log the simple bearing to dataflash
-    if (g.log_bitmask != 0) {
+    if (should_log(MASK_LOG_ANY)) {
         Log_Write_Data(DATA_INIT_SIMPLE_BEARING, ahrs.yaw_sensor);
     }
 }
@@ -1399,13 +1391,13 @@ static void read_AHRS(void)
 static void update_altitude()
 {
     // read in baro altitude
-    baro_alt            = read_barometer();
+    read_barometer();
 
     // read in sonar altitude
     sonar_alt           = read_sonar();
 
     // write altitude info to dataflash logs
-    if (g.log_bitmask & MASK_LOG_CTUN) {
+    if (should_log(MASK_LOG_CTUN)) {
         Log_Write_Control_Tuning();
     }
 }

ArduCopter/Attitude.pde

@@ -151,8 +151,8 @@ static int16_t get_pilot_desired_throttle(int16_t throttle_control)
 // get_pilot_desired_climb_rate - transform pilot's throttle input to
 // climb rate in cm/s.  we use radio_in instead of control_in to get the full range
 // without any deadzone at the bottom
-#define THROTTLE_IN_DEADBAND_TOP (THROTTLE_IN_MIDDLE+THROTTLE_IN_DEADBAND)  // top of the deadband
-#define THROTTLE_IN_DEADBAND_BOTTOM (THROTTLE_IN_MIDDLE-THROTTLE_IN_DEADBAND)  // bottom of the deadband
+#define THROTTLE_IN_DEADBAND_TOP (THROTTLE_IN_MIDDLE+g.throttle_deadzone)  // top of the deadband
+#define THROTTLE_IN_DEADBAND_BOTTOM (THROTTLE_IN_MIDDLE-g.throttle_deadzone)  // bottom of the deadband
 static int16_t get_pilot_desired_climb_rate(int16_t throttle_control)
 {
     int16_t desired_rate = 0;
@@ -165,13 +165,16 @@ static int16_t get_pilot_desired_climb_rate(int16_t throttle_control)
     // ensure a reasonable throttle value
     throttle_control = constrain_int16(throttle_control,0,1000);
 
+    // ensure a reasonable deadzone
+    g.throttle_deadzone = constrain_int16(g.throttle_deadzone, 0, 400);
+
     // check throttle is above, below or in the deadband
     if (throttle_control < THROTTLE_IN_DEADBAND_BOTTOM) {
         // below the deadband
-        desired_rate = (int32_t)g.pilot_velocity_z_max * (throttle_control-THROTTLE_IN_DEADBAND_BOTTOM) / (THROTTLE_IN_MIDDLE - THROTTLE_IN_DEADBAND);
+        desired_rate = (int32_t)g.pilot_velocity_z_max * (throttle_control-THROTTLE_IN_DEADBAND_BOTTOM) / (THROTTLE_IN_MIDDLE - g.throttle_deadzone);
     }else if (throttle_control > THROTTLE_IN_DEADBAND_TOP) {
         // above the deadband
-        desired_rate = (int32_t)g.pilot_velocity_z_max * (throttle_control-THROTTLE_IN_DEADBAND_TOP) / (THROTTLE_IN_MIDDLE - THROTTLE_IN_DEADBAND);
+        desired_rate = (int32_t)g.pilot_velocity_z_max * (throttle_control-THROTTLE_IN_DEADBAND_TOP) / (THROTTLE_IN_MIDDLE - g.throttle_deadzone);
     }else{
         // must be in the deadband
         desired_rate = 0;

ArduCopter/GCS_Mavlink.pde

@@ -199,11 +199,14 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan)
     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);
+    if (!ins.get_gyro_health_all() || !ins.gyro_calibrated_ok_all()) {
+        control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_3D_GYRO;
+    }
+    if (!ins.get_accel_health_all()) {
+        control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_3D_ACCEL;
     }
 
-    if (!ahrs.healthy()) {
+    if (ahrs.initialised() && !ahrs.healthy()) {
         // AHRS subsystem is unhealthy
         control_sensors_health &= ~MAV_SYS_STATUS_AHRS;
     }
@@ -221,9 +224,10 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan)
     case AP_Terrain::TerrainStatusDisabled:
         break;
     case AP_Terrain::TerrainStatusUnhealthy:
-        control_sensors_present |= MAV_SYS_STATUS_TERRAIN;
-        control_sensors_enabled |= MAV_SYS_STATUS_TERRAIN;
-        break;
+        // To-Do: restore unhealthy terrain status reporting once terrain is used in copter
+        //control_sensors_present |= MAV_SYS_STATUS_TERRAIN;
+        //control_sensors_enabled |= MAV_SYS_STATUS_TERRAIN;
+        //break;
     case AP_Terrain::TerrainStatusOK:
         control_sensors_present |= MAV_SYS_STATUS_TERRAIN;
         control_sensors_enabled |= MAV_SYS_STATUS_TERRAIN;
@@ -276,8 +280,7 @@ static void NOINLINE send_location(mavlink_channel_t chan)
 
 static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
 {
-    Vector3f targets;
-    get_angle_targets_for_reporting(targets);
+    const Vector3f &targets = attitude_control.angle_ef_targets();
     mavlink_msg_nav_controller_output_send(
         chan,
         targets.x / 1.0e2f,
@@ -479,10 +482,14 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
         break;
 
     case MSG_EXTENDED_STATUS1:
-        CHECK_PAYLOAD_SIZE(SYS_STATUS);
-        send_extended_status1(chan);
-        CHECK_PAYLOAD_SIZE(POWER_STATUS);
-        gcs[chan-MAVLINK_COMM_0].send_power_status();
+        // send extended status only once vehicle has been initialised
+        // to avoid unnecessary errors being reported to user
+        if (ap.initialised) {
+            CHECK_PAYLOAD_SIZE(SYS_STATUS);
+            send_extended_status1(chan);
+            CHECK_PAYLOAD_SIZE(POWER_STATUS);
+            gcs[chan-MAVLINK_COMM_0].send_power_status();
+        }
         break;
 
     case MSG_EXTENDED_STATUS2:
@@ -877,6 +884,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
         mavlink_set_mode_t packet;
         mavlink_msg_set_mode_decode(msg, &packet);
 
+        // exit immediately if this command is not meant for this vehicle
+        if (mavlink_check_target(packet.target_system, 0)) {
+            break;
+        }
+
         // only accept custom modes because there is no easy mapping from Mavlink flight modes to AC flight modes
         if (packet.base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
             if (set_mode(packet.custom_mode)) {
@@ -897,6 +909,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 
     case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:         // MAV ID: 21
     {
+        // mark the firmware version in the tlog
+        send_text_P(SEVERITY_LOW, PSTR(FIRMWARE_STRING));
+
+#if defined(PX4_GIT_VERSION) && defined(NUTTX_GIT_VERSION)
+        send_text_P(SEVERITY_LOW, PSTR("PX4: " PX4_GIT_VERSION " NuttX: " NUTTX_GIT_VERSION));
+#endif
+        send_text_P(SEVERITY_LOW, PSTR("Frame: " FRAME_CONFIG_STRING));
         handle_param_request_list(msg);
         break;
     }
@@ -1044,12 +1063,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
         case MAV_CMD_CONDITION_YAW:
             // param1 : target angle [0-360]
             // param2 : speed during change [deg per second]
-            // param3 : direction (not supported)
+            // param3 : direction (-1:ccw, +1:cw)
             // param4 : relative offset (1) or absolute angle (0)
             if ((packet.param1 >= 0.0f)   &&
             	(packet.param1 <= 360.0f) &&
             	((packet.param4 == 0) || (packet.param4 == 1))) {
-            	set_auto_yaw_look_at_heading(packet.param1, packet.param2, (uint8_t)packet.param4);
+            	set_auto_yaw_look_at_heading(packet.param1, packet.param2, (int8_t)packet.param3, (uint8_t)packet.param4);
                 result = MAV_RESULT_ACCEPTED;
             } else {
                 result = MAV_RESULT_FAILED;
@@ -1091,10 +1110,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             break;
 
         case MAV_CMD_PREFLIGHT_CALIBRATION:
-            if (packet.param1 == 1 ||
-                packet.param2 == 1) {
-                ins.init_accel();
-                ahrs.set_trim(Vector3f(0,0,0));             // clear out saved trim
+            if (packet.param1 == 1) {
+                // gyro offset calibration
+                ins.init_gyro();
+                // reset ahrs gyro bias
+                ahrs.reset_gyro_drift();
                 result = MAV_RESULT_ACCEPTED;
             }
             if (packet.param3 == 1) {
@@ -1138,14 +1158,18 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             if (packet.param1 == 1.0f) {
                 // run pre_arm_checks and arm_checks and display failures
                 pre_arm_checks(true);
-                if(ap.pre_arm_check && arm_checks(true)) {
-                    init_arm_motors();
+                if(ap.pre_arm_check && arm_checks(true, true)) {
+                    if (init_arm_motors()) {
                     result = MAV_RESULT_ACCEPTED;
+                    } else {
+                        AP_Notify::flags.arming_failed = true;  // init_arm_motors function will reset flag back to false
+                        result = MAV_RESULT_UNSUPPORTED;
+                    }
                 }else{
                     AP_Notify::flags.arming_failed = true;  // init_arm_motors function will reset flag back to false
                     result = MAV_RESULT_UNSUPPORTED;
                 }
-            } else if (packet.param1 == 0.0f)  {
+            } else if (packet.param1 == 0.0f && (manual_flight_mode(control_mode) || ap.land_complete))  {
                 init_disarm_motors();
                 result = MAV_RESULT_ACCEPTED;
             } else {
@@ -1279,11 +1303,21 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
     case MAVLINK_MSG_ID_RADIO:
     case MAVLINK_MSG_ID_RADIO_STATUS:       // MAV ID: 109
     {
-        handle_radio_status(msg, DataFlash, (g.log_bitmask & MASK_LOG_PM) != 0);
+        handle_radio_status(msg, DataFlash, should_log(MASK_LOG_PM));
         break;
     }
 
-    case MAVLINK_MSG_ID_LOG_REQUEST_LIST ... MAVLINK_MSG_ID_LOG_REQUEST_END:    // MAV ID: 117 ... 122
+    case MAVLINK_MSG_ID_LOG_REQUEST_DATA:
+    case MAVLINK_MSG_ID_LOG_ERASE:
+        in_log_download = true;
+        // fallthru
+    case MAVLINK_MSG_ID_LOG_REQUEST_LIST:
+        if (!in_mavlink_delay && !motors.armed()) {
+            handle_log_message(msg, DataFlash);
+        }
+        break;
+    case MAVLINK_MSG_ID_LOG_REQUEST_END:
+        in_log_download = false;
         if (!in_mavlink_delay && !motors.armed()) {
             handle_log_message(msg, DataFlash);
         }

ArduCopter/Log.pde

@@ -173,7 +173,7 @@ struct PACKED log_AutoTune {
     float   new_gain_sp;       // newly calculated gain
 };
 
-// Write an Current data packet
+// Write an Autotune data packet
 static void Log_Write_AutoTune(uint8_t axis, uint8_t tune_step, float rate_min, float rate_max, float new_gain_rp, float new_gain_rd, float new_gain_sp)
 {
     struct log_AutoTune pkt = {
@@ -195,7 +195,7 @@ struct PACKED log_AutoTuneDetails {
     float   rate_cds;       // current rotation rate in centi-degrees / second
 };
 
-// Write an Current data packet
+// Write an Autotune data packet
 static void Log_Write_AutoTuneDetails(int16_t angle_cd, float rate_cds)
 {
     struct log_AutoTuneDetails pkt = {
@@ -464,13 +464,14 @@ struct PACKED log_Attitude {
     int16_t  pitch;
     uint16_t control_yaw;
     uint16_t yaw;
+    uint16_t error_rp;
+    uint16_t error_yaw;
 };
 
 // Write an attitude packet
 static void Log_Write_Attitude()
 {
-    Vector3f targets;
-    get_angle_targets_for_reporting(targets);
+    const Vector3f &targets = attitude_control.angle_ef_targets();
     struct log_Attitude pkt = {
         LOG_PACKET_HEADER_INIT(LOG_ATTITUDE_MSG),
         time_ms         : hal.scheduler->millis(),
@@ -479,7 +480,9 @@ static void Log_Write_Attitude()
         control_pitch   : (int16_t)targets.y,
         pitch           : (int16_t)ahrs.pitch_sensor,
         control_yaw     : (uint16_t)targets.z,
-        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));
 
@@ -530,7 +533,7 @@ struct PACKED log_Event {
 // Wrote an event packet
 static void Log_Write_Event(uint8_t id)
 {
-    if (g.log_bitmask != 0) {
+    if (should_log(MASK_LOG_ANY)) {
         struct log_Event pkt = {
             LOG_PACKET_HEADER_INIT(LOG_EVENT_MSG),
             id  : id
@@ -548,7 +551,7 @@ struct PACKED log_Data_Int16t {
 // Write an int16_t data packet
 static void Log_Write_Data(uint8_t id, int16_t value)
 {
-    if (g.log_bitmask != 0) {
+    if (should_log(MASK_LOG_ANY)) {
         struct log_Data_Int16t pkt = {
             LOG_PACKET_HEADER_INIT(LOG_DATA_INT16_MSG),
             id          : id,
@@ -567,7 +570,7 @@ struct PACKED log_Data_UInt16t {
 // Write an uint16_t data packet
 static void Log_Write_Data(uint8_t id, uint16_t value)
 {
-    if (g.log_bitmask != 0) {
+    if (should_log(MASK_LOG_ANY)) {
         struct log_Data_UInt16t pkt = {
             LOG_PACKET_HEADER_INIT(LOG_DATA_UINT16_MSG),
             id          : id,
@@ -586,7 +589,7 @@ struct PACKED log_Data_Int32t {
 // Write an int32_t data packet
 static void Log_Write_Data(uint8_t id, int32_t value)
 {
-    if (g.log_bitmask != 0) {
+    if (should_log(MASK_LOG_ANY)) {
         struct log_Data_Int32t pkt = {
             LOG_PACKET_HEADER_INIT(LOG_DATA_INT32_MSG),
             id          : id,
@@ -605,7 +608,7 @@ struct PACKED log_Data_UInt32t {
 // Write a uint32_t data packet
 static void Log_Write_Data(uint8_t id, uint32_t value)
 {
-    if (g.log_bitmask != 0) {
+    if (should_log(MASK_LOG_ANY)) {
         struct log_Data_UInt32t pkt = {
             LOG_PACKET_HEADER_INIT(LOG_DATA_UINT32_MSG),
             id          : id,
@@ -624,7 +627,7 @@ struct PACKED log_Data_Float {
 // Write a float data packet
 static void Log_Write_Data(uint8_t id, float value)
 {
-    if (g.log_bitmask != 0) {
+    if (should_log(MASK_LOG_ANY)) {
         struct log_Data_Float pkt = {
             LOG_PACKET_HEADER_INIT(LOG_DATA_FLOAT_MSG),
             id          : id,
@@ -685,7 +688,7 @@ static const struct LogStructure log_structure[] PROGMEM = {
     { LOG_PERFORMANCE_MSG, sizeof(log_Performance), 
       "PM",  "HHIhBHB",    "NLon,NLoop,MaxT,PMT,I2CErr,INSErr,INAVErr" },
     { LOG_ATTITUDE_MSG, sizeof(log_Attitude),       
-      "ATT", "IccccCC",      "TimeMS,DesRoll,Roll,DesPitch,Pitch,DesYaw,Yaw" },
+      "ATT", "IccccCCCC",    "TimeMS,DesRoll,Roll,DesPitch,Pitch,DesYaw,Yaw,ErrRP,ErrYaw" },
     { LOG_MODE_MSG, sizeof(log_Mode),
       "MODE", "Mh",          "Mode,ThrCrs" },
     { LOG_STARTUP_MSG, sizeof(log_Startup),         
@@ -715,7 +718,8 @@ static void Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page)
  #endif
 
     cliSerial->printf_P(PSTR("\n" FIRMWARE_STRING
-                             "\nFree RAM: %u\n"),
+                             "\nFree RAM: %u\n"
+                             "\nFrame: " FRAME_CONFIG_STRING "\n"),
                         (unsigned) hal.util->available_memory());
 
     cliSerial->println_P(PSTR(HAL_BOARD_NAME));
@@ -746,6 +750,7 @@ static void start_logging()
             if (hal.util->get_system_id(sysid)) {
                 DataFlash.Log_Write_Message(sysid);
             }
+            DataFlash.Log_Write_Message_P(PSTR("Frame: " FRAME_CONFIG_STRING));
 
             // log the flight mode
             Log_Write_Mode(control_mode);

ArduCopter/Parameters.h

@@ -81,7 +81,7 @@ public:
 
         // Misc
         //
-        k_param_log_bitmask = 20,
+        k_param_log_bitmask_old = 20,           // Deprecated
         k_param_log_last_filenumber,            // *** Deprecated - remove
                                                 // with next eeprom number
                                                 // change
@@ -119,7 +119,11 @@ public:
         k_param_sonar, // sonar object
         k_param_ekfcheck_thresh,
         k_param_terrain,
-        k_param_acro_expo,              // 56
+        k_param_acro_expo,
+        k_param_throttle_deadzone,
+        k_param_optflow,
+        k_param_dcmcheck_thresh,        // 59
+        k_param_log_bitmask,
 
         // 65: AP_Limits Library
         k_param_limits = 65,            // deprecated - remove
@@ -365,6 +369,7 @@ public:
     AP_Int16        failsafe_throttle_value;
     AP_Int16        throttle_cruise;
     AP_Int16        throttle_mid;
+    AP_Int16        throttle_deadzone;
 
     // Flight modes
     //
@@ -378,7 +383,7 @@ public:
 
     // Misc
     //
-    AP_Int16        log_bitmask;
+    AP_Int32        log_bitmask;
     AP_Int8         esc_calibrate;
     AP_Int8         radio_tuning;
     AP_Int16        radio_tuning_high;
@@ -390,6 +395,7 @@ public:
 
     AP_Int8         land_repositioning;
     AP_Float        ekfcheck_thresh;
+    AP_Float        dcmcheck_thresh;
 
 #if FRAME_CONFIG ==     HELI_FRAME
     // Heli

ArduCopter/Parameters.pde

@@ -295,6 +295,15 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @Increment: 1
     GSCALAR(throttle_mid,        "THR_MID",    THR_MID_DEFAULT),
 
+    // @Param: THR_DZ
+    // @DisplayName: Throttle deadzone
+    // @Description: The deadzone above and below mid throttle.  Used in AltHold, Loiter, PosHold flight modes
+    // @User: Standard
+    // @Range: 0 300
+    // @Units: pwm
+    // @Increment: 1
+    GSCALAR(throttle_deadzone,  "THR_DZ",    THR_DZ_DEFAULT),
+
     // @Param: FLTMODE1
     // @DisplayName: Flight Mode 1
     // @Description: Flight mode when Channel 5 pwm is <= 1230
@@ -345,8 +354,8 @@ const AP_Param::Info var_info[] PROGMEM = {
 
     // @Param: LOG_BITMASK
     // @DisplayName: Log bitmask
-    // @Description: 2 byte bitmap of log types to enable
-    // @Values: 830:Default,894:Default+RCIN,958:Default+IMU,1854:Default+Motors,-6146:NearlyAll,0:Disabled
+    // @Description: 4 byte bitmap of log types to enable
+    // @Values: 830:Default,894:Default+RCIN,958:Default+IMU,1854:Default+Motors,-6146:NearlyAll-AC315,43006:NearlyAll,131070:All+DisarmedLogging,0:Disabled
     // @User: Standard
     GSCALAR(log_bitmask,    "LOG_BITMASK",          DEFAULT_LOG_BITMASK),
 
@@ -381,7 +390,7 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @Param: FRAME
     // @DisplayName: Frame Orientation (+, X or V)
     // @Description: Controls motor mixing for multicopters.  Not used for Tri or Traditional Helicopters.
-    // @Values: 0:Plus, 1:X, 2:V, 3:H, 4:V-Tail, 10:Y6B (New)
+    // @Values: 0:Plus, 1:X, 2:V, 3:H, 4:V-Tail, 5:A-Tail, 10:Y6B (New)
     // @User: Standard
     GSCALAR(frame_orientation, "FRAME",             AP_MOTORS_X_FRAME),
 
@@ -447,12 +456,19 @@ const AP_Param::Info var_info[] PROGMEM = {
     GSCALAR(land_repositioning, "LAND_REPOSITION",     LAND_REPOSITION_DEFAULT),
 
     // @Param: EKF_CHECK_THRESH
-    // @DisplayName: EKF and InertialNav check compass and velocity variance threshold
+    // @DisplayName: EKF check compass and velocity variance threshold
     // @Description: Allows setting the maximum acceptable compass and velocity variance (0 to disable check)
     // @Values: 0:Disabled, 0.6:Default, 1.0:Relaxed
     // @User: Advanced
     GSCALAR(ekfcheck_thresh, "EKF_CHECK_THRESH",    EKFCHECK_THRESHOLD_DEFAULT),
 
+    // @Param: DCM_CHECK_THRESH
+    // @DisplayName: DCM yaw error threshold
+    // @Description: Allows setting the maximum acceptable yaw error as a sin of the yaw error (0 to disable check)
+    // @Values: 0:Disabled, 0.8:Default, 0.98:Relaxed
+    // @User: Advanced
+    GSCALAR(dcmcheck_thresh, "DCM_CHECK_THRESH",    DCMCHECK_THRESHOLD_DEFAULT),
+
 #if FRAME_CONFIG ==     HELI_FRAME
     // @Group: HS1_
     // @Path: ../libraries/RC_Channel/RC_Channel.cpp
@@ -589,7 +605,7 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @Param: ACRO_EXPO
     // @DisplayName: Acro Expo
     // @Description: Acro roll/pitch Expo to allow faster rotation when stick at edges
-    // @Values: 0:Disabled,0.2:Low,0.3:Medium,0.4:High
+    // @Values: 0:Disabled,0.1:Very Low,0.2:Low,0.3:Medium,0.4:High,0.5:Very High
     // @User: Advanced
     GSCALAR(acro_expo,  "ACRO_EXPO",    ACRO_EXPO_DEFAULT),
 
@@ -799,7 +815,7 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @Param: THR_ACCEL_IMAX
     // @DisplayName: Throttle acceleration controller I gain maximum
     // @Description: Throttle acceleration controller I gain maximum.  Constrains the maximum pwm that the I term will generate
-    // @Range: 0 500
+    // @Range: 0 1000
     // @Units: Percent*10
     // @User: Standard
 
@@ -993,7 +1009,7 @@ const AP_Param::Info var_info[] PROGMEM = {
 
     // @Group: BRD_
     // @Path: ../libraries/AP_BoardConfig/AP_BoardConfig.cpp
-    GOBJECT(BoardConfig,            "BRD_",       AP_BoardConfig),    
+    GOBJECT(BoardConfig,            "BRD_",       AP_BoardConfig),
 
 #if SPRAYER == ENABLED
     // @Group: SPRAY_
@@ -1126,12 +1142,13 @@ const AP_Param::ConversionInfo conversion_table[] PROGMEM = {
     { Parameters::k_param_volt_div_ratio,     0,      AP_PARAM_FLOAT, "BATT_VOLT_MULT" },
     { Parameters::k_param_curr_amp_per_volt,  0,      AP_PARAM_FLOAT, "BATT_AMP_PERVOLT" },
     { Parameters::k_param_pack_capacity,      0,      AP_PARAM_INT32, "BATT_CAPACITY" },
+    { Parameters::k_param_log_bitmask_old,    0,      AP_PARAM_INT16, "LOG_BITMASK" },
 };
 
 static void load_parameters(void)
 {
     if (!AP_Param::check_var_info()) {
-        cliSerial->printf_P(PSTR("Bad var table\n"));        
+        cliSerial->printf_P(PSTR("Bad var table\n"));
         hal.scheduler->panic(PSTR("Bad var table"));
     }
 

ArduCopter/ReleaseNotes.txt

@@ -1,5 +1,125 @@
 ArduCopter Release Notes:
 ------------------------------------------------------------------
+ArduCopter 3.2.1 11-Feb-2015 / 3.2.1-rc2 30-Jan-2015
+Changes from 3.2.1-rc1
+1) Bug Fixes:
+    a) prevent infinite loop with linked jump commands
+    b) Pixhawk memory corruption fix when connecting via USB
+    c) vehicle stops at fence altitude limit in Loiter, AltHold, PosHold
+    d) protect against multiple arming messages from GCS causing silent gyro calibration failure    
+------------------------------------------------------------------
+ArduCopter 3.2.1-rc1 08-Jan-2015
+Changes from 3.2
+1) Enhancements:
+    a) reduced twitch when passing Spline waypoints
+    b) Faster disarm after landing in Auto, Land, RTL
+    c) Pixhawk LED turns green before arming only after GPS HDOP falls below 2.3 (only in flight modes requiring GPS)
+2) Safety Features:
+    a) Add desired descent rate check to reduce chance of false-positive on landing check
+    b) improved MPU6k health monitoring and re-configuration in case of in-flight failure
+    c) Rally point distance check reduced to 300m (reduces chance of RTL to far away forgotten Rally point)
+    d) auto-disarm if vehicle is landed for 15seconds even in Auto, Guided, RTL, Circle
+    e) fence breach while vehicle is landed causes vehicle to disarm (previously did RTL)
+3) Bug Fixes:
+    a) Check flight mode even when arming from GCS (previously it was possible to arm in RTL mode if arming was initiated from GCS)
+    b) Send vehicle target destination in RTL, Guided (allows GCS to show where vehicle is flying to in these modes)
+    c) PosHold wind compensation fix
+------------------------------------------------------------------
+ArduCopter 3.2 07-Nov2014 / 3.2-rc14 31-Oct-2014
+Changes from 3.2-rc13
+1) Safety Features:
+    a) fail to arm if second gyro calibration fails (can be disabled with ARMING_CHECK)
+2) Bug fixes:
+    a) DCM-check to require one continuous second of bad heading before triggering LAND
+    b) I2C bug that could lead to Pixhawk freezing up if I2C bus is noisy
+    c) reset DCM and EKF gyro bias estimates after gyro calibration (DCM heading could drift after takeoff due to sudden change in gyro values)
+    d) use primary GPS for LED status (instead of always using first GPS)
+------------------------------------------------------------------
+ArduCopter 3.2-rc13 23-Oct-2014
+Changes from 3.2-rc12
+1) DCM check triggers LAND if yaw disagrees with GPS by > 60deg (configure with DCM_CHECK_THRESH param) and in Loiter, PosHold, Auto, etc 
+2) Safety features:
+    a) landing detector checks baro climbrate between -1.5 ~ +1.5 m/s
+    b) sanity check AHRS_RP_P and AHRS_YAW_P are never less than 0.05
+    c) check set-mode requests from GCS are for this vehicle
+3) Bug fixes:
+    a) fix ch6 tuning of wp-speed (was getting stuck at zero)
+    b) parachute servo set to off position on startup
+    c) Auto Takeoff timing bug fix that could cause severe lean on takeoff
+    d) timer fix for "slow start" of motors on Pixhawk (timer was incorrectly based on 100hz APM2 main loop speed)
+4) reduced number of relays from 4 to 2 (saves memory and flash required on APM boards)
+5) reduced number of range finders from 2 to 1 (saves memory and flash on APM boards)
+6) allow logging from startup when LOG_BITMASK set to "All+DisarmedLogging" 
+------------------------------------------------------------------
+ArduCopter 3.2-rc12 10-Oct-2014
+Changes from 3.2-rc11
+1) disable sonar on APM1 and TradHeli (APM1 & APM2) to allow code to fit 
+2) Add pre-arm and health check that gyro calibration succeeded 
+3) Bug fix to EKF reporting invalid position and velocity when switched on in flight with Ch7/Ch8 switch 
+------------------------------------------------------------------
+ArduCopter 3.2-rc11 06-Oct-2014
+Changes from 3.2-rc10
+1) reduce lean on take-off in Auto by resetting horizontal position targets 
+2) TradHeli landing check ignores overall throttle output
+3) reduce AHRS bad messages by delaying 20sec after init to allow EKF to settle (Pixhawk only)
+4) Bug fixes:
+    a) fix THR_MIN scaling issue that could cause landing-detector to fail to detect landing when ch3 min~max > 1000 pwm
+    b) fix Mediatek GPS configuration so update rate is set correctly to 5hz
+    c) fix to Condition-Yaw mission command to support relative angles
+    d) EKF bug fixes when recovering from GPS glitches (affects only Pixhawks using EKF)
+------------------------------------------------------------------
+ArduCopter 3.2-rc10 24-Sep-2014
+Changes from 3.2-rc9
+1) two-stage land-detector to reduce motor run-up when landing in Loiter, PosHold, RTL, Auto
+2) Allow passthrough from input to output of channels 9 ~ 14 (thanks Emile!)
+3) Add 4hz filter to vertical velocity error during AltHold
+4) Safety Feature:
+    a) increase Alt Disparity pre-arm check threshold to 2m (was 1m)
+    b) reset battery failsafe after disarming/arming (thanks AndKe!)
+    c) EKF only apply centrifugal corrections when GPS has at least 6 satellites (Pixhawk with EKF enabled only)
+5) Bug fixes:
+    a) to default compass devid to zero when no compass connected
+    b) reduce motor run-up while landing in RTL
+------------------------------------------------------------------
+ArduCopter 3.2-rc9 11-Sep-2014
+Changes from 3.2-rc8
+1) FRAM bug fix that could stop Mission or Parameter changes from being saved (Pixhawk, VRBrain only)
+------------------------------------------------------------------
+ArduCopter 3.2-rc8 11-Sep-2014
+Changes from 3.2-rc7
+1) EKF reduced ripple to resolve copter motor pulsing
+2) Default Param changes:
+    a) AltHold Rate P reduced from 6 to 5
+    b) AltHold Accel P reduced from 0.75 to 0.5, I from 1.5 to 1.0
+    c) EKF check threshold increased from 0.6 to 0.8 to reduce false positives
+3) sensor health flags sent to GCS only after initialisation to remove false alerts 
+4) suppress bad terrain data alerts
+5) Bug Fix:
+    a)PX4 dataflash RAM useage reduced to 8k so it works again  
+------------------------------------------------------------------
+ArduCopter 3.2-rc7 04-Sep-2014
+Changes from 3.2-rc6
+1) Safety Items:
+    a) Landing check made more strict (climb rate requirement reduced to 30cm/s, overall throttle < 25%, rotation < 20deg/sec)
+    b) pre-arm check that accels are consistent (Pixhawk only, must be within 1m/sec/sec of each other)
+    c) pre-arm check that gyros are consistent (Pixhawk only, must be within 20deg/sec of each other)
+    d) report health of all accels and gyros (not just primary) to ground station 
+------------------------------------------------------------------
+ArduCopter 3.2-rc6 31-Aug-2014
+Changes from 3.2-rc5 
+1) Spline twitch when passing through a waypoint largely resolved
+2) THR_DZ param added to allow user configuration of throttle deadzone during AltHold, Loiter, PosHold
+3) Landing check made more strict (climb rate must be -40~40cm/s for 1 full second)
+4) LAND_REPOSITION param default set to 1
+5) TradHeli with flybar passes through pilot inputs directly to swash when in ACRO mode
+6) Safety Items:
+    a) EKF check disabled when using inertial nav (caused too many false positives)
+    b) pre-arm check of internal vs external compass direction (must be within 45deg of each other)
+7) Bug Fixes:
+    a) resolve NaN in angle targets when vehicle hits gimbal lock in ACRO mode
+    b) resolve GPS driver buffer overflow that could lead to missed GPS messages on Pixhawk/PX4 boards
+    c) resolve false "compass not calibrated" warnings on Pixhawk/PX4 caused by missing device id initialisation 
+------------------------------------------------------------------
 ArduCopter 3.2-rc5 15-Aug-2014
 Changes from 3.2-rc4 
 1) Pixhawk's max num waypoints increased to 718

ArduCopter/commands.pde

@@ -14,7 +14,7 @@ static void init_home()
     inertial_nav.setup_home_position();
 
     // log new home position which mission library will pull from ahrs
-    if (g.log_bitmask & MASK_LOG_CMD) {
+    if (should_log(MASK_LOG_CMD)) {
         AP_Mission::Mission_Command temp_cmd;
         if (mission.read_cmd_from_storage(0, temp_cmd)) {
             Log_Write_Cmd(temp_cmd);

ArduCopter/commands_logic.pde

@@ -33,7 +33,7 @@ static void auto_spline_start(const Vector3f& destination, bool stopped_at_start
 static bool start_command(const AP_Mission::Mission_Command& cmd)
 {
     // To-Do: logging when new commands start/end
-    if (g.log_bitmask & MASK_LOG_CMD) {
+    if (should_log(MASK_LOG_CMD)) {
         Log_Write_Cmd(cmd);
     }
 
@@ -151,16 +151,6 @@ static bool start_command(const AP_Mission::Mission_Command& cmd)
         break;
 #endif
 
-#if MOUNT == ENABLED
-    case MAV_CMD_DO_MOUNT_CONFIGURE:                    // Mission command to configure a camera mount |Mount operation mode (see MAV_CONFIGURE_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty|
-        camera_mount.configure_cmd();
-        break;
-
-    case MAV_CMD_DO_MOUNT_CONTROL:                      // Mission command to control a camera mount |pitch(deg*100) or lat, depending on mount mode.| roll(deg*100) or lon depending on mount mode| yaw(deg*100) or alt (in cm) depending on mount mode| Empty| Empty| Empty| Empty|
-        camera_mount.control_cmd();
-        break;
-#endif
-
 #if PARACHUTE == ENABLED
     case MAV_CMD_DO_PARACHUTE:                          // Mission command to configure or release parachute
         do_parachute(cmd);
@@ -275,7 +265,7 @@ static void exit_mission()
     }else{
 #if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED
         // disarm when the landing detector says we've landed and throttle is at minimum
-        if (g.rc_3.control_in == 0 || failsafe.radio) {
+        if (ap.throttle_zero || failsafe.radio) {
             init_disarm_motors();
         }
 #else
@@ -782,6 +772,7 @@ static void do_yaw(const AP_Mission::Mission_Command& cmd)
 	set_auto_yaw_look_at_heading(
 		cmd.content.yaw.angle_deg,
 		cmd.content.yaw.turn_rate_dps,
+		cmd.content.yaw.direction,
 		cmd.content.yaw.relative_angle);
 }
 
@@ -913,7 +904,7 @@ static void do_take_picture()
 {
 #if CAMERA == ENABLED
     camera.trigger_pic();
-    if (g.log_bitmask & MASK_LOG_CAMERA) {
+    if (should_log(MASK_LOG_CAMERA)) {
         DataFlash.Log_Write_Camera(ahrs, gps, current_loc);
     }
 #endif

ArduCopter/config.h

@@ -84,6 +84,13 @@
 #if HAL_CPU_CLASS < HAL_CPU_CLASS_75
  # define PARACHUTE DISABLED
  # define AC_RALLY DISABLED
+ # define CLI_ENABLED           DISABLED
+ # define FRSKY_TELEM_ENABLED   DISABLED
+#endif
+
+// disable sonar on APM1 and TradHeli/APM2
+#if (CONFIG_HAL_BOARD == HAL_BOARD_APM1 || (CONFIG_HAL_BOARD == HAL_BOARD_APM2 && FRAME_CONFIG == HELI_FRAME))
+ # define CONFIG_SONAR          DISABLED
 #endif
 
 #if HAL_CPU_CLASS < HAL_CPU_CLASS_75 || CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
@@ -110,6 +117,28 @@
  # define FRAME_CONFIG   QUAD_FRAME
 #endif
 
+#if FRAME_CONFIG == QUAD_FRAME
+ # define FRAME_CONFIG_STRING "QUAD"
+#elif FRAME_CONFIG == TRI_FRAME
+ # define FRAME_CONFIG_STRING "TRI"
+#elif FRAME_CONFIG == HEXA_FRAME
+ # define FRAME_CONFIG_STRING "HEXA"
+#elif FRAME_CONFIG == Y6_FRAME
+ # define FRAME_CONFIG_STRING "Y6"
+#elif FRAME_CONFIG == OCTA_FRAME
+ # define FRAME_CONFIG_STRING "OCTA"
+#elif FRAME_CONFIG == OCTA_QUAD_FRAME
+ # define FRAME_CONFIG_STRING "OCTA_QUAD"
+#elif FRAME_CONFIG == HELI_FRAME
+ # define FRAME_CONFIG_STRING "HELI"
+#elif FRAME_CONFIG == SINGLE_FRAME
+ # define FRAME_CONFIG_STRING "SINGLE"
+#elif FRAME_CONFIG == COAX_FRAME
+ # define FRAME_CONFIG_STRING "COAX"
+#else
+ # define FRAME_CONFIG_STRING "UNKNOWN"
+#endif
+
 /////////////////////////////////////////////////////////////////////////////////
 // TradHeli defaults
 #if FRAME_CONFIG == HELI_FRAME
@@ -273,7 +302,7 @@
  # define FAILSAFE_GPS_TIMEOUT_MS       5000    // gps failsafe triggers after 5 seconds with no GPS
 #endif
 #ifndef GPS_HDOP_GOOD_DEFAULT
- # define GPS_HDOP_GOOD_DEFAULT         200     // minimum hdop that represents a good position.  used during pre-arm checks if fence is enabled
+ # define GPS_HDOP_GOOD_DEFAULT         230     // minimum hdop that represents a good position.  used during pre-arm checks if fence is enabled
 #endif
 
 // GCS failsafe
@@ -299,15 +328,33 @@
 #define FS_GCS_ENABLED_ALWAYS_RTL           1
 #define FS_GCS_ENABLED_CONTINUE_MISSION     2
 
+// pre-arm baro vs inertial nav max alt disparity
+#ifndef PREARM_MAX_ALT_DISPARITY_CM
+ # define PREARM_MAX_ALT_DISPARITY_CM       200     // barometer and inertial nav altitude must be within this many centimeters
+#endif
+
 // pre-arm check max velocity
 #ifndef PREARM_MAX_VELOCITY_CMS
  # define PREARM_MAX_VELOCITY_CMS           50.0f   // vehicle must be travelling under 50cm/s before arming
 #endif
 
+// arming check's maximum acceptable accelerometer vector difference (in m/s/s) between primary and backup accelerometers
+#ifndef PREARM_MAX_ACCEL_VECTOR_DIFF
+  #define PREARM_MAX_ACCEL_VECTOR_DIFF  1.0f    // pre arm accel check will fail if primary and backup accelerometer vectors differ by 1m/s/s
+#endif
+
+// arming check's maximum acceptable rotation rate difference (in rad/sec) between primary and backup gyros
+#ifndef PREARM_MAX_GYRO_VECTOR_DIFF
+  #define PREARM_MAX_GYRO_VECTOR_DIFF   0.35f   // pre arm gyro check will fail if primary and backup gyro vectors differ by 0.35 rad/sec (=20deg/sec)
+#endif
+
 //////////////////////////////////////////////////////////////////////////////
-//  EKF Checker
+//  EKF & DCM Checker
 #ifndef EKFCHECK_THRESHOLD_DEFAULT
- # define EKFCHECK_THRESHOLD_DEFAULT    0.6f    // EKF checker's default compass and velocity variance above which the EKF's horizontal position will be considered bad
+ # define EKFCHECK_THRESHOLD_DEFAULT    0.8f    // EKF checker's default compass and velocity variance above which the EKF's horizontal position will be considered bad
+#endif
+#ifndef DCMCHECK_THRESHOLD_DEFAULT
+ # define DCMCHECK_THRESHOLD_DEFAULT    0.8f    // DCM checker's default yaw error threshold above which we will abandon horizontal position hold.  The units are sin(angle) so 0.8 = about 60degrees of error
 #endif
 
 //////////////////////////////////////////////////////////////////////////////
@@ -338,6 +385,11 @@
  #endif
 #endif
 
+// arming check's maximum acceptable vector difference between internal and external compass after vectors are normalized to field length of 1.0
+#ifndef COMPASS_ACCEPTABLE_VECTOR_DIFF
+  #define COMPASS_ACCEPTABLE_VECTOR_DIFF    0.75    // pre arm compass check will fail if internal vs external compass direction differ by more than 45 degrees
+ #endif
+
 //////////////////////////////////////////////////////////////////////////////
 //  OPTICAL_FLOW
 #ifndef OPTFLOW                         // sets global enabled/disabled flag for optflow (as seen in CLI)
@@ -442,11 +494,26 @@
 #ifndef LAND_DETECTOR_TRIGGER
  # define LAND_DETECTOR_TRIGGER 50    // number of 50hz iterations with near zero climb rate and low throttle that triggers landing complete.
 #endif
+#ifndef LAND_DETECTOR_MAYBE_TRIGGER
+ # define LAND_DETECTOR_MAYBE_TRIGGER   10  // number of 50hz iterations with near zero climb rate and low throttle that means we might be landed (used to reset horizontal position targets to prevent tipping over)
+#endif
+#ifndef LAND_DETECTOR_CLIMBRATE_MAX
+# define LAND_DETECTOR_CLIMBRATE_MAX    30  // vehicle climb rate must be between -30 and +30 cm/s
+#endif
+#ifndef LAND_DETECTOR_BARO_CLIMBRATE_MAX
+# define LAND_DETECTOR_BARO_CLIMBRATE_MAX   150  // barometer climb rate must be between -150cm/s ~ +150cm/s
+#endif
+#ifndef LAND_DETECTOR_DESIRED_CLIMBRATE_MAX
+# define LAND_DETECTOR_DESIRED_CLIMBRATE_MAX    -20    // vehicle desired climb rate must be below -20cm/s
+#endif
+#ifndef LAND_DETECTOR_ROTATION_MAX
+ # define LAND_DETECTOR_ROTATION_MAX    0.50f   // vehicle rotation must be below 0.5 rad/sec (=30deg/sec for) vehicle to consider itself landed
+#endif
 #ifndef LAND_REQUIRE_MIN_THROTTLE_TO_DISARM // require pilot to reduce throttle to minimum before vehicle will disarm
  # define LAND_REQUIRE_MIN_THROTTLE_TO_DISARM ENABLED
 #endif
 #ifndef LAND_REPOSITION_DEFAULT
- # define LAND_REPOSITION_DEFAULT   0   // by default the pilot cannot override roll/pitch during landing
+ # define LAND_REPOSITION_DEFAULT   1   // by default the pilot can override roll/pitch during landing
 #endif
 #ifndef LAND_WITH_DELAY_MS
  # define LAND_WITH_DELAY_MS        4000    // default delay (in milliseconds) when a land-with-delay is triggered during a failsafe event
@@ -584,7 +651,7 @@
  # define RATE_PITCH_D       		0.004f
 #endif
 #ifndef RATE_PITCH_IMAX
- # define RATE_PITCH_IMAX        	500
+ # define RATE_PITCH_IMAX        	1000
 #endif
 
 #ifndef RATE_YAW_P
@@ -654,8 +721,8 @@
  # define THR_MAX_DEFAULT       1000            // maximum throttle sent to the motors
 #endif
 
-#ifndef THROTTLE_IN_DEADBAND
-# define THROTTLE_IN_DEADBAND    100            // the throttle input channel's deadband in PWM
+#ifndef THR_DZ_DEFAULT
+# define THR_DZ_DEFAULT         100             // the deadzone above and below mid throttle while in althold or loiter
 #endif
 
 #ifndef ALT_HOLD_P
@@ -664,7 +731,21 @@
 
 // RATE control
 #ifndef THROTTLE_RATE_P
- # define THROTTLE_RATE_P       6.0f
+ # define THROTTLE_RATE_P       5.0f
+#endif
+
+// Throttle Accel control
+#ifndef THROTTLE_ACCEL_P
+ # define THROTTLE_ACCEL_P      0.50f
+#endif
+#ifndef THROTTLE_ACCEL_I
+ # define THROTTLE_ACCEL_I      1.00f
+#endif
+#ifndef THROTTLE_ACCEL_D
+ # define THROTTLE_ACCEL_D      0.0f
+#endif
+#ifndef THROTTLE_ACCEL_IMAX
+ # define THROTTLE_ACCEL_IMAX   800
 #endif
 
 // default maximum vertical velocity and acceleration the pilot may request
@@ -684,20 +765,6 @@
  # define ALT_HOLD_ACCEL_MAX 250    // if you change this you must also update the duplicate declaration in AC_WPNav.h
 #endif
 
-// Throttle Accel control
-#ifndef THROTTLE_ACCEL_P
- # define THROTTLE_ACCEL_P  0.75f
-#endif
-#ifndef THROTTLE_ACCEL_I
- # define THROTTLE_ACCEL_I  1.50f
-#endif
-#ifndef THROTTLE_ACCEL_D
- # define THROTTLE_ACCEL_D 0.0f
-#endif
-#ifndef THROTTLE_ACCEL_IMAX
- # define THROTTLE_ACCEL_IMAX 500
-#endif
-
 //////////////////////////////////////////////////////////////////////////////
 // Dataflash logging control
 //

ArduCopter/control_acro.pde

@@ -59,6 +59,11 @@ static void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int
         // expo variables
         float rp_in, rp_in3, rp_out;
 
+        // range check expo
+        if (g.acro_expo > 1.0f) {
+            g.acro_expo = 1.0f;
+        }
+
         // roll expo
         rp_in = float(roll_in)/ROLL_PITCH_INPUT_MAX;
         rp_in3 = rp_in*rp_in*rp_in;

ArduCopter/control_auto.pde

@@ -102,13 +102,14 @@ static void auto_takeoff_run()
 {
     // if not auto armed set throttle to zero and exit immediately
     if(!ap.auto_armed) {
+        // initialise wpnav targets
+        wp_nav.shift_wp_origin_to_current_pos();
         // reset attitude control targets
         attitude_control.relax_bf_rate_controller();
         attitude_control.set_yaw_target_to_current_heading();
         attitude_control.set_throttle_out(0, false);
         // tell motors to do a slow start
         motors.slow_start(true);
-        // To-Do: re-initialise wpnav targets
         return;
     }
 
@@ -287,6 +288,11 @@ static void auto_land_run()
         return;
     }
 
+    // relax loiter targets if we might be landed
+    if (land_complete_maybe()) {
+        wp_nav.loiter_soften_for_landing();
+    }
+
     // process pilot's input
     if (!failsafe.radio) {
         if (g.land_repositioning) {
@@ -309,7 +315,7 @@ static void auto_land_run()
     wp_nav.update_loiter();
 
     // call z-axis position controller
-    pos_control.set_alt_target_from_climb_rate(get_throttle_land(), G_Dt);
+    pos_control.set_alt_target_from_climb_rate(get_throttle_land(), G_Dt, true);
     pos_control.update_z_controller();
 
     // roll & pitch from waypoint controller, yaw rate from pilot
@@ -470,7 +476,7 @@ void set_auto_yaw_mode(uint8_t yaw_mode)
 }
 
 // set_auto_yaw_look_at_heading - sets the yaw look at heading for auto mode 
-static void set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, uint8_t relative_angle)
+static void set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, uint8_t relative_angle)
 {
     // get current yaw target
     int32_t curr_yaw_target = attitude_control.angle_ef_targets().z;
@@ -481,7 +487,10 @@ static void set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, u
         yaw_look_at_heading = wrap_360_cd(angle_deg * 100);
     } else {
         // relative angle
-        yaw_look_at_heading = wrap_360_cd(angle_deg * 100);
+        if (direction < 0) {
+            angle_deg = -angle_deg;
+        }
+        yaw_look_at_heading = wrap_360_cd((angle_deg*100+curr_yaw_target));
     }
 
     // get turn speed

ArduCopter/control_autotune.pde

@@ -53,10 +53,10 @@
 #define AUTOTUNE_PI_RATIO_FOR_TESTING      0.1f    // I is set 10x smaller than P during testing
 #define AUTOTUNE_RP_RATIO_FINAL            1.0f    // I is set 1x P after testing
 #define AUTOTUNE_RD_MIN                  0.002f    // minimum Rate D value
-#define AUTOTUNE_RD_MAX                  0.015f    // maximum Rate D value
+#define AUTOTUNE_RD_MAX                  0.020f    // maximum Rate D value
 #define AUTOTUNE_RP_MIN                   0.01f    // minimum Rate P value
-#define AUTOTUNE_RP_MAX                   0.25f    // maximum Rate P value
-#define AUTOTUNE_SP_MAX                   15.0f    // maximum Stab P value
+#define AUTOTUNE_RP_MAX                   0.35f    // maximum Rate P value
+#define AUTOTUNE_SP_MAX                   20.0f    // maximum Stab P value
 #define AUTOTUNE_SUCCESS_COUNT                4    // how many successful iterations we need to freeze at current gains
 
 // Auto Tune message ids for ground station

ArduCopter/control_guided.pde

@@ -64,6 +64,7 @@ void guided_pos_control_start()
     set_auto_yaw_mode(get_default_auto_yaw_mode(false));
 }
 
+#if NAV_GUIDED == ENABLED
 // initialise guided mode's velocity controller
 void guided_vel_control_start()
 {
@@ -77,6 +78,7 @@ void guided_vel_control_start()
     // initialise velocity controller
     pos_control.init_vel_controller_xyz();
 }
+#endif
 
 // guided_set_destination - sets guided mode's target destination
 static void guided_set_destination(const Vector3f& destination)
@@ -89,6 +91,7 @@ static void guided_set_destination(const Vector3f& destination)
     wp_nav.set_wp_destination(destination);
 }
 
+#if NAV_GUIDED == ENABLED
 // guided_set_velocity - sets guided mode's target velocity
 static void guided_set_velocity(const Vector3f& velocity)
 {
@@ -100,6 +103,7 @@ static void guided_set_velocity(const Vector3f& velocity)
     // set position controller velocity target
     pos_control.set_desired_velocity(velocity);
 }
+#endif
 
 // guided_run - runs the guided controller
 // should be called at 100hz or more
@@ -128,9 +132,12 @@ static void guided_run()
         guided_pos_control_run();
         break;
 
+#if NAV_GUIDED == ENABLED
     case Guided_Velocity:
         // run velocity controller
         guided_vel_control_run();
+        break;
+#endif
     }
  }
 
@@ -140,13 +147,14 @@ static void guided_takeoff_run()
 {
     // if not auto armed set throttle to zero and exit immediately
     if(!ap.auto_armed) {
+        // initialise wpnav targets
+        wp_nav.shift_wp_origin_to_current_pos();
         // reset attitude control targets
         attitude_control.relax_bf_rate_controller();
         attitude_control.set_yaw_target_to_current_heading();
         attitude_control.set_throttle_out(0, false);
         // tell motors to do a slow start
         motors.slow_start(true);
-        // To-Do: re-initialise wpnav targets
         return;
     }
 

ArduCopter/control_land.pde

@@ -1,7 +1,5 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
-// counter to verify landings
-static uint16_t land_detector;
 static bool land_with_gps;
 
 static uint32_t land_start_time;
@@ -61,7 +59,7 @@ static void land_gps_run()
 
 #if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED
         // disarm when the landing detector says we've landed and throttle is at minimum
-        if (ap.land_complete && (g.rc_3.control_in == 0 || failsafe.radio)) {
+        if (ap.land_complete && (ap.throttle_zero || failsafe.radio)) {
             init_disarm_motors();
         }
 #else
@@ -73,6 +71,11 @@ static void land_gps_run()
         return;
     }
 
+    // relax loiter target if we might be landed
+    if (land_complete_maybe()) {
+        wp_nav.loiter_soften_for_landing();
+    }
+
     // process pilot inputs
     if (!failsafe.radio) {
         if (g.land_repositioning) {
@@ -107,7 +110,7 @@ static void land_gps_run()
     }
 
     // update altitude target and call position controller
-    pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt);
+    pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt, true);
     pos_control.update_z_controller();
 }
 
@@ -126,7 +129,7 @@ static void land_nogps_run()
         attitude_control.set_throttle_out(0, false);
 #if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED
         // disarm when the landing detector says we've landed and throttle is at minimum
-        if (ap.land_complete && (g.rc_3.control_in == 0 || failsafe.radio)) {
+        if (ap.land_complete && (ap.throttle_zero || failsafe.radio)) {
             init_disarm_motors();
         }
 #else
@@ -165,7 +168,7 @@ static void land_nogps_run()
     }
 
     // call position controller
-    pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt);
+    pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt, true);
     pos_control.update_z_controller();
 }
 
@@ -187,33 +190,6 @@ static float get_throttle_land()
     }
 }
 
-// update_land_detector - checks if we have landed and updates the ap.land_complete flag
-// returns true if we have landed
-static bool update_land_detector()
-{
-    // detect whether we have landed by watching for low climb rate and minimum throttle
-    if (abs(climb_rate) < 40 && motors.limit.throttle_lower) {
-        if (!ap.land_complete) {
-            // run throttle controller if accel based throttle controller is enabled and active (active means it has been given a target)
-            if( land_detector < LAND_DETECTOR_TRIGGER) {
-                land_detector++;
-            }else{
-                set_land_complete(true);
-                land_detector = 0;
-            }
-        }
-    } else if ((motors.get_throttle_out() > get_non_takeoff_throttle()) || failsafe.radio) {
-        // we've sensed movement up or down so reset land_detector
-        land_detector = 0;
-        if(ap.land_complete) {
-            set_land_complete(false);
-        }
-    }
-
-    // return current state of landing
-    return ap.land_complete;
-}
-
 // land_do_not_use_GPS - forces land-mode to not use the GPS but instead rely on pilot input for roll and pitch
 //  called during GPS failsafe to ensure that if we were already in LAND mode that we do not use the GPS
 //  has no effect if we are not already in LAND mode
@@ -228,3 +204,8 @@ static void set_mode_land_with_pause()
     set_mode(LAND);
     land_pause = true;
 }
+
+// landing_with_GPS - returns true if vehicle is landing using GPS
+static bool landing_with_GPS() {
+    return (control_mode == LAND && land_with_gps);
+}

ArduCopter/control_loiter.pde

@@ -68,6 +68,11 @@ static void loiter_run()
         wp_nav.clear_pilot_desired_acceleration();
     }
 
+    // relax loiter target if we might be landed
+    if (land_complete_maybe()) {
+        wp_nav.loiter_soften_for_landing();
+    }
+
     // when landed reset targets and output zero throttle
     if (ap.land_complete) {
         wp_nav.init_loiter_target();

ArduCopter/control_poshold.pde

@@ -77,7 +77,7 @@ static struct {
 
     // loiter related variables
     int16_t controller_to_pilot_timer_roll;     // cycles to mix controller and pilot controls in POSHOLD_CONTROLLER_TO_PILOT
-    int16_t controller_to_pilot_timer_pitch;        // cycles to mix controller and pilot controls in POSHOLD_CONTROLLER_TO_PILOT
+    int16_t controller_to_pilot_timer_pitch;    // cycles to mix controller and pilot controls in POSHOLD_CONTROLLER_TO_PILOT
     int16_t controller_final_roll;              // final roll angle from controller as we exit brake or loiter mode (used for mixing with pilot input)
     int16_t controller_final_pitch;             // final pitch angle from controller as we exit brake or loiter mode (used for mixing with pilot input)
 
@@ -85,8 +85,8 @@ static struct {
     Vector2f wind_comp_ef;                      // wind compensation in earth frame, filtered lean angles from position controller
     int16_t wind_comp_roll;                     // roll angle to compensate for wind
     int16_t wind_comp_pitch;                    // pitch angle to compensate for wind
-    int8_t  wind_comp_start_timer;              // counter to delay start of wind compensation for a short time after loiter is engaged
-    int8_t  wind_comp_timer;         // counter to reduce wind comp roll/pitch lean angle calcs to 10hz
+    uint16_t wind_comp_start_timer;             // counter to delay start of wind compensation for a short time after loiter is engaged
+    int8_t  wind_comp_timer;                    // counter to reduce wind comp roll/pitch lean angle calcs to 10hz
 
     // final output
     int16_t roll;   // final roll angle sent to attitude controller
@@ -183,6 +183,11 @@ static void poshold_run()
         }
     }
 
+    // relax loiter target if we might be landed
+    if (land_complete_maybe()) {
+        wp_nav.loiter_soften_for_landing();
+    }
+
     // if landed initialise loiter targets, set throttle to zero and exit
     if (ap.land_complete) {
         wp_nav.init_loiter_target();

ArduCopter/control_rtl.pde

@@ -325,15 +325,35 @@ static void rtl_land_run()
     int16_t roll_control = 0, pitch_control = 0;
     float target_yaw_rate = 0;
     // if not auto armed set throttle to zero and exit immediately
-    if(!ap.auto_armed || !inertial_nav.position_ok()) {
+    if(!ap.auto_armed || ap.land_complete) {
         attitude_control.relax_bf_rate_controller();
         attitude_control.set_yaw_target_to_current_heading();
         attitude_control.set_throttle_out(0, false);
         // set target to current position
         wp_nav.init_loiter_target();
+
+#if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED
+        // disarm when the landing detector says we've landed and throttle is at minimum
+        if (ap.land_complete && (ap.throttle_zero || failsafe.radio)) {
+            init_disarm_motors();
+        }
+#else
+        // disarm when the landing detector says we've landed
+        if (ap.land_complete) {
+            init_disarm_motors();
+        }
+#endif
+
+        // check if we've completed this stage of RTL
+        rtl_state_complete = ap.land_complete;
         return;
     }
 
+    // relax loiter target if we might be landed
+    if (land_complete_maybe()) {
+        wp_nav.loiter_soften_for_landing();
+    }
+
     // process pilot's input
     if (!failsafe.radio) {
         if (g.land_repositioning) {
@@ -357,7 +377,7 @@ static void rtl_land_run()
 
     // call z-axis position controller
     float cmb_rate = get_throttle_land();
-    pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt);
+    pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt, true);
     pos_control.update_z_controller();
 
     // roll & pitch from waypoint controller, yaw rate from pilot
@@ -365,18 +385,6 @@ static void rtl_land_run()
 
     // check if we've completed this stage of RTL
     rtl_state_complete = ap.land_complete;
-
-#if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED
-    // disarm when the landing detector says we've landed and throttle is at minimum
-    if (ap.land_complete && (g.rc_3.control_in == 0 || failsafe.radio)) {
-        init_disarm_motors();
-    }
-#else
-    // disarm when the landing detector says we've landed
-    if (ap.land_complete) {
-        init_disarm_motors();
-    }
-#endif
 }
 
 // get_RTL_alt - return altitude which vehicle should return home at

ArduCopter/crash_check.pde

@@ -25,7 +25,7 @@ void crash_check()
 #endif
 
     // 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)) {
+    if (!motors.armed() || (!ap.throttle_zero && !failsafe.radio)) {
         inverted_count = 0;
         return;
     }

ArduCopter/defines.h

@@ -195,8 +195,10 @@ enum AutoMode {
 // Guided modes
 enum GuidedMode {
     Guided_TakeOff,
-    Guided_WP,
-    Guided_Velocity
+    Guided_WP
+#if NAV_GUIDED == ENABLED
+    ,Guided_Velocity
+#endif
 };
 
 // RTL states
@@ -263,6 +265,8 @@ enum FlipState {
 #define MASK_LOG_COMPASS                (1<<13)
 #define MASK_LOG_INAV                   (1<<14) // deprecated
 #define MASK_LOG_CAMERA                 (1<<15)
+#define MASK_LOG_WHEN_DISARMED          (1UL<<16)
+#define MASK_LOG_ANY                    0xFFFF
 
 // DATA - event logging
 #define DATA_MAVLINK_FLOAT              1
@@ -275,6 +279,7 @@ enum FlipState {
 #define DATA_DISARMED                   11
 #define DATA_AUTO_ARMED                 15
 #define DATA_TAKEOFF                    16
+#define DATA_LAND_COMPLETE_MAYBE        17
 #define DATA_LAND_COMPLETE              18
 #define DATA_NOT_LANDED                 28
 #define DATA_LOST_GPS                   19
@@ -329,8 +334,8 @@ enum FlipState {
 #define ERROR_SUBSYSTEM_FLIP                13
 #define ERROR_SUBSYSTEM_AUTOTUNE            14
 #define ERROR_SUBSYSTEM_PARACHUTE           15
-#define ERROR_SUBSYSTEM_EKF_CHECK           16
-#define ERROR_SUBSYSTEM_FAILSAFE_EKF        17
+#define ERROR_SUBSYSTEM_EKFINAV_CHECK       16
+#define ERROR_SUBSYSTEM_FAILSAFE_EKFINAV    17
 #define ERROR_SUBSYSTEM_BARO                18
 // general error codes
 #define ERROR_CODE_ERROR_RESOLVED           0
@@ -356,8 +361,8 @@ enum FlipState {
 // parachute failed to deploy because of low altitude
 #define ERROR_CODE_PARACHUTE_TOO_LOW        2
 // EKF check definitions
-#define ERROR_CODE_EKF_CHECK_BAD_VARIANCE   2
-#define ERROR_CODE_EKF_CHECK_BAD_VARIANCE_CLEARED    0
+#define ERROR_CODE_EKFINAV_CHECK_BAD_VARIANCE       2
+#define ERROR_CODE_EKFINAV_CHECK_VARIANCE_CLEARED   0
 // Baro specific error codes
 #define ERROR_CODE_BARO_GLITCH              2
 

ArduCopter/ekf_check.pde

@@ -11,61 +11,52 @@
  # define EKF_CHECK_ITERATIONS_MAX          10      // 1 second (ie. 10 iterations at 10hz) of bad variances signals a failure
 #endif
 
-#ifndef EKF_CHECK_COMPASS_INAV_CONVERSION
- # define EKF_CHECK_COMPASS_INAV_CONVERSION 0.0075f // converts the inertial nav's acceleration corrections to a form that is comparable to the ekf variance
-#endif
-
 #ifndef EKF_CHECK_WARNING_TIME
  # define EKF_CHECK_WARNING_TIME            (30*1000)   // warning text messages are sent to ground no more than every 30 seconds
 #endif
 
+// Enumerator for types of check
+enum EKFCheckType {
+    CHECK_NONE = 0,
+    CHECK_DCM = 1,
+    CHECK_EKF = 2
+};
+
 ////////////////////////////////////////////////////////////////////////////////
 // EKF_check strucutre
 ////////////////////////////////////////////////////////////////////////////////
 static struct {
-    uint8_t fail_count_compass; // number of iterations ekf's compass variance has been out of tolerances
+    uint8_t fail_count_compass; 		// number of iterations ekf or dcm have been out of tolerances
 
-    uint8_t bad_compass : 1;    // true if compass variance is bad
+    uint8_t bad_compass : 1;    // true if dcm or ekf should be considered untrusted (fail_count_compass has exceeded EKF_CHECK_ITERATIONS_MAX)
 
     uint32_t last_warn_time;    // system time of last warning in milliseconds.  Used to throttle text warnings sent to GCS
 } ekf_check_state;
 
-// ekf_check - detects ekf variances that are out of tolerance
+// ekf_dcm_check - detects if ekf variances or dcm yaw errors that are out of tolerance and triggers failsafe
 // should be called at 10hz
-void ekf_check()
+void ekf_dcm_check()
 {
-    // return immediately if motors are not armed, ekf check is disabled, no inertial-nav position yet or usb is connected
-    if (!motors.armed() || g.ekfcheck_thresh == 0.0f || !inertial_nav.position_ok() || ap.usb_connected) {
+    EKFCheckType check_type = CHECK_NONE;
+
+    // decide if we should check ekf or dcm
+    if (ahrs.have_inertial_nav() && g.ekfcheck_thresh > 0.0f) {
+        check_type = CHECK_EKF;
+    } else if (g.dcmcheck_thresh > 0.0f) {
+        check_type = CHECK_DCM;
+    }
+
+    // return immediately if motors are not armed, ekf check is disabled, not using ekf or usb is connected
+    if (!motors.armed() || ap.usb_connected || check_type == CHECK_NONE) {
         ekf_check_state.fail_count_compass = 0;
-        ekf_check_state.bad_compass = 0;
+        ekf_check_state.bad_compass = false;
         AP_Notify::flags.ekf_bad = ekf_check_state.bad_compass;
         failsafe_ekf_off_event();   // clear failsafe
         return;
     }
 
-    // variances
-    float compass_variance = 0;
-    float vel_variance = 9.0;   // default set high to enable failsafe trigger if not using EKF
-
-#if AP_AHRS_NAVEKF_AVAILABLE
-    if (ahrs.have_inertial_nav()) {
-        // use EKF to get variance
-        float posVar, hgtVar, tasVar;
-        Vector3f magVar;
-        Vector2f offset;
-        ahrs.get_NavEKF().getVariances(vel_variance, posVar, hgtVar, magVar, tasVar, offset);
-        compass_variance = magVar.length();
-    } else {
-        // use complementary filter's acceleration corrections multiplied by conversion factor to make them general in the same range as the EKF's variances
-        compass_variance = safe_sqrt(inertial_nav.accel_correction_hbf.x * inertial_nav.accel_correction_hbf.x + inertial_nav.accel_correction_hbf.y * inertial_nav.accel_correction_hbf.y) * EKF_CHECK_COMPASS_INAV_CONVERSION;
-    }
-#else
-    // use complementary filter's acceleration corrections multiplied by conversion factor to make them general in the same range as the EKF's variances
-    compass_variance = safe_sqrt(inertial_nav.accel_correction_hbf.x * inertial_nav.accel_correction_hbf.x + inertial_nav.accel_correction_hbf.y * inertial_nav.accel_correction_hbf.y) * EKF_CHECK_COMPASS_INAV_CONVERSION;
-#endif
-
     // compare compass and velocity variance vs threshold
-    if (compass_variance >= g.ekfcheck_thresh && vel_variance > g.ekfcheck_thresh) {
+    if ((check_type == CHECK_EKF && ekf_over_threshold()) || (check_type == CHECK_DCM && dcm_over_threshold())) {
         // if compass is not yet flagged as bad
         if (!ekf_check_state.bad_compass) {
             // increase counter
@@ -76,25 +67,29 @@ void ekf_check()
                 ekf_check_state.fail_count_compass = EKF_CHECK_ITERATIONS_MAX;
                 ekf_check_state.bad_compass = true;
                 // log an error in the dataflash
-                Log_Write_Error(ERROR_SUBSYSTEM_EKF_CHECK, ERROR_CODE_EKF_CHECK_BAD_VARIANCE);
+                Log_Write_Error(ERROR_SUBSYSTEM_EKFINAV_CHECK, ERROR_CODE_EKFINAV_CHECK_BAD_VARIANCE);
                 // send message to gcs
                 if ((hal.scheduler->millis() - ekf_check_state.last_warn_time) > EKF_CHECK_WARNING_TIME) {
-                    gcs_send_text_P(SEVERITY_HIGH,PSTR("EKF variance"));
+                    if (check_type == CHECK_EKF) {
+                        gcs_send_text_P(SEVERITY_HIGH,PSTR("EKF variance"));
+                    } else {
+                        gcs_send_text_P(SEVERITY_HIGH,PSTR("DCM bad heading"));
+                    }
                     ekf_check_state.last_warn_time = hal.scheduler->millis();
                 }
                 failsafe_ekf_event();
             }
         }
     } else {
-        // if compass is flagged as bad
-        if (ekf_check_state.bad_compass) {
-            // reduce counter
+        // reduce counter
+        if (ekf_check_state.fail_count_compass > 0) {
             ekf_check_state.fail_count_compass--;
-            // if counter reaches zero then clear flag
-            if (ekf_check_state.fail_count_compass == 0) {
+
+            // if compass is flagged as bad and the counter reaches zero then clear flag
+            if (ekf_check_state.bad_compass && ekf_check_state.fail_count_compass == 0) {
                 ekf_check_state.bad_compass = false;
                 // log recovery in the dataflash
-                Log_Write_Error(ERROR_SUBSYSTEM_EKF_CHECK, ERROR_CODE_EKF_CHECK_BAD_VARIANCE_CLEARED);
+                Log_Write_Error(ERROR_SUBSYSTEM_EKFINAV_CHECK, ERROR_CODE_EKFINAV_CHECK_VARIANCE_CLEARED);
                 // clear failsafe
                 failsafe_ekf_off_event();
             }
@@ -104,18 +99,47 @@ void ekf_check()
     // set AP_Notify flags
     AP_Notify::flags.ekf_bad = ekf_check_state.bad_compass;
 
-    // To-Do: add check for althold when vibrations are high
     // To-Do: add ekf variances to extended status
-    // To-Do: add counter measures to try and recover from bad EKF
-    // To-Do: add check into GPS position_ok() to return false if ekf xy not healthy?
-    // To-Do: ensure it compiles for AVR
 }
 
+// dcm_over_threshold - returns true if the dcm yaw error is over the tolerance
+static bool dcm_over_threshold()
+{
+    // return true if yaw error is over the threshold
+    return (g.dcmcheck_thresh > 0.0f && ahrs.get_error_yaw() > g.dcmcheck_thresh);
+}
+
+// ekf_over_threshold - returns true if the ekf's variance are over the tolerance
+static bool ekf_over_threshold()
+{
+#if AP_AHRS_NAVEKF_AVAILABLE
+    // return false immediately if disabled
+    if (g.ekfcheck_thresh <= 0.0f) {
+        return false;
+    }
+
+    // use EKF to get variance
+    float posVar, hgtVar, tasVar;
+    Vector3f magVar;
+    Vector2f offset;
+    float compass_variance;
+    float vel_variance;
+    ahrs.get_NavEKF().getVariances(vel_variance, posVar, hgtVar, magVar, tasVar, offset);
+    compass_variance = magVar.length();
+
+    // return true if compass and velocity variance over the threshold
+    return (compass_variance >= g.ekfcheck_thresh && vel_variance >= g.ekfcheck_thresh);
+#else
+    return false;
+#endif
+}
+
+
 // failsafe_ekf_event - perform ekf failsafe
 static void failsafe_ekf_event()
 {
-    // return immediately if ekf failsafe already triggered or disabled
-    if (failsafe.ekf || g.ekfcheck_thresh <= 0.0f) {
+    // return immediately if ekf failsafe already triggered
+    if (failsafe.ekf) {
         return;
     }
 
@@ -126,7 +150,7 @@ static void failsafe_ekf_event()
 
     // EKF failsafe event has occurred
     failsafe.ekf = true;
-    Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_EKF, ERROR_CODE_FAILSAFE_OCCURRED);
+    Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_EKFINAV, ERROR_CODE_FAILSAFE_OCCURRED);
 
     // take action based on flight mode
     if (mode_requires_GPS(control_mode)) {
@@ -149,5 +173,5 @@ static void failsafe_ekf_off_event(void)
 
     // clear flag and log recovery
     failsafe.ekf = false;
-    Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_EKF, ERROR_CODE_FAILSAFE_RESOLVED);
+    Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_EKFINAV, ERROR_CODE_FAILSAFE_RESOLVED);
 }

ArduCopter/events.pde

@@ -16,7 +16,7 @@ static void failsafe_radio_on_event()
         case STABILIZE:
         case ACRO:
             // if throttle is zero OR vehicle is landed disarm motors
-            if (g.rc_3.control_in == 0 || ap.land_complete) {
+            if (ap.throttle_zero || ap.land_complete) {
                 init_disarm_motors();
 
             // if failsafe_throttle is FS_THR_ENABLED_ALWAYS_LAND then land immediately
@@ -115,7 +115,7 @@ static void failsafe_battery_event(void)
             case STABILIZE:
             case ACRO:
                 // if throttle is zero OR vehicle is landed disarm motors
-                if (g.rc_3.control_in == 0 || ap.land_complete) {
+                if (ap.throttle_zero || ap.land_complete) {
                     init_disarm_motors();
                 }else{
                     // set mode to RTL or LAND
@@ -274,7 +274,7 @@ static void failsafe_gcs_check()
         case ACRO:
         case SPORT:
             // if throttle is zero disarm motors
-            if (g.rc_3.control_in == 0) {
+            if (ap.throttle_zero) {
                 init_disarm_motors();
             }else if(home_distance > wp_nav.get_wp_radius()) {
                 if (!set_mode(RTL)) {

ArduCopter/fence.pde

@@ -30,7 +30,7 @@ void fence_check()
 
             // disarm immediately if we think we are on the ground
             // don't disarm if the high-altitude fence has been broken because it's likely the user has pulled their throttle to zero to bring it down
-            if(manual_flight_mode(control_mode) && g.rc_3.control_in == 0 && !failsafe.radio && ((fence.get_breaches() & AC_FENCE_TYPE_ALT_MAX)== 0)){
+            if(ap.land_complete || manual_flight_mode(control_mode) && ap.throttle_zero && !failsafe.radio && ((fence.get_breaches() & AC_FENCE_TYPE_ALT_MAX)== 0)){
                 init_disarm_motors();
             }else{
                 // if we are within 100m of the fence, RTL

ArduCopter/flight_mode.pde

@@ -227,6 +227,13 @@ static void exit_mode(uint8_t old_control_mode, uint8_t new_control_mode)
         // this assumes all manual flight modes use get_pilot_desired_throttle to translate pilot input to output throttle
         set_accel_throttle_I_from_pilot_throttle(get_pilot_desired_throttle(g.rc_3.control_in));
     }
+    
+#if FRAME_CONFIG == HELI_FRAME
+    // firmly reset the flybar passthrough to false when exiting acro mode.
+    if (old_control_mode == ACRO) {
+        attitude_control.use_flybar_passthrough(false);
+    }
+#endif //HELI_FRAME
 }
 
 // returns true or false whether mode requires GPS
@@ -252,8 +259,6 @@ static bool manual_flight_mode(uint8_t mode) {
     switch(mode) {
         case ACRO:
         case STABILIZE:
-        case DRIFT:
-        case SPORT:
             return true;
         default:
             return false;
@@ -262,6 +267,15 @@ static bool manual_flight_mode(uint8_t mode) {
     return false;
 }
 
+// mode_allows_arming - returns true if vehicle can be armed in the specified mode
+//  arming_from_gcs should be set to true if the arming request comes from the ground station
+static bool mode_allows_arming(uint8_t mode, bool arming_from_gcs) {
+    if (manual_flight_mode(mode) || mode == LOITER || mode == ALT_HOLD || mode == POSHOLD || (arming_from_gcs && mode == GUIDED)) {
+        return true;
+    }
+    return false;
+}
+
 //
 // print_flight_mode - prints flight mode to serial port.
 //
@@ -320,8 +334,3 @@ print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
     }
 }
 
-// get_angle_targets_for_reporting() - returns 3d vector of roll, pitch and yaw target angles for logging and reporting to GCS
-static void get_angle_targets_for_reporting(Vector3f& targets)
-{
-    targets = attitude_control.angle_ef_targets();
-}

ArduCopter/heli_control_acro.pde

@@ -7,6 +7,9 @@
 // heli_acro_init - initialise acro controller
 static bool heli_acro_init(bool ignore_checks)
 {
+    // if heli is equipped with a flybar, then tell the attitude controller to pass through controls directly to servos
+    attitude_control.use_flybar_passthrough(motors.has_flybar());
+
     // always successfully enter acro
     return true;
 }
@@ -31,19 +34,33 @@ static void heli_acro_run()
     if(motors.armed() && heli_flags.init_targets_on_arming) {
         heli_flags.init_targets_on_arming=false;
         attitude_control.relax_bf_rate_controller();
-        
     }   
 
-    // To-Do: add support for flybarred helis
-
-    // convert the input to the desired body frame rate
-    get_pilot_desired_angle_rates(g.rc_1.control_in, g.rc_2.control_in, g.rc_4.control_in, target_roll, target_pitch, target_yaw);
-
-    // run attitude controller
-    attitude_control.rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw);
+    if (!motors.has_flybar()){
+        // convert the input to the desired body frame rate
+        get_pilot_desired_angle_rates(g.rc_1.control_in, g.rc_2.control_in, g.rc_4.control_in, target_roll, target_pitch, target_yaw);
+        // run attitude controller
+        attitude_control.rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw);
+    }else{
+        // flybar helis only need yaw rate control
+        get_pilot_desired_yaw_rate(g.rc_4.control_in, target_yaw);
+        // run attitude controller
+        attitude_control.passthrough_bf_roll_pitch_rate_yaw(g.rc_1.control_in, g.rc_2.control_in, target_yaw);
+    }
 
     // output pilot's throttle without angle boost
     attitude_control.set_throttle_out(g.rc_3.control_in, false);
 }
 
+// get_pilot_desired_yaw_rate - transform pilot's yaw input into a desired yaw angle rate
+// returns desired yaw rate in centi-degrees-per-second
+static void get_pilot_desired_yaw_rate(int16_t yaw_in, float &yaw_out)
+{
+    // calculate rate request
+    float rate_bf_yaw_request = yaw_in * g.acro_yaw_p;
+
+    // hand back rate request
+    yaw_out = rate_bf_yaw_request;
+}
+
 #endif  //HELI_FRAME

ArduCopter/land_detector.pde

@@ -0,0 +1,43 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+// counter to verify landings
+static uint16_t land_detector = LAND_DETECTOR_TRIGGER;  // we assume we are landed
+
+// land_complete_maybe - return true if we may have landed (used to reset loiter targets during landing)
+static bool land_complete_maybe()
+{
+    return (ap.land_complete || ap.land_complete_maybe);
+}
+
+// update_land_detector - checks if we have landed and updates the ap.land_complete flag
+// called at 50hz
+static void update_land_detector()
+{
+    bool climb_rate_low = (abs(climb_rate) < LAND_DETECTOR_CLIMBRATE_MAX) && (abs(baro_climbrate) < LAND_DETECTOR_BARO_CLIMBRATE_MAX);
+    bool target_climb_rate_low = !pos_control.is_active_z() || (pos_control.get_desired_velocity().z <= LAND_DETECTOR_DESIRED_CLIMBRATE_MAX);
+    bool motor_at_lower_limit = motors.limit.throttle_lower;
+    bool throttle_low = (FRAME_CONFIG == HELI_FRAME) || (motors.get_throttle_out() < get_non_takeoff_throttle());
+    bool not_rotating_fast = (ahrs.get_gyro().length() < LAND_DETECTOR_ROTATION_MAX);
+
+    if (climb_rate_low && target_climb_rate_low && motor_at_lower_limit && throttle_low && not_rotating_fast) {
+        if (!ap.land_complete) {
+            // increase counter until we hit the trigger then set land complete flag
+            if( land_detector < LAND_DETECTOR_TRIGGER) {
+                land_detector++;
+            }else{
+                set_land_complete(true);
+                land_detector = LAND_DETECTOR_TRIGGER;
+            }
+        }
+    } else {
+        // we've sensed movement up or down so reset land_detector
+        land_detector = 0;
+        // if throttle output is high then clear landing flag
+        if (motors.get_throttle_out() > get_non_takeoff_throttle()) {
+            set_land_complete(false);
+        }
+    }
+
+    // set land maybe flag
+    set_land_complete_maybe(land_detector >= LAND_DETECTOR_MAYBE_TRIGGER);
+}

ArduCopter/motor_test.pde

@@ -96,9 +96,6 @@ static bool mavlink_motor_test_check(mavlink_channel_t chan)
 //  returns MAV_RESULT_ACCEPTED on success, MAV_RESULT_FAILED on failure
 static uint8_t mavlink_motor_test_start(mavlink_channel_t chan, uint8_t motor_seq, uint8_t throttle_type, uint16_t throttle_value, float timeout_sec)
 {
-    // debug
-    cliSerial->printf_P(PSTR("\nMotTest Seq:%d TT:%d Thr:%d TimOut:%4.2f"),(int)motor_seq, (int)throttle_type, (int)throttle_value, (float)timeout_sec);
-
     // if test has not started try to start it
     if (!ap.motor_test) {
         // perform checks that it is ok to start test

ArduCopter/motors.pde

@@ -5,12 +5,13 @@
 #define AUTO_TRIM_DELAY         100 // called at 10hz so 10 seconds
 #define AUTO_DISARMING_DELAY    15  // called at 1hz so 15 seconds
 
+static uint8_t auto_disarming_counter;
+
 // arm_motors_check - checks for pilot input to arm or disarm the copter
 // called at 10hz
 static void arm_motors_check()
 {
     static int16_t arming_counter;
-    bool allow_arming = false;
 
     // ensure throttle is down
     if (g.rc_3.control_in > 0) {
@@ -18,30 +19,6 @@ static void arm_motors_check()
         return;
     }
 
-    // allow arming/disarming in fully manual flight modes ACRO, STABILIZE, SPORT and DRIFT
-    if (manual_flight_mode(control_mode)) {
-        allow_arming = true;
-    }
-
-    // allow arming/disarming in Loiter and AltHold if landed
-    if (ap.land_complete && (control_mode == LOITER || control_mode == ALT_HOLD || control_mode == POSHOLD || control_mode == AUTOTUNE)) {
-        allow_arming = true;
-    }
-
-    // kick out other flight modes
-    if (!allow_arming) {
-        arming_counter = 0;
-        return;
-    }
-
-    #if FRAME_CONFIG == HELI_FRAME
-    // heli specific arming check
-    if (!motors.allow_arming()){
-        arming_counter = 0;
-        return;
-    }
-    #endif  // HELI_FRAME
-
     int16_t tmp = g.rc_4.control_in;
 
     // full right
@@ -56,8 +33,12 @@ static void arm_motors_check()
         if (arming_counter == ARM_DELAY && !motors.armed()) {
             // run pre-arm-checks and display failures
             pre_arm_checks(true);
-            if(ap.pre_arm_check && arm_checks(true)) {
-                init_arm_motors();
+            if(ap.pre_arm_check && arm_checks(true,false)) {
+                if (!init_arm_motors()) {
+                    // reset arming counter if arming fail
+                    arming_counter = 0;
+                    AP_Notify::flags.arming_failed = true;
+                }
             }else{
                 // reset arming counter if pre-arm checks fail
                 arming_counter = 0;
@@ -68,10 +49,16 @@ static void arm_motors_check()
         // arm the motors and configure for flight
         if (arming_counter == AUTO_TRIM_DELAY && motors.armed() && control_mode == STABILIZE) {
             auto_trim_counter = 250;
+            // ensure auto-disarm doesn't trigger immediately
+            auto_disarming_counter = 0;
         }
 
     // full left
     }else if (tmp < -4000) {
+        if (!manual_flight_mode(control_mode) && !ap.land_complete) {
+            arming_counter = 0;
+            return;
+        }
 
         // increase the counter to a maximum of 1 beyond the disarm delay
         if( arming_counter <= DISARM_DELAY ) {
@@ -94,18 +81,14 @@ static void arm_motors_check()
 // called at 1hz
 static void auto_disarm_check()
 {
-    static uint8_t auto_disarming_counter;
-
     // exit immediately if we are already disarmed or throttle is not zero
-    if (!motors.armed() || g.rc_3.control_in > 0) {
+    if (!motors.armed() || !ap.throttle_zero) {
         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 == ALT_HOLD || control_mode == LOITER || control_mode == OF_LOITER ||
-                                                                  control_mode == DRIFT || control_mode == SPORT || control_mode == AUTOTUNE ||
-                                                                  control_mode == POSHOLD))) {
+    if (manual_flight_mode(control_mode) || ap.land_complete) {
         auto_disarming_counter++;
 
         if(auto_disarming_counter >= AUTO_DISARMING_DELAY) {
@@ -118,13 +101,21 @@ static void auto_disarm_check()
 }
 
 // init_arm_motors - performs arming process including initialisation of barometer and gyros
-static void init_arm_motors()
+//  returns false in the unlikely case that arming fails (because of a gyro calibration failure)
+static bool init_arm_motors()
 {
 	// arming marker
     // Flag used to track if we have armed the motors the first time.
     // This is used to decide if we should run the ground_start routine
     // which calibrates the IMU
     static bool did_ground_start = false;
+    static bool in_arm_motors = false;
+
+    // exit immediately if already in this function
+    if (in_arm_motors) {
+        return false;
+    }
+    in_arm_motors = true;
 
     // disable cpu failsafe because initialising everything takes a while
     failsafe_disable();
@@ -132,6 +123,9 @@ static void init_arm_motors()
     // disable inertial nav errors temporarily
     inertial_nav.ignore_next_error();
 
+    // reset battery failsafe
+    set_failsafe_battery(false);
+
     // notify that arming will occur (we do this early to give plenty of warning)
     AP_Notify::flags.armed = true;
     // call update_notify a few times to ensure the message gets out
@@ -162,8 +156,16 @@ static void init_arm_motors()
     }
 
     if(did_ground_start == false) {
-        did_ground_start = true;
         startup_ground(true);
+        // final check that gyros calibrated successfully
+        if (((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_INS)) && !ins.gyro_calibrated_ok_all()) {
+            gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Gyro cal failed"));
+            AP_Notify::flags.armed = false;
+            failsafe_enable();
+            in_arm_motors = false;
+            return false;
+        }
+        did_ground_start = true;
     }
 
     // fast baro calibration to reset ground pressure
@@ -188,8 +190,8 @@ static void init_arm_motors()
         motors.output_min();
         failsafe_enable();
         AP_Notify::flags.armed = false;
-        AP_Notify::flags.arming_failed = false;
-        return;
+        in_arm_motors = false;
+        return false;
     }
 
 #if SPRAYER == ENABLED
@@ -197,6 +199,9 @@ static void init_arm_motors()
     sprayer.test_pump(false);
 #endif
 
+    // short delay to allow reading of rc inputs
+    delay(30);
+
     // enable output to motors
     output_min();
 
@@ -211,13 +216,26 @@ static void init_arm_motors()
 
     // reenable failsafe
     failsafe_enable();
+
+    // flag exiting this function
+    in_arm_motors = false;
+
+    // return success
+    return true;
 }
 
 // perform pre-arm checks and set ap.pre_arm_check flag
 static void pre_arm_checks(bool display_failure)
 {
+    // exit immediately if already armed
+    if (motors.armed()) {
+        return;
+    }
+
     // exit immediately if we've already successfully performed the pre-arm check
+    // run gps checks because results may change and affect LED colour
     if (ap.pre_arm_check) {
+        pre_arm_gps_checks(display_failure);
         return;
     }
 
@@ -247,7 +265,7 @@ static void pre_arm_checks(bool display_failure)
             return;
         }
         // check Baro & inav alt are within 1m
-        if(fabs(inertial_nav.get_altitude() - baro_alt) > 100) {
+        if(fabs(inertial_nav.get_altitude() - baro_alt) > PREARM_MAX_ALT_DISPARITY_CM) {
             if (display_failure) {
                 gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Alt disparity"));
             }
@@ -266,7 +284,7 @@ static void pre_arm_checks(bool display_failure)
         }
 
         // check compass learning is on or offsets have been set
-        if(!compass.learn_offsets_enabled() && !compass.configured()) {
+        if(!compass.configured()) {
             if (display_failure) {
                 gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Compass not calibrated"));
             }
@@ -290,21 +308,40 @@ static void pre_arm_checks(bool display_failure)
             }
             return;
         }
+
+#if COMPASS_MAX_INSTANCES > 1
+        // check all compasses point in roughly same direction
+        if (compass.get_count() > 1) {
+            Vector3f prime_mag_vec = compass.get_field();
+            prime_mag_vec.normalize();
+            for(uint8_t i=0; i<compass.get_count(); i++) {
+                // get next compass
+                Vector3f mag_vec = compass.get_field(i);
+                mag_vec.normalize();
+                Vector3f vec_diff = mag_vec - prime_mag_vec;
+                if (vec_diff.length() > COMPASS_ACCEPTABLE_VECTOR_DIFF) {
+                    if (display_failure) {
+                        gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: compasses inconsistent"));
+                    }
+                    return;
+                }
+            }
+        }
+#endif
+
     }
 
     // check GPS
-    if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & 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 (!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(display_failure))) {
-            return;
+    // check fence is initialised
+    if(!fence.pre_arm_check()) {
+        if (display_failure) {
+            gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: check fence"));
         }
-#endif
+        return;
     }
 
     // check INS
@@ -317,13 +354,63 @@ static void pre_arm_checks(bool display_failure)
             return;
         }
 
-        // check accels and gyros are healthy
-        if(!ins.healthy()) {
+        // check accels are healthy
+        if(!ins.get_accel_health_all()) {
             if (display_failure) {
-                gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: INS not healthy"));
+                gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Accels not healthy"));
             }
             return;
         }
+
+#if INS_MAX_INSTANCES > 1
+        // check all accelerometers point in roughly same direction
+        if (ins.get_accel_count() > 1) {
+            const Vector3f &prime_accel_vec = ins.get_accel();
+            for(uint8_t i=0; i<ins.get_accel_count(); i++) {
+                // get next accel vector
+                const Vector3f &accel_vec = ins.get_accel(i);
+                Vector3f vec_diff = accel_vec - prime_accel_vec;
+                if (vec_diff.length() > PREARM_MAX_ACCEL_VECTOR_DIFF) {
+                    if (display_failure) {
+                        gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Accels inconsistent"));
+                    }
+                    return;
+                }
+            }
+        }
+#endif
+
+        // check gyros are healthy
+        if(!ins.get_gyro_health_all()) {
+            if (display_failure) {
+                gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Gyros not healthy"));
+            }
+            return;
+        }
+
+        // check gyros calibrated successfully
+        if(!ins.gyro_calibrated_ok_all()) {
+            if (display_failure) {
+                gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Gyro cal failed"));
+            }
+            return;
+        }
+
+#if INS_MAX_INSTANCES > 1
+        // check all gyros are consistent
+        if (ins.get_gyro_count() > 1) {
+            for(uint8_t i=0; i<ins.get_gyro_count(); i++) {
+                // get rotation rate difference between gyro #i and primary gyro
+                Vector3f vec_diff = ins.get_gyro(i) - ins.get_gyro();
+                if (vec_diff.length() > PREARM_MAX_GYRO_VECTOR_DIFF) {
+                    if (display_failure) {
+                        gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Gyros inconsistent"));
+                    }
+                    return;
+                }
+            }
+        }
+#endif
     }
 #if CONFIG_HAL_BOARD != HAL_BOARD_VRBRAIN
 #ifndef CONFIG_ARCH_BOARD_PX4FMU_V1
@@ -418,13 +505,39 @@ static void pre_arm_rc_checks()
 // performs pre_arm gps related checks and returns true if passed
 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
+    // return true immediately if gps check is disabled
+    if (!(g.arming_check == ARMING_CHECK_ALL || g.arming_check & ARMING_CHECK_GPS)) {
+        AP_Notify::flags.pre_arm_gps_check = true;
+        return true;
+    }
+
+    // check if flight mode requires GPS
+    bool gps_required = mode_requires_GPS(control_mode);
+
+    // if GPS failsafe will triggers even in stabilize mode we need GPS before arming
+    if (g.failsafe_gps_enabled == FS_GPS_LAND_EVEN_STABILIZE) {
+        gps_required = true;
+    }
+
+#if AC_FENCE == ENABLED
+    // if circular fence is enabled we need GPS
+    if ((fence.get_enabled_fences() & AC_FENCE_TYPE_CIRCLE) != 0) {
+        gps_required = true;
+    }
+#endif
+
+    // return true if GPS is not required
+    if (!gps_required) {
+        AP_Notify::flags.pre_arm_gps_check = true;
+        return true;
+    }
 
     // check GPS is not glitching
     if (gps_glitch.glitching()) {
         if (display_failure) {
             gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: GPS Glitch"));
         }
+        AP_Notify::flags.pre_arm_gps_check = false;
         return false;
     }
 
@@ -433,14 +546,17 @@ static bool pre_arm_gps_checks(bool display_failure)
         if (display_failure) {
             gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Need 3D Fix"));
         }
+        AP_Notify::flags.pre_arm_gps_check = false;
         return false;
     }
 
     // check speed is below 50cm/s
+    float speed_cms = inertial_nav.get_velocity().length();     // speed according to inertial nav in cm/s
     if (speed_cms == 0 || speed_cms > PREARM_MAX_VELOCITY_CMS) {
         if (display_failure) {
             gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Bad Velocity"));
         }
+        AP_Notify::flags.pre_arm_gps_check = false;
         return false;
     }
 
@@ -449,17 +565,38 @@ static bool pre_arm_gps_checks(bool display_failure)
         if (display_failure) {
             gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: High GPS HDOP"));
         }
+        AP_Notify::flags.pre_arm_gps_check = false;
         return false;
     }
 
     // if we got here all must be ok
+    AP_Notify::flags.pre_arm_gps_check = true;
     return true;
 }
 
 // arm_checks - perform final checks before arming
 // always called just before arming.  Return true if ok to arm
-static bool arm_checks(bool display_failure)
+static bool arm_checks(bool display_failure, bool arming_from_gcs)
 {
+    // always check if the current mode allows arming
+    if (!mode_allows_arming(control_mode, arming_from_gcs)) {
+        if (display_failure) {
+            gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Mode not armable"));
+        }
+        return false;
+    }
+
+    // always check if rotor is spinning on heli
+    #if FRAME_CONFIG == HELI_FRAME
+    // heli specific arming check
+    if (!motors.allow_arming()){
+        if (display_failure) {
+            gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Rotor not spinning"));
+        }
+        return false;
+    }
+    #endif  // HELI_FRAME
+
     // succeed if arming checks are disabled
     if (g.arming_check == ARMING_CHECK_NONE) {
         return true;
@@ -467,7 +604,7 @@ static bool arm_checks(bool display_failure)
 
     // check Baro & inav alt are within 1m
     if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_BARO)) {
-        if(fabs(inertial_nav.get_altitude() - baro_alt) > 100) {
+        if(fabs(inertial_nav.get_altitude() - baro_alt) > PREARM_MAX_ALT_DISPARITY_CM) {
             if (display_failure) {
                 gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Alt disparity"));
             }
@@ -475,11 +612,9 @@ static bool arm_checks(bool display_failure)
         }
     }
 
-    // check gps is ok if required - note this same check is also done in pre-arm checks
-    if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & 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 gps
+    if (!pre_arm_gps_checks(display_failure)) {
+        return false;
     }
 
     // check parameters
@@ -546,6 +681,7 @@ static void init_disarm_motors()
 
     // we are not in the air
     set_land_complete(true);
+    set_land_complete_maybe(true);
 
     // reset the mission
     mission.reset();
@@ -557,7 +693,9 @@ static void init_disarm_motors()
     Log_Write_Event(DATA_DISARMED);
 
     // suspend logging
-    DataFlash.EnableWrites(false);
+    if (!(g.log_bitmask & MASK_LOG_WHEN_DISARMED)) {
+        DataFlash.EnableWrites(false);
+    }
 
     // disable gps velocity based centrefugal force compensation
     ahrs.set_correct_centrifugal(false);

ArduCopter/navigation.pde

@@ -38,7 +38,7 @@ static void calc_wp_distance()
     // get target from loiter or wpinav controller
     if (control_mode == LOITER || control_mode == CIRCLE) {
         wp_distance = wp_nav.get_loiter_distance_to_target();
-    }else if (control_mode == AUTO) {
+    }else if (control_mode == AUTO || control_mode == RTL || (control_mode == GUIDED && guided_mode == Guided_WP)) {
         wp_distance = wp_nav.get_wp_distance_to_destination();
     }else{
         wp_distance = 0;
@@ -51,7 +51,7 @@ static void calc_wp_bearing()
     // get target from loiter or wpinav controller
     if (control_mode == LOITER || control_mode == CIRCLE) {
         wp_bearing = wp_nav.get_loiter_bearing_to_target();
-    } else if (control_mode == AUTO) {
+    } else if (control_mode == AUTO || control_mode == RTL || (control_mode == GUIDED && guided_mode == Guided_WP)) {
         wp_bearing = wp_nav.get_wp_bearing_to_destination();
     } else {
         wp_bearing = 0;

ArduCopter/radio.pde

@@ -38,6 +38,9 @@ static void init_rc_in()
 
     // set default dead zones
     default_dead_zones();
+
+    // initialise throttle_zero flag
+    ap.throttle_zero = true;
 }
 
  // init_rc_out -- initialise motors and check if pilot wants to perform ESC calibration
@@ -62,13 +65,13 @@ static void init_rc_out()
             // we will enter esc_calibrate mode on next reboot
             g.esc_calibrate.set_and_save(1);
             // display message on console
-            cliSerial->printf_P(PSTR("Entering ESC Calibration: please restart APM.\n"));
+            cliSerial->printf_P(PSTR("Entering ESC Cal: restart APM.\n"));
             // turn on esc calibration notification
             AP_Notify::flags.esc_calibration = true;
             // block until we restart
             while(1) { delay(5); }
         }else{
-            cliSerial->printf_P(PSTR("ESC Calibration active: passing throttle through to ESCs.\n"));
+            cliSerial->printf_P(PSTR("ESC Cal: passing throttle through to ESCs.\n"));
             // clear esc flag
             g.esc_calibrate.set_and_save(0);
             // pass through user throttle to escs
@@ -97,9 +100,11 @@ void output_min()
 
 static void read_radio()
 {
-    static uint32_t last_update = 0;
+    static uint32_t last_update_ms = 0;
+    uint32_t tnow_ms = millis();
+
     if (hal.rcin->new_input()) {
-        last_update = millis();
+        last_update_ms = tnow_ms;
         ap.new_radio_frame = true;
         uint16_t periods[8];
         hal.rcin->read(periods,8);
@@ -107,6 +112,7 @@ static void read_radio()
         g.rc_2.set_pwm(periods[rcmap.pitch()-1]);
 
         set_throttle_and_failsafe(periods[rcmap.throttle()-1]);
+        set_throttle_zero_flag(g.rc_3.control_in);
 
         g.rc_4.set_pwm(periods[rcmap.yaw()-1]);
         g.rc_5.set_pwm(periods[4]);
@@ -114,6 +120,13 @@ static void read_radio()
         g.rc_7.set_pwm(periods[6]);
         g.rc_8.set_pwm(periods[7]);
 
+        // read channels 9 ~ 14
+        for (uint8_t i=8; i<RC_MAX_CHANNELS; i++) {
+            if (RC_Channel::rc_channel(i) != NULL) {
+                RC_Channel::rc_channel(i)->set_pwm(RC_Channel::rc_channel(i)->read());
+            }
+        }
+
         // flag we must have an rc receiver attached
         if (!failsafe.rc_override_active) {
             ap.rc_receiver_present = true;
@@ -122,7 +135,7 @@ static void read_radio()
         // update output on any aux channels, for manual passthru
         RC_Channel_aux::output_ch_all();
     }else{
-        uint32_t elapsed = millis() - last_update;
+        uint32_t elapsed = tnow_ms - last_update_ms;
         // turn on throttle failsafe if no update from the RC Radio for 500ms or 2000ms if we are using RC_OVERRIDE
         if (((!failsafe.rc_override_active && (elapsed >= FS_RADIO_TIMEOUT_MS)) || (failsafe.rc_override_active && (elapsed >= FS_RADIO_RC_OVERRIDE_TIMEOUT_MS))) &&
             (g.failsafe_throttle && motors.armed() && !failsafe.radio)) {
@@ -174,6 +187,22 @@ static void set_throttle_and_failsafe(uint16_t throttle_pwm)
     }
 }
 
+#define THROTTLE_ZERO_DEBOUNCE_TIME_MS 400
+// set_throttle_zero_flag - set throttle_zero flag from debounced throttle control_in
+static void set_throttle_zero_flag(int16_t throttle_control)
+{
+    static uint32_t last_nonzero_throttle_ms = 0;
+    uint32_t tnow_ms = millis();
+
+    // if non-zero throttle immediately set as non-zero
+    if (throttle_control > 0) {
+        last_nonzero_throttle_ms = tnow_ms;
+        ap.throttle_zero = false;
+    } else if (tnow_ms - last_nonzero_throttle_ms > THROTTLE_ZERO_DEBOUNCE_TIME_MS) {
+        ap.throttle_zero = true;
+    }
+}
+
 static void trim_radio()
 {
     for (uint8_t i = 0; i < 30; i++) {

ArduCopter/sensors.pde

@@ -19,13 +19,14 @@ static void init_barometer(bool full_calibration)
 }
 
 // return barometric altitude in centimeters
-static int32_t read_barometer(void)
+static void read_barometer(void)
 {
     barometer.read();
-    if (g.log_bitmask & MASK_LOG_IMU) {
+    if (should_log(MASK_LOG_IMU)) {
         Log_Write_Baro();
     }
-    int32_t balt = barometer.get_altitude() * 100.0f;
+    baro_alt = barometer.get_altitude() * 100.0f;
+    baro_climbrate = barometer.get_climb_rate() * 100.0f;
 
     // run glitch protection and update AP_Notify if home has been initialised
     baro_glitch.check_alt();
@@ -38,9 +39,6 @@ static int32_t read_barometer(void)
         }
         AP_Notify::flags.baro_glitching = report_baro_glitch;
     }
-
-    // return altitude
-    return balt;
 }
 
 // return sonar altitude in centimeters

ArduCopter/setup.pde

@@ -9,6 +9,7 @@
 // Functions called from the setup menu
 static int8_t   setup_factory           (uint8_t argc, const Menu::arg *argv);
 static int8_t   setup_show              (uint8_t argc, const Menu::arg *argv);
+static int8_t   setup_set               (uint8_t argc, const Menu::arg *argv);
 #ifdef WITH_ESC_CALIB
 static int8_t   esc_calib               (uint8_t argc, const Menu::arg *argv);
 #endif
@@ -19,6 +20,7 @@ const struct Menu::command setup_menu_commands[] PROGMEM = {
     // =======          ===============
     {"reset",                       setup_factory},
     {"show",                        setup_show},
+    {"set",                         setup_set},
 #ifdef WITH_ESC_CALIB
     {"esc_calib",                   esc_calib},
 #endif
@@ -66,6 +68,65 @@ setup_factory(uint8_t argc, const Menu::arg *argv)
     return(0);
 }
 
+//Set a parameter to a specified value. It will cast the value to the current type of the
+//parameter and make sure it fits in case of INT8 and INT16
+static int8_t setup_set(uint8_t argc, const Menu::arg *argv)
+{
+    int8_t value_int8;
+    int16_t value_int16;
+
+    AP_Param *param;
+    enum ap_var_type p_type;
+
+    if(argc!=3)
+    {
+        cliSerial->printf_P(PSTR("Invalid command. Usage: set <name> <value>\n"));
+        return 0;
+    }
+
+    param = AP_Param::find(argv[1].str, &p_type);
+    if(!param)
+    {
+        cliSerial->printf_P(PSTR("Param not found: %s\n"), argv[1].str);
+        return 0;
+    }
+
+    switch(p_type)
+    {
+        case AP_PARAM_INT8:
+            value_int8 = (int8_t)(argv[2].i);
+            if(argv[2].i!=value_int8)
+            {
+                cliSerial->printf_P(PSTR("Value out of range for type INT8\n"));
+                return 0;
+            }
+            ((AP_Int8*)param)->set_and_save(value_int8);
+            break;
+        case AP_PARAM_INT16:
+            value_int16 = (int16_t)(argv[2].i);
+            if(argv[2].i!=value_int16)
+            {
+                cliSerial->printf_P(PSTR("Value out of range for type INT16\n"));
+                return 0;
+            }
+            ((AP_Int16*)param)->set_and_save(value_int16);
+            break;
+
+        //int32 and float don't need bounds checking, just use the value provoded by Menu::arg
+        case AP_PARAM_INT32:
+            ((AP_Int32*)param)->set_and_save(argv[2].i);
+            break;
+        case AP_PARAM_FLOAT:
+            ((AP_Float*)param)->set_and_save(argv[2].f);
+            break;
+        default:
+            cliSerial->printf_P(PSTR("Cannot set parameter of type %d.\n"), p_type);
+            break;
+    }
+
+    return 0;
+}
+
 // Print the current configuration.
 // Called by the setup menu 'show' command.
 static int8_t

ArduCopter/system.pde

@@ -288,12 +288,16 @@ static void init_ardupilot()
     // mid-flight, so set the serial ports non-blocking once we are
     // ready to fly
     hal.uartA->set_blocking_writes(false);
+    hal.uartB->set_blocking_writes(false);
     hal.uartC->set_blocking_writes(false);
     if (hal.uartD != NULL) {
         hal.uartD->set_blocking_writes(false);
     }
 
     cliSerial->print_P(PSTR("\nReady to FLY "));
+
+    // flag that initialisation has completed
+    ap.initialised = true;
 }
 
 
@@ -316,11 +320,17 @@ static void startup_ground(bool force_gyro_cal)
     report_ins();
  #endif
 
+    // reset ahrs gyro bias
+    if (force_gyro_cal) {
+        ahrs.reset_gyro_drift();
+    }
+
     // setup fast AHRS gains to get right attitude
     ahrs.set_fast_gains(true);
 
     // set landed flag
     set_land_complete(true);
+    set_land_complete_maybe(true);
 }
 
 // returns true if the GPS is ok and home position is set
@@ -346,7 +356,7 @@ static void update_auto_armed()
             return;
         }
         // if in stabilize or acro flight mode and throttle is zero, auto-armed should become false
-        if(manual_flight_mode(control_mode) && g.rc_3.control_in == 0 && !failsafe.radio) {
+        if(manual_flight_mode(control_mode) && ap.throttle_zero && !failsafe.radio) {
             set_auto_armed(false);
         }
     }else{
@@ -354,12 +364,12 @@ static void update_auto_armed()
         
 #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()) {
+        if(motors.armed() && !ap.throttle_zero && 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) {
+        if(motors.armed() && !ap.throttle_zero) {
             set_auto_armed(true);
         }
 #endif // HELI_FRAME
@@ -399,3 +409,24 @@ static void telemetry_send(void)
                                 (AP_Frsky_Telem::FrSkyProtocol)g.serial2_protocol.get());
 #endif
 }
+
+/*
+  should we log a message type now?
+ */
+static bool should_log(uint32_t mask)
+{
+#if LOGGING_ENABLED == ENABLED
+    if (!(mask & g.log_bitmask) || in_mavlink_delay) {
+        return false;
+    }
+    bool ret = motors.armed() || (g.log_bitmask & MASK_LOG_WHEN_DISARMED) != 0;
+    if (ret && !DataFlash.logging_started() && !in_log_download) {
+        // we have to set in_mavlink_delay to prevent logging while
+        // writing headers
+        start_logging();
+    }
+    return ret;
+#else
+    return false;
+#endif
+}

ArduCopter/test.pde

@@ -58,13 +58,13 @@ test_baro(uint8_t argc, const Menu::arg *argv)
 
     while(1) {
         delay(100);
-        alt = read_barometer();
+        read_barometer();
 
         if (!barometer.healthy()) {
             cliSerial->println_P(PSTR("not healthy"));
         } else {
             cliSerial->printf_P(PSTR("Alt: %0.2fm, Raw: %f Temperature: %.1f\n"),
-                                alt / 100.0,
+                                baro_alt / 100.0,
                                 barometer.get_pressure(), 
                                 barometer.get_temperature());
         }

Tools/Replay/LogReader.cpp

@@ -144,7 +144,7 @@ void LogReader::process_plane(uint8_t type, uint8_t *data, uint16_t length)
         memcpy(&msg, data, sizeof(msg));
         wait_timestamp(msg.time_ms);
         compass.setHIL(Vector3f(msg.mag_x - msg.offset_x, msg.mag_y - msg.offset_y, msg.mag_z - msg.offset_z));
-        compass.set_and_save_offsets(0, msg.offset_x, msg.offset_y, msg.offset_z);
+        compass.set_offsets(0, Vector3f(msg.offset_x, msg.offset_y, msg.offset_z));
         break;
     }
 
@@ -186,7 +186,7 @@ void LogReader::process_rover(uint8_t type, uint8_t *data, uint16_t length)
         memcpy(&msg, data, sizeof(msg));
         wait_timestamp(msg.time_ms);
         compass.setHIL(Vector3f(msg.mag_x - msg.offset_x, msg.mag_y - msg.offset_y, msg.mag_z - msg.offset_z));
-        compass.set_and_save_offsets(0, msg.offset_x, msg.offset_y, msg.offset_z);
+        compass.set_offsets(0, Vector3f(msg.offset_x, msg.offset_y, msg.offset_z));
         break;
     }
 
@@ -216,7 +216,7 @@ void LogReader::process_copter(uint8_t type, uint8_t *data, uint16_t length)
         memcpy(&msg, data, sizeof(msg));
         wait_timestamp(msg.time_ms);
         compass.setHIL(Vector3f(msg.mag_x - msg.offset_x, msg.mag_y - msg.offset_y, msg.mag_z - msg.offset_z));
-        compass.set_and_save_offsets(0, msg.offset_x, msg.offset_y, msg.offset_z);
+        compass.set_offsets(0, Vector3f(msg.offset_x, msg.offset_y, msg.offset_z));
         break;
     }
 
@@ -250,6 +250,10 @@ void LogReader::process_copter(uint8_t type, uint8_t *data, uint16_t length)
 
 bool LogReader::set_parameter(const char *name, float value)
 {
+    if (strcmp(name, "GPS_TYPE") == 0) {
+        // ignore this one
+        return true;
+    }
     enum ap_var_type var_type;
     AP_Param *vp = AP_Param::find(name, &var_type);
     if (vp == NULL) {
@@ -470,6 +474,18 @@ bool LogReader::update(uint8_t &type)
         break;        
     }
         
+    case LOG_AHR2_MSG: {
+        struct log_AHRS msg;
+        if(sizeof(msg) != f.length) {
+            printf("Bad AHR2 length %u should be %u\n", (unsigned)f.length, (unsigned)sizeof(msg));
+            exit(1);
+        }
+        memcpy(&msg, data, sizeof(msg));
+        wait_timestamp(msg.time_ms);
+        ahr2_attitude = Vector3f(msg.roll*0.01f, msg.pitch*0.01f, msg.yaw*0.01f);
+        break;
+    }
+
 
     default:
         if (vehicle == VEHICLE_PLANE) {

Tools/Replay/LogReader.h

@@ -3,6 +3,7 @@ enum log_messages {
     // plane specific messages
     LOG_PLANE_ATTITUDE_MSG = 9,
     LOG_PLANE_COMPASS_MSG  = 11,
+    LOG_PLANE_COMPASS2_MSG  = 15,
     LOG_PLANE_AIRSPEED_MSG  = 17,
 
     // copter specific messages
@@ -25,6 +26,7 @@ public:
     bool wait_type(uint8_t type);
 
     const Vector3f &get_attitude(void) const { return attitude; }
+    const Vector3f &get_ahr2_attitude(void) const { return ahr2_attitude; }
     const Vector3f &get_inavpos(void) const { return inavpos; }
     const Vector3f &get_sim_attitude(void) const { return sim_attitude; }
     const float &get_relalt(void) const { return rel_altitude; }
@@ -57,6 +59,7 @@ private:
     struct log_Format formats[LOGREADER_MAX_FORMATS];
 
     Vector3f attitude;
+    Vector3f ahr2_attitude;
     Vector3f sim_attitude;
     Vector3f inavpos;
     float rel_altitude;

Tools/Replay/Parameters.h

@@ -15,7 +15,8 @@ public:
         k_param_ins,
         k_param_ahrs,
         k_param_airspeed,
-        k_param_NavEKF
+        k_param_NavEKF,
+        k_param_compass
     };
     AP_Int8 dummy;
 };

Tools/Replay/Parameters.pde

@@ -34,6 +34,10 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @Path: ../libraries/AP_NavEKF/AP_NavEKF.cpp
     GOBJECTN(ahrs.get_NavEKF(), NavEKF, "EKF_", NavEKF),
 
+    // @Group: COMPASS_
+    // @Path: ../libraries/AP_Compass/AP_Compass.cpp
+    GOBJECT(compass, "COMPASS_", Compass),
+
     AP_VAREND
 };
 

Tools/Replay/Replay.pde

@@ -103,6 +103,8 @@ static struct {
     float value;
 } user_parameters[100];
 
+// setup the var_info table
+AP_Param param_loader(var_info);
 
 static void usage(void)
 {
@@ -228,7 +230,7 @@ void setup()
     ekf3f = fopen("EKF3.dat", "w");
     ekf4f = fopen("EKF4.dat", "w");
 
-    fprintf(plotf, "time SIM.Roll SIM.Pitch SIM.Yaw BAR.Alt FLIGHT.Roll FLIGHT.Pitch FLIGHT.Yaw FLIGHT.dN FLIGHT.dE FLIGHT.Alt DCM.Roll DCM.Pitch DCM.Yaw EKF.Roll EKF.Pitch EKF.Yaw INAV.dN INAV.dE INAV.Alt EKF.dN EKF.dE EKF.Alt\n");
+    fprintf(plotf, "time SIM.Roll SIM.Pitch SIM.Yaw BAR.Alt FLIGHT.Roll FLIGHT.Pitch FLIGHT.Yaw FLIGHT.dN FLIGHT.dE FLIGHT.Alt AHR2.Roll AHR2.Pitch AHR2.Yaw DCM.Roll DCM.Pitch DCM.Yaw EKF.Roll EKF.Pitch EKF.Yaw INAV.dN INAV.dE INAV.Alt EKF.dN EKF.dE EKF.Alt\n");
     fprintf(plotf2, "time E1 E2 E3 VN VE VD PN PE PD GX GY GZ WN WE MN ME MD MX MY MZ E1ref E2ref E3ref\n");
     fprintf(ekf1f, "timestamp TimeMS Roll Pitch Yaw VN VE VD PN PE PD GX GY GZ\n");
     fprintf(ekf2f, "timestamp TimeMS AX AY AZ VWN VWE MN ME MD MX MY MZ\n");
@@ -391,10 +393,13 @@ void loop()
                     barometer.get_altitude(),
                     LogReader.get_attitude().x,
                     LogReader.get_attitude().y,
-                    LogReader.get_attitude().z,
+                    wrap_180_cd(LogReader.get_attitude().z*100)*0.01f,
                     LogReader.get_inavpos().x,
                     LogReader.get_inavpos().y,
                     LogReader.get_relalt(),
+                    LogReader.get_ahr2_attitude().x,
+                    LogReader.get_ahr2_attitude().y,
+                    wrap_180_cd(LogReader.get_ahr2_attitude().z*100)*0.01f,
                     degrees(DCM_attitude.x),
                     degrees(DCM_attitude.y),
                     degrees(DCM_attitude.z),

Tools/autotest/ArduPlane.parm

@@ -65,3 +65,10 @@ ACRO_LOCKING     1
 ELEVON_OUTPUT    0
 VTAIL_OUTPUT     0
 SKIP_GYRO_CAL    1
+# we need small INS_ACC offsets so INS is recognised as being calibrated
+INS_ACCOFFS_X   0.001
+INS_ACCOFFS_Y   0.001
+INS_ACCOFFS_Z   0.001
+INS_ACCSCAL_X   1.001
+INS_ACCSCAL_Y   1.001
+INS_ACCSCAL_Z   1.001

Tools/autotest/Rover.parm

@@ -18,3 +18,10 @@ MODE6        0
 STEER2SRV_P     1.8
 SIM_GPS_DELAY 2
 NAVL1_PERIOD 6
+# we need small INS_ACC offsets so INS is recognised as being calibrated
+INS_ACCOFFS_X   0.001
+INS_ACCOFFS_Y   0.001
+INS_ACCOFFS_Z   0.001
+INS_ACCSCAL_X   1.001
+INS_ACCSCAL_Y   1.001
+INS_ACCSCAL_Z   1.001

Tools/autotest/arducopter.py

@@ -160,7 +160,7 @@ def fly_square(mavproxy, mav, side=50, timeout=120):
     save_wp(mavproxy, mav)
 
     # switch back to stabilize mode
-    mavproxy.send('rc 3 1400\n')
+    mavproxy.send('rc 3 1430\n')
     mavproxy.send('switch 6\n')
     wait_mode(mav, 'STABILIZE')
 
@@ -620,7 +620,7 @@ def fly_simple(mavproxy, mav, side=50, timeout=120):
     # switch to stabilize mode
     mavproxy.send('switch 6\n')
     wait_mode(mav, 'STABILIZE')
-    mavproxy.send('rc 3 1400\n')
+    mavproxy.send('rc 3 1430\n')
 
     # fly south 50m
     print("# Flying south %u meters" % side)
@@ -685,7 +685,7 @@ def fly_super_simple(mavproxy, mav, timeout=45):
     # switch to stabilize mode
     mavproxy.send('switch 6\n')
     wait_mode(mav, 'STABILIZE')
-    mavproxy.send('rc 3 1400\n')
+    mavproxy.send('rc 3 1430\n')
 
     # start copter yawing slowly
     mavproxy.send('rc 4 1550\n')

Tools/autotest/copter_AVC2013_params.parm

@@ -48,6 +48,13 @@ SIM_WIND_SPD    0
 SIM_WIND_TURB   0
 SIM_BARO_RND    0
 SIM_MAG_RND     0
+# we need small INS_ACC offsets so INS is recognised as being calibrated
+INS_ACCOFFS_X   0.001
+INS_ACCOFFS_Y   0.001
+INS_ACCOFFS_Z   0.001
+INS_ACCSCAL_X   1.001
+INS_ACCSCAL_Y   1.001
+INS_ACCSCAL_Z   1.001
 # flightmodes
 # switch 1 Circle
 # switch 2 LAND

Tools/autotest/copter_params.parm

@@ -46,6 +46,13 @@ SIM_WIND_SPD    0
 SIM_WIND_TURB   0
 SIM_BARO_RND    0
 SIM_MAG_RND     0
+# we need small INS_ACC offsets so INS is recognised as being calibrated
+INS_ACCOFFS_X   0.001
+INS_ACCOFFS_Y   0.001
+INS_ACCOFFS_Z   0.001
+INS_ACCSCAL_X   1.001
+INS_ACCSCAL_Y   1.001
+INS_ACCSCAL_Z   1.001
 # flightmodes
 # switch 1 Circle
 # switch 2 LAND

Tools/autotest/run_in_terminal_window.sh

@@ -13,8 +13,14 @@ elif [ -x /usr/bin/konsole ]; then
   /usr/bin/konsole --hold -e $*
 elif [ -x /usr/bin/gnome-terminal ]; then
   /usr/bin/gnome-terminal -e "$*"
+elif [ -n "$STY" ]; then
+  # We are running inside of screen, try to start it there
+  /usr/bin/screen -X screen -t $name $*
 else
-  echo "ERROR: Please install xterm"
-  exit 1
+  filename="/tmp/$name.log"
+  echo "Window access not found, logging to $filename"
+  cmd="$1"
+  shift
+  ( run_cmd.sh $cmd $* &>$filename < /dev/null ) &
 fi
 exit 0

Tools/scripts/build_all_travis.sh

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

Tools/scripts/build_vrbrain_binaries.sh

@@ -0,0 +1,277 @@
+#!/bin/bash
+# script to build the latest binaries for each vehicle type, ready to upload
+# Andrew Tridgell, March 2013
+
+export PATH=$PATH:/bin:/usr/bin
+
+export TMPDIR=$PWD/build.tmp.$$
+echo $TMDIR
+rm -rf $TMPDIR
+echo "Building in $TMPDIR"
+
+date
+git checkout master
+githash=$(git rev-parse HEAD)
+
+hdate=$(date +"%Y-%m/%Y-%m-%d-%H:%m")
+mkdir -p binaries/$hdate
+binaries=$PWD/../buildlogs/binaries
+
+error_count=0
+
+. config.mk
+
+# checkout the right version of the tree
+checkout() {
+    branch="$1"
+    git stash
+    if [ "$branch" = "master" ]; then
+	vbranch="master"
+	vbranch2="master"
+    fi
+    if [ "$branch" = "for_merge" ]; then
+	vbranch="for_merge"
+	vbranch2="for_merge"
+    fi
+    if [ "$branch" = "for_merge-3.2" ]; then
+	vbranch="for_merge-3.2"
+	vbranch2="for_merge"
+    fi
+    if [ "$branch" = "tone_alarm" ]; then
+	vbranch="ToneAlarm"
+	vbranch2="ToneAlarm"
+    fi
+    if [ "$branch" = "tone_alarm-3.2" ]; then
+	vbranch="ToneAlarm-3.2"
+	vbranch2="ToneAlarm"
+    fi
+
+    echo "Checkout with branch $branch"
+
+    git remote update
+    git checkout -B "$vbranch" remotes/origin/"$vbranch"
+    git pull -v --progress  "origin" "$vbranch" || return 1
+
+    git log -1
+
+    pushd ../../vrbrain_nuttx
+    git remote update
+    git checkout -B "$vbranch2" remotes/origin/"$vbranch2"
+    git pull -v --progress  "origin" "$vbranch2" || {
+        popd
+        return 1
+    }
+    git log -1
+    popd
+
+    return 0
+}
+
+# check if we should skip this build because we have already
+# built this version
+skip_build() {
+    [ "$FORCE_BUILD" = "1" ] && return 1
+    tag="$1"
+    ddir="$2"
+    bname=$(basename $ddir)
+    ldir=$(dirname $(dirname $(dirname $ddir)))/$tag/$bname
+    [ -d "$ldir" ] || {
+	echo "$ldir doesn't exist - building"
+	return 1
+    }
+    oldversion=$(cat "$ldir/git-version.txt" | head -1)
+    newversion=$(git log -1 | head -1)
+    [ "$oldversion" = "$newversion" ] && {
+	echo "Skipping build - version match $newversion"
+	return 0
+    }
+    echo "$ldir needs rebuild"
+    return 1
+}
+
+addfwversion() {
+    destdir="$1"
+    git log -1 > "$destdir/git-version.txt"
+    [ -f APM_Config.h ] && {
+	version=$(grep 'define.THISFIRMWARE' *.pde 2> /dev/null | cut -d'"' -f2)
+	echo >> "$destdir/git-version.txt"
+	echo "APMVERSION: $version" >> "$destdir/git-version.txt"
+    }    
+}
+
+# copy the built firmware to the right directory
+copyit() {
+    file="$1"
+    dir="$2"
+    tag="$3"
+    bname=$(basename $dir)
+    tdir=$(dirname $(dirname $(dirname $dir)))/$tag/$bname
+    if [ "$tag" = "latest" ]; then
+	mkdir -p "$dir"
+	/bin/cp "$file" "$dir"
+	addfwversion "$dir"
+    fi
+    echo "Copying $file to $tdir"
+    mkdir -p "$tdir"
+    addfwversion "$tdir"
+
+    rsync "$file" "$tdir"
+}
+
+# build plane binaries
+build_arduplane() {
+    tag="$1"
+    echo "Building ArduPlane $tag binaries"
+    pushd ArduPlane
+    test -n "$VRBRAIN_ROOT" && {
+	echo "Building ArduPlane VRBRAIN binaries"
+	ddir=$binaries/Plane/$hdate/VRX
+        checkout $tag || {
+            echo "Failed checkout of ArduPlane VRBRAIN $tag"
+            error_count=$((error_count+1))
+            continue
+        }
+	skip_build $tag $ddir || {
+	    make vrbrain-clean &&
+	    make vrbrain || {
+                echo "Failed build of ArduPlane VRBRAIN $tag"
+                error_count=$((error_count+1))
+                continue
+            }
+	    copyit ArduPlane-vrbrain-v45.vrx $ddir $tag && 
+	    copyit ArduPlane-vrbrain-v45.bin $ddir $tag && 
+	    copyit ArduPlane-vrbrain-v45.hex $ddir $tag &&
+	    copyit ArduPlane-vrbrain-v51.vrx $ddir $tag && 
+	    copyit ArduPlane-vrbrain-v51.bin $ddir $tag && 
+	    copyit ArduPlane-vrbrain-v51.hex $ddir $tag &&
+	    copyit ArduPlane-vrbrain-v51P.vrx $ddir $tag && 
+	    copyit ArduPlane-vrbrain-v51P.bin $ddir $tag && 
+	    copyit ArduPlane-vrbrain-v51P.hex $ddir $tag &&
+	    copyit ArduPlane-vrbrain-v51Pro.vrx $ddir $tag && 
+	    copyit ArduPlane-vrbrain-v51Pro.bin $ddir $tag && 
+	    copyit ArduPlane-vrbrain-v51Pro.hex $ddir $tag &&
+	    copyit ArduPlane-vrbrain-v51ProP.vrx $ddir $tag && 
+	    copyit ArduPlane-vrbrain-v51ProP.bin $ddir $tag && 
+	    copyit ArduPlane-vrbrain-v51ProP.hex $ddir $tag &&
+	    copyit ArduPlane-vrubrain-v51.vrx $ddir $tag && 
+	    copyit ArduPlane-vrubrain-v51.bin $ddir $tag && 
+	    copyit ArduPlane-vrubrain-v51.hex $ddir $tag && 
+	    copyit ArduPlane-vrubrain-v51P.vrx $ddir $tag && 
+	    copyit ArduPlane-vrubrain-v51P.bin $ddir $tag && 
+	    copyit ArduPlane-vrubrain-v51P.hex $ddir $tag
+	}
+    }
+    checkout "master"
+    popd
+}
+
+# build copter binaries
+build_arducopter() {
+    tag="$1"
+    echo "Building ArduCopter $tag binaries from $(pwd)"
+    pushd ArduCopter
+    frames="quad tri hexa y6 octa octa-quad heli"
+    test -n "$VRBRAIN_ROOT" && {
+        checkout $tag || {
+            echo "Failed checkout of ArduCopter VRBRAIN $tag"
+            error_count=$((error_count+1))
+            checkout "master"
+            popd
+            return
+        }
+	make vrbrain-clean || return
+	for f in $frames quad-hil heli-hil; do
+	    echo "Building ArduCopter VRBRAIN-$f binaries"
+	    ddir="$binaries/Copter/$hdate/VRX-$f"
+	    skip_build $tag $ddir && continue
+            rm -rf ../Build.ArduCopter
+	    make vrbrain-$f || {
+                echo "Failed build of ArduCopter VRBRAIN $tag"
+                error_count=$((error_count+1))
+                continue
+            }
+	    copyit ArduCopter-vrbrain-v45.vrx $ddir $tag && 
+	    copyit ArduCopter-vrbrain-v45.bin $ddir $tag && 
+	    copyit ArduCopter-vrbrain-v45.hex $ddir $tag &&
+	    copyit ArduCopter-vrbrain-v51.vrx $ddir $tag && 
+	    copyit ArduCopter-vrbrain-v51.bin $ddir $tag && 
+	    copyit ArduCopter-vrbrain-v51.hex $ddir $tag &&
+	    copyit ArduCopter-vrbrain-v51P.vrx $ddir $tag && 
+	    copyit ArduCopter-vrbrain-v51P.bin $ddir $tag && 
+	    copyit ArduCopter-vrbrain-v51P.hex $ddir $tag &&
+	    copyit ArduCopter-vrbrain-v51Pro.vrx $ddir $tag && 
+	    copyit ArduCopter-vrbrain-v51Pro.bin $ddir $tag && 
+	    copyit ArduCopter-vrbrain-v51Pro.hex $ddir $tag &&
+	    copyit ArduCopter-vrbrain-v51ProP.vrx $ddir $tag && 
+	    copyit ArduCopter-vrbrain-v51ProP.bin $ddir $tag && 
+	    copyit ArduCopter-vrbrain-v51ProP.hex $ddir $tag &&
+	    copyit ArduCopter-vrubrain-v51.vrx $ddir $tag && 
+	    copyit ArduCopter-vrubrain-v51.bin $ddir $tag && 
+	    copyit ArduCopter-vrubrain-v51.hex $ddir $tag && 
+	    copyit ArduCopter-vrubrain-v51P.vrx $ddir $tag && 
+	    copyit ArduCopter-vrubrain-v51P.bin $ddir $tag && 
+	    copyit ArduCopter-vrubrain-v51P.hex $ddir $tag
+	done
+    }
+    checkout "master"
+    popd
+}
+
+# build rover binaries
+build_rover() {
+    tag="$1"
+    echo "Building APMrover2 $tag binaries from $(pwd)"
+    pushd APMrover2
+    test -n "$VRBRAIN_ROOT" && {
+	echo "Building APMrover2 VRBRAIN binaries"
+	ddir=$binaries/Rover/$hdate/VRX
+        checkout $tag || {
+            checkout "master"
+            popd
+            return
+        }
+	skip_build $tag $ddir || {
+	    make vrbrain-clean &&
+	    make vrbrain || {
+                echo "Failed build of APMrover2 VRBRAIN $tag"
+                error_count=$((error_count+1))
+                checkout APMrover2 "latest" ""
+                popd
+                return
+            }
+	    copyit APMrover2-vrbrain-v45.vrx $ddir $tag && 
+	    copyit APMrover2-vrbrain-v45.bin $ddir $tag && 
+	    copyit APMrover2-vrbrain-v45.hex $ddir $tag &&
+	    copyit APMrover2-vrbrain-v51.vrx $ddir $tag && 
+	    copyit APMrover2-vrbrain-v51.bin $ddir $tag && 
+	    copyit APMrover2-vrbrain-v51.hex $ddir $tag &&
+	    copyit APMrover2-vrbrain-v51P.vrx $ddir $tag && 
+	    copyit APMrover2-vrbrain-v51P.bin $ddir $tag && 
+	    copyit APMrover2-vrbrain-v51P.hex $ddir $tag &&
+	    copyit APMrover2-vrbrain-v51Pro.vrx $ddir $tag && 
+	    copyit APMrover2-vrbrain-v51Pro.bin $ddir $tag && 
+	    copyit APMrover2-vrbrain-v51Pro.hex $ddir $tag &&
+	    copyit APMrover2-vrbrain-v51ProP.vrx $ddir $tag && 
+	    copyit APMrover2-vrbrain-v51ProP.bin $ddir $tag && 
+	    copyit APMrover2-vrbrain-v51ProP.hex $ddir $tag &&
+	    copyit APMrover2-vrubrain-v51.vrx $ddir $tag && 
+	    copyit APMrover2-vrubrain-v51.bin $ddir $tag && 
+	    copyit APMrover2-vrubrain-v51.hex $ddir $tag && 
+	    copyit APMrover2-vrubrain-v51P.vrx $ddir $tag && 
+	    copyit APMrover2-vrubrain-v51P.bin $ddir $tag && 
+	    copyit APMrover2-vrubrain-v51P.hex $ddir $tag
+	}
+    }
+    checkout "master"
+    popd
+}
+
+for build in for_merge for_merge-3.2 tone_alarm tone_alarm-3.2; do
+    build_arduplane $build
+    build_arducopter $build
+    build_rover $build
+done
+
+rm -rf $TMPDIR
+
+exit $error_count

Tools/scripts/install-prereqs-ubuntu.sh

@@ -6,6 +6,7 @@ OPT="/opt"
 
 BASE_PKGS="gawk make git arduino-core curl"
 SITL_PKGS="g++ python-pip python-matplotlib python-serial python-wxgtk2.8 python-scipy python-opencv python-numpy python-pyparsing ccache"
+AVR_PKGS="gcc-avr binutils-avr avr-libc"
 PYTHON_PKGS="pymavlink MAVProxy droneapi"
 PX4_PKGS="python-serial python-argparse openocd flex bison libncurses5-dev \
           autoconf texinfo build-essential libftdi-dev libtool zlib1g-dev \
@@ -15,9 +16,9 @@ ASSUME_YES=false
 
 # GNU Tools for ARM Embedded Processors
 # (see https://launchpad.net/gcc-arm-embedded/)
-ARM_ROOT="gcc-arm-none-eabi-4_8-2013q4"
-ARM_TARBALL="$ARM_ROOT-20131204-linux.tar.bz2"
-ARM_TARBALL_URL="https://launchpad.net/gcc-arm-embedded/4.8/4.8-2013-q4-major/+download/$ARM_TARBALL"
+ARM_ROOT="gcc-arm-none-eabi-4_7-2014q2"
+ARM_TARBALL="$ARM_ROOT-20140408-linux.tar.bz2"
+ARM_TARBALL_URL="http://firmware.diydrones.com/Tools/PX4-tools/$ARM_TARBALL"
 
 # Ardupilot Tools
 ARDUPILOT_TOOLS="ardupilot/Tools/autotest"
@@ -57,7 +58,7 @@ sudo usermod -a -G dialout $USER
 
 $APT_GET remove modemmanager
 $APT_GET update
-$APT_GET install $BASE_PKGS $SITL_PKGS $PX4_PKGS $UBUNTU64_PKGS
+$APT_GET install $BASE_PKGS $SITL_PKGS $PX4_PKGS $UBUNTU64_PKGS $AVR_PKGS
 sudo pip -q install $PYTHON_PKGS
 
 
@@ -69,6 +70,10 @@ if [ ! -d PX4NuttX ]; then
     git clone https://github.com/diydrones/PX4NuttX.git
 fi
 
+if [ ! -d uavcan ]; then
+    git clone https://github.com/diydrones/uavcan.git
+fi
+
 if [ ! -d VRNuttX ]; then
     git clone https://github.com/virtualrobotix/vrbrain_nuttx.git VRNuttX
 fi

libraries/AC_AttitudeControl/AC_AttitudeControl.cpp

@@ -3,8 +3,6 @@
 #include "AC_AttitudeControl.h"
 #include <AP_HAL.h>
 
-extern const AP_HAL::HAL& hal;
-
 // table of user settable parameters
 const AP_Param::GroupInfo AC_AttitudeControl::var_info[] PROGMEM = {
 
@@ -382,18 +380,19 @@ void AC_AttitudeControl::rate_bf_roll_pitch_yaw(float roll_rate_bf, float pitch_
     } else {
         _acro_angle_switch = 4500;
         integrate_bf_rate_error_to_angle_errors();
-        frame_conversion_bf_to_ef(_angle_bf_error, angle_ef_error);
-        _angle_ef_target.x = wrap_180_cd_float(angle_ef_error.x + _ahrs.roll_sensor);
-        _angle_ef_target.y = wrap_180_cd_float(angle_ef_error.y + _ahrs.pitch_sensor);
-        _angle_ef_target.z = wrap_360_cd_float(angle_ef_error.z + _ahrs.yaw_sensor);
+        if (frame_conversion_bf_to_ef(_angle_bf_error, angle_ef_error)) {
+            _angle_ef_target.x = wrap_180_cd_float(angle_ef_error.x + _ahrs.roll_sensor);
+            _angle_ef_target.y = wrap_180_cd_float(angle_ef_error.y + _ahrs.pitch_sensor);
+            _angle_ef_target.z = wrap_360_cd_float(angle_ef_error.z + _ahrs.yaw_sensor);
+        }
         if (_angle_ef_target.y > 9000.0f) {
             _angle_ef_target.x = wrap_180_cd_float(_angle_ef_target.x + 18000.0f);
-            _angle_ef_target.y = wrap_180_cd_float(18000.0f - _angle_ef_target.x);
+            _angle_ef_target.y = wrap_180_cd_float(18000.0f - _angle_ef_target.y);
             _angle_ef_target.z = wrap_360_cd_float(_angle_ef_target.z + 18000.0f);
         }
         if (_angle_ef_target.y < -9000.0f) {
             _angle_ef_target.x = wrap_180_cd_float(_angle_ef_target.x + 18000.0f);
-            _angle_ef_target.y = wrap_180_cd_float(-18000.0f - _angle_ef_target.x);
+            _angle_ef_target.y = wrap_180_cd_float(-18000.0f - _angle_ef_target.y);
             _angle_ef_target.z = wrap_360_cd_float(_angle_ef_target.z + 18000.0f);
         }
     }
@@ -432,12 +431,17 @@ void AC_AttitudeControl::frame_conversion_ef_to_bf(const Vector3f& ef_vector, Ve
 }
 
 // frame_conversion_bf_to_ef - converts body frame vector to earth frame vector
-void AC_AttitudeControl::frame_conversion_bf_to_ef(const Vector3f& bf_vector, Vector3f& ef_vector)
+bool AC_AttitudeControl::frame_conversion_bf_to_ef(const Vector3f& bf_vector, Vector3f& ef_vector)
 {
-    // convert earth frame rates to body frame rates
+    // avoid divide by zero
+    if (_ahrs.cos_pitch() == 0.0f) {
+        return false;
+    }
+    // convert earth frame angle or rates to body frame
     ef_vector.x = bf_vector.x + _ahrs.sin_roll() * (_ahrs.sin_pitch()/_ahrs.cos_pitch()) * bf_vector.y + _ahrs.cos_roll() * (_ahrs.sin_pitch()/_ahrs.cos_pitch()) * bf_vector.z;
     ef_vector.y = _ahrs.cos_roll()  * bf_vector.y - _ahrs.sin_roll() * bf_vector.z;
     ef_vector.z = (_ahrs.sin_roll() / _ahrs.cos_pitch()) * bf_vector.y + (_ahrs.cos_roll() / _ahrs.cos_pitch()) * bf_vector.z;
+    return true;
 }
 
 //
@@ -668,7 +672,6 @@ void AC_AttitudeControl::accel_limiting(bool enable_limits)
         _accel_rp_max = 0.0f;
         _accel_y_max = 0.0f;
     }
-    hal.console->printf_P(PSTR("AccLim:%d"),(int)enable_limits);
 }
 
 //

libraries/AC_AttitudeControl/AC_AttitudeControl.h

@@ -116,11 +116,12 @@ public:
     //
     // earth-frame <-> body-frame conversion functions
     //
-    // frame_conversion_ef_to_bf - converts earth frame rate targets to body frame rate targets
+    // frame_conversion_ef_to_bf - converts earth frame angles or rates to body frame
     void frame_conversion_ef_to_bf(const Vector3f& ef_vector, Vector3f &bf_vector);
 
-    // frame_conversion_bf_to_ef - converts body frame rate targets to earth frame rate targets
-    void frame_conversion_bf_to_ef(const Vector3f& bf_vector, Vector3f &ef_vector);
+    // frame_conversion_bf_to_ef - converts body frame angles or rates to earth frame
+    //  returns false if conversion fails due to gimbal lock
+    bool frame_conversion_bf_to_ef(const Vector3f& bf_vector, Vector3f &ef_vector);
 
     //
     // public accessor functions

libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp

@@ -10,7 +10,7 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] PROGMEM = {
     // @DisplayName: Angle Rate Roll-Pitch max
     // @Description: maximum rotation rate in roll/pitch axis requested by angle controller used in stabilize, loiter, rtl, auto flight modes
     // @Units: Centi-Degrees/Sec
-    // @Range: 90000 250000
+    // @Range: 9000 36000
     // @Increment: 500
     // @User: Advanced
     AP_GROUPINFO("RATE_RP_MAX", 0, AC_AttitudeControl_Heli, _angle_rate_rp_max, AC_ATTITUDE_CONTROL_RATE_RP_MAX_DEFAULT),
@@ -19,7 +19,7 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] PROGMEM = {
     // @DisplayName: Angle Rate Yaw max
     // @Description: maximum rotation rate in roll/pitch axis requested by angle controller used in stabilize, loiter, rtl, auto flight modes
     // @Units: Centi-Degrees/Sec
-    // @Range: 90000 250000
+    // @Range: 4500 18000
     // @Increment: 500
     // @User: Advanced
     AP_GROUPINFO("RATE_Y_MAX",  1, AC_AttitudeControl_Heli, _angle_rate_y_max, AC_ATTITUDE_CONTROL_RATE_Y_MAX_DEFAULT),
@@ -37,8 +37,9 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] PROGMEM = {
     // @DisplayName: Acceleration Max for Roll/Pitch
     // @Description: Maximum acceleration in roll/pitch axis
     // @Units: Centi-Degrees/Sec/Sec
-    // @Range: 20000 100000
-    // @Increment: 100
+    // @Range: 0 180000
+    // @Increment: 1000
+    // @Values: 0:Disabled, 72000:Slow, 108000:Medium, 162000:Fast
     // @User: Advanced
     AP_GROUPINFO("ACCEL_RP_MAX", 3, AC_AttitudeControl_Heli, _accel_rp_max, AC_ATTITUDE_CONTROL_ACCEL_RP_MAX_DEFAULT),
 
@@ -46,8 +47,9 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] PROGMEM = {
     // @DisplayName: Acceleration Max for Yaw
     // @Description: Maximum acceleration in yaw axis
     // @Units: Centi-Degrees/Sec/Sec
-    // @Range: 20000 100000
-    // @Increment: 100
+    // @Range: 0 72000
+    // @Values: 0:Disabled, 18000:Slow, 36000:Medium, 54000:Fast
+    // @Increment: 1000
     // @User: Advanced
     AP_GROUPINFO("ACCEL_Y_MAX",  4, AC_AttitudeControl_Heli, _accel_y_max, AC_ATTITUDE_CONTROL_ACCEL_Y_MAX_DEFAULT),
 
@@ -61,6 +63,65 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] PROGMEM = {
     AP_GROUPEND
 };
 
+// passthrough_bf_roll_pitch_rate_yaw - passthrough the pilots roll and pitch inputs directly to swashplate for flybar acro mode
+void AC_AttitudeControl_Heli::passthrough_bf_roll_pitch_rate_yaw(float roll_passthrough, float pitch_passthrough, float yaw_rate_bf)
+{
+    // store roll and pitch passthroughs
+    _passthrough_roll = roll_passthrough;
+    _passthrough_pitch = pitch_passthrough;
+
+    // set rate controller to use pass through
+    _flags_heli.flybar_passthrough = true;
+
+    // set bf rate targets to current body frame rates (i.e. relax and be ready for vehicle to switch out of acro)
+    _rate_bf_desired.x = _ahrs.get_gyro().x * AC_ATTITUDE_CONTROL_DEGX100;
+    _rate_bf_desired.y = _ahrs.get_gyro().y * AC_ATTITUDE_CONTROL_DEGX100;
+
+    // accel limit desired yaw rate
+    if (_accel_y_max > 0.0f) {
+        float rate_change_limit = _accel_y_max * _dt;
+        float rate_change = yaw_rate_bf - _rate_bf_desired.z;
+        rate_change = constrain_float(rate_change, -rate_change_limit, rate_change_limit);
+        _rate_bf_desired.z += rate_change;
+    } else {
+        _rate_bf_desired.z = yaw_rate_bf;
+    }
+
+    integrate_bf_rate_error_to_angle_errors();
+    _angle_bf_error.x = 0;
+    _angle_bf_error.y = 0;
+
+    // update our earth-frame angle targets
+    Vector3f angle_ef_error;
+    if (frame_conversion_bf_to_ef(_angle_bf_error, angle_ef_error)) {
+        _angle_ef_target.x = wrap_180_cd_float(angle_ef_error.x + _ahrs.roll_sensor);
+        _angle_ef_target.y = wrap_180_cd_float(angle_ef_error.y + _ahrs.pitch_sensor);
+        _angle_ef_target.z = wrap_360_cd_float(angle_ef_error.z + _ahrs.yaw_sensor);
+    }
+
+    // handle flipping over pitch axis
+    if (_angle_ef_target.y > 9000.0f) {
+        _angle_ef_target.x = wrap_180_cd_float(_angle_ef_target.x + 18000.0f);
+        _angle_ef_target.y = wrap_180_cd_float(18000.0f - _angle_ef_target.x);
+        _angle_ef_target.z = wrap_360_cd_float(_angle_ef_target.z + 18000.0f);
+    }
+    if (_angle_ef_target.y < -9000.0f) {
+        _angle_ef_target.x = wrap_180_cd_float(_angle_ef_target.x + 18000.0f);
+        _angle_ef_target.y = wrap_180_cd_float(-18000.0f - _angle_ef_target.x);
+        _angle_ef_target.z = wrap_360_cd_float(_angle_ef_target.z + 18000.0f);
+    }
+
+    // convert body-frame angle errors to body-frame rate targets
+    update_rate_bf_targets();
+
+    // set body-frame roll/pitch rate target to current desired rates which are the vehicle's actual rates
+    _rate_bf_target.x = _rate_bf_desired.x;
+    _rate_bf_target.y = _rate_bf_desired.y;
+
+    // add desired target to yaw
+    _rate_bf_target.z += _rate_bf_desired.z;
+}
+
 //
 // rate controller (body-frame) methods
 //
@@ -70,8 +131,13 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] PROGMEM = {
 void AC_AttitudeControl_Heli::rate_controller_run()
 {	
     // call rate controllers and send output to motors object
-    // To-Do: should the outputs from get_rate_roll, pitch, yaw be int16_t which is the input to the motors library?
-    rate_bf_to_motor_roll_pitch(_rate_bf_target.x, _rate_bf_target.y);
+    // if using a flybar passthrough roll and pitch directly to motors
+    if (_flags_heli.flybar_passthrough) {
+        _motors.set_roll(_passthrough_roll);
+        _motors.set_pitch(_passthrough_pitch);
+    } else {
+        rate_bf_to_motor_roll_pitch(_rate_bf_target.x, _rate_bf_target.y);
+    }
     _motors.set_yaw(rate_bf_to_motor_yaw(_rate_bf_target.z));
 }
 

libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h

@@ -26,11 +26,15 @@ public:
                         ) :
         AC_AttitudeControl(ahrs, aparm, motors,
                            p_angle_roll, p_angle_pitch, p_angle_yaw,
-                           pid_rate_roll, pid_rate_pitch, pid_rate_yaw)
+                           pid_rate_roll, pid_rate_pitch, pid_rate_yaw),
+        _passthrough_roll(0), _passthrough_pitch(0)
 		{
             AP_Param::setup_object_defaults(this, var_info);
 		}
 
+    // passthrough_bf_roll_pitch_rate_yaw - roll and pitch are passed through directly, body-frame rate target for yaw
+    void passthrough_bf_roll_pitch_rate_yaw(float roll_passthrough, float pitch_passthrough, float yaw_rate_bf);
+
 	// rate_controller_run - run lowest level body-frame rate controller and send outputs to the motors
 	// should be called at 100hz or more
 	virtual void rate_controller_run();
@@ -38,6 +42,9 @@ public:
 	// use_leaky_i - controls whether we use leaky i term for body-frame to motor output stage
 	void use_leaky_i(bool leaky_i) {  _flags_heli.leaky_i = leaky_i; }
     
+    // use_flybar_passthrough - controls whether we pass-through control inputs to swash-plate
+	void use_flybar_passthrough(bool passthrough) {  _flags_heli.flybar_passthrough = passthrough; }
+    
     void update_feedforward_filter_rates(float time_step);
 
     // user settable parameters
@@ -47,10 +54,11 @@ private:
 
     // To-Do: move these limits flags into the heli motors class
     struct AttControlHeliFlags {
-        uint8_t limit_roll      :   1;  // 1 if we have requested larger roll angle than swash can physically move
-        uint8_t limit_pitch     :   1;  // 1 if we have requested larger pitch angle than swash can physically move
-        uint8_t limit_yaw       :   1;  // 1 if we have requested larger yaw angle than tail servo can physically move
-        uint8_t leaky_i         :   1;  // 1 if we should use leaky i term for body-frame rate to motor stage
+        uint8_t limit_roll          :   1;  // 1 if we have requested larger roll angle than swash can physically move
+        uint8_t limit_pitch         :   1;  // 1 if we have requested larger pitch angle than swash can physically move
+        uint8_t limit_yaw           :   1;  // 1 if we have requested larger yaw angle than tail servo can physically move
+        uint8_t leaky_i             :   1;  // 1 if we should use leaky i term for body-frame rate to motor stage
+        uint8_t flybar_passthrough  :   1;  // 1 if we should pass through pilots roll & pitch input directly to swash-plate
     } _flags_heli;
 
     //
@@ -68,6 +76,7 @@ private:
     // get_angle_boost - calculate total body frame throttle required to produce the given earth frame throttle
     virtual int16_t get_angle_boost(int16_t throttle_pwm);
     
+    
     // LPF filters to act on Rate Feedforward terms to linearize output.
     // Due to complicated aerodynamic effects, feedforwards acting too fast can lead
     // to jerks on rate change requests.
@@ -75,6 +84,9 @@ private:
     LowPassFilterInt32 roll_feedforward_filter;
     LowPassFilterInt32 yaw_feedforward_filter;
 
+    // pass through for roll and pitch
+    int16_t _passthrough_roll;
+    int16_t _passthrough_pitch;
 };
 
 #endif //AC_ATTITUDECONTROL_HELI_H

libraries/AC_AttitudeControl/AC_PosControl.cpp

@@ -84,6 +84,10 @@ void AC_PosControl::set_dt(float delta_sec)
 
     // update rate controller's d filter
     _pid_alt_accel.set_d_lpf_alpha(POSCONTROL_ACCEL_Z_DTERM_FILTER, _dt);
+
+    // update rate z-axis velocity error and accel error filters
+    _vel_error_filter.set_cutoff_frequency(_dt,POSCONTROL_VEL_ERROR_CUTOFF_FREQ);
+    _accel_error_filter.set_cutoff_frequency(_dt,POSCONTROL_ACCEL_ERROR_CUTOFF_FREQ);
 }
 
 /// set_speed_z - sets maximum climb and descent rates
@@ -118,6 +122,8 @@ void AC_PosControl::set_accel_z(float accel_cmss)
 void AC_PosControl::set_alt_target_with_slew(float alt_cm, float dt)
 {
     float alt_change = alt_cm-_pos_target.z;
+    
+    _vel_desired.z = constrain_float(alt_change * dt, _speed_down_cms, _speed_up_cms);
 
     // adjust desired alt if motors have not hit their limits
     if ((alt_change<0 && !_motors.limit.throttle_lower) || (alt_change>0 && !_motors.limit.throttle_upper)) {
@@ -133,13 +139,21 @@ void AC_PosControl::set_alt_target_with_slew(float alt_cm, float dt)
 ///     should be called continuously (with dt set to be the expected time between calls)
 ///     actual position target will be moved no faster than the speed_down and speed_up
 ///     target will also be stopped if the motors hit their limits or leash length is exceeded
-void AC_PosControl::set_alt_target_from_climb_rate(float climb_rate_cms, float dt)
+void AC_PosControl::set_alt_target_from_climb_rate(float climb_rate_cms, float dt, bool force_descend)
 {
     // adjust desired alt if motors have not hit their limits
-    // To-Do: add check of _limit.pos_up and _limit.pos_down?
-    if ((climb_rate_cms<0 && !_motors.limit.throttle_lower) || (climb_rate_cms>0 && !_motors.limit.throttle_upper)) {
+    // To-Do: add check of _limit.pos_down?
+    if ((climb_rate_cms<0 && (!_motors.limit.throttle_lower || force_descend)) || (climb_rate_cms>0 && !_motors.limit.throttle_upper && !_limit.pos_up)) {
         _pos_target.z += climb_rate_cms * dt;
     }
+
+    // do not let target alt get above limit
+    if (_alt_max > 0 && _pos_target.z > _alt_max) {
+        _pos_target.z = _alt_max;
+        _limit.pos_up = true;
+    }
+
+    _vel_desired.z = climb_rate_cms;
 }
 
 // get_alt_error - returns altitude error in cm
@@ -249,12 +263,6 @@ void AC_PosControl::pos_to_rate_z()
     _limit.pos_up = false;
     _limit.pos_down = false;
 
-    // do not let target alt get above limit
-    if (_alt_max > 0 && _pos_target.z > _alt_max) {
-        _pos_target.z = _alt_max;
-        _limit.pos_up = true;
-    }
-
     // calculate altitude error
     _pos_error.z = _pos_target.z - curr_alt;
 
@@ -334,10 +342,12 @@ void AC_PosControl::rate_to_accel_z()
     if (_flags.reset_rate_to_accel_z) {
         // Reset Filter
         _vel_error.z = 0;
+        _vel_error_filter.reset(0);
         desired_accel = 0;
         _flags.reset_rate_to_accel_z = false;
     } else {
-        _vel_error.z = (_vel_target.z - curr_vel.z);
+        // calculate rate error and filter with cut off frequency of 2 Hz
+        _vel_error.z = _vel_error_filter.apply(_vel_target.z - curr_vel.z);
     }
 
     // calculate p
@@ -365,11 +375,11 @@ void AC_PosControl::accel_to_throttle(float accel_target_z)
     if (_flags.reset_accel_to_throttle) {
         // Reset Filter
         _accel_error.z = 0;
+        _accel_error_filter.reset(0);
         _flags.reset_accel_to_throttle = false;
     } else {
         // calculate accel error and Filter with fc = 2 Hz
-        // To-Do: replace constant below with one that is adjusted for update rate
-        _accel_error.z = _accel_error.z + 0.11164f * (constrain_float(accel_target_z - z_accel_meas, -32000, 32000) - _accel_error.z);
+        _accel_error.z = _accel_error_filter.apply(constrain_float(accel_target_z - z_accel_meas, -32000, 32000));
     }
 
     // separately calculate p, i, d values for logging
@@ -469,7 +479,7 @@ void AC_PosControl::get_stopping_point_xy(Vector3f &stopping_point) const
     float vel_total = pythagorous2(curr_vel.x, curr_vel.y);
 
     // avoid divide by zero by using current position if the velocity is below 10cm/s, kP is very low or acceleration is zero
-    if (kP <= 0.0f || _accel_cms <= 0.0f) {
+    if (kP <= 0.0f || _accel_cms <= 0.0f || vel_total == 0.0f) {
         stopping_point.x = curr_pos.x;
         stopping_point.y = curr_pos.y;
         return;
@@ -545,6 +555,7 @@ void AC_PosControl::update_xy_controller(bool use_desired_velocity)
     uint32_t now = hal.scheduler->millis();
     if ((now - _last_update_xy_ms) >= POSCONTROL_ACTIVE_TIMEOUT_MS) {
         init_xy_controller();
+        now = _last_update_xy_ms;
     }
 
     // check if xy leash needs to be recalculated

libraries/AC_AttitudeControl/AC_PosControl.h

@@ -40,6 +40,9 @@
 
 #define POSCONTROL_VEL_UPDATE_TIME              0.020f  // 50hz update rate on high speed CPUs (Pixhawk, Flymaple)
 
+#define POSCONTROL_VEL_ERROR_CUTOFF_FREQ        4.0     // 4hz low-pass filter on velocity error
+#define POSCONTROL_ACCEL_ERROR_CUTOFF_FREQ      2.0     // 2hz low-pass filter on accel error
+
 class AC_PosControl
 {
 public:
@@ -64,8 +67,8 @@ public:
     ///
 
     /// set_alt_max - sets maximum altitude above home in cm
-    /// set to zero to disable limit
-    /// To-Do: update this intermittantly from main code after checking if fence is enabled/disabled
+    ///   only enforced when set_alt_target_from_climb_rate is used
+    ///   set to zero to disable limit
     void set_alt_max(float alt) { _alt_max = alt; }
 
     /// set_speed_z - sets maximum climb and descent rates
@@ -106,7 +109,8 @@ public:
     ///     should be called continuously (with dt set to be the expected time between calls)
     ///     actual position target will be moved no faster than the speed_down and speed_up
     ///     target will also be stopped if the motors hit their limits or leash length is exceeded
-    void set_alt_target_from_climb_rate(float climb_rate_cms, float dt);
+    ///     set force_descend to true during landing to allow target to move low enough to slow the motors
+    void set_alt_target_from_climb_rate(float climb_rate_cms, float dt, bool force_descend = false);
 
     /// set_alt_target_to_current_alt - set altitude target to current altitude
     void set_alt_target_to_current_alt() { _pos_target.z = _inav.get_altitude(); }
@@ -367,6 +371,8 @@ private:
     float       _distance_to_target;    // distance to position target - for reporting only
     uint8_t     _xy_step;               // used to decide which portion of horizontal position controller to run during this iteration
     float       _dt_xy;                 // time difference in seconds between horizontal position updates
+    LowPassFilterFloat _vel_error_filter;   // low-pass-filter on z-axis velocity error
+    LowPassFilterFloat _accel_error_filter; // low-pass-filter on z-axis accelerometer error
 
     // velocity controller internal variables
     uint8_t     _vel_xyz_step;          // used to decide which portion of velocity controller to run during this iteration

libraries/AC_WPNav/AC_WPNav.cpp

@@ -176,6 +176,16 @@ void AC_WPNav::init_loiter_target()
     _pilot_accel_rgt_cms = 0;
 }
 
+/// loiter_soften_for_landing - reduce response for landing
+void AC_WPNav::loiter_soften_for_landing()
+{
+    const Vector3f& curr_pos = _inav.get_position();
+
+    // set target position to current position
+    _pos_control.set_xy_target(curr_pos.x, curr_pos.y);
+    _pos_control.freeze_ff_xy();
+}
+
 /// set_loiter_velocity - allows main code to pass the maximum velocity for loiter
 void AC_WPNav::set_loiter_velocity(float velocity_cms)
 {
@@ -331,7 +341,7 @@ void AC_WPNav::wp_and_spline_init()
 void AC_WPNav::set_speed_xy(float speed_cms)
 {
     // range check new target speed and update position controller
-    if (_wp_speed_cms >= WPNAV_WP_SPEED_MIN) {
+    if (speed_cms >= WPNAV_WP_SPEED_MIN) {
         _wp_speed_cms = speed_cms;
         _pos_control.set_speed_xy(_wp_speed_cms);
         // flag that wp leash must be recalculated
@@ -404,6 +414,33 @@ void AC_WPNav::set_wp_origin_and_destination(const Vector3f& origin, const Vecto
     _limited_speed_xy_cms = constrain_float(speed_along_track,0,_wp_speed_cms);
 }
 
+/// shift_wp_origin_to_current_pos - shifts the origin and destination so the origin starts at the current position
+///     used to reset the position just before takeoff
+///     relies on set_wp_destination or set_wp_origin_and_destination having been called first
+void AC_WPNav::shift_wp_origin_to_current_pos()
+{
+    // return immediately if vehicle is not at the origin
+    if (_track_desired > 0.0f) {
+        return;
+    }
+
+    // get current and target locations
+    const Vector3f curr_pos = _inav.get_position();
+    const Vector3f pos_target = _pos_control.get_pos_target();
+
+    // calculate difference between current position and target
+    Vector3f pos_diff = curr_pos - pos_target;
+
+    // shift origin and destination
+    _origin += pos_diff;
+    _destination += pos_diff;
+
+    // move pos controller target and disable feed forward
+    _pos_control.set_pos_target(curr_pos);
+    _pos_control.freeze_ff_xy();
+    _pos_control.freeze_ff_z();
+}
+
 /// get_wp_stopping_point_xy - returns vector to stopping point based on a horizontal position and velocity
 void AC_WPNav::get_wp_stopping_point_xy(Vector3f& stopping_point) const
 {
@@ -695,7 +732,7 @@ void AC_WPNav::set_spline_origin_and_destination(const Vector3f& origin, const V
             // before beginning it's spline path to the next waypoint. Note: we are using the previous segment's origin and destination
             _spline_origin_vel = (_destination - _origin);
             _spline_time = 0.0f;	// To-Do: this should be set based on how much overrun there was from straight segment?
-            _spline_vel_scaler = 0.0f;    // To-Do: this should be set based on speed at end of prev straight segment?
+            _spline_vel_scaler = _pos_control.get_vel_target().length();    // start velocity target from current target velocity
         }else{
             // previous segment is splined, vehicle will fly through origin
             // we can use the previous segment's destination velocity as this segment's origin velocity
@@ -707,7 +744,7 @@ void AC_WPNav::set_spline_origin_and_destination(const Vector3f& origin, const V
             }else{
                 _spline_time = 0.0f;
             }
-            _spline_vel_scaler = 0.0f;
+            // Note: we leave _spline_vel_scaler as it was from end of previous segment
         }
     }
 

libraries/AC_WPNav/AC_WPNav.h

@@ -69,6 +69,9 @@ public:
     /// init_loiter_target - initialize's loiter position and feed-forward velocity from current pos and velocity
     void init_loiter_target();
 
+    /// loiter_soften_for_landing - reduce response for landing
+    void loiter_soften_for_landing();
+
     /// set_loiter_velocity - allows main code to pass the maximum velocity for loiter
     void set_loiter_velocity(float velocity_cms);
 
@@ -132,6 +135,11 @@ public:
     /// set_wp_origin_and_destination - set origin and destination waypoints using position vectors (distance from home in cm)
     void set_wp_origin_and_destination(const Vector3f& origin, const Vector3f& destination);
 
+    /// shift_wp_origin_to_current_pos - shifts the origin and destination so the origin starts at the current position
+    ///     used to reset the position just before takeoff
+    ///     relies on set_wp_destination or set_wp_origin_and_destination having been called first
+    void shift_wp_origin_to_current_pos();
+
     /// get_wp_stopping_point_xy - calculates stopping point based on current position, velocity, waypoint acceleration
     ///		results placed in stopping_position vector
     void get_wp_stopping_point_xy(Vector3f& stopping_point) const;

libraries/AP_AHRS/AP_AHRS.cpp

@@ -247,3 +247,15 @@ void AP_AHRS::update_trig(void)
     _sin_pitch = -temp.c.x;
     _sin_roll = temp.c.y / _cos_pitch;
 }
+
+/*
+  update the centi-degree values
+ */
+void AP_AHRS::update_cd_values(void)
+{
+    roll_sensor  = degrees(roll) * 100;
+    pitch_sensor = degrees(pitch) * 100;
+    yaw_sensor   = degrees(yaw) * 100;
+    if (yaw_sensor < 0)
+        yaw_sensor += 36000;
+}

libraries/AP_AHRS/AP_AHRS.h

@@ -32,6 +32,8 @@
 #include <AP_Param.h>
 
 #define AP_AHRS_TRIM_LIMIT 10.0f        // maximum trim angle in degrees
+#define AP_AHRS_RP_P_MIN   0.05f        // minimum value for AHRS_RP_P parameter
+#define AP_AHRS_YAW_P_MIN  0.05f        // minimum value for AHRS_YAW_P parameter
 
 enum AHRS_VehicleClass {
     AHRS_VEHICLE_UNKNOWN,
@@ -179,6 +181,10 @@ public:
     // return the current estimate of the gyro drift
     virtual const Vector3f &get_gyro_drift(void) const = 0;
 
+    // reset the current gyro drift estimate
+    //  should be called if gyro offsets are recalculated
+    virtual void reset_gyro_drift(void) = 0;
+
     // reset the current attitude, used on new IMU calibration
     virtual void reset(bool recover_eulers=false) = 0;
 
@@ -338,6 +344,9 @@ public:
     // is the AHRS subsystem healthy?
     virtual bool healthy(void) = 0;
 
+    // true if the AHRS has completed initialisation
+    virtual bool initialised(void) const { return true; };
+
 protected:
     AHRS_VehicleClass _vehicle_class;
 
@@ -364,6 +373,9 @@ protected:
     //      should be called after _dcm_matrix is updated
     void update_trig(void);
 
+    // update roll_sensor, pitch_sensor and yaw_sensor
+    void update_cd_values(void);
+
     // pointer to compass object, if available
     Compass         * _compass;
 

libraries/AP_AHRS/AP_AHRS_DCM.cpp

@@ -36,6 +36,15 @@ extern const AP_HAL::HAL& hal;
 // http://gentlenav.googlecode.com/files/fastRotations.pdf
 #define SPIN_RATE_LIMIT 20
 
+// reset the current gyro drift estimate
+//  should be called if gyro offsets are recalculated
+void
+AP_AHRS_DCM::reset_gyro_drift(void)
+{
+    _omega_I.zero();
+    _omega_I_sum.zero();
+    _omega_I_sum_time = 0;
+}
 
 // run a full DCM update round
 void
@@ -143,7 +152,7 @@ AP_AHRS_DCM::check_matrix(void)
 {
     if (_dcm_matrix.is_nan()) {
         //Serial.printf("ERROR: DCM matrix NAN\n");
-        reset(true);
+        AP_AHRS_DCM::reset(true);
         return;
     }
     // some DCM matrix values can lead to an out of range error in
@@ -161,7 +170,7 @@ AP_AHRS_DCM::check_matrix(void)
             // in real trouble. All we can do is reset
             //Serial.printf("ERROR: DCM matrix error. _dcm_matrix.c.x=%f\n",
             //	   _dcm_matrix.c.x);
-            reset(true);
+            AP_AHRS_DCM::reset(true);
         }
     }
 }
@@ -242,7 +251,7 @@ AP_AHRS_DCM::normalize(void)
         // Our solution is blowing up and we will force back
         // to last euler angles
         _last_failure_ms = hal.scheduler->millis();
-        reset(true);
+        AP_AHRS_DCM::reset(true);
     }
 }
 
@@ -362,7 +371,7 @@ AP_AHRS_DCM::drift_correction_yaw(void)
     float yaw_error;
     float yaw_deltat;
 
-    if (use_compass()) {
+    if (AP_AHRS_DCM::use_compass()) {
         /*
           we are using compass for yaw
          */
@@ -442,6 +451,11 @@ AP_AHRS_DCM::drift_correction_yaw(void)
     // integration at higher rates
     float spin_rate = _omega.length();
 
+    // sanity check _kp_yaw
+    if (_kp_yaw < AP_AHRS_YAW_P_MIN) {
+        _kp_yaw = AP_AHRS_YAW_P_MIN;
+    }
+
     // update the proportional control to drag the
     // yaw back to the right value. We use a gain
     // that depends on the spin rate. See the fastRotations.pdf
@@ -684,7 +698,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
     // reduce the impact of the gps/accelerometers on yaw when we are
     // flat, but still allow for yaw correction using the
     // accelerometers at high roll angles as long as we have a GPS
-    if (use_compass()) {
+    if (AP_AHRS_DCM::use_compass()) {
         if (have_gps() && gps_gain == 1.0f) {
             error[besti].z *= sinf(fabsf(roll));
         } else {
@@ -715,6 +729,11 @@ AP_AHRS_DCM::drift_correction(float deltat)
     // base the P gain on the spin rate
     float spin_rate = _omega.length();
 
+    // sanity check _kp value
+    if (_kp < AP_AHRS_RP_P_MIN) {
+        _kp = AP_AHRS_RP_P_MIN;
+    }
+
     // we now want to calculate _omega_P and _omega_I. The
     // _omega_P value is what drags us quickly to the
     // accelerometer reading.
@@ -837,12 +856,7 @@ AP_AHRS_DCM::euler_angles(void)
     _body_dcm_matrix.rotateXYinv(_trim);
     _body_dcm_matrix.to_euler(&roll, &pitch, &yaw);
 
-    roll_sensor     = degrees(roll)  * 100;
-    pitch_sensor    = degrees(pitch) * 100;
-    yaw_sensor      = degrees(yaw)   * 100;
-
-    if (yaw_sensor < 0)
-        yaw_sensor += 36000;
+    update_cd_values();
 }
 
 /* reporting of DCM state for MAVLink */

libraries/AP_AHRS/AP_AHRS_DCM.h

@@ -75,6 +75,10 @@ public:
         return _omega_I;
     }
 
+    // reset the current gyro drift estimate
+    //  should be called if gyro offsets are recalculated
+    void reset_gyro_drift(void);
+
     // Methods
     void            update(void);
     void            reset(bool recover_eulers = false);

libraries/AP_AHRS/AP_AHRS_NavEKF.cpp

@@ -50,16 +50,35 @@ const Vector3f &AP_AHRS_NavEKF::get_gyro_drift(void) const
     return _gyro_bias;
 }
 
+// reset the current gyro drift estimate
+//  should be called if gyro offsets are recalculated
+void AP_AHRS_NavEKF::reset_gyro_drift(void)
+{
+    // update DCM
+    AP_AHRS_DCM::reset_gyro_drift();
+
+    // reset the EKF gyro bias states
+    EKF.resetGyroBias();
+}
+
 void AP_AHRS_NavEKF::update(void)
 {
+    // we need to restore the old DCM attitude values as these are
+    // used internally in DCM to calculate error values for gyro drift
+    // correction
+    roll = _dcm_attitude.x;
+    pitch = _dcm_attitude.y;
+    yaw = _dcm_attitude.z;
+    update_cd_values();
+
     AP_AHRS_DCM::update();
 
     // keep DCM attitude available for get_secondary_attitude()
     _dcm_attitude(roll, pitch, yaw);
 
     if (!ekf_started) {
-        // if we have a GPS lock we can start the EKF
-        if (get_gps().status() >= AP_GPS::GPS_OK_FIX_3D) {
+        // if we have a GPS lock and more than 6 satellites, we can start the EKF
+        if (get_gps().status() >= AP_GPS::GPS_OK_FIX_3D && get_gps().num_sats() >= _gps_minsats) {
             if (start_time_ms == 0) {
                 start_time_ms = hal.scheduler->millis();
             }
@@ -78,11 +97,8 @@ void AP_AHRS_NavEKF::update(void)
             roll  = eulers.x;
             pitch = eulers.y;
             yaw   = eulers.z;
-            roll_sensor  = degrees(roll) * 100;
-            pitch_sensor = degrees(pitch) * 100;
-            yaw_sensor   = degrees(yaw) * 100;
-            if (yaw_sensor < 0)
-                yaw_sensor += 36000;
+
+            update_cd_values();
             update_trig();
 
             // keep _gyro_bias for get_gyro_drift()
@@ -166,6 +182,9 @@ bool AP_AHRS_NavEKF::airspeed_estimate(float *airspeed_ret) const
 // true if compass is being used
 bool AP_AHRS_NavEKF::use_compass(void)
 {
+    if (using_EKF()) {
+        return EKF.use_compass();
+    }
     return AP_AHRS_DCM::use_compass();
 }
 
@@ -263,5 +282,12 @@ bool AP_AHRS_NavEKF::healthy(void)
     return AP_AHRS_DCM::healthy();    
 }
 
+// true if the AHRS has completed initialisation
+bool AP_AHRS_NavEKF::initialised(void) const
+{
+    // initialisation complete 10sec after ekf has started
+    return (ekf_started && (hal.scheduler->millis() - start_time_ms > AP_AHRS_NAVEKF_SETTLE_TIME_MS));
+};
+
 #endif // AP_AHRS_NAVEKF_AVAILABLE
 

libraries/AP_AHRS/AP_AHRS_NavEKF.h

@@ -28,6 +28,7 @@
 #include <AP_NavEKF.h>
 
 #define AP_AHRS_NAVEKF_AVAILABLE 1
+#define AP_AHRS_NAVEKF_SETTLE_TIME_MS 20000     // time in milliseconds the ekf needs to settle after being started
 
 class AP_AHRS_NavEKF : public AP_AHRS_DCM
 {
@@ -48,6 +49,10 @@ public:
     // return the current drift correction integrator value
     const Vector3f &get_gyro_drift(void) const;
 
+    // reset the current gyro drift estimate
+    //  should be called if gyro offsets are recalculated
+    void reset_gyro_drift(void);
+
     void            update(void);
     void            reset(bool recover_eulers = false);
 
@@ -95,6 +100,9 @@ public:
     // is the AHRS subsystem healthy?
     bool healthy(void);
 
+    // true if the AHRS has completed initialisation
+    bool initialised(void) const;
+
 private:
     bool using_EKF(void) const;
 

libraries/AP_Airspeed/AP_Airspeed.cpp

@@ -60,7 +60,11 @@ extern const AP_HAL::HAL& hal;
 #elif defined(CONFIG_ARCH_BOARD_VRBRAIN_V51)
  #define ARSPD_DEFAULT_PIN 0
 #elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V51)
+#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
+ #define ARSPD_DEFAULT_PIN 1
+#else
  #define ARSPD_DEFAULT_PIN 0
+#endif
 #elif defined(CONFIG_ARCH_BOARD_VRHERO_V10)
  #define ARSPD_DEFAULT_PIN 0
 #endif

libraries/AP_Compass/AP_Compass_VRBRAIN.cpp

@@ -57,10 +57,8 @@ bool AP_Compass_VRBRAIN::init(void)
 	}
 
     for (uint8_t i=0; i<_num_instances; i++) {
-#ifdef DEVIOCGDEVICEID
         // get device id
         _dev_id[i] = ioctl(_mag_fd[i], DEVIOCGDEVICEID, 0);
-#endif
 
         // average over up to 20 samples
         if (ioctl(_mag_fd[i], SENSORIOCSQUEUEDEPTH, 20) != 0) {
@@ -70,6 +68,11 @@ bool AP_Compass_VRBRAIN::init(void)
 
         // remember if the compass is external
         _is_external[i] = (ioctl(_mag_fd[i], MAGIOCGEXTERNAL, 0) > 0);
+#if defined(CONFIG_ARCH_BOARD_VRBRAIN_V45)
+		//deal with situations where user has cut internal mag on VRBRAIN 4.5 
+		//and uses only one external mag attached to the internal I2C bus
+		_is_external[i] = _external.load() ? _external.get() : _is_external[i];
+#endif
         if (_is_external[i]) {
             hal.console->printf("Using external compass[%u]\n", (unsigned)i);
         }
@@ -108,9 +111,10 @@ bool AP_Compass_VRBRAIN::read(void)
         // a noop on most boards
         _sum[i].rotate(MAG_BOARD_ORIENTATION);
 
+#if !defined(CONFIG_ARCH_BOARD_VRBRAIN_V45)
         // override any user setting of COMPASS_EXTERNAL 
         _external.set(_is_external[0]);
-
+#endif
         if (_is_external[i]) {
             // add user selectable orientation
             _sum[i].rotate((enum Rotation)_orientation.get());

libraries/AP_Compass/Compass.cpp

@@ -120,13 +120,13 @@ const AP_Param::GroupInfo Compass::var_info[] PROGMEM = {
     // @DisplayName: Compass device id
     // @Description: Compass device id.  Automatically detected, do not set manually
     // @User: Advanced
-    AP_GROUPINFO("DEV_ID",  15, Compass, _dev_id[0], COMPASS_EXPECTED_DEV_ID),
+    AP_GROUPINFO("DEV_ID",  15, Compass, _dev_id[0], 0),
 
     // @Param: DEV_ID2
     // @DisplayName: Compass2 device id
     // @Description: Second compass's device id.  Automatically detected, do not set manually
     // @User: Advanced
-    AP_GROUPINFO("DEV_ID2", 16, Compass, _dev_id[1], COMPASS_EXPECTED_DEV_ID2),
+    AP_GROUPINFO("DEV_ID2", 16, Compass, _dev_id[1], 0),
 #endif
 
 #if COMPASS_MAX_INSTANCES > 2
@@ -134,7 +134,7 @@ const AP_Param::GroupInfo Compass::var_info[] PROGMEM = {
     // @DisplayName: Compass3 device id
     // @Description: Third compass's device id.  Automatically detected, do not set manually
     // @User: Advanced
-    AP_GROUPINFO("DEV_ID3", 17, Compass, _dev_id[2], COMPASS_EXPECTED_DEV_ID3),
+    AP_GROUPINFO("DEV_ID3", 17, Compass, _dev_id[2], 0),
 #endif
 
     AP_GROUPEND
@@ -169,6 +169,15 @@ Compass::init()
     return true;
 }
 
+void
+Compass::set_offsets(uint8_t i, const Vector3f &offsets)
+{
+    // sanity check compass instance provided
+    if (i < COMPASS_MAX_INSTANCES) {
+        _offset[i].set(offsets);
+    }
+}
+
 void
 Compass::set_and_save_offsets(uint8_t i, const Vector3f &offsets)
 {

libraries/AP_Compass/Compass.h

@@ -52,25 +52,6 @@
 #define COMPASS_MAX_INSTANCES 1
 #endif
 
-// default compass device ids for each board type to most common set-up to reduce eeprom usage
-#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
-# define COMPASS_EXPECTED_DEV_ID  73225 // external hmc5883
-# define COMPASS_EXPECTED_DEV_ID2 -1    // internal ldm303d
-# define COMPASS_EXPECTED_DEV_ID3 0
-#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX
-# define COMPASS_EXPECTED_DEV_ID  0
-# define COMPASS_EXPECTED_DEV_ID2 0
-# define COMPASS_EXPECTED_DEV_ID3 0
-#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
-# define COMPASS_EXPECTED_DEV_ID  0
-# define COMPASS_EXPECTED_DEV_ID2 0
-# define COMPASS_EXPECTED_DEV_ID3 0
-#else
-# define COMPASS_EXPECTED_DEV_ID  0
-# define COMPASS_EXPECTED_DEV_ID2 0
-# define COMPASS_EXPECTED_DEV_ID3 0
-#endif
-
 class Compass
 {
 public:
@@ -106,6 +87,13 @@ public:
     ///
     float calculate_heading(const Matrix3f &dcm_matrix) const;
 
+    /// Sets offset x/y/z values.
+    ///
+    /// @param  i                   compass instance
+    /// @param  offsets             Offsets to the raw mag_ values.
+    ///
+    void set_offsets(uint8_t i, const Vector3f &offsets);
+
     /// Sets and saves the compass offset x/y/z values.
     ///
     /// @param  i                   compass instance

libraries/AP_GPS/AP_GPS.cpp

@@ -72,8 +72,8 @@ void AP_GPS::init(DataFlash_Class *dataflash)
 {
     _DataFlash = dataflash;
     hal.uartB->begin(38400UL, 256, 16);
-#if GPS_MAX_INSTANCES > 1
     primary_instance = 0;
+#if GPS_MAX_INSTANCES > 1
     if (hal.uartE != NULL) {
         hal.uartE->begin(38400UL, 256, 16);        
     }
@@ -334,9 +334,6 @@ AP_GPS::update(void)
         update_instance(i);
     }
 
-    // update notify with gps status. We always base this on the first GPS
-    AP_Notify::flags.gps_status = state[0].status;
-
 #if GPS_MAX_INSTANCES > 1
     // work out which GPS is the primary, and how many sensors we have
     for (uint8_t i=0; i<GPS_MAX_INSTANCES; i++) {
@@ -368,6 +365,8 @@ AP_GPS::update(void)
 #else
     num_instances = 1;
 #endif // GPS_MAX_INSTANCES
+	// update notify with gps status. We always base this on the primary_instance
+    AP_Notify::flags.gps_status = state[primary_instance].status;
 }
 
 /*

libraries/AP_GPS/AP_GPS_SIRF.h

@@ -25,7 +25,7 @@
 #include <AP_Common.h>
 #include <AP_GPS.h>
 
-#define SIRF_SET_BINARY "$PSRF100,0,38400,8,1,0*3C"
+#define SIRF_SET_BINARY "$PSRF100,0,38400,8,1,0*3C\r\n"
 
 class AP_GPS_SIRF : public AP_GPS_Backend {
 public:

libraries/AP_GPS/AP_GPS_UBLOX.cpp

@@ -44,8 +44,6 @@ extern const AP_HAL::HAL& hal;
 #define UBLOX_HW_LOGGING 0
 #endif
 
-bool AP_GPS_UBLOX::logging_started = false;
-
 AP_GPS_UBLOX::AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) :
     AP_GPS_Backend(_gps, _state, _port),
     _step(0),
@@ -249,53 +247,13 @@ AP_GPS_UBLOX::read(void)
 // Private Methods /////////////////////////////////////////////////////////////
 #if UBLOX_HW_LOGGING
 
-#define LOG_MSG_UBX1 200
-#define LOG_MSG_UBX2 201
-
-struct PACKED log_Ubx1 {
-    LOG_PACKET_HEADER;
-    uint32_t timestamp;
-    uint8_t  instance;
-    uint16_t noisePerMS;
-    uint8_t  jamInd;
-    uint8_t  aPower;
-};
-
-struct PACKED log_Ubx2 {
-    LOG_PACKET_HEADER;
-    uint32_t timestamp;
-    uint8_t  instance;
-    int8_t   ofsI;
-    uint8_t  magI;
-    int8_t   ofsQ;
-    uint8_t  magQ;
-};
-
-static const struct LogStructure ubx_log_structures[] PROGMEM = {
-    { LOG_MSG_UBX1, sizeof(log_Ubx1),
-      "UBX1", "IBHBB",  "TimeMS,Instance,noisePerMS,jamInd,aPower" },
-    { LOG_MSG_UBX2, sizeof(log_Ubx2),
-      "UBX2", "IBbBbB", "TimeMS,Instance,ofsI,magI,ofsQ,magQ" }
-};
-
-void AP_GPS_UBLOX::write_logging_headers(void)
-{
-    if (!logging_started) {
-        logging_started = true;
-        gps._DataFlash->AddLogFormats(ubx_log_structures, 2);
-    }
-}
-
 void AP_GPS_UBLOX::log_mon_hw(void)
 {
     if (gps._DataFlash == NULL || !gps._DataFlash->logging_started()) {
         return;
     }
-    // log mon_hw message
-    write_logging_headers();
-
     struct log_Ubx1 pkt = {
-        LOG_PACKET_HEADER_INIT(LOG_MSG_UBX1),
+        LOG_PACKET_HEADER_INIT(LOG_UBX1_MSG),
         timestamp  : hal.scheduler->millis(),
         instance   : state.instance,
         noisePerMS : _buffer.mon_hw_60.noisePerMS,
@@ -315,11 +273,9 @@ void AP_GPS_UBLOX::log_mon_hw2(void)
     if (gps._DataFlash == NULL || !gps._DataFlash->logging_started()) {
         return;
     }
-    // log mon_hw message
-    write_logging_headers();
 
     struct log_Ubx2 pkt = {
-        LOG_PACKET_HEADER_INIT(LOG_MSG_UBX2),
+        LOG_PACKET_HEADER_INIT(LOG_UBX2_MSG),
         timestamp : hal.scheduler->millis(),
         instance  : state.instance,
         ofsI      : _buffer.mon_hw2.ofsI,

libraries/AP_GPS/AP_GPS_UBLOX.h

@@ -256,9 +256,6 @@ private:
     bool            _new_speed:1;
     bool            need_rate_update:1;
 
-    // have we written the logging headers to DataFlash?
-    static bool     logging_started;
-
     uint8_t         _disable_counter;
 
     // Buffer parse & GPS state update

libraries/AP_HAL/AP_HAL_Boards.h

@@ -215,13 +215,15 @@
 #define HAL_BOARD_NAME "VRBRAIN"
 #define HAL_CPU_CLASS HAL_CPU_CLASS_150
 #define HAL_OS_POSIX_IO 1
-#define HAL_STORAGE_SIZE            4096
+#define HAL_STORAGE_SIZE            8192
 #define HAL_STORAGE_SIZE_AVAILABLE  HAL_STORAGE_SIZE
 #define HAL_BOARD_LOG_DIRECTORY "/fs/microsd/APM/LOGS"
+#define HAL_BOARD_TERRAIN_DIRECTORY "/fs/microsd/APM/TERRAIN"
 #define HAL_INS_DEFAULT HAL_INS_VRBRAIN
 #define HAL_BARO_DEFAULT HAL_BARO_VRBRAIN
 #define HAL_COMPASS_DEFAULT HAL_COMPASS_VRBRAIN
 #define HAL_SERIAL0_BAUD_DEFAULT 115200
+#define CONFIG_HAL_BOARD_SUBTYPE HAL_BOARD_SUBTYPE_NONE
 
 #else
 #error "Unknown CONFIG_HAL_BOARD type"

libraries/AP_HAL/Scheduler.h

@@ -3,6 +3,7 @@
 #define __AP_HAL_SCHEDULER_H__
 
 #include "AP_HAL_Namespace.h"
+#include "AP_HAL_Boards.h"
 
 #include <stdint.h>
 #include <AP_Progmem.h>
@@ -14,6 +15,11 @@ public:
     virtual void     delay(uint16_t ms) = 0;
     virtual uint32_t millis() = 0;
     virtual uint32_t micros() = 0;
+#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150
+    // offer non-wrapping 64 bit versions on faster CPUs
+    virtual uint64_t millis64() = 0;
+    virtual uint64_t micros64() = 0;
+#endif
     virtual void     delay_microseconds(uint16_t us) = 0;
     virtual void     register_delay_callback(AP_HAL::Proc,
                                              uint16_t min_time_ms) = 0;

libraries/AP_HAL_AVR_SITL/SITL_State.cpp

@@ -65,7 +65,7 @@ uint16_t SITL_State::current_pin_value;
 float SITL_State::_current;
 
 AP_Baro_HIL *SITL_State::_barometer;
-AP_InertialSensor_HIL *SITL_State::_ins;
+AP_InertialSensor *SITL_State::_ins;
 SITLScheduler *SITL_State::_scheduler;
 AP_Compass_HIL *SITL_State::_compass;
 
@@ -212,7 +212,7 @@ void SITL_State::_sitl_setup(void)
 	// find the barometer object if it exists
 	_sitl = (SITL *)AP_Param::find_object("SIM_");
 	_barometer = (AP_Baro_HIL *)AP_Param::find_object("GND_");
-	_ins = (AP_InertialSensor_HIL *)AP_Param::find_object("INS_");
+	_ins = (AP_InertialSensor *)AP_Param::find_object("INS_");
 	_compass = (AP_Compass_HIL *)AP_Param::find_object("COMPASS_");
 
     if (_sitl != NULL) {

libraries/AP_HAL_AVR_SITL/SITL_State.h

@@ -121,7 +121,7 @@ private:
     static bool _motors_on;
 
     static AP_Baro_HIL *_barometer;
-    static AP_InertialSensor_HIL *_ins;
+    static AP_InertialSensor *_ins;
     static SITLScheduler *_scheduler;
     static AP_Compass_HIL *_compass;
 

libraries/AP_HAL_AVR_SITL/Scheduler.cpp

@@ -70,52 +70,64 @@ double SITLScheduler::_cyg_sec()
 }
 #endif
 
-uint32_t SITLScheduler::_micros() 
+uint64_t SITLScheduler::_micros64() 
 {
 #ifdef __CYGWIN__
-	return (uint32_t)(_cyg_sec() * 1.0e6);
+	return (uint64_t)(_cyg_sec() * 1.0e6);
 #else   
 	struct timeval tp;
 	gettimeofday(&tp,NULL);
-	return 1.0e6*((tp.tv_sec + (tp.tv_usec*1.0e-6)) - 
+	uint64_t ret = 1.0e6*((tp.tv_sec + (tp.tv_usec*1.0e-6)) - 
 		      (_sketch_start_time.tv_sec +
 		       (_sketch_start_time.tv_usec*1.0e-6)));
+    return ret;
 #endif
 }
 
+uint64_t SITLScheduler::micros64() 
+{
+    return _micros64();
+}
+
 uint32_t SITLScheduler::micros() 
 {
-    return _micros();
+    return micros64() & 0xFFFFFFFF;
+}
+
+uint64_t SITLScheduler::millis64() 
+{
+#ifdef __CYGWIN__
+	// 1000 ms in a second
+	return (uint64_t)(_cyg_sec() * 1000);
+#else
+	struct timeval tp;
+	gettimeofday(&tp,NULL);
+	uint64_t ret = 1.0e3*((tp.tv_sec + (tp.tv_usec*1.0e-6)) - 
+		      (_sketch_start_time.tv_sec +
+		       (_sketch_start_time.tv_usec*1.0e-6)));
+    return ret;
+#endif
 }
 
 uint32_t SITLScheduler::millis() 
 {
-#ifdef __CYGWIN__
-	// 1000 ms in a second
-	return (uint32_t)(_cyg_sec() * 1000);
-#else
-	struct timeval tp;
-	gettimeofday(&tp,NULL);
-	return 1.0e3*((tp.tv_sec + (tp.tv_usec*1.0e-6)) - 
-		      (_sketch_start_time.tv_sec +
-		       (_sketch_start_time.tv_usec*1.0e-6)));
-#endif
+    return millis64() & 0xFFFFFFFF;
 }
 
 void SITLScheduler::delay_microseconds(uint16_t usec) 
 {
-	uint32_t start = micros();
-	while (micros() - start < usec) {
-		usleep(usec - (micros() - start));
+	uint64_t start = micros64();
+	while (micros64() - start < usec) {
+		usleep(usec - (micros64() - start));
 	}
 }
 
 void SITLScheduler::delay(uint16_t ms)
 {
-	uint32_t start = micros();
+	uint64_t start = micros64();
     
     while (ms > 0) {
-        while ((micros() - start) >= 1000) {
+        while ((micros64() - start) >= 1000) {
             ms--;
             if (ms == 0) break;
             start += 1000;

libraries/AP_HAL_AVR_SITL/Scheduler.h

@@ -19,6 +19,8 @@ public:
     void     delay(uint16_t ms);
     uint32_t millis();
     uint32_t micros();
+    uint64_t millis64();
+    uint64_t micros64();
     void     delay_microseconds(uint16_t us);
     void     register_delay_callback(AP_HAL::Proc, uint16_t min_time_ms);
 
@@ -43,7 +45,7 @@ public:
     void     sitl_end_atomic();
 
     // callable from interrupt handler
-    static uint32_t _micros();
+    static uint64_t _micros64();
     static void timer_event() { _run_timer_procs(true); _run_io_procs(true); }
 
 private:

libraries/AP_HAL_AVR_SITL/sitl_ins.cpp

@@ -52,7 +52,7 @@ float SITL_State::_gyro_drift(void)
 		return 0;
 	}
 	double period  = _sitl->drift_time * 2;
-	double minutes = fmod(_scheduler->_micros() / 60.0e6, period);
+	double minutes = fmod(_scheduler->micros64() / 60.0e6, period);
 	if (minutes < period/2) {
 		return minutes * ToRad(_sitl->drift_speed);
 	}

libraries/AP_HAL_Empty/Scheduler.cpp

@@ -14,14 +14,22 @@ void EmptyScheduler::init(void* machtnichts)
 void EmptyScheduler::delay(uint16_t ms)
 {}
 
-uint32_t EmptyScheduler::millis() {
+uint64_t EmptyScheduler::millis64() {
     return 10000;
 }
 
-uint32_t EmptyScheduler::micros() {
+uint64_t EmptyScheduler::micros64() {
     return 200000;
 }
 
+uint32_t EmptyScheduler::millis() {
+    return millis64();
+}
+
+uint32_t EmptyScheduler::micros() {
+    return micros64();
+}
+
 void EmptyScheduler::delay_microseconds(uint16_t us)
 {}
 

libraries/AP_HAL_Empty/Scheduler.h

@@ -11,6 +11,8 @@ public:
     void     delay(uint16_t ms);
     uint32_t millis();
     uint32_t micros();
+    uint64_t millis64();
+    uint64_t micros64();
     void     delay_microseconds(uint16_t us);
     void     register_delay_callback(AP_HAL::Proc,
                 uint16_t min_time_ms);

libraries/AP_HAL_PX4/Scheduler.cpp

@@ -76,6 +76,16 @@ void PX4Scheduler::init(void *unused)
 	pthread_create(&_io_thread_ctx, &thread_attr, (pthread_startroutine_t)&PX4::PX4Scheduler::_io_thread, this);
 }
 
+uint64_t PX4Scheduler::micros64() 
+{
+    return hrt_absolute_time();
+}
+
+uint64_t PX4Scheduler::millis64() 
+{
+    return micros64() / 1000;
+}
+
 uint32_t PX4Scheduler::micros() 
 {
     return (uint32_t)(hrt_absolute_time() - _sketch_start_time);
@@ -83,7 +93,7 @@ uint32_t PX4Scheduler::micros()
 
 uint32_t PX4Scheduler::millis() 
 {
-    return hrt_absolute_time() / 1000;
+    return millis64() & 0xFFFFFFFF;
 }
 
 /**
@@ -106,9 +116,9 @@ void PX4Scheduler::delay_microseconds(uint16_t usec)
         perf_end(_perf_delay);
         return;
     }
-	uint32_t start = micros();
-    uint32_t dt;
-	while ((dt=(micros() - start)) < usec) {
+	uint64_t start = micros64();
+    uint64_t dt;
+	while ((dt=(micros64() - start)) < usec) {
 		up_udelay(usec - dt);
 	}
     perf_end(_perf_delay);
@@ -121,9 +131,9 @@ void PX4Scheduler::delay(uint16_t ms)
         return;
     }
     perf_begin(_perf_delay);
-	uint64_t start = hrt_absolute_time();
+	uint64_t start = micros64();
     
-    while ((hrt_absolute_time() - start)/1000 < ms && 
+    while ((micros64() - start)/1000 < ms && 
            !_px4_thread_should_exit) {
         delay_microseconds_semaphore(1000);
         if (_min_delay_cb_ms <= ms) {

libraries/AP_HAL_PX4/Scheduler.h

@@ -29,6 +29,8 @@ public:
     void     delay(uint16_t ms);
     uint32_t millis();
     uint32_t micros();
+    uint64_t millis64();
+    uint64_t micros64();
     void     delay_microseconds(uint16_t us);
     void     register_delay_callback(AP_HAL::Proc, uint16_t min_time_ms);
     void     register_timer_process(AP_HAL::MemberProc);

libraries/AP_HAL_PX4/Storage.cpp

@@ -176,12 +176,12 @@ void PX4Storage::_storage_open(void)
  */
 void PX4Storage::_mark_dirty(uint16_t loc, uint16_t length)
 {
-	uint16_t end = loc + length;
-	while (loc < end) {
-		uint8_t line = (loc >> PX4_STORAGE_LINE_SHIFT);
-		_dirty_mask |= 1 << line;
-		loc += PX4_STORAGE_LINE_SIZE;
-	}
+    uint16_t end = loc + length;
+    for (uint8_t line=loc>>PX4_STORAGE_LINE_SHIFT;
+         line <= end>>PX4_STORAGE_LINE_SHIFT;
+         line++) {
+        _dirty_mask |= 1U << line;
+    }
 }
 
 void PX4Storage::read_block(void *dst, uint16_t loc, size_t n) 
@@ -261,13 +261,6 @@ void PX4Storage::_timer_tick(void)
 			_fd = -1;
 			perf_count(_perf_errors);
 		}
-		if (_dirty_mask == 0) {
-			if (fsync(_fd) != 0) {
-				close(_fd);
-				_fd = -1;
-				perf_count(_perf_errors);
-			}
-		}
 	}
 	perf_end(_perf_storage);
 }

libraries/AP_HAL_PX4/UARTDriver.cpp

@@ -503,7 +503,7 @@ void PX4UARTDriver::_timer_tick(void)
             // split into two writes
             uint16_t n1 = _writebuf_size - _writebuf_head;
             int ret = _write_fd(&_writebuf[_writebuf_head], n1);
-            if (ret == n1 && n != n1) {
+            if (ret == n1 && n > n1) {
                 _write_fd(&_writebuf[_writebuf_head], n - n1);                
             }
         }
@@ -523,7 +523,7 @@ void PX4UARTDriver::_timer_tick(void)
             uint16_t n1 = _readbuf_size - _readbuf_tail;
             assert(_readbuf_tail+n1 <= _readbuf_size);
             int ret = _read_fd(&_readbuf[_readbuf_tail], n1);
-            if (ret == n1 && n != n1) {
+            if (ret == n1 && n > n1) {
                 assert(_readbuf_tail+(n-n1) <= _readbuf_size);
                 _read_fd(&_readbuf[_readbuf_tail], n - n1);                
             }

libraries/AP_HAL_VRBRAIN/AnalogIn.cpp

@@ -18,6 +18,7 @@
 #include <uORB/topics/system_power.h>
 #include <GCS_MAVLink.h>
 #include <errno.h>
+#include <AP_Vehicle.h>
 
 #define ANLOGIN_DEBUGGING 0
 
@@ -57,6 +58,11 @@ static const struct {
     { 10, 3.3f/4096 },
     { 11, 3.3f/4096 },
 #elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V51)
+#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
+    {  1, 3.3f/4096 },
+    {  2, 3.3f/4096 },
+    {  3, 3.3f/4096 },
+#endif
     { 10, 3.3f/4096 },
 #elif defined(CONFIG_ARCH_BOARD_VRHERO_V10)
     { 10, 3.3f/4096 },

libraries/AP_HAL_VRBRAIN/GPIO.cpp

@@ -44,22 +44,23 @@ void VRBRAINGPIO::init()
     if (ioctl(_led_fd, LED_OFF, LED_GREEN) != 0) {
          hal.console->printf("GPIO: Unable to setup GPIO LED GREEN\n");
     }
-#if !defined(CONFIG_ARCH_BOARD_VRHERO_V10)
+#if defined(LED_EXT1)
     if (ioctl(_led_fd, LED_OFF, LED_EXT1) != 0) {
          hal.console->printf("GPIO: Unable to setup GPIO LED EXT 1\n");
     }
 #endif
-#if !defined(CONFIG_ARCH_BOARD_VRHERO_V10)
+#if defined(LED_EXT2)
     if (ioctl(_led_fd, LED_OFF, LED_EXT2) != 0) {
          hal.console->printf("GPIO: Unable to setup GPIO LED EXT 2\n");
     }
 #endif
-#if !defined(CONFIG_ARCH_BOARD_VRHERO_V10) && !defined(CONFIG_ARCH_BOARD_VRUBRAIN_V51)
+#if defined(LED_EXT3)
     if (ioctl(_led_fd, LED_OFF, LED_EXT3) != 0) {
          hal.console->printf("GPIO: Unable to setup GPIO LED EXT 3\n");
     }
 #endif
 
+#if defined(BUZZER_EXT)
     _buzzer_fd = open(BUZZER_DEVICE_PATH, O_RDWR);
     if (_buzzer_fd == -1) {
         hal.scheduler->panic("Unable to open " BUZZER_DEVICE_PATH);
@@ -67,6 +68,15 @@ void VRBRAINGPIO::init()
     if (ioctl(_buzzer_fd, BUZZER_OFF, BUZZER_EXT) != 0) {
         hal.console->printf("GPIO: Unable to setup GPIO BUZZER\n");
     }
+#endif
+
+#if defined(GPIO_GPIO0_OUTPUT)
+    stm32_configgpio(GPIO_GPIO0_OUTPUT);
+#endif
+
+#if defined(GPIO_GPIO1_OUTPUT)
+    stm32_configgpio(GPIO_GPIO1_OUTPUT);
+#endif
 }
 
 void VRBRAINGPIO::pinMode(uint8_t pin, uint8_t output)
@@ -85,6 +95,16 @@ uint8_t VRBRAINGPIO::read(uint8_t pin)
 {
 
     switch (pin) {
+        case EXTERNAL_RELAY1_PIN:
+#if defined(GPIO_GPIO0_OUTPUT)
+        	return (stm32_gpioread(GPIO_GPIO0_OUTPUT))?HIGH:LOW;
+#endif
+            break;
+        case EXTERNAL_RELAY2_PIN:
+#if defined(GPIO_GPIO1_OUTPUT)
+        	return (stm32_gpioread(GPIO_GPIO1_OUTPUT))?HIGH:LOW;
+#endif
+            break;
     }
     return LOW;
 }
@@ -118,7 +138,7 @@ void VRBRAINGPIO::write(uint8_t pin, uint8_t value)
             break;
 
         case EXTERNAL_LED_GPS:
-#if !defined(CONFIG_ARCH_BOARD_VRHERO_V10)
+#if defined(LED_EXT1)
             if (value == LOW) {
                 ioctl(_led_fd, LED_OFF, LED_EXT1);
             } else {
@@ -128,7 +148,7 @@ void VRBRAINGPIO::write(uint8_t pin, uint8_t value)
             break;
 
         case EXTERNAL_LED_ARMED:
-#if !defined(CONFIG_ARCH_BOARD_VRHERO_V10)
+#if defined(LED_EXT2)
             if (value == LOW) {
                 ioctl(_led_fd, LED_OFF, LED_EXT2);
             } else {
@@ -144,12 +164,26 @@ void VRBRAINGPIO::write(uint8_t pin, uint8_t value)
             break;
 
         case BUZZER_PIN:
+#if defined(BUZZER_EXT)
             if (value == LOW) {
                 ioctl(_buzzer_fd, BUZZER_OFF, BUZZER_EXT);
             } else {
                 ioctl(_buzzer_fd, BUZZER_ON, BUZZER_EXT);
             }
+#endif
             break;
+
+        case EXTERNAL_RELAY1_PIN:
+#if defined(GPIO_GPIO0_OUTPUT)
+        	stm32_gpiowrite(GPIO_GPIO0_OUTPUT, (value==HIGH));
+#endif
+        	break;
+        case EXTERNAL_RELAY2_PIN:
+#if defined(GPIO_GPIO1_OUTPUT)
+        	stm32_gpiowrite(GPIO_GPIO1_OUTPUT, (value==HIGH));
+#endif
+        	break;
+
     }
 }
 
@@ -170,13 +204,13 @@ void VRBRAINGPIO::toggle(uint8_t pin)
             break;
 
         case EXTERNAL_LED_GPS:
-#if !defined(CONFIG_ARCH_BOARD_VRHERO_V10)
+#if defined(LED_EXT1)
 			ioctl(_led_fd, LED_TOGGLE, LED_EXT1);
 #endif
             break;
 
         case EXTERNAL_LED_ARMED:
-#if !defined(CONFIG_ARCH_BOARD_VRHERO_V10)
+#if defined(LED_EXT2)
 			ioctl(_led_fd, LED_TOGGLE, LED_EXT2);
 #endif
             break;
@@ -190,7 +224,9 @@ void VRBRAINGPIO::toggle(uint8_t pin)
             break;
 
         case BUZZER_PIN:
+#if defined(BUZZER_EXT)
 			ioctl(_buzzer_fd, BUZZER_TOGGLE, BUZZER_EXT);
+#endif
             break;
 
         default:

libraries/AP_HAL_VRBRAIN/GPIO.h

@@ -13,6 +13,8 @@
  # define EXTERNAL_LED_MOTOR1       30
  # define EXTERNAL_LED_MOTOR2       31
  # define BUZZER_PIN                32
+ # define EXTERNAL_RELAY1_PIN       33
+ # define EXTERNAL_RELAY2_PIN       34
  # define HAL_GPIO_LED_ON           HIGH
  # define HAL_GPIO_LED_OFF          LOW
 

libraries/AP_HAL_VRBRAIN/HAL_VRBRAIN_Class.cpp

@@ -67,7 +67,7 @@ static VRBRAINGPIO gpioDriver;
 #define UARTB_DEFAULT_DEVICE "/dev/ttyS0"
 #define UARTC_DEFAULT_DEVICE "/dev/ttyS2"
 #define UARTD_DEFAULT_DEVICE "/dev/null"
-#define UARTE_DEFAULT_DEVICE "/dev/null"
+#define UARTE_DEFAULT_DEVICE "/dev/ttyS1"
 #elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V51)
 #define UARTA_DEFAULT_DEVICE "/dev/ttyACM0"
 #define UARTB_DEFAULT_DEVICE "/dev/ttyS0"

libraries/AP_HAL_VRBRAIN/Storage.cpp

@@ -177,11 +177,11 @@ void VRBRAINStorage::_storage_open(void)
 void VRBRAINStorage::_mark_dirty(uint16_t loc, uint16_t length)
 {
 	uint16_t end = loc + length;
-	while (loc < end) {
-		uint8_t line = (loc >> VRBRAIN_STORAGE_LINE_SHIFT);
-		_dirty_mask |= 1 << line;
-		loc += VRBRAIN_STORAGE_LINE_SIZE;
-	}
+        for (uint8_t line=loc>>VRBRAIN_STORAGE_LINE_SHIFT;
+             line <= end>>VRBRAIN_STORAGE_LINE_SHIFT;
+             line++) {
+            _dirty_mask |= 1U << line;
+        }
 }
 
 void VRBRAINStorage::read_block(void *dst, uint16_t loc, size_t n)

libraries/AP_HAL_VRBRAIN/UARTDriver.cpp

@@ -503,7 +503,7 @@ void VRBRAINUARTDriver::_timer_tick(void)
             // split into two writes
             uint16_t n1 = _writebuf_size - _writebuf_head;
             int ret = _write_fd(&_writebuf[_writebuf_head], n1);
-            if (ret == n1 && n != n1) {
+            if (ret == n1 && n > n1) {
                 _write_fd(&_writebuf[_writebuf_head], n - n1);                
             }
         }
@@ -523,7 +523,7 @@ void VRBRAINUARTDriver::_timer_tick(void)
             uint16_t n1 = _readbuf_size - _readbuf_tail;
             assert(_readbuf_tail+n1 <= _readbuf_size);
             int ret = _read_fd(&_readbuf[_readbuf_tail], n1);
-            if (ret == n1 && n != n1) {
+            if (ret == n1 && n > n1) {
                 assert(_readbuf_tail+(n-n1) <= _readbuf_size);
                 _read_fd(&_readbuf[_readbuf_tail], n - n1);                
             }

libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp

@@ -25,12 +25,12 @@ void AP_InertialNav_NavEKF::init()
 void AP_InertialNav_NavEKF::update(float dt)
 {
     AP_InertialNav::update(dt);
-    _ahrs.get_relative_position_NED(_relpos_cm);
+    _ahrs_ekf.get_NavEKF().getPosNED(_relpos_cm);
     _relpos_cm *= 100; // convert to cm
 
     _haveabspos = _ahrs.get_position(_abspos);
 
-    _ahrs.get_velocity_NED(_velocity_cm);
+    _ahrs_ekf.get_NavEKF().getVelNED(_velocity_cm);
     _velocity_cm *= 100; // convert to cm/s
 
     // InertialNav is NEU

libraries/AP_InertialNav/AP_InertialNav_NavEKF.h

@@ -14,9 +14,10 @@ class AP_InertialNav_NavEKF : public AP_InertialNav
 {
 public:
     // Constructor
-    AP_InertialNav_NavEKF(AP_AHRS &ahrs, AP_Baro &baro, GPS_Glitch& gps_glitch, Baro_Glitch& baro_glitch) :
+    AP_InertialNav_NavEKF(AP_AHRS_NavEKF &ahrs, AP_Baro &baro, GPS_Glitch& gps_glitch, Baro_Glitch& baro_glitch) :
         AP_InertialNav(ahrs, baro, gps_glitch, baro_glitch),
-        _haveabspos(false)
+        _haveabspos(false),
+        _ahrs_ekf(ahrs)
         {
         }
 
@@ -113,6 +114,7 @@ private:
     Vector3f _velocity_cm; // NEU
     struct Location _abspos;
     bool _haveabspos;
+    AP_AHRS_NavEKF &_ahrs_ekf;
 };
 
 #endif // __AP_INERTIALNAV_NAVEKF_H__

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -11,18 +11,22 @@
    maximum number of INS instances available on this platform. If more
    than 1 then redundent sensors may be available
  */
-#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_LINUX || CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
-#define INS_MAX_INSTANCES 3
-#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+#if HAL_CPU_CLASS > HAL_CPU_CLASS_16
 #define INS_MAX_INSTANCES 3
+#define INS_MAX_BACKENDS  6
 #else
 #define INS_MAX_INSTANCES 1
+#define INS_MAX_BACKENDS  1
 #endif
 
+
 #include <stdint.h>
 #include <AP_HAL.h>
 #include <AP_Math.h>
 #include "AP_InertialSensor_UserInteract.h"
+
+class AP_InertialSensor_Backend;
+
 /* AP_InertialSensor is an abstraction for gyro and accel measurements
  * which are correctly aligned to the body axes and scaled to SI units.
  *
@@ -32,12 +36,11 @@
  */
 class AP_InertialSensor
 {
+    friend class AP_InertialSensor_Backend;
+
 public:
     AP_InertialSensor();
 
-    // empty virtual destructor
-    virtual ~AP_InertialSensor() {}
-
     enum Start_style {
         COLD_START = 0,
         WARM_START
@@ -64,22 +67,28 @@ public:
     ///
     /// @param style	The initialisation startup style.
     ///
-    virtual void init( Start_style style,
-                       Sample_rate sample_rate);
+    void init( Start_style style,
+               Sample_rate sample_rate);
 
     /// Perform cold startup initialisation for just the accelerometers.
     ///
     /// @note This should not be called unless ::init has previously
     ///       been called, as ::init may perform other work.
     ///
-    virtual void init_accel();
+    void init_accel();
+
+
+    /// Register a new gyro/accel driver, allocating an instance
+    /// number
+    uint8_t register_gyro(void);
+    uint8_t register_accel(void);
 
 #if !defined( __AVR_ATmega1280__ )
     // perform accelerometer calibration including providing user instructions
     // and feedback
-    virtual bool calibrate_accel(AP_InertialSensor_UserInteract *interact,
-                                 float& trim_roll,
-                                 float& trim_pitch);
+    bool calibrate_accel(AP_InertialSensor_UserInteract *interact,
+                         float& trim_roll,
+                         float& trim_pitch);
 #endif
 
     /// calibrated - returns true if the accelerometers have been calibrated
@@ -93,61 +102,66 @@ public:
     /// @note This should not be called unless ::init has previously
     ///       been called, as ::init may perform other work
     ///
-    virtual void init_gyro(void);
+    void init_gyro(void);
 
     /// Fetch the current gyro values
     ///
     /// @returns	vector of rotational rates in radians/sec
     ///
     const Vector3f     &get_gyro(uint8_t i) const { return _gyro[i]; }
-    const Vector3f     &get_gyro(void) const { return get_gyro(_get_primary_gyro()); }
-    virtual void       set_gyro(uint8_t instance, const Vector3f &gyro) {}
+    const Vector3f     &get_gyro(void) const { return get_gyro(_primary_gyro); }
+    void               set_gyro(uint8_t instance, const Vector3f &gyro);
 
     // set gyro offsets in radians/sec
     const Vector3f &get_gyro_offsets(uint8_t i) const { return _gyro_offset[i]; }
-    const Vector3f &get_gyro_offsets(void) const { return get_gyro_offsets(_get_primary_gyro()); }
+    const Vector3f &get_gyro_offsets(void) const { return get_gyro_offsets(_primary_gyro); }
 
     /// Fetch the current accelerometer values
     ///
     /// @returns	vector of current accelerations in m/s/s
     ///
     const Vector3f     &get_accel(uint8_t i) const { return _accel[i]; }
-    const Vector3f     &get_accel(void) const { return get_accel(get_primary_accel()); }
-    virtual void       set_accel(uint8_t instance, const Vector3f &accel) {}
+    const Vector3f     &get_accel(void) const { return get_accel(_primary_accel); }
+    void               set_accel(uint8_t instance, const Vector3f &accel);
+
+    uint32_t get_gyro_error_count(uint8_t i) const { return _gyro_error_count[i]; }
+    uint32_t get_accel_error_count(uint8_t i) const { return _accel_error_count[i]; }
 
     // multi-device interface
-    virtual bool get_gyro_health(uint8_t instance) const { return true; }
-    bool get_gyro_health(void) const { return get_gyro_health(_get_primary_gyro()); }
-    virtual uint8_t get_gyro_count(void) const { return 1; };
+    bool get_gyro_health(uint8_t instance) const { return _gyro_healthy[instance]; }
+    bool get_gyro_health(void) const { return get_gyro_health(_primary_gyro); }
+    bool get_gyro_health_all(void) const;
+    uint8_t get_gyro_count(void) const { return _gyro_count; }
+    bool gyro_calibrated_ok(uint8_t instance) const { return _gyro_cal_ok[instance]; }
+    bool gyro_calibrated_ok_all() const;
 
-    virtual bool get_accel_health(uint8_t instance) const { return true; }
-    bool get_accel_health(void) const { return get_accel_health(get_primary_accel()); }
-    virtual uint8_t get_accel_count(void) const { return 1; };
+    bool get_accel_health(uint8_t instance) const { return _accel_healthy[instance]; }
+    bool get_accel_health(void) const { return get_accel_health(_primary_accel); }
+    bool get_accel_health_all(void) const;
+    uint8_t get_accel_count(void) const { return _accel_count; };
 
     // get accel offsets in m/s/s
     const Vector3f &get_accel_offsets(uint8_t i) const { return _accel_offset[i]; }
-    const Vector3f &get_accel_offsets(void) const { return get_accel_offsets(get_primary_accel()); }
+    const Vector3f &get_accel_offsets(void) const { return get_accel_offsets(_primary_accel); }
 
     // get accel scale
     const Vector3f &get_accel_scale(uint8_t i) const { return _accel_scale[i]; }
-    const Vector3f &get_accel_scale(void) const { return get_accel_scale(get_primary_accel()); }
-
-    /* Update the sensor data, so that getters are nonblocking.
-     * Returns a bool of whether data was updated or not.
-     */
-    virtual bool update() = 0;
+    const Vector3f &get_accel_scale(void) const { return get_accel_scale(_primary_accel); }
 
     /* get_delta_time returns the time period in seconds
      * overwhich the sensor data was collected
      */
-    virtual float get_delta_time() const = 0;
+    float get_delta_time() const { return _delta_time; }
 
     // return the maximum gyro drift rate in radians/s/s. This
     // depends on what gyro chips are being used
-    virtual float get_gyro_drift_rate(void) = 0;
+    float get_gyro_drift_rate(void) const { return ToRad(0.5f/60); }
 
-    // wait for a sample to be available, with timeout in milliseconds
-    virtual bool wait_for_sample(uint16_t timeout_ms) = 0;
+    // update gyro and accel values from accumulated samples
+    void update(void);
+
+    // wait for a sample to be available
+    void wait_for_sample(void);
 
     // class level parameters
     static const struct AP_Param::GroupInfo var_info[];
@@ -165,24 +179,28 @@ public:
     }
 
     // get_filter - return filter in hz
-    virtual uint8_t get_filter() const { return _mpu6000_filter.get(); }
+    uint8_t get_filter() const { return _mpu6000_filter.get(); }
 
-    virtual uint16_t error_count(void) const { return 0; }
-    virtual bool healthy(void) const { return get_gyro_health() && get_accel_health(); }
+    // return the selected sample rate
+    Sample_rate get_sample_rate(void) const { return _sample_rate; }
 
-    virtual uint8_t get_primary_accel(void) const { return 0; }
+    uint16_t error_count(void) const { return 0; }
+    bool healthy(void) const { return get_gyro_health() && get_accel_health(); }
 
-protected:
+    uint8_t get_primary_accel(void) const { return 0; }
 
-    virtual uint8_t _get_primary_gyro(void) const { return 0; }
+    // enable HIL mode
+    void set_hil_mode(void) { _hil_mode = true; }
 
-    // sensor specific init to be overwritten by descendant classes
-    virtual uint16_t        _init_sensor( Sample_rate sample_rate ) = 0;
+private:
 
-    // no-save implementations of accel and gyro initialisation routines
-    virtual void  _init_accel();
+    // load backend drivers
+    void _add_backend(AP_InertialSensor_Backend *(detect)(AP_InertialSensor &));
+    void _detect_backends(void);
 
-    virtual void _init_gyro();
+    // accel and gyro initialisation
+    void _init_accel();
+    void _init_gyro();
 
 #if !defined( __AVR_ATmega1280__ )
     // Calibration routines borrowed from Rolfe Schmidt
@@ -190,50 +208,98 @@ protected:
     // original sketch available at http://rolfeschmidt.com/mathtools/skimetrics/adxl_gn_calibration.pde
 
     // _calibrate_accel - perform low level accel calibration
-    virtual bool            _calibrate_accel(Vector3f accel_sample[6], Vector3f& accel_offsets, Vector3f& accel_scale);
-    virtual void            _calibrate_update_matrices(float dS[6], float JS[6][6], float beta[6], float data[3]);
-    virtual void            _calibrate_reset_matrices(float dS[6], float JS[6][6]);
-    virtual void            _calibrate_find_delta(float dS[6], float JS[6][6], float delta[6]);
-    virtual void            _calculate_trim(Vector3f accel_sample, float& trim_roll, float& trim_pitch);
+    bool _calibrate_accel(Vector3f accel_sample[6], Vector3f& accel_offsets, Vector3f& accel_scale);
+    void _calibrate_update_matrices(float dS[6], float JS[6][6], float beta[6], float data[3]);
+    void _calibrate_reset_matrices(float dS[6], float JS[6][6]);
+    void _calibrate_find_delta(float dS[6], float JS[6][6], float delta[6]);
+    void _calculate_trim(Vector3f accel_sample, float& trim_roll, float& trim_pitch);
 #endif
 
+    // check if we have 3D accel calibration
+    void check_3D_calibration(void);
+
     // save parameters to eeprom
     void  _save_parameters();
 
-    // Most recent accelerometer reading obtained by ::update
+    // backend objects
+    AP_InertialSensor_Backend *_backends[INS_MAX_BACKENDS];
+
+    // number of gyros and accel drivers. Note that most backends
+    // provide both accel and gyro data, so will increment both
+    // counters on initialisation
+    uint8_t _gyro_count;
+    uint8_t _accel_count;
+    uint8_t _backend_count;
+
+    // the selected sample rate
+    Sample_rate _sample_rate;
+    
+    // Most recent accelerometer reading
     Vector3f _accel[INS_MAX_INSTANCES];
 
-    // previous accelerometer reading obtained by ::update
-    Vector3f _previous_accel[INS_MAX_INSTANCES];
-
-    // Most recent gyro reading obtained by ::update
+    // Most recent gyro reading
     Vector3f _gyro[INS_MAX_INSTANCES];
 
     // product id
     AP_Int16 _product_id;
 
     // accelerometer scaling and offsets
-    AP_Vector3f             _accel_scale[INS_MAX_INSTANCES];
-    AP_Vector3f             _accel_offset[INS_MAX_INSTANCES];
-    AP_Vector3f             _gyro_offset[INS_MAX_INSTANCES];
+    AP_Vector3f _accel_scale[INS_MAX_INSTANCES];
+    AP_Vector3f _accel_offset[INS_MAX_INSTANCES];
+    AP_Vector3f _gyro_offset[INS_MAX_INSTANCES];
 
     // filtering frequency (0 means default)
-    AP_Int8                 _mpu6000_filter;
+    AP_Int8     _mpu6000_filter;
 
     // board orientation from AHRS
-    enum Rotation			_board_orientation;
+    enum Rotation _board_orientation;
+
+    // calibrated_ok flags
+    bool _gyro_cal_ok[INS_MAX_INSTANCES];
+
+    // primary accel and gyro
+    uint8_t _primary_gyro;
+    uint8_t _primary_accel;
+
+    // has wait_for_sample() found a sample?
+    bool _have_sample:1;
+
+    // are we in HIL mode?
+    bool _hil_mode:1;
+
+    // do we have offsets/scaling from a 3D calibration?
+    bool _have_3D_calibration:1;
+
+    // the delta time in seconds for the last sample
+    float _delta_time;
+
+    // last time a wait_for_sample() returned a sample
+    uint32_t _last_sample_usec;
+
+    // target time for next wait_for_sample() return
+    uint32_t _next_sample_usec;
+    
+    // time between samples in microseconds
+    uint32_t _sample_period_usec;
+
+    // health of gyros and accels
+    bool _gyro_healthy[INS_MAX_INSTANCES];
+    bool _accel_healthy[INS_MAX_INSTANCES];
+
+    uint32_t _accel_error_count[INS_MAX_INSTANCES];
+    uint32_t _gyro_error_count[INS_MAX_INSTANCES];
 };
 
-#include "AP_InertialSensor_Oilpan.h"
+#include "AP_InertialSensor_Backend.h"
 #include "AP_InertialSensor_MPU6000.h"
-#include "AP_InertialSensor_HIL.h"
 #include "AP_InertialSensor_PX4.h"
-#include "AP_InertialSensor_VRBRAIN.h"
+#include "AP_InertialSensor_Oilpan.h"
+#include "AP_InertialSensor_MPU9250.h"
+#include "AP_InertialSensor_L3G4200D.h"
+#include "AP_InertialSensor_Flymaple.h"
+#include "AP_InertialSensor_MPU9150.h"
+#include "AP_InertialSensor_HIL.h"
 #include "AP_InertialSensor_UserInteract_Stream.h"
 #include "AP_InertialSensor_UserInteract_MAVLink.h"
-#include "AP_InertialSensor_Flymaple.h"
-#include "AP_InertialSensor_L3G4200D.h"
-#include "AP_InertialSensor_MPU9150.h"
-#include "AP_InertialSensor_MPU9250.h"
 
 #endif // __AP_INERTIAL_SENSOR_H__

libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp

@@ -0,0 +1,72 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#include <AP_HAL.h>
+#include "AP_InertialSensor.h"
+#include "AP_InertialSensor_Backend.h"
+
+AP_InertialSensor_Backend::AP_InertialSensor_Backend(AP_InertialSensor &imu) :
+    _imu(imu),
+    _product_id(AP_PRODUCT_ID_NONE)
+{}
+
+/*
+  rotate gyro vector and add the gyro offset
+ */
+void AP_InertialSensor_Backend::_rotate_and_offset_gyro(uint8_t instance, const Vector3f &gyro)
+{
+    _imu._gyro[instance] = gyro;
+    _imu._gyro[instance].rotate(_imu._board_orientation);
+    _imu._gyro[instance] -= _imu._gyro_offset[instance];
+    _imu._gyro_healthy[instance] = true;
+}
+
+/*
+  rotate accel vector, scale and add the accel offset
+ */
+void AP_InertialSensor_Backend::_rotate_and_offset_accel(uint8_t instance, const Vector3f &accel)
+{
+    _imu._accel[instance] = accel;
+    _imu._accel[instance].rotate(_imu._board_orientation);
+
+    const Vector3f &accel_scale = _imu._accel_scale[instance].get();
+    _imu._accel[instance].x *= accel_scale.x;
+    _imu._accel[instance].y *= accel_scale.y;
+    _imu._accel[instance].z *= accel_scale.z;
+    _imu._accel[instance] -= _imu._accel_offset[instance];
+    _imu._accel_healthy[instance] = true;
+}
+
+// set accelerometer error_count
+void AP_InertialSensor_Backend::_set_accel_error_count(uint8_t instance, uint32_t error_count)
+{
+    _imu._accel_error_count[instance] = error_count;
+}
+
+// set gyro error_count
+void AP_InertialSensor_Backend::_set_gyro_error_count(uint8_t instance, uint32_t error_count)
+{
+    _imu._gyro_error_count[instance] = error_count;
+}
+
+/*
+  return the default filter frequency in Hz for the sample rate
+  
+  This uses the sample_rate as a proxy for what type of vehicle it is
+  (ie. plane and rover run at 50Hz). Copters need a bit more filter
+  bandwidth
+ */
+uint8_t AP_InertialSensor_Backend::_default_filter(void) const
+{
+    switch (_imu.get_sample_rate()) {
+    case AP_InertialSensor::RATE_50HZ:
+        // on Rover and plane use a lower filter rate
+        return 15;
+    case AP_InertialSensor::RATE_100HZ:
+        return 30;
+    case AP_InertialSensor::RATE_200HZ:
+        return 30;
+    case AP_InertialSensor::RATE_400HZ:
+        return 30;
+    }
+    return 30;
+}

libraries/AP_InertialSensor/AP_InertialSensor_Backend.h

@@ -0,0 +1,87 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/*
+  IMU driver backend class. Each supported gyro/accel sensor type
+  needs to have an object derived from this class.
+
+  Note that drivers can implement just gyros or just accels, and can
+  also provide multiple gyro/accel instances.
+ */
+#ifndef __AP_INERTIALSENSOR_BACKEND_H__
+#define __AP_INERTIALSENSOR_BACKEND_H__
+
+class AP_InertialSensor_Backend
+{
+public:
+    AP_InertialSensor_Backend(AP_InertialSensor &imu);
+
+    // we declare a virtual destructor so that drivers can
+    // override with a custom destructor if need be.
+    virtual ~AP_InertialSensor_Backend(void) {}
+
+    /* 
+     * Update the sensor data. Called by the frontend to transfer
+     * accumulated sensor readings to the frontend structure via the
+     * _rotate_and_offset_gyro() and _rotate_and_offset_accel() functions
+     */
+    virtual bool update() = 0;
+
+    /* 
+     * return true if at least one accel sample is available in the backend
+     * since the last call to update()
+     */
+    virtual bool accel_sample_available() = 0;
+
+    /* 
+     * return true if at least one gyro sample is available in the backend
+     * since the last call to update()
+     */
+    virtual bool gyro_sample_available() = 0;
+
+    /*
+      return the product ID
+     */
+    int16_t product_id(void) const { return _product_id; }
+
+protected:
+    // access to frontend
+    AP_InertialSensor &_imu;
+
+    // rotate gyro vector and offset
+    void _rotate_and_offset_gyro(uint8_t instance, const Vector3f &gyro);
+
+    // rotate accel vector, scale and offset
+    void _rotate_and_offset_accel(uint8_t instance, const Vector3f &accel);
+
+    // set accelerometer error_count
+    void _set_accel_error_count(uint8_t instance, uint32_t error_count);
+
+    // set gyro error_count
+    void _set_gyro_error_count(uint8_t instance, uint32_t error_count);
+
+    // backend should fill in its product ID from AP_PRODUCT_ID_*
+    int16_t _product_id;
+
+    // return the default filter frequency in Hz for the sample rate
+    uint8_t _default_filter(void) const;
+
+    // note that each backend is also expected to have a static detect()
+    // function which instantiates an instance of the backend sensor
+    // driver if the sensor is available
+};
+
+#endif // __AP_INERTIALSENSOR_BACKEND_H__

libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp

@@ -14,7 +14,7 @@
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 /*
-  Flymaple port by Mike McCauley
+  Flymaple IMU driver by Mike McCauley
  */
 
 // Interface to the Flymaple sensors:
@@ -28,20 +28,6 @@
 
 const extern AP_HAL::HAL& hal;
 
-/// Statics
-Vector3f AP_InertialSensor_Flymaple::_accel_filtered;
-uint32_t AP_InertialSensor_Flymaple::_accel_samples;
-Vector3f AP_InertialSensor_Flymaple::_gyro_filtered;
-uint32_t AP_InertialSensor_Flymaple::_gyro_samples;
-uint64_t AP_InertialSensor_Flymaple::_last_accel_timestamp;
-uint64_t AP_InertialSensor_Flymaple::_last_gyro_timestamp;
-LowPassFilter2p AP_InertialSensor_Flymaple::_accel_filter_x(800, 10);
-LowPassFilter2p AP_InertialSensor_Flymaple::_accel_filter_y(800, 10);
-LowPassFilter2p AP_InertialSensor_Flymaple::_accel_filter_z(800, 10);
-LowPassFilter2p AP_InertialSensor_Flymaple::_gyro_filter_x(800, 10);
-LowPassFilter2p AP_InertialSensor_Flymaple::_gyro_filter_y(800, 10);
-LowPassFilter2p AP_InertialSensor_Flymaple::_gyro_filter_z(800, 10);
-
 // This is how often we wish to make raw samples of the sensors in Hz
 const uint32_t  raw_sample_rate_hz = 800;
 // And the equivalent time between samples in microseconds
@@ -77,25 +63,39 @@ const uint32_t  raw_sample_interval_us = (1000000 / raw_sample_rate_hz);
 // Result wil be radians/sec
 #define FLYMAPLE_GYRO_SCALE_R_S (1.0f / 14.375f) * (3.1415926f / 180.0f)
 
-uint16_t AP_InertialSensor_Flymaple::_init_sensor( Sample_rate sample_rate ) 
+AP_InertialSensor_Flymaple::AP_InertialSensor_Flymaple(AP_InertialSensor &imu) :
+    AP_InertialSensor_Backend(imu),
+    _have_gyro_sample(false),
+    _have_accel_sample(false),
+    _accel_filter_x(raw_sample_rate_hz, 10),
+    _accel_filter_y(raw_sample_rate_hz, 10),
+    _accel_filter_z(raw_sample_rate_hz, 10),
+    _gyro_filter_x(raw_sample_rate_hz, 10),
+    _gyro_filter_y(raw_sample_rate_hz, 10),
+    _gyro_filter_z(raw_sample_rate_hz, 10),
+    _last_gyro_timestamp(0),
+    _last_accel_timestamp(0)
+{}
+
+/*
+  detect the sensor
+ */
+AP_InertialSensor_Backend *AP_InertialSensor_Flymaple::detect(AP_InertialSensor &_imu)
 {
-    // Sensors are raw sampled at 800Hz.
-    // Here we figure the divider to get the rate that update should be called
-    switch (sample_rate) {
-    case RATE_50HZ:
-        _sample_divider = raw_sample_rate_hz / 50;
-        _default_filter_hz = 10;
-        break;
-    case RATE_100HZ:
-        _sample_divider = raw_sample_rate_hz / 100;
-        _default_filter_hz = 20;
-        break;
-    case RATE_200HZ:
-    default:
-        _sample_divider = raw_sample_rate_hz / 200;
-        _default_filter_hz = 20;
-        break;
+    AP_InertialSensor_Flymaple *sensor = new AP_InertialSensor_Flymaple(_imu);
+    if (sensor == NULL) {
+        return NULL;
     }
+    if (!sensor->_init_sensor()) {
+        delete sensor;
+        return NULL;
+    }
+    return sensor;
+}
+
+bool AP_InertialSensor_Flymaple::_init_sensor(void) 
+{
+    _default_filter_hz = _default_filter();
 
     // get pointer to i2c bus semaphore
     AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore();
@@ -146,12 +146,17 @@ uint16_t AP_InertialSensor_Flymaple::_init_sensor( Sample_rate sample_rate )
     hal.scheduler->delay(1);
 
     // Set up the filter desired
-    _set_filter_frequency(_mpu6000_filter);
+    _set_filter_frequency(_imu.get_filter());
 
-   // give back i2c semaphore
+    // give back i2c semaphore
     i2c_sem->give();
 
-    return AP_PRODUCT_ID_FLYMAPLE;
+    _gyro_instance = _imu.register_gyro();
+    _accel_instance = _imu.register_accel();
+
+    _product_id = AP_PRODUCT_ID_FLYMAPLE;
+
+    return true;
 }
 
 /*
@@ -170,109 +175,46 @@ void AP_InertialSensor_Flymaple::_set_filter_frequency(uint8_t filter_hz)
     _gyro_filter_z.set_cutoff_frequency(raw_sample_rate_hz, filter_hz);
 }
 
-/*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */
 
 // This takes about 20us to run
 bool AP_InertialSensor_Flymaple::update(void) 
 {
-    if (!wait_for_sample(100)) {
-        return false;
-    }
-    Vector3f accel_scale = _accel_scale[0].get();
+    Vector3f accel, gyro;
 
-    // Not really needed since Flymaple _accumulate runs in the main thread
     hal.scheduler->suspend_timer_procs();
-
-    // base the time on the gyro timestamp, as that is what is
-    // multiplied by time to integrate in DCM
-    _delta_time = (_last_gyro_timestamp - _last_update_usec) * 1.0e-6f;
-    _last_update_usec = _last_gyro_timestamp;
-
-    _previous_accel[0] = _accel[0];
-
-    _accel[0] = _accel_filtered;
-    _accel_samples = 0;
-
-    _gyro[0] = _gyro_filtered;
-    _gyro_samples = 0;
-
+    accel = _accel_filtered;
+    gyro = _gyro_filtered;
+    _have_gyro_sample = false;
+    _have_accel_sample = false;
     hal.scheduler->resume_timer_procs();
 
-    // add offsets and rotation
-    _accel[0].rotate(_board_orientation);
-
     // Adjust for chip scaling to get m/s/s
-    _accel[0] *= FLYMAPLE_ACCELEROMETER_SCALE_M_S;
-
-    // Now the calibration scale factor
-    _accel[0].x *= accel_scale.x;
-    _accel[0].y *= accel_scale.y;
-    _accel[0].z *= accel_scale.z;
-    _accel[0]   -= _accel_offset[0];
-
-    _gyro[0].rotate(_board_orientation);
+    accel *= FLYMAPLE_ACCELEROMETER_SCALE_M_S;
+    _rotate_and_offset_accel(_accel_instance, accel);
 
     // Adjust for chip scaling to get radians/sec
-    _gyro[0] *= FLYMAPLE_GYRO_SCALE_R_S;
-    _gyro[0] -= _gyro_offset[0];
+    gyro *= FLYMAPLE_GYRO_SCALE_R_S;
+    _rotate_and_offset_gyro(_gyro_instance, gyro);
 
-    if (_last_filter_hz != _mpu6000_filter) {
-        _set_filter_frequency(_mpu6000_filter);
-        _last_filter_hz = _mpu6000_filter;
+    if (_last_filter_hz != _imu.get_filter()) {
+        _set_filter_frequency(_imu.get_filter());
+        _last_filter_hz = _imu.get_filter();
     }
 
     return true;
 }
 
-bool AP_InertialSensor_Flymaple::get_gyro_health(void) const
-{
-    if (_last_gyro_timestamp == 0) {
-        // not initialised yet, show as healthy to prevent scary GCS
-        // warnings
-        return true;
-    }
-    uint64_t now = hal.scheduler->micros();
-    if ((now - _last_gyro_timestamp) >= (2 * raw_sample_interval_us)) {
-        // gyros have not updated
-        return false;
-    }
-    return true;
-}
-
-bool AP_InertialSensor_Flymaple::get_accel_health(void) const
-{
-    if (_last_accel_timestamp == 0) {
-        // not initialised yet, show as healthy to prevent scary GCS
-        // warnings
-        return true;
-    }
-    uint64_t now = hal.scheduler->micros();
-    if ((now - _last_accel_timestamp) >= (2 * raw_sample_interval_us)) {
-        // gyros have not updated
-        return false;
-    }
-    return true;
-}
-
-float AP_InertialSensor_Flymaple::get_delta_time(void) const
-{
-    return _delta_time;
-}
-
-float AP_InertialSensor_Flymaple::get_gyro_drift_rate(void) 
-{
-    // Dont really know this for the ITG-3200.
-    // 0.5 degrees/second/minute
-    return ToRad(0.5/60);
-}
-
 // This needs to get called as often as possible.
 // Its job is to accumulate samples as fast as is reasonable for the accel and gyro
 // sensors.
-// Cant call this from within the system timers, since the long I2C reads (up to 1ms) 
-// with interrupts disabled breaks lots of things
-// Therefore must call this as often as possible from
-// within the mainline and thropttle the reads here to suit the sensors
+// Note that this is called from gyro_sample_available() and
+// accel_sample_available(), which is really not good enough for
+// 800Hz, as it is common for the main loop to take more than 1.5ms
+// before wait_for_sample() is called again. We can't just call this
+// from a timer as timers run with interrupts disabled, and the I2C
+// operations take too long
+// So we are stuck with a suboptimal solution. The results are not so
+// good in terms of timing. It may be better with the FIFOs enabled
 void AP_InertialSensor_Flymaple::_accumulate(void)
 {
     // get pointer to i2c bus semaphore
@@ -285,7 +227,7 @@ void AP_InertialSensor_Flymaple::_accumulate(void)
     // Read accelerometer
     // ADXL345 is in the default FIFO bypass mode, so the FIFO is not used
     uint8_t buffer[6];
-    uint64_t now = hal.scheduler->micros();
+    uint32_t now = hal.scheduler->micros();
     // This takes about 250us at 400kHz I2C speed
     if ((now - _last_accel_timestamp) >= raw_sample_interval_us
         && hal.i2c->readRegisters(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_DATAX0, 6, buffer) == 0)
@@ -300,7 +242,7 @@ void AP_InertialSensor_Flymaple::_accumulate(void)
         _accel_filtered = Vector3f(_accel_filter_x.apply(x),
                                    _accel_filter_y.apply(y),
                                    _accel_filter_z.apply(z));
-        _accel_samples++;
+        _have_accel_sample = true;
         _last_accel_timestamp = now;
     }
 
@@ -317,7 +259,7 @@ void AP_InertialSensor_Flymaple::_accumulate(void)
         _gyro_filtered = Vector3f(_gyro_filter_x.apply(x),
                                   _gyro_filter_y.apply(y),
                                   _gyro_filter_z.apply(z));
-        _gyro_samples++;
+        _have_gyro_sample = true;
         _last_gyro_timestamp = now;
     }
 
@@ -325,26 +267,4 @@ void AP_InertialSensor_Flymaple::_accumulate(void)
     i2c_sem->give();
 }
 
-bool AP_InertialSensor_Flymaple::_sample_available(void)
-{
-    _accumulate();
-    return min(_accel_samples, _gyro_samples) / _sample_divider > 0;
-}
-
-bool AP_InertialSensor_Flymaple::wait_for_sample(uint16_t timeout_ms)
-{
-    if (_sample_available()) {
-        return true;
-    }
-    uint32_t start = hal.scheduler->millis();
-    while ((hal.scheduler->millis() - start) < timeout_ms) {
-        hal.scheduler->delay_microseconds(100);
-        if (_sample_available()) {
-            return true;
-        }
-    }
-    return false;
-}
-
 #endif // CONFIG_HAL_BOARD
-