Copter: add current based compass compensation

ArduCopter/ArduCopter.pde

@@ -1079,6 +1079,11 @@ static void medium_loop()
     case 0:
         medium_loopCounter++;
 
+        // read battery before compass because it may be used for motor interference compensation
+        if (g.battery_monitoring != 0) {
+            read_battery();
+        }
+
 #if HIL_MODE != HIL_MODE_ATTITUDE                                                               // don't execute in HIL mode
         if(g.compass_enabled) {
             if (compass.read()) {
@@ -1154,10 +1159,6 @@ static void medium_loop()
     case 4:
         medium_loopCounter = 0;
 
-        if (g.battery_monitoring != 0) {
-            read_battery();
-        }
-
         // Accel trims      = hold > 2 seconds
         // Throttle cruise  = switch less than 1 second
         // --------------------------------------------
@@ -1327,6 +1328,7 @@ static void super_slow_loop()
         Log_Write_Data(DATA_AP_STATE, ap.value);
     }
 
+    // log battery info to the dataflash
     if (g.log_bitmask & MASK_LOG_CURRENT && motors.armed())
         Log_Write_Current();
 

ArduCopter/GCS_Mavlink.pde

@@ -208,7 +208,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
         battery_current = current_amps1 * 100;
     }
 
-    if (g.battery_monitoring == 3) {
+    if (g.battery_monitoring == BATT_MONITOR_VOLTAGE_ONLY) {
         /*setting a out-of-range value.
          *  It informs to external devices that
          *  it cannot be calculated properly just by voltage*/

ArduCopter/Parameters.pde

@@ -82,7 +82,7 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @Description: Controls enabling monitoring of the battery's voltage and current
     // @Values: 0:Disabled,3:Voltage Only,4:Voltage and Current
     // @User: Standard
-    GSCALAR(battery_monitoring, "BATT_MONITOR", DISABLED),
+    GSCALAR(battery_monitoring, "BATT_MONITOR", BATT_MONITOR_DISABLED),
 
     // @Param: FS_BATT_ENABLE
     // @DisplayName: Battery Failsafe Enable

ArduCopter/defines.h

@@ -350,6 +350,10 @@ enum gcs_severity {
 #define BATTERY_VOLTAGE(x) (x->voltage_average()*g.volt_div_ratio)
 #define CURRENT_AMPS(x) (x->voltage_average()-CURR_AMPS_OFFSET)*g.curr_amp_per_volt
 
+#define BATT_MONITOR_DISABLED               0
+#define BATT_MONITOR_VOLTAGE_ONLY           3
+#define BATT_MONITOR_VOLTAGE_AND_CURRENT    4
+
 /* ************************************************************** */
 /* Expansion PIN's that people can use for various things. */
 

ArduCopter/sensors.pde

@@ -111,23 +111,26 @@ static void read_battery(void)
 {
     static uint8_t low_battery_counter = 0;
 
-    if(g.battery_monitoring == 0) {
+    if(g.battery_monitoring == BATT_MONITOR_DISABLED) {
         battery_voltage1 = 0;
         return;
     }
 
-    if(g.battery_monitoring == 3 || g.battery_monitoring == 4) {
+    if(g.battery_monitoring == BATT_MONITOR_VOLTAGE_ONLY || g.battery_monitoring == BATT_MONITOR_VOLTAGE_AND_CURRENT) {
         batt_volt_analog_source->set_pin(g.battery_volt_pin);
         battery_voltage1 = BATTERY_VOLTAGE(batt_volt_analog_source);
     }
-    if(g.battery_monitoring == 4) {
+    if(g.battery_monitoring == BATT_MONITOR_VOLTAGE_AND_CURRENT) {
         batt_curr_analog_source->set_pin(g.battery_curr_pin);
         current_amps1    = CURRENT_AMPS(batt_curr_analog_source);
         current_total1   += current_amps1 * 0.02778f;            // called at 100ms on average, .0002778 is 1/3600 (conversion to hours)
+
+        // update compass with current value
+        compass.set_current(current_amps1);
     }
 
     // check for low voltage or current if the low voltage check hasn't already been triggered
-    if (!ap.low_battery && ( battery_voltage1 < g.low_voltage || (g.battery_monitoring == 4 && current_total1 > g.pack_capacity))) {
+    if (!ap.low_battery && ( battery_voltage1 < g.low_voltage || (g.battery_monitoring == BATT_MONITOR_VOLTAGE_AND_CURRENT && current_total1 > g.pack_capacity))) {
         low_battery_counter++;
         if( low_battery_counter >= BATTERY_FS_COUNTER ) {
             low_battery_counter = BATTERY_FS_COUNTER;   // ensure counter does not overflow

ArduCopter/setup.pde

@@ -463,13 +463,32 @@ setup_compass(uint8_t argc, const Menu::arg *argv)
 static int8_t
 setup_compassmot(uint8_t argc, const Menu::arg *argv)
 {
+    int8_t   comp_type;             // throttle or current based compensation
     Vector3f compass_base;          // compass vector when throttle is zero
     Vector3f motor_impact;          // impact of motors on compass vector
     Vector3f motor_impact_scaled;   // impact of motors on compass vector scaled with throttle
     Vector3f motor_compensation;    // final compensation to be stored to eeprom
-    float throttle_pct;             // throttle as a percentage 0.0 ~ 1.0
+    float    throttle_pct;          // throttle as a percentage 0.0 ~ 1.0
     uint32_t last_run_time;
-    uint8_t print_counter = 0;
+    uint8_t  print_counter = 49;
+    bool     updated = false;       // have we updated the compensation vector at least once
+
+    // default compensation type to use current if possible
+    if( g.battery_monitoring == BATT_MONITOR_VOLTAGE_AND_CURRENT ) {
+        comp_type = AP_COMPASS_MOT_COMP_CURRENT;
+    }else{
+        comp_type = AP_COMPASS_MOT_COMP_THROTTLE;
+    }
+
+    // check if user wants throttle compensation
+    if( !strcmp_P(argv[1].str, PSTR("t")) || !strcmp_P(argv[1].str, PSTR("T")) ) {
+        comp_type = AP_COMPASS_MOT_COMP_THROTTLE;
+    }
+
+    // check if user wants current compensation
+    if( !strcmp_P(argv[1].str, PSTR("c")) || !strcmp_P(argv[1].str, PSTR("C")) ) {
+        comp_type = AP_COMPASS_MOT_COMP_CURRENT;
+    }
 
     // check compass is enabled
     if( !g.compass_enabled ) {
@@ -477,30 +496,39 @@ setup_compassmot(uint8_t argc, const Menu::arg *argv)
         return 0;
     }
 
+    // check if we have a current monitor
+    if( comp_type == AP_COMPASS_MOT_COMP_CURRENT && g.battery_monitoring != BATT_MONITOR_VOLTAGE_AND_CURRENT ) {
+        cliSerial->print_P(PSTR("current monitor disabled, exiting"));
+        return 0;
+    }
+
     // initialise compass
     init_compass();
 
-    // set motor compensation to zero
+    // disable motor compensation
+    compass.motor_compensation_type(AP_COMPASS_MOT_COMP_DISABLED);
     compass.set_motor_compensation(Vector3f(0,0,0));
 
     // print warning that motors will spin
-    cliSerial->print_P(PSTR("This setup records the impact on the compass of spinning up the motors.  The motors will spin!\n"));
-
     // ask user to raise throttle
-    cliSerial->print_P(PSTR("Hold throttle low, then raise as high as possible (without taking off) for 10 sec.\n"));
-
     // inform how to stop test
-    cliSerial->print_P(PSTR("At any time you may press any key to exit.\n"));
+    cliSerial->print_P(PSTR("This setup records the impact on the compass of spinning up the motors.  The motors will spin!\nHold throttle low, then raise as high as safely possible for 10 sec.\nAt any time you may press any key to exit.\nmeasuring compass vs "));
 
-    // wait 1 second for user to read message
+    // inform what type of compensation we are attempting
-    delay(2000);
+    if( comp_type == AP_COMPASS_MOT_COMP_CURRENT ) {
+        cliSerial->print_P(PSTR("CURRENT\n"));
+    }else{
+        cliSerial->print_P(PSTR("THROTTLE\n"));
+    }
 
+    // clear out user input
     while( cliSerial->available() ) {
         cliSerial->read();
     }
 
-    // disable throttle failsafe
+    // disable throttle and battery failsafe
     g.failsafe_throttle = FS_THR_DISABLED;
+    g.failsafe_battery_enabled = false;
 
     // read radio
     read_radio();
@@ -561,6 +589,9 @@ setup_compassmot(uint8_t argc, const Menu::arg *argv)
             // read some compass values
             compass.read();
 
+            // read current
+            read_battery();
+
             // calculate scaling for throttle
             throttle_pct = (float)g.rc_3.control_in / 1000.0f;
             throttle_pct = constrain(throttle_pct,0.0f,1.0f);
@@ -574,22 +605,36 @@ setup_compassmot(uint8_t argc, const Menu::arg *argv)
                 // causing printing to happen as soon as throttle is lifted
                 print_counter = 49;
             }else{
+
                 // calculate diff from compass base and scale with throttle
                 motor_impact.x = compass.mag_x - compass_base.x;
                 motor_impact.y = compass.mag_y - compass_base.y;
                 motor_impact.z = compass.mag_z - compass_base.z;
 
-                // scale by throttle
+                // throttle based compensation
-                motor_impact_scaled = motor_impact / throttle_pct;
+                if( comp_type == AP_COMPASS_MOT_COMP_THROTTLE ) {
+                    // scale by throttle
+                    motor_impact_scaled = motor_impact / throttle_pct;
 
-                // adjust the motor compensation to negate the impact
+                    // adjust the motor compensation to negate the impact
-                motor_compensation = motor_compensation * 0.99f - motor_impact_scaled * 0.01f;
+                    motor_compensation = motor_compensation * 0.99f - motor_impact_scaled * 0.01f;
+                    updated = true;
+                }else{
+                    // current based compensation if more than 3amps being drawn
+                    motor_impact_scaled = motor_impact / current_amps1;
+
+                    // adjust the motor compensation to negate the impact if drawing over 3amps
+                    if( current_amps1 >= 3.0f ) {
+                        motor_compensation = motor_compensation * 0.99f - motor_impact_scaled * 0.01f;
+                        updated = true;
+                    }
+                }
 
                 // display output at 1hz if throttle is above zero
                 print_counter++;
                 if(print_counter >= 50) {
                     print_counter = 0;
-                    cliSerial->printf_P(PSTR("thr:%d mot x:%4.1f\ty:%4.1f\tz:%4.1f\tcomp x:%4.1f\ty:%4.1f\tz:%4.1f\n"),(int)g.rc_3.control_in, (float)motor_impact.x, (float)motor_impact.y, (float)motor_impact.z, (float)motor_compensation.x, (float)motor_compensation.y, (float)motor_compensation.z);
+                    cliSerial->printf_P(PSTR("thr:%d cur:%4.2f mot x:%4.1f y:%4.1f z:%4.1f  comp x:%4.2f y:%4.2f z:%4.2f\n"),(int)g.rc_3.control_in, (float)current_amps1, (float)motor_impact.x, (float)motor_impact.y, (float)motor_impact.z, (float)motor_compensation.x, (float)motor_compensation.y, (float)motor_compensation.z);
                 }
             }
         }else{
@@ -607,9 +652,19 @@ setup_compassmot(uint8_t argc, const Menu::arg *argv)
         cliSerial->read();
     }
 
+    // print one more time so the last thing printed matches what appears in the report_compass
+    cliSerial->printf_P(PSTR("thr:%d cur:%4.2f mot x:%4.1f y:%4.1f z:%4.1f  comp x:%4.2f y:%4.2f z:%4.2f\n"),(int)g.rc_3.control_in, (float)current_amps1, (float)motor_impact.x, (float)motor_impact.y, (float)motor_impact.z, (float)motor_compensation.x, (float)motor_compensation.y, (float)motor_compensation.z);
+
     // set and save motor compensation
-    compass.set_motor_compensation(motor_compensation);
+    if( updated ) {
-    compass.save_motor_compensation();
+        compass.motor_compensation_type(comp_type);
+        compass.set_motor_compensation(motor_compensation);
+        compass.save_motor_compensation();
+    }else{
+        // compensation vector never updated, report failure
+        cliSerial->printf_P(PSTR("Failed! Compensation disabled.  Did you forget to raise the throttle high enough?"));
+        compass.motor_compensation_type(AP_COMPASS_MOT_COMP_DISABLED);
+    }
 
     // display new motor offsets and save
     report_compass();
@@ -946,9 +1001,9 @@ static void report_batt_monitor()
 {
     cliSerial->printf_P(PSTR("\nBatt Mon:\n"));
     print_divider();
-    if(g.battery_monitoring == 0) print_enabled(false);
+    if(g.battery_monitoring == BATT_MONITOR_DISABLED) print_enabled(false);
-    if(g.battery_monitoring == 3) cliSerial->printf_P(PSTR("volts"));
+    if(g.battery_monitoring == BATT_MONITOR_VOLTAGE_ONLY) cliSerial->printf_P(PSTR("volts"));
-    if(g.battery_monitoring == 4) cliSerial->printf_P(PSTR("volts and cur"));
+    if(g.battery_monitoring == BATT_MONITOR_VOLTAGE_AND_CURRENT) cliSerial->printf_P(PSTR("volts and cur"));
     print_blanks(2);
 }
 
@@ -1044,11 +1099,22 @@ static void report_compass()
                     offsets.z);
 
     // motor compensation
-    Vector3f motor_compensation = compass.get_motor_compensation();
+    cliSerial->print_P(PSTR("Motor Comp: "));
-    cliSerial->printf_P(PSTR("Mot comp: %4.0f, %4.0f, %4.0f\n"),
+    if( compass.motor_compensation_type() == AP_COMPASS_MOT_COMP_DISABLED ) {
-                    motor_compensation.x,
+        cliSerial->print_P(PSTR("Off\n"));
-                    motor_compensation.y,
+    }else{
-                    motor_compensation.z);
+        if( compass.motor_compensation_type() == AP_COMPASS_MOT_COMP_THROTTLE ) {
+            cliSerial->print_P(PSTR("Throttle"));
+        }
+        if( compass.motor_compensation_type() == AP_COMPASS_MOT_COMP_CURRENT ) {
+            cliSerial->print_P(PSTR("Current"));
+        }
+        Vector3f motor_compensation = compass.get_motor_compensation();
+        cliSerial->printf_P(PSTR("\nComp Vec: %4.2f, %4.2f, %4.2f\n"),
+                        motor_compensation.x,
+                        motor_compensation.y,
+                        motor_compensation.z);
+    }
     print_blanks(1);
 }
 

ArduCopter/test.pde

@@ -680,7 +680,7 @@ test_battery(uint8_t argc, const Menu::arg *argv)
         delay(100);
         read_radio();
         read_battery();
-        if (g.battery_monitoring == 3) {
+        if (g.battery_monitoring == BATT_MONITOR_VOLTAGE_ONLY) {
             cliSerial->printf_P(PSTR("V: %4.4f\n"),
                             battery_voltage1,
                             current_amps1,