Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: add current based compass compensation

This commit is contained in:
Randy Mackay 2013-03-03 23:02:36 +09:00
parent b8d492b504
commit cb84ec9d9b
7 changed files with 112 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ArduCopter/ArduCopter.pde
View File

@ -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();

2
ArduCopter/GCS_Mavlink.pde
View File

@ -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*/

2
ArduCopter/Parameters.pde
View File

@ -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

4
ArduCopter/defines.h
View File

@ -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. */

11
ArduCopter/sensors.pde
View File

@ -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

118
ArduCopter/setup.pde
View File

@ -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
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();
@ -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
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{
@ -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();
@ -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);
}
@ -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);
}

2
ArduCopter/test.pde
View File

@ -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,