mirror of https://github.com/ArduPilot/ardupilot
Copter: move visual odometry update function into AP_VisualOdom
This commit is contained in:
parent
bf9b4dca70
commit
f64ad2f433
|
@ -106,7 +106,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
|
|||
SCHED_TASK_CLASS(AP_Beacon, &copter.g2.beacon, update, 400, 50),
|
||||
#endif
|
||||
#if VISUAL_ODOMETRY_ENABLED == ENABLED
|
||||
SCHED_TASK(update_visual_odom, 400, 50),
|
||||
SCHED_TASK_CLASS(AP_VisualOdom, &copter.g2.visual_odom, update, 400, 50),
|
||||
#endif
|
||||
SCHED_TASK(update_altitude, 10, 100),
|
||||
SCHED_TASK(run_nav_updates, 50, 100),
|
||||
|
|
|
@ -549,11 +549,6 @@ private:
|
|||
// last esc calibration notification update
|
||||
uint32_t esc_calibration_notify_update_ms;
|
||||
|
||||
#if VISUAL_ODOMETRY_ENABLED == ENABLED
|
||||
// last visual odometry update time
|
||||
uint32_t visual_odom_last_update_ms;
|
||||
#endif
|
||||
|
||||
// Top-level logic
|
||||
// setup the var_info table
|
||||
AP_Param param_loader;
|
||||
|
@ -837,7 +832,6 @@ private:
|
|||
void update_proximity();
|
||||
void update_sensor_status_flags(void);
|
||||
void init_visual_odom();
|
||||
void update_visual_odom();
|
||||
void winch_init();
|
||||
void winch_update();
|
||||
|
||||
|
|
|
@ -403,29 +403,6 @@ void Copter::init_visual_odom()
|
|||
#endif
|
||||
}
|
||||
|
||||
// update visual odometry sensor
|
||||
void Copter::update_visual_odom()
|
||||
{
|
||||
#if VISUAL_ODOMETRY_ENABLED == ENABLED
|
||||
// check for updates
|
||||
if (g2.visual_odom.enabled() && (g2.visual_odom.get_last_update_ms() != visual_odom_last_update_ms)) {
|
||||
visual_odom_last_update_ms = g2.visual_odom.get_last_update_ms();
|
||||
float time_delta_sec = g2.visual_odom.get_time_delta_usec() / 1000000.0f;
|
||||
ahrs.writeBodyFrameOdom(g2.visual_odom.get_confidence(),
|
||||
g2.visual_odom.get_position_delta(),
|
||||
g2.visual_odom.get_angle_delta(),
|
||||
time_delta_sec,
|
||||
visual_odom_last_update_ms,
|
||||
g2.visual_odom.get_pos_offset());
|
||||
// log sensor data
|
||||
logger.Write_VisualOdom(time_delta_sec,
|
||||
g2.visual_odom.get_angle_delta(),
|
||||
g2.visual_odom.get_position_delta(),
|
||||
g2.visual_odom.get_confidence());
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
// winch and wheel encoder initialisation
|
||||
void Copter::winch_init()
|
||||
{
|
||||
|
|
Loading…
Reference in New Issue