Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: implement per-motor compass compensation

Browse Source
This commit is contained in:
Andrew Tridgell 2017-06-15 13:17:19 +10:00
parent ca30f6aec8
commit 1f76f69d80
3 changed files with 21 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduCopter/ArduCopter.cpp
View File

@ -349,6 +349,7 @@ void Copter::update_batt_compass(void)
if(g.compass_enabled) { if(g.compass_enabled) {
// update compass with throttle value - used for compassmot // update compass with throttle value - used for compassmot
compass.set_throttle(motors->get_throttle()); compass.set_throttle(motors->get_throttle());
compass.set_voltage(battery.voltage());
compass.read(); compass.read();
// log compass information // log compass information
if (should_log(MASK_LOG_COMPASS) && !ahrs.have_ekf_logging()) { if (should_log(MASK_LOG_COMPASS) && !ahrs.have_ekf_logging()) {

1
ArduCopter/GCS_Mavlink.cpp
View File

@ -1184,6 +1184,7 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
// param3 : throttle (range depends upon param2) // param3 : throttle (range depends upon param2)
// param4 : timeout (in seconds) // param4 : timeout (in seconds)
// param5 : num_motors (in sequence) // param5 : num_motors (in sequence)
// param6 : compass learning (0: disabled, 1: enabled)
result = copter.mavlink_motor_test_start(chan, (uint8_t)packet.param1, (uint8_t)packet.param2, (uint16_t)packet.param3, result = copter.mavlink_motor_test_start(chan, (uint8_t)packet.param1, (uint8_t)packet.param2, (uint16_t)packet.param3,
packet.param4, (uint8_t)packet.param5); packet.param4, (uint8_t)packet.param5);
break; break;

19
ArduCopter/motor_test.cpp
View File

@ -51,6 +51,11 @@ void Copter::motor_test_output()
// calculate pwm based on throttle type // calculate pwm based on throttle type
switch (motor_test_throttle_type) { switch (motor_test_throttle_type) {
case MOTOR_TEST_COMPASS_CAL:
compass.set_voltage(battery.voltage());
compass.per_motor_calibration_update();
// fall through
case MOTOR_TEST_THROTTLE_PERCENT: case MOTOR_TEST_THROTTLE_PERCENT:
// sanity check motor_test_throttle value // sanity check motor_test_throttle value
#if FRAME_CONFIG != HELI_FRAME #if FRAME_CONFIG != HELI_FRAME
@ -129,6 +134,8 @@ MAV_RESULT Copter::mavlink_motor_test_start(mavlink_channel_t chan, uint8_t moto
} }
// if test has not started try to start it // if test has not started try to start it
if (!ap.motor_test) { if (!ap.motor_test) {
gcs().send_text(MAV_SEVERITY_INFO, "starting motor test");
/* perform checks that it is ok to start test /* perform checks that it is ok to start test
The RC calibrated check can be skipped if direct pwm is The RC calibrated check can be skipped if direct pwm is
supplied supplied
@ -150,6 +157,7 @@ MAV_RESULT Copter::mavlink_motor_test_start(mavlink_channel_t chan, uint8_t moto
g.failsafe_throttle = FS_THR_DISABLED; g.failsafe_throttle = FS_THR_DISABLED;
g.failsafe_battery_enabled = FS_BATT_DISABLED; g.failsafe_battery_enabled = FS_BATT_DISABLED;
g.failsafe_gcs = FS_GCS_DISABLED; g.failsafe_gcs = FS_GCS_DISABLED;
g.fs_ekf_action = 0;
// turn on notify leds // turn on notify leds
AP_Notify::flags.esc_calibration = true; AP_Notify::flags.esc_calibration = true;
@ -166,6 +174,10 @@ MAV_RESULT Copter::mavlink_motor_test_start(mavlink_channel_t chan, uint8_t moto
motor_test_throttle_type = throttle_type; motor_test_throttle_type = throttle_type;
motor_test_throttle_value = throttle_value; motor_test_throttle_value = throttle_value;
if (motor_test_throttle_type == MOTOR_TEST_COMPASS_CAL) {
compass.per_motor_calibration_start();
}
// return success // return success
return MAV_RESULT_ACCEPTED; return MAV_RESULT_ACCEPTED;
} }
@ -178,6 +190,8 @@ void Copter::motor_test_stop()
return; return;
} }
gcs().send_text(MAV_SEVERITY_INFO, "finished motor test");
// flag test is complete // flag test is complete
ap.motor_test = false; ap.motor_test = false;
@ -192,7 +206,12 @@ void Copter::motor_test_stop()
g.failsafe_throttle.load(); g.failsafe_throttle.load();
g.failsafe_battery_enabled.load(); g.failsafe_battery_enabled.load();
g.failsafe_gcs.load(); g.failsafe_gcs.load();
g.fs_ekf_action.load();
if (motor_test_throttle_type == MOTOR_TEST_COMPASS_CAL) {
compass.per_motor_calibration_end();
}
// turn off notify leds // turn off notify leds
AP_Notify::flags.esc_calibration = false; AP_Notify::flags.esc_calibration = false;
} }