mirror of https://github.com/ArduPilot/ardupilot
Copter: added compassmot over MAVLink
use the MAVLink interact code to allow for compassmot over MAVLink Pair-Programmed-With: Randy Mackay <rmackay9@yahoo.com>
This commit is contained in:
Andrew Tridgell
parent
50cbc661ef
commit
76b60a1a52
|
|
@ -186,7 +186,8 @@ static AP_Scheduler scheduler;
|
|||
// AP_Notify instance
|
||||
static AP_Notify notify;
|
||||
|
||||
|
||||
// used to detect MAVLink acks from GCS to stop compassmot
|
||||
static uint8_t command_ack_counter;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// prototypes
|
||||
|
|
|
|||
|
|
@ -1143,6 +1143,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_COMMAND_ACK:
|
||||
{
|
||||
command_ack_counter++;
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_COMMAND_LONG:
|
||||
{
|
||||
// decode
|
||||
|
|
@ -1178,6 +1184,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|||
break;
|
||||
|
||||
case MAV_CMD_PREFLIGHT_CALIBRATION:
|
||||
result = MAV_RESULT_ACCEPTED;
|
||||
if (packet.param1 == 1 ||
|
||||
packet.param2 == 1) {
|
||||
ins.init_accel();
|
||||
|
|
@ -1198,7 +1205,10 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|||
ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
|
||||
}
|
||||
}
|
||||
result = MAV_RESULT_ACCEPTED;
|
||||
if (packet.param6 == 1) {
|
||||
// compassmot calibration
|
||||
result = mavlink_compassmot(chan);
|
||||
}
|
||||
break;
|
||||
|
||||
case MAV_CMD_COMPONENT_ARM_DISARM:
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
@ -14,7 +14,6 @@ static int8_t setup_sonar (uint8_t argc, const Menu::arg *argv);
|
|||
const struct Menu::command setup_menu_commands[] PROGMEM = {
|
||||
// command function called
|
||||
// ======= ===============
|
||||
{"compassmot", setup_compassmot},
|
||||
{"reset", setup_factory},
|
||||
{"set", setup_set},
|
||||
{"show", setup_show},
|
||||
|
|
@ -42,228 +41,6 @@ setup_mode(uint8_t argc, const Menu::arg *argv)
|
|||
return 0;
|
||||
}
|
||||
|
||||
// setup_compassmot - sets compass's motor interference parameters
|
||||
static int8_t
|
||||
setup_compassmot(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
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;
|
||||
uint8_t print_counter = 49;
|
||||
bool updated = false; // have we updated the compensation vector at least once
|
||||
|
||||
// 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 calibration
|
||||
pre_arm_rc_checks();
|
||||
if(!ap.pre_arm_rc_check) {
|
||||
cliSerial->print_P(PSTR("radio not calibrated\n"));
|
||||
return 0;
|
||||
}
|
||||
|
||||
// check compass is enabled
|
||||
if( !g.compass_enabled ) {
|
||||
cliSerial->print_P(PSTR("compass disabled\n"));
|
||||
return 0;
|
||||
}
|
||||
|
||||
// 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
|
||||
cliSerial->print_P(PSTR("This records the impact on the compass of running the motors. Motors will spin!\nHold throttle low, then raise to mid for 5 sec, then quickly back to low.\nPress any key to exit.\n\nmeasuring compass vs "));
|
||||
|
||||
// inform what type of compensation we are attempting
|
||||
if( comp_type == AP_COMPASS_MOT_COMP_CURRENT ) {
|
||||
cliSerial->print_P(PSTR("CURRENT\n"));
|
||||
}else{
|
||||
cliSerial->print_P(PSTR("THROTTLE\n"));
|
||||
}
|
||||
|
||||
// clear out user input
|
||||
while( cliSerial->available() ) {
|
||||
cliSerial->read();
|
||||
}
|
||||
|
||||
// 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 ) {
|
||||
cliSerial->print_P(PSTR("throttle not zero\n"));
|
||||
return 0;
|
||||
}
|
||||
|
||||
// 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() ) {
|
||||
cliSerial->print_P(PSTR("check compass\n"));
|
||||
return 0;
|
||||
}
|
||||
|
||||
// store initial x,y,z compass values
|
||||
compass_base = compass.get_field();
|
||||
|
||||
// initialise motor compensation
|
||||
motor_compensation = Vector3f(0,0,0);
|
||||
|
||||
// clear out any user input
|
||||
while( cliSerial->available() ) {
|
||||
cliSerial->read();
|
||||
}
|
||||
|
||||
// enable motors and pass through throttle
|
||||
init_rc_out();
|
||||
output_min();
|
||||
motors.armed(true);
|
||||
|
||||
// initialise run time
|
||||
last_run_time = millis();
|
||||
|
||||
// main run while there is no user input and the compass is healthy
|
||||
while(!cliSerial->available() && compass.healthy()) {
|
||||
|
||||
// 50hz loop
|
||||
if( millis() - last_run_time > 20 ) {
|
||||
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
|
||||
print_counter = 49;
|
||||
}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());
|
||||
|
||||
// display output at 1hz if throttle is above zero
|
||||
print_counter++;
|
||||
if(print_counter >= 50) {
|
||||
print_counter = 0;
|
||||
display_compassmot_info(motor_impact, motor_compensation);
|
||||
}
|
||||
}
|
||||
}else{
|
||||
// grab some compass values
|
||||
compass.accumulate();
|
||||
}
|
||||
}
|
||||
|
||||
// stop motors
|
||||
motors.output_min();
|
||||
motors.armed(false);
|
||||
|
||||
// clear out any user input
|
||||
while( cliSerial->available() ) {
|
||||
cliSerial->read();
|
||||
}
|
||||
|
||||
// print one more time so the last thing printed matches what appears in the report_compass
|
||||
display_compassmot_info(motor_impact, motor_compensation);
|
||||
|
||||
// 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;
|
||||
}
|
||||
cliSerial->printf_P(PSTR("\nInterference at full throttle is %d%% of mag field\n\n"),(int)interference_pct);
|
||||
}else{
|
||||
// compensation vector never updated, report failure
|
||||
cliSerial->printf_P(PSTR("Failed! Compensation disabled. Did you forget to raise the throttle high enough?"));
|
||||
compass.motor_compensation_type(AP_COMPASS_MOT_COMP_DISABLED);
|
||||
}
|
||||
|
||||
// display new motor offsets and save
|
||||
report_compass();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
// display_compassmot_info - displays a status line for compassmot process
|
||||
static void display_compassmot_info(Vector3f& motor_impact, Vector3f& motor_compensation)
|
||||
{
|
||||
// print one more time so the last thing printed matches what appears in the report_compass
|
||||
cliSerial->printf_P(PSTR("thr:%d cur:%4.2f mot x:%4.1f y:%4.1f z:%4.1f comp x:%4.2f y:%4.2f z:%4.2f\n"),(int)g.rc_3.control_in, (float)battery.current_amps(), (float)motor_impact.x, (float)motor_impact.y, (float)motor_impact.z, (float)motor_compensation.x, (float)motor_compensation.y, (float)motor_compensation.z);
|
||||
}
|
||||
|
||||
static int8_t
|
||||
setup_optflow(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
|
|
|
|||
Loading…
Reference in New Issue