Browse Source
use the MAVLink interact code to allow for compassmot over MAVLink Pair-Programmed-With: Randy Mackay <rmackay9@yahoo.com>master
Andrew Tridgell
11 years ago
4 changed files with 219 additions and 225 deletions
@ -0,0 +1,206 @@ |
|||||||
|
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- |
||||||
|
|
||||||
|
/* |
||||||
|
compass/motor interference calibration |
||||||
|
*/ |
||||||
|
|
||||||
|
// setup_compassmot - sets compass's motor interference parameters |
||||||
|
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 |
||||||
|
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; // 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 |
||||||
|
uint8_t command_ack_start = command_ack_counter; |
||||||
|
|
||||||
|
// default compensation type to use current if possible |
||||||
|
if (battery.monitoring() == AP_BATT_MONITOR_VOLTAGE_AND_CURRENT) { |
||||||
|
comp_type = AP_COMPASS_MOT_COMP_CURRENT; |
||||||
|
}else{ |
||||||
|
comp_type = AP_COMPASS_MOT_COMP_THROTTLE; |
||||||
|
} |
||||||
|
|
||||||
|
// check if radio is calibrated |
||||||
|
pre_arm_rc_checks(); |
||||||
|
if (!ap.pre_arm_rc_check) { |
||||||
|
gcs[chan-MAVLINK_COMM_0].send_text_P(SEVERITY_HIGH, PSTR("radio not calibrated")); |
||||||
|
return 1; |
||||||
|
} |
||||||
|
|
||||||
|
// check compass is enabled |
||||||
|
if ( !g.compass_enabled ) { |
||||||
|
gcs[chan-MAVLINK_COMM_0].send_text_P(SEVERITY_HIGH, PSTR("compass disabled\n")); |
||||||
|
return 1; |
||||||
|
} |
||||||
|
|
||||||
|
// initialise compass |
||||||
|
init_compass(); |
||||||
|
|
||||||
|
// 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 |
||||||
|
// ask user to raise throttle |
||||||
|
// inform how to stop test |
||||||
|
gcs[chan-MAVLINK_COMM_0].send_text_P(SEVERITY_HIGH, PSTR("STARTING CALIBRATION")); |
||||||
|
|
||||||
|
// inform what type of compensation we are attempting |
||||||
|
if ( comp_type == AP_COMPASS_MOT_COMP_CURRENT ) { |
||||||
|
gcs[chan-MAVLINK_COMM_0].send_text_P(SEVERITY_HIGH, PSTR("CURRENT")); |
||||||
|
} else{ |
||||||
|
gcs[chan-MAVLINK_COMM_0].send_text_P(SEVERITY_HIGH, PSTR("THROTTLE")); |
||||||
|
} |
||||||
|
|
||||||
|
// disable throttle and battery failsafe |
||||||
|
g.failsafe_throttle = FS_THR_DISABLED; |
||||||
|
g.failsafe_battery_enabled = FS_BATT_DISABLED; |
||||||
|
|
||||||
|
// read radio |
||||||
|
read_radio(); |
||||||
|
|
||||||
|
// exit immediately if throttle is not zero |
||||||
|
if ( g.rc_3.control_in != 0 ) { |
||||||
|
gcs[chan-MAVLINK_COMM_0].send_text_P(SEVERITY_HIGH, PSTR("throttle not zero")); |
||||||
|
return 1; |
||||||
|
} |
||||||
|
|
||||||
|
// get some initial compass readings |
||||||
|
last_run_time = millis(); |
||||||
|
while ( millis() - last_run_time < 2000 ) { |
||||||
|
compass.accumulate(); |
||||||
|
} |
||||||
|
compass.read(); |
||||||
|
|
||||||
|
// exit immediately if the compass is not healthy |
||||||
|
if ( !compass.healthy() ) { |
||||||
|
gcs[chan-MAVLINK_COMM_0].send_text_P(SEVERITY_HIGH, PSTR("check compass")); |
||||||
|
return 0; |
||||||
|
} |
||||||
|
|
||||||
|
// store initial x,y,z compass values |
||||||
|
compass_base = compass.get_field(); |
||||||
|
|
||||||
|
// initialise motor compensation |
||||||
|
motor_compensation = Vector3f(0,0,0); |
||||||
|
|
||||||
|
// enable motors and pass through throttle |
||||||
|
init_rc_out(); |
||||||
|
output_min(); |
||||||
|
motors.armed(true); |
||||||
|
|
||||||
|
// initialise run time |
||||||
|
last_run_time = millis(); |
||||||
|
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()) { |
||||||
|
// 50hz loop |
||||||
|
if( millis() - last_run_time < 20 ) { |
||||||
|
// grab some compass values |
||||||
|
compass.accumulate(); |
||||||
|
hal.scheduler->delay(5); |
||||||
|
continue; |
||||||
|
} |
||||||
|
last_run_time = millis(); |
||||||
|
|
||||||
|
// read radio input |
||||||
|
read_radio(); |
||||||
|
|
||||||
|
// pass through throttle to motors |
||||||
|
motors.throttle_pass_through(); |
||||||
|
|
||||||
|
// 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_float(throttle_pct,0.0f,1.0f); |
||||||
|
|
||||||
|
// if throttle is zero, update base x,y,z values |
||||||
|
if( throttle_pct == 0.0f ) { |
||||||
|
compass_base = compass_base * 0.99f + compass.get_field() * 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; |
||||||
|
|
||||||
|
// 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; |
||||||
|
updated = true; |
||||||
|
} else { |
||||||
|
// current based compensation if more than 3amps being drawn |
||||||
|
motor_impact_scaled = motor_impact / 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; |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
// record maximum throttle and current |
||||||
|
throttle_pct_max = max(throttle_pct_max, throttle_pct); |
||||||
|
current_amps_max = max(current_amps_max, battery.current_amps()); |
||||||
|
|
||||||
|
} |
||||||
|
if (hal.scheduler->millis() - last_send_time > 500) { |
||||||
|
last_send_time = hal.scheduler->millis(); |
||||||
|
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); |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
// stop motors |
||||||
|
motors.output_min(); |
||||||
|
motors.armed(false); |
||||||
|
|
||||||
|
// set and save motor compensation |
||||||
|
if( updated ) { |
||||||
|
compass.motor_compensation_type(comp_type); |
||||||
|
compass.set_motor_compensation(motor_compensation); |
||||||
|
compass.save_motor_compensation(); |
||||||
|
|
||||||
|
// calculate and display interference compensation 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; |
||||||
|
}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; |
||||||
|
} |
||||||
|
} else { |
||||||
|
// compensation vector never updated, report failure |
||||||
|
gcs[chan-MAVLINK_COMM_0].send_text_P(SEVERITY_HIGH, PSTR("Failed! Compensation disabled")); |
||||||
|
compass.motor_compensation_type(AP_COMPASS_MOT_COMP_DISABLED); |
||||||
|
} |
||||||
|
|
||||||
|
// display new motor offsets and save |
||||||
|
report_compass(); |
||||||
|
|
||||||
|
return 0; |
||||||
|
} |
||||||
|
|
||||||
Loading…
Reference in new issue