Browse Source

Copter: remove compass accumulate

master
Randy Mackay 7 years ago committed by Andrew Tridgell
parent
commit
346e9e36db
  1. 4
      ArduCopter/ArduCopter.cpp
  2. 2
      ArduCopter/Copter.h
  3. 2
      ArduCopter/compassmot.cpp
  4. 11
      ArduCopter/sensors.cpp

4
ArduCopter/ArduCopter.cpp

@ -117,7 +117,6 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { @@ -117,7 +117,6 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
#endif
SCHED_TASK(three_hz_loop, 3, 75),
SCHED_TASK_CLASS(AP_ServoRelayEvents, &copter.ServoRelayEvents, update_events, 50, 75),
SCHED_TASK(compass_accumulate, 100, 100),
SCHED_TASK_CLASS(AP_Baro, &copter.barometer, accumulate, 50, 90),
#if PRECISION_LANDING == ENABLED
SCHED_TASK(update_precland, 400, 50),
@ -456,6 +455,9 @@ void Copter::one_hz_loop() @@ -456,6 +455,9 @@ void Copter::one_hz_loop()
// indicates that the sensor or subsystem is present but not
// functioning correctly
update_sensor_status_flags();
// init compass location for declination
init_compass_location();
}
// called at 50hz

2
ArduCopter/Copter.h

@ -862,7 +862,7 @@ private: @@ -862,7 +862,7 @@ private:
bool rangefinder_alt_ok();
void rpm_update();
void init_compass();
void compass_accumulate(void);
void init_compass_location();
void init_optflow();
void update_optical_flow(void);
void compass_cal_update(void);

2
ArduCopter/compassmot.cpp

@ -137,8 +137,6 @@ MAV_RESULT Copter::mavlink_compassmot(mavlink_channel_t chan) @@ -137,8 +137,6 @@ MAV_RESULT Copter::mavlink_compassmot(mavlink_channel_t chan)
while (command_ack_start == command_ack_counter && compass.healthy() && motors->armed()) {
// 50hz loop
if (millis() - last_run_time < 20) {
// grab some compass values
compass.accumulate();
hal.scheduler->delay(5);
continue;
}

11
ArduCopter/sensors.cpp

@ -102,17 +102,10 @@ void Copter::init_compass() @@ -102,17 +102,10 @@ void Copter::init_compass()
}
/*
if the compass is enabled then try to accumulate a reading
also update initial location used for declination
initialise compass's location used for declination
*/
void Copter::compass_accumulate(void)
void Copter::init_compass_location()
{
if (!g.compass_enabled) {
return;
}
compass.accumulate();
// update initial location used for declination
if (!ap.compass_init_location) {
Location loc;

Loading…
Cancel
Save