Browse Source

Plane: moved accel cal update to vehicle code

gps-1.3.1
Andrew Tridgell 4 years ago
parent
commit
baec0d83f2
  1. 1
      ArduPlane/ArduPlane.cpp
  2. 1
      ArduPlane/Plane.h
  3. 14
      ArduPlane/sensors.cpp

1
ArduPlane/ArduPlane.cpp

@ -64,7 +64,6 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = { @@ -64,7 +64,6 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = {
SCHED_TASK(read_rangefinder, 50, 100),
SCHED_TASK_CLASS(AP_ICEngine, &plane.g2.ice_control, update, 10, 100),
SCHED_TASK_CLASS(Compass, &plane.compass, cal_update, 50, 50),
SCHED_TASK(accel_cal_update, 10, 50),
#if OPTFLOW == ENABLED
SCHED_TASK_CLASS(OpticalFlow, &plane.optflow, update, 50, 50),
#endif

1
ArduPlane/Plane.h

@ -1032,7 +1032,6 @@ private: @@ -1032,7 +1032,6 @@ private:
void read_rangefinder(void);
void read_airspeed(void);
void rpm_update(void);
void accel_cal_update(void);
// system.cpp
void init_ardupilot() override;

14
ArduPlane/sensors.cpp

@ -34,20 +34,6 @@ void Plane::read_rangefinder(void) @@ -34,20 +34,6 @@ void Plane::read_rangefinder(void)
rangefinder_height_update();
}
/*
Accel calibration
*/
void Plane::accel_cal_update() {
if (hal.util->get_soft_armed()) {
return;
}
ins.acal_update();
float trim_roll, trim_pitch;
if(ins.get_new_trim(trim_roll, trim_pitch)) {
ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
}
}
/*
ask airspeed sensor for a new value
*/

Loading…
Cancel
Save