Copter: add compass_cal update function

This commit is contained in:
Jonathan Challinger 2015-07-01 12:36:53 -07:00 committed by Andrew Tridgell
parent 137bd25220
commit 7d67a00aa3
2 changed files with 8 additions and 0 deletions

View File

@ -534,6 +534,7 @@ private:
static const struct LogStructure log_structure[]; static const struct LogStructure log_structure[];
void compass_accumulate(void); void compass_accumulate(void);
void compass_cal_update(void);
void barometer_accumulate(void); void barometer_accumulate(void);
void perf_update(void); void perf_update(void);
void fast_loop(); void fast_loop();

View File

@ -174,6 +174,13 @@ void Copter::read_receiver_rssi(void)
receiver_rssi = rssi.read_receiver_rssi_uint8(); receiver_rssi = rssi.read_receiver_rssi_uint8();
} }
void Copter::compass_cal_update()
{
if (!hal.util->get_soft_armed()) {
compass.compass_cal_update();
}
}
#if EPM_ENABLED == ENABLED #if EPM_ENABLED == ENABLED
// epm update - moves epm pwm output back to neutral after grab or release is completed // epm update - moves epm pwm output back to neutral after grab or release is completed
void Copter::epm_update() void Copter::epm_update()