From 76b60a1a52884377bd9512c86e90c08831a17976 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 7 Feb 2014 16:31:34 +1100 Subject: [PATCH] Copter: added compassmot over MAVLink use the MAVLink interact code to allow for compassmot over MAVLink Pair-Programmed-With: Randy Mackay --- ArduCopter/ArduCopter.pde | 3 +- ArduCopter/GCS_Mavlink.pde | 12 +- ArduCopter/compassmot.pde | 206 ++++++++++++++++++++++++++++++++++ ArduCopter/setup.pde | 223 ------------------------------------- 4 files changed, 219 insertions(+), 225 deletions(-) create mode 100644 ArduCopter/compassmot.pde diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 521f50a482..cfc4aa445c 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -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 diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 70bc5b46d4..fc378f3d23 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -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: diff --git a/ArduCopter/compassmot.pde b/ArduCopter/compassmot.pde new file mode 100644 index 0000000000..d61ca077e5 --- /dev/null +++ b/ArduCopter/compassmot.pde @@ -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; +} + diff --git a/ArduCopter/setup.pde b/ArduCopter/setup.pde index 1cb0a06299..d93a713bee 100644 --- a/ArduCopter/setup.pde +++ b/ArduCopter/setup.pde @@ -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) {