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

101
ArduCopter/compassmot.pde
View File

@ -8,14 +8,14 @@
static uint8_t mavlink_compassmot(mavlink_channel_t chan)
{
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
Vector3f compass_base[COMPASS_MAX_INSTANCES]; // compass vector when throttle is zero
Vector3f motor_impact[COMPASS_MAX_INSTANCES]; // impact of motors on compass vector
Vector3f motor_impact_scaled[COMPASS_MAX_INSTANCES]; // impact of motors on compass vector scaled with throttle
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_max = 0.0f; // maximum throttle reached (as a percentage 0~1.0)
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_send_time;
bool updated = false; // have we updated the compensation vector at least once
@ -38,10 +38,12 @@ static uint8_t mavlink_compassmot(mavlink_channel_t chan)
// check compass health
compass.read();
if (!compass.healthy(0)) {
gcs[chan-MAVLINK_COMM_0].send_text_P(SEVERITY_HIGH, PSTR("check compass"));
ap.compass_mot = false;
return 1;
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"));
ap.compass_mot = false;
return 1;
}
}
// check if radio is calibrated
@ -103,7 +105,9 @@ static uint8_t mavlink_compassmot(mavlink_channel_t chan)
// disable motor compensation
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
last_run_time = millis();
@ -113,10 +117,11 @@ static uint8_t mavlink_compassmot(mavlink_channel_t chan)
compass.read();
// store initial x,y,z compass values
compass_base = compass.get_field();
// initialise motor compensation
motor_compensation = Vector3f(0,0,0);
// initialise interference percentage
for (uint8_t i=0; i<compass.get_count(); i++) {
compass_base[i] = compass.get_field(i);
interference_pct[i] = 0.0f;
}
// enable motors and pass through throttle
init_rc_out();
@ -128,7 +133,7 @@ static uint8_t mavlink_compassmot(mavlink_channel_t chan)
last_send_time = millis();
// 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
if (millis() - last_run_time < 20) {
// grab some compass values
@ -153,43 +158,57 @@ static uint8_t mavlink_compassmot(mavlink_channel_t chan)
// calculate scaling for throttle
throttle_pct = (float)g.rc_3.control_in / 1000.0f;
throttle_pct = constrain_float(throttle_pct,0.0f,1.0f);
// if throttle is near zero, update base x,y,z values
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
} else {
// 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
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;
// for each compass
for (uint8_t i=0; i<compass.get_count(); i++) {
// scale by throttle
motor_impact_scaled[i] = motor_impact[i] / throttle_pct;
// adjust the motor compensation to negate the impact
motor_compensation[i] = motor_compensation[i] * 0.99f - motor_impact_scaled[i] * 0.01f;
}
updated = true;
} else {
// current based compensation if more than 3amps being drawn
motor_impact_scaled = motor_impact / battery.current_amps();
// for each compass
for (uint8_t i=0; i<compass.get_count(); i++) {
// current based compensation if more than 3amps being drawn
motor_impact_scaled[i] = motor_impact[i] / battery.current_amps();
// adjust the motor compensation to negate the impact if drawing over 3amps
if( battery.current_amps() >= 3.0f ) {
motor_compensation = motor_compensation * 0.99f - motor_impact_scaled * 0.01f;
updated = true;
// adjust the motor compensation to negate the impact if drawing over 3amps
if (battery.current_amps() >= 3.0f) {
motor_compensation[i] = motor_compensation[i] * 0.99f - motor_impact_scaled[i] * 0.01f;
updated = true;
}
}
}
// calculate interference percentage at full throttle as % of total mag field
if (comp_type == AP_COMPASS_MOT_COMP_THROTTLE) {
// interference is impact@fullthrottle / mag field * 100
interference_pct = motor_compensation.length() / (float)COMPASS_MAGFIELD_EXPECTED * 100.0f;
for (uint8_t i=0; i<compass.get_count(); i++) {
// interference is impact@fullthrottle / mag field * 100
interference_pct[i] = motor_compensation[i].length() / (float)COMPASS_MAGFIELD_EXPECTED * 100.0f;
}
}else{
// 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;
for (uint8_t i=0; i<compass.get_count(); i++) {
// interference is impact/amp * (max current seen / max throttle seen) / mag field * 100
interference_pct[i] = motor_compensation[i].length() * (current_amps_max/throttle_pct_max) / (float)COMPASS_MAGFIELD_EXPECTED * 100.0f;
}
}
// record maximum throttle and current
@ -202,10 +221,10 @@ static uint8_t mavlink_compassmot(mavlink_channel_t chan)
mavlink_msg_compassmot_status_send(chan,
g.rc_3.control_in,
battery.current_amps(),
interference_pct,
motor_compensation.x,
motor_compensation.y,
motor_compensation.z);
interference_pct[compass.get_primary()],
motor_compensation[compass.get_primary()].x,
motor_compensation[compass.get_primary()].y,
motor_compensation[compass.get_primary()].z);
}
}
@ -216,7 +235,9 @@ static uint8_t mavlink_compassmot(mavlink_channel_t chan)
// set and save motor compensation
if (updated) {
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();
// display success message
gcs[chan-MAVLINK_COMM_0].send_text_P(SEVERITY_HIGH, PSTR("Calibration Successful!"));