Skip to content

Commit

Permalink
Copter: add current based compass compensation
Browse files Browse the repository at this point in the history
  • Loading branch information
@rmackay9
rmackay9 committed Mar 3, 2013
1 parent b8d492b commit cb84ec9
Show file tree
Hide file tree
Showing 7 changed files with 112 additions and 37 deletions.
10 changes: 6 additions & 4 deletions ArduCopter/ArduCopter.pde
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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()) {
Expand Down Expand Up @@ -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
// --------------------------------------------
Expand Down Expand Up @@ -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();

Expand Down
2 changes: 1 addition & 1 deletion ArduCopter/GCS_Mavlink.pde
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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*/
Expand Down
2 changes: 1 addition & 1 deletion ArduCopter/Parameters.pde
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
4 changes: 4 additions & 0 deletions ArduCopter/defines.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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. */

Expand Down
11 changes: 7 additions & 4 deletions ArduCopter/sensors.pde
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
118 changes: 92 additions & 26 deletions ArduCopter/setup.pde
View file
Original file line number Diff line number Diff line change
Expand Up @@ -463,44 +463,72 @@ 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 ) {
cliSerial->print_P(PSTR("compass disabled, exiting"));
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
delay(2000);
// inform what type of compensation we are attempting
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();
Expand Down Expand Up @@ -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);
Expand All @@ -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
motor_impact_scaled = motor_impact / throttle_pct;
// throttle based compensation
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
motor_compensation = motor_compensation * 0.99f - motor_impact_scaled * 0.01f;
// adjust the motor compensation to negate the impact
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{
Expand All @@ -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);
compass.save_motor_compensation();
if( updated ) {
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();
Expand Down Expand Up @@ -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 == 3) cliSerial->printf_P(PSTR("volts"));
if(g.battery_monitoring == 4) cliSerial->printf_P(PSTR("volts and cur"));
if(g.battery_monitoring == BATT_MONITOR_DISABLED) print_enabled(false);
if(g.battery_monitoring == BATT_MONITOR_VOLTAGE_ONLY) cliSerial->printf_P(PSTR("volts"));
if(g.battery_monitoring == BATT_MONITOR_VOLTAGE_AND_CURRENT) cliSerial->printf_P(PSTR("volts and cur"));
print_blanks(2);
}

Expand Down Expand Up @@ -1044,11 +1099,22 @@ static void report_compass()
offsets.z);

// motor compensation
Vector3f motor_compensation = compass.get_motor_compensation();
cliSerial->printf_P(PSTR("Mot comp: %4.0f, %4.0f, %4.0f\n"),
motor_compensation.x,
motor_compensation.y,
motor_compensation.z);
cliSerial->print_P(PSTR("Motor Comp: "));
if( compass.motor_compensation_type() == AP_COMPASS_MOT_COMP_DISABLED ) {
cliSerial->print_P(PSTR("Off\n"));
}else{
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);
}

Expand Down
2 changes: 1 addition & 1 deletion ArduCopter/test.pde
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down

0 comments on commit cb84ec9

Please sign in to comment.