Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

Rover: move visual odometry update function into AP_VisualOdom

Browse Source
This commit is contained in:
Peter Barker 2018-04-06 22:16:35 +10:00 committed by Peter Barker
parent f64ad2f433
commit 76910f9283
3 changed files with 3 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
APMrover2/APMrover2.cpp
View File

@ -54,7 +54,9 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = {
SCHED_TASK_CLASS(AP_Beacon, &rover.g2.beacon, update, 50, 200),
SCHED_TASK_CLASS(AP_Proximity, &rover.g2.proximity, update, 50, 200),
SCHED_TASK_CLASS(AP_WindVane, &rover.g2.windvane, update, 20, 100),
SCHED_TASK(update_visual_odom, 50, 200),
#if VISUAL_ODOMETRY_ENABLED == ENABLED
SCHED_TASK_CLASS(AP_VisualOdom, &rover.g2.visual_odom, update, 50, 200),
#endif
SCHED_TASK(update_wheel_encoder, 50, 200),
SCHED_TASK(update_compass, 10, 200),
SCHED_TASK(update_mission, 50, 200),

4
APMrover2/Rover.h
View File

@ -336,9 +336,6 @@ private:
// Store the time the last GPS message was received.
uint32_t last_gps_msg_ms{0};
// last visual odometry update time
uint32_t visual_odom_last_update_ms;
// last wheel encoder update times
float wheel_encoder_last_angle_rad[WHEELENCODER_MAX_INSTANCES]; // distance in radians at time of last update to EKF
float wheel_encoder_last_distance_m[WHEELENCODER_MAX_INSTANCES]; // distance in meters at time of last update to EKF (for reporting to GCS)
@ -493,7 +490,6 @@ private:
void init_compass_location(void);
void init_beacon();
void init_visual_odom();
void update_visual_odom();
void update_wheel_encoder();
void compass_cal_update(void);
void compass_save(void);

21
APMrover2/sensors.cpp
View File

@ -44,27 +44,6 @@ void Rover::init_visual_odom()
g2.visual_odom.init();
}
// update visual odometry sensor
void Rover::update_visual_odom()
{
// 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();
const 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());
}
}
// update wheel encoders
void Rover::update_wheel_encoder()
{