mirror of https://github.com/ArduPilot/ardupilot
Copter: compass mot configures all compasses
This commit is contained in:
parent
a6c29ba1c3
commit
3c702e5fc5
|
@ -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,11 +38,13 @@ static uint8_t mavlink_compassmot(mavlink_channel_t chan)
|
|||
|
||||
// check compass health
|
||||
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"));
|
||||
ap.compass_mot = false;
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
||||
// check if radio is calibrated
|
||||
pre_arm_rc_checks();
|
||||
|
@ -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
|
||||
|
@ -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_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) {
|
||||
// for each compass
|
||||
for (uint8_t i=0; i<compass.get_count(); i++) {
|
||||
// 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
|
||||
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;
|
||||
} else {
|
||||
// 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 = 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
|
||||
if( battery.current_amps() >= 3.0f ) {
|
||||
motor_compensation = motor_compensation * 0.99f - motor_impact_scaled * 0.01f;
|
||||
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) {
|
||||
for (uint8_t i=0; i<compass.get_count(); i++) {
|
||||
// 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{
|
||||
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 = 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
|
||||
|
@ -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!"));
|
||||
|
|
Loading…
Reference in New Issue