ArduCopter/ArduCopter.pde

 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
-#define THISFIRMWARE "ArduCopter V3.1.2-rc1"
+#define THISFIRMWARE "ArduCopter V3.1.5-rc1"
 /*
    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
@@ -1236,6 +1236,7 @@ static void update_GPS(void)
 {
     static uint32_t last_gps_reading;           // time of last gps message
     static uint8_t ground_start_count = 10;     // counter used to grab at least 10 reads before commiting the Home location
+    bool report_gps_glitch;
 
     g_gps->update();
 
@@ -1251,13 +1252,14 @@ static void update_GPS(void)
         // run glitch protection and update AP_Notify if home has been initialised
         if (ap.home_is_set) {
             gps_glitch.check_position();
-            if (AP_Notify::flags.gps_glitching != gps_glitch.glitching()) {
+            report_gps_glitch = (gps_glitch.glitching() && !ap.usb_connected);
+            if (AP_Notify::flags.gps_glitching != report_gps_glitch) {
                 if (gps_glitch.glitching()) {
                     Log_Write_Error(ERROR_SUBSYSTEM_GPS, ERROR_CODE_GPS_GLITCH);
                 }else{
                     Log_Write_Error(ERROR_SUBSYSTEM_GPS, ERROR_CODE_ERROR_RESOLVED);
                 }
-                AP_Notify::flags.gps_glitching = gps_glitch.glitching();
+                AP_Notify::flags.gps_glitching = report_gps_glitch;
             }
         }
     }
@@ -1550,6 +1552,7 @@ bool set_roll_pitch_mode(uint8_t new_roll_pitch_mode)
 
     switch( new_roll_pitch_mode ) {
         case ROLL_PITCH_STABLE:
+            reset_roll_pitch_in_filters(g.rc_1.control_in, g.rc_2.control_in);
             roll_pitch_initialised = true;
             break;
         case ROLL_PITCH_ACRO:
@@ -1558,9 +1561,12 @@ bool set_roll_pitch_mode(uint8_t new_roll_pitch_mode)
             acro_pitch_rate = 0;
             roll_pitch_initialised = true;
             break;
-        case ROLL_PITCH_AUTO:
         case ROLL_PITCH_STABLE_OF:
         case ROLL_PITCH_DRIFT:
+            reset_roll_pitch_in_filters(g.rc_1.control_in, g.rc_2.control_in);
+            roll_pitch_initialised = true;
+            break;
+        case ROLL_PITCH_AUTO:
         case ROLL_PITCH_LOITER:
         case ROLL_PITCH_SPORT:
             roll_pitch_initialised = true;
@@ -1570,6 +1576,7 @@ bool set_roll_pitch_mode(uint8_t new_roll_pitch_mode)
         case ROLL_PITCH_AUTOTUNE:
             // only enter autotune mode from stabilized roll-pitch mode when armed and flying
             if (roll_pitch_mode == ROLL_PITCH_STABLE && motors.armed() && !ap.land_complete) {
+                reset_roll_pitch_in_filters(g.rc_1.control_in, g.rc_2.control_in);
                 // auto_tune_start returns true if it wants the roll-pitch mode changed to autotune
                 roll_pitch_initialised = auto_tune_start();
             }
@@ -1672,8 +1679,13 @@ void update_roll_pitch_mode(void)
         // apply SIMPLE mode transform
         update_simple_mode();
 
-        // update loiter target from user controls
-        wp_nav.move_loiter_target(g.rc_1.control_in, g.rc_2.control_in, 0.01f);
+        if(failsafe.radio) {
+            // don't allow loiter target to move during failsafe
+            wp_nav.move_loiter_target(0.0f, 0.0f, 0.01f);
+        } else {
+            // update loiter target from user controls
+            wp_nav.move_loiter_target(g.rc_1.control_in, g.rc_2.control_in, 0.01f);
+        }
 
         // copy latest output from nav controller to stabilize controller
         control_roll = wp_nav.get_desired_roll();

ArduCopter/Attitude.pde

 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
+// local variables
+float roll_in_filtered;     // roll-in in filtered with RC_FEEL_RP parameter
+float pitch_in_filtered;    // pitch-in filtered with RC_FEEL_RP parameter
+
+static void reset_roll_pitch_in_filters(int16_t roll_in, int16_t pitch_in)
+{
+    roll_in_filtered = constrain_int16(roll_in, -ROLL_PITCH_INPUT_MAX, ROLL_PITCH_INPUT_MAX);
+    pitch_in_filtered = constrain_int16(pitch_in, -ROLL_PITCH_INPUT_MAX, ROLL_PITCH_INPUT_MAX);
+}
+
 // get_pilot_desired_angle - transform pilot's roll or pitch input into a desired lean angle
 // returns desired angle in centi-degrees
 static void get_pilot_desired_lean_angles(int16_t roll_in, int16_t pitch_in, int16_t &roll_out, int16_t &pitch_out)
@@ -11,10 +21,31 @@ static void get_pilot_desired_lean_angles(int16_t roll_in, int16_t pitch_in, int
     roll_in = constrain_int16(roll_in, -ROLL_PITCH_INPUT_MAX, ROLL_PITCH_INPUT_MAX);
     pitch_in = constrain_int16(pitch_in, -ROLL_PITCH_INPUT_MAX, ROLL_PITCH_INPUT_MAX);
 
-    // return immediately if no scaling required
+    // filter input for feel
+    if (g.rc_feel_rp >= RC_FEEL_RP_VERY_CRISP) {
+        // no filtering required
+        roll_in_filtered = roll_in;
+        pitch_in_filtered = pitch_in;
+    }else{
+        float filter_gain;
+        if (g.rc_feel_rp >= RC_FEEL_RP_CRISP) {
+            filter_gain = 0.5;
+        } else if(g.rc_feel_rp >= RC_FEEL_RP_MEDIUM) {
+            filter_gain = 0.3;
+        } else if(g.rc_feel_rp >= RC_FEEL_RP_SOFT) {
+            filter_gain = 0.05;
+        } else {
+            // must be RC_FEEL_RP_VERY_SOFT
+            filter_gain = 0.02;
+        }
+        roll_in_filtered = roll_in_filtered * (1.0 - filter_gain) + (float)roll_in * filter_gain;
+        pitch_in_filtered = pitch_in_filtered * (1.0 - filter_gain) + (float)pitch_in * filter_gain;
+    }
+
+    // return filtered roll if no scaling required
     if (g.angle_max == ROLL_PITCH_INPUT_MAX) {
-        roll_out = roll_in;
-        pitch_out = pitch_in;
+        roll_out = (int16_t)roll_in_filtered;
+        pitch_out = (int16_t)pitch_in_filtered;
         return;
     }
 
@@ -25,8 +56,8 @@ static void get_pilot_desired_lean_angles(int16_t roll_in, int16_t pitch_in, int
     }
 
     // convert pilot input to lean angle
-    roll_out = roll_in * _scaler;
-    pitch_out = pitch_in * _scaler;
+    roll_out = (int16_t)(roll_in_filtered * _scaler);
+    pitch_out = (int16_t)(pitch_in_filtered * _scaler);
 }
 
 static void

ArduCopter/GCS_Mavlink.pde

@@ -185,7 +185,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
     if (g.compass_enabled && compass.healthy() && ahrs.use_compass()) {
         control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG;
     }
-    if (g_gps != NULL && g_gps->status() > GPS::NO_GPS && !gps_glitch.glitching()) {
+    if (g_gps != NULL && g_gps->status() > GPS::NO_GPS && (!gps_glitch.glitching()||ap.usb_connected)) {
         control_sensors_health |= MAV_SYS_STATUS_SENSOR_GPS;
     }
     if (ap.rc_receiver_present && !failsafe.radio) {

ArduCopter/Parameters.h

@@ -96,7 +96,8 @@ public:
         k_param_battery,
         k_param_fs_batt_mah,
         k_param_angle_rate_max,
-        k_param_rssi_range,             // 39
+        k_param_rssi_range,
+        k_param_rc_feel_rp,             // 40
 
         // 65: AP_Limits Library
         k_param_limits = 65,            // deprecated - remove
@@ -321,6 +322,7 @@ public:
     AP_Int8         wp_yaw_behavior;            // controls how the autopilot controls yaw during missions
     AP_Int16        angle_max;                  // maximum lean angle of the copter in centi-degrees
     AP_Int32        angle_rate_max;             // maximum rotation rate in roll/pitch axis requested by angle controller used in stabilize, loiter, rtl, auto flight modes
+    AP_Int8         rc_feel_rp;                 // controls vehicle response to user input with 0 being extremely soft and 100 begin extremely crisp
     
     // Waypoints
     //

ArduCopter/Parameters.pde

@@ -435,7 +435,14 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @Range 90000 250000
     // @User: Advanced
     GSCALAR(angle_rate_max, "ANGLE_RATE_MAX",  ANGLE_RATE_MAX),
-    
+
+    // @Param: RC_FEEL_RP
+    // @DisplayName: RC Feel Roll/Pitch
+    // @Description: RC feel for roll/pitch which controls vehicle response to user input with 0 being extremely soft and 100 begin crisp
+    // @User: Advanced
+    // @Values: 0:Very Soft, 25:Soft, 50:Medium, 75:Crisp, 100:Very Crisp
+    GSCALAR(rc_feel_rp, "RC_FEEL_RP",  RC_FEEL_RP_VERY_CRISP),
+
 #if FRAME_CONFIG ==     HELI_FRAME
     // @Group: HS1_
     // @Path: ../libraries/RC_Channel/RC_Channel.cpp

ArduCopter/ReleaseNotes.txt

 ArduCopter Release Notes:
 ------------------------------------------------------------------
+ArduCopter 3.1.5-rc1 14-May-2014
+Changes from 3.1.4
+1) Bug Fix to ignore roll and pitch inputs to loiter controller when in radio failsafe
+2) Bug Fix to allow compassmot to work on Pixhawk
+------------------------------------------------------------------
+ArduCopter 3.1.4 8-May-2014 / 3.1.4-rc1 2-May-2014
+Changes from 3.1.3
+1) Bug Fix for Pixhawk/PX4 NuttX I2C memory corruption when errors are found on I2C bus
+------------------------------------------------------------------
+ArduCopter 3.1.3 7-Apr-2014
+Changes from 3.1.2
+1) Stability patch fix which could cause motors to go to min at full throttle and with large roll/pitch inputs
+------------------------------------------------------------------
+ArduCopter 3.1.2 13-Feb-2014 / ArduCopter 3.1.2-rc2 12-Feb-2014
+Changes from 3.1.2-rc1
+1) GPS Glitch detection disabled when connected via USB
+2) RC_FEEL_RP param added for adjusting responsiveness to pilot roll/pitch input in Stabilize, Drift, AltHold modes
+------------------------------------------------------------------
 ArduCopter 3.1.2-rc1 30-Jan-2014
 Changes from 3.1.1
 1) Pixhawk baro bug fix to SPI communication which could cause large altitude estimate jumps at high temperatures

ArduCopter/defines.h

@@ -171,6 +171,13 @@
 #define ACRO_TRAINER_LEVELING   1
 #define ACRO_TRAINER_LIMITED    2
 
+// RC Feel roll/pitch definitions
+#define RC_FEEL_RP_VERY_SOFT        0
+#define RC_FEEL_RP_SOFT             25
+#define RC_FEEL_RP_MEDIUM           50
+#define RC_FEEL_RP_CRISP            75
+#define RC_FEEL_RP_VERY_CRISP       100
+
 // Commands - Note that APM now uses a subset of the MAVLink protocol
 // commands.  See enum MAV_CMD in the GCS_Mavlink library
 #define CMD_BLANK 0 // there is no command stored in the mem location

libraries/AP_Compass/AP_Compass_PX4.cpp

@@ -92,6 +92,9 @@ bool AP_Compass_PX4::read(void)
     }
 
     for (uint8_t i=0; i<_num_instances; i++) {
+        // avoid division by zero if we haven't received any mag reports
+        if (_count[i] == 0) continue;
+
         _sum[i] /= _count[i];
         _sum[i] *= 1000;
 

libraries/AP_Motors/AP_MotorsMatrix.cpp

@@ -250,7 +250,7 @@ void AP_MotorsMatrix::output_armed()
         thr_adj = _rc_throttle->radio_out - out_best_thr_pwm;
 
         // calc upper and lower limits of thr_adj
-        int16_t thr_adj_max = out_max_pwm-(out_best_thr_pwm+rpy_high);
+        int16_t thr_adj_max = max(out_max_pwm-(out_best_thr_pwm+rpy_high),0);
 
         // if we are increasing the throttle (situation #2 above)..
         if (thr_adj > 0) {