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

Copter: compass mot configures all compasses

This commit is contained in:
Randy Mackay 2014-07-22 16:31:45 +09:00
parent a6c29ba1c3
commit 3c702e5fc5
1 changed files with 61 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

75
ArduCopter/compassmot.pde
View File

@ -8,14 +8,14 @@
static uint8_t mavlink_compassmot(mavlink_channel_t chan) static uint8_t mavlink_compassmot(mavlink_channel_t chan)
{ {
int8_t comp_type; // throttle or current based compensation int8_t comp_type; // throttle or current based compensation
Vector3f compass_base; // compass vector when throttle is zero Vector3f compass_base[COMPASS_MAX_INSTANCES]; // compass vector when throttle is zero
Vector3f motor_impact; // impact of motors on compass vector Vector3f motor_impact[COMPASS_MAX_INSTANCES]; // impact of motors on compass vector
Vector3f motor_impact_scaled; // impact of motors on compass vector scaled with throttle Vector3f motor_impact_scaled[COMPASS_MAX_INSTANCES]; // impact of motors on compass vector scaled with throttle
Vector3f motor_compensation; // final compensation to be stored to eeprom Vector3f motor_compensation[COMPASS_MAX_INSTANCES]; // 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
float throttle_pct_max = 0.0f; // maximum throttle reached (as a percentage 0~1.0) float throttle_pct_max = 0.0f; // maximum throttle reached (as a percentage 0~1.0)
float current_amps_max = 0.0f; // maximum current reached float current_amps_max = 0.0f; // maximum current reached
float interference_pct = 0.0f; // interference as a percentage of total mag field (for reporting purposes only) float interference_pct[COMPASS_MAX_INSTANCES]; // interference as a percentage of total mag field (for reporting purposes only)
uint32_t last_run_time; uint32_t last_run_time;
uint32_t last_send_time; uint32_t last_send_time;
bool updated = false; // have we updated the compensation vector at least once bool updated = false; // have we updated the compensation vector at least once
@ -38,11 +38,13 @@ static uint8_t mavlink_compassmot(mavlink_channel_t chan)
// check compass health // check compass health
compass.read(); compass.read();
if (!compass.healthy(0)) { for (uint8_t i=0; i<compass.get_count(); i++) {
if (!compass.healthy(i)) {
gcs[chan-MAVLINK_COMM_0].send_text_P(SEVERITY_HIGH, PSTR("check compass")); gcs[chan-MAVLINK_COMM_0].send_text_P(SEVERITY_HIGH, PSTR("check compass"));
ap.compass_mot = false; ap.compass_mot = false;
return 1; return 1;
} }
}
// check if radio is calibrated // check if radio is calibrated
pre_arm_rc_checks(); pre_arm_rc_checks();
@ -103,7 +105,9 @@ static uint8_t mavlink_compassmot(mavlink_channel_t chan)
// disable motor compensation // disable motor compensation
compass.motor_compensation_type(AP_COMPASS_MOT_COMP_DISABLED); compass.motor_compensation_type(AP_COMPASS_MOT_COMP_DISABLED);
compass.set_motor_compensation(Vector3f(0,0,0)); for (uint8_t i=0; i<compass.get_count(); i++) {
compass.set_motor_compensation(i, Vector3f(0,0,0));
}
// get initial compass readings // get initial compass readings
last_run_time = millis(); last_run_time = millis();
@ -113,10 +117,11 @@ static uint8_t mavlink_compassmot(mavlink_channel_t chan)
compass.read(); compass.read();
// store initial x,y,z compass values // store initial x,y,z compass values
compass_base = compass.get_field(); // initialise interference percentage
for (uint8_t i=0; i<compass.get_count(); i++) {
// initialise motor compensation compass_base[i] = compass.get_field(i);
motor_compensation = Vector3f(0,0,0); interference_pct[i] = 0.0f;
}
// enable motors and pass through throttle // enable motors and pass through throttle
init_rc_out(); init_rc_out();
@ -128,7 +133,7 @@ static uint8_t mavlink_compassmot(mavlink_channel_t chan)
last_send_time = millis(); last_send_time = millis();
// main run while there is no user input and the compass is healthy // main run while there is no user input and the compass is healthy
while (command_ack_start == command_ack_counter && compass.healthy(0) && motors.armed()) { while (command_ack_start == command_ack_counter && compass.healthy(compass.get_primary()) && motors.armed()) {
// 50hz loop // 50hz loop
if (millis() - last_run_time < 20) { if (millis() - last_run_time < 20) {
// grab some compass values // grab some compass values
@ -156,40 +161,54 @@ static uint8_t mavlink_compassmot(mavlink_channel_t chan)
// if throttle is near zero, update base x,y,z values // if throttle is near zero, update base x,y,z values
if (throttle_pct <= 0.0f) { if (throttle_pct <= 0.0f) {
compass_base = compass_base * 0.99f + compass.get_field() * 0.01f; for (uint8_t i=0; i<compass.get_count(); i++) {
compass_base[i] = compass_base[i] * 0.99f + compass.get_field(i) * 0.01f;
}
// causing printing to happen as soon as throttle is lifted // causing printing to happen as soon as throttle is lifted
} else { } else {
// calculate diff from compass base and scale with throttle // calculate diff from compass base and scale with throttle
motor_impact = compass.get_field() - compass_base; for (uint8_t i=0; i<compass.get_count(); i++) {
motor_impact[i] = compass.get_field(i) - compass_base[i];
}
// throttle based compensation // throttle based compensation
if (comp_type == AP_COMPASS_MOT_COMP_THROTTLE) { if (comp_type == AP_COMPASS_MOT_COMP_THROTTLE) {
// for each compass
for (uint8_t i=0; i<compass.get_count(); i++) {
// scale by throttle // scale by throttle
motor_impact_scaled = motor_impact / throttle_pct; motor_impact_scaled[i] = motor_impact[i] / 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[i] = motor_compensation[i] * 0.99f - motor_impact_scaled[i] * 0.01f;
}
updated = true; updated = true;
} else { } else {
// for each compass
for (uint8_t i=0; i<compass.get_count(); i++) {
// current based compensation if more than 3amps being drawn // current based compensation if more than 3amps being drawn
motor_impact_scaled = motor_impact / battery.current_amps(); motor_impact_scaled[i] = motor_impact[i] / battery.current_amps();
// adjust the motor compensation to negate the impact if drawing over 3amps // adjust the motor compensation to negate the impact if drawing over 3amps
if( battery.current_amps() >= 3.0f ) { if (battery.current_amps() >= 3.0f) {
motor_compensation = motor_compensation * 0.99f - motor_impact_scaled * 0.01f; motor_compensation[i] = motor_compensation[i] * 0.99f - motor_impact_scaled[i] * 0.01f;
updated = true; updated = true;
} }
} }
}
// calculate interference percentage at full throttle as % of total mag field // calculate interference percentage at full throttle as % of total mag field
if (comp_type == AP_COMPASS_MOT_COMP_THROTTLE) { if (comp_type == AP_COMPASS_MOT_COMP_THROTTLE) {
for (uint8_t i=0; i<compass.get_count(); i++) {
// interference is impact@fullthrottle / mag field * 100 // interference is impact@fullthrottle / mag field * 100
interference_pct = motor_compensation.length() / (float)COMPASS_MAGFIELD_EXPECTED * 100.0f; interference_pct[i] = motor_compensation[i].length() / (float)COMPASS_MAGFIELD_EXPECTED * 100.0f;
}
}else{ }else{
for (uint8_t i=0; i<compass.get_count(); i++) {
// interference is impact/amp * (max current seen / max throttle seen) / mag field * 100 // interference is impact/amp * (max current seen / max throttle seen) / mag field * 100
interference_pct = motor_compensation.length() * (current_amps_max/throttle_pct_max) / (float)COMPASS_MAGFIELD_EXPECTED * 100.0f; interference_pct[i] = motor_compensation[i].length() * (current_amps_max/throttle_pct_max) / (float)COMPASS_MAGFIELD_EXPECTED * 100.0f;
}
} }
// record maximum throttle and current // record maximum throttle and current
@ -202,10 +221,10 @@ static uint8_t mavlink_compassmot(mavlink_channel_t chan)
mavlink_msg_compassmot_status_send(chan, mavlink_msg_compassmot_status_send(chan,
g.rc_3.control_in, g.rc_3.control_in,
battery.current_amps(), battery.current_amps(),
interference_pct, interference_pct[compass.get_primary()],
motor_compensation.x, motor_compensation[compass.get_primary()].x,
motor_compensation.y, motor_compensation[compass.get_primary()].y,
motor_compensation.z); motor_compensation[compass.get_primary()].z);
} }
} }
@ -216,7 +235,9 @@ static uint8_t mavlink_compassmot(mavlink_channel_t chan)
// set and save motor compensation // set and save motor compensation
if (updated) { if (updated) {
compass.motor_compensation_type(comp_type); compass.motor_compensation_type(comp_type);
compass.set_motor_compensation(motor_compensation); for (uint8_t i=0; i<compass.get_count(); i++) {
compass.set_motor_compensation(i, motor_compensation[i]);
}
compass.save_motor_compensation(); compass.save_motor_compensation();
// display success message // display success message
gcs[chan-MAVLINK_COMM_0].send_text_P(SEVERITY_HIGH, PSTR("Calibration Successful!")); gcs[chan-MAVLINK_COMM_0].send_text_P(SEVERITY_HIGH, PSTR("Calibration Successful!"));