Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: implement per-motor compass compensation

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) {
// update compass with throttle value - used for compassmot
compass.set_throttle(motors->get_throttle());
compass.set_voltage(battery.voltage());
compass.read();
// log compass information
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)
// param4 : timeout (in seconds)
// 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,
packet.param4, (uint8_t)packet.param5);
break;

19
ArduCopter/motor_test.cpp
View File

@ -51,6 +51,11 @@ void Copter::motor_test_output()
// calculate pwm based on 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:
// sanity check motor_test_throttle value
#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 (!ap.motor_test) {
gcs().send_text(MAV_SEVERITY_INFO, "starting motor test");
/* perform checks that it is ok to start test
The RC calibrated check can be skipped if direct pwm is
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_battery_enabled = FS_BATT_DISABLED;
g.failsafe_gcs = FS_GCS_DISABLED;
g.fs_ekf_action = 0;
// turn on notify leds
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_value = throttle_value;
if (motor_test_throttle_type == MOTOR_TEST_COMPASS_CAL) {
compass.per_motor_calibration_start();
}
// return success
return MAV_RESULT_ACCEPTED;
}
@ -178,6 +190,8 @@ void Copter::motor_test_stop()
return;
}
gcs().send_text(MAV_SEVERITY_INFO, "finished motor test");
// flag test is complete
ap.motor_test = false;
@ -192,7 +206,12 @@ void Copter::motor_test_stop()
g.failsafe_throttle.load();
g.failsafe_battery_enabled.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
AP_Notify::flags.esc_calibration = false;
}