zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Copter: compass mot configures all compasses

master
Randy Mackay 11 years ago
parent
a6c29ba1c3
commit
3c702e5fc5
1 changed files with 61 additions and 40 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 101
      ArduCopter/compassmot.pde

101
ArduCopter/compassmot.pde
Unescape View File

@ -8,14 +8,14 @@ @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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!"));

Write Preview
Loading…
Cancel
Save