You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
95 lines
3.7 KiB
95 lines
3.7 KiB
#include "Rover.h" |
|
|
|
#include <AP_RangeFinder/AP_RangeFinder_Backend.h> |
|
|
|
// check for new compass data - 10Hz |
|
void Rover::update_compass(void) |
|
{ |
|
compass.read(); |
|
} |
|
|
|
// Save compass offsets |
|
void Rover::compass_save() { |
|
if (AP::compass().available() && |
|
compass.get_learn_type() >= Compass::LEARN_INTERNAL && |
|
!arming.is_armed()) { |
|
compass.save_offsets(); |
|
} |
|
} |
|
|
|
// update wheel encoders |
|
void Rover::update_wheel_encoder() |
|
{ |
|
// exit immediately if not enabled |
|
if (g2.wheel_encoder.num_sensors() == 0) { |
|
return; |
|
} |
|
|
|
// update encoders |
|
g2.wheel_encoder.update(); |
|
|
|
// save cumulative distances at current time (in meters) for reporting to GCS |
|
for (uint8_t i = 0; i < g2.wheel_encoder.num_sensors(); i++) { |
|
wheel_encoder_last_distance_m[i] = g2.wheel_encoder.get_distance(i); |
|
} |
|
|
|
// send wheel encoder delta angle and delta time to EKF |
|
// this should not be done at more than 50hz |
|
// initialise on first iteration |
|
if (!wheel_encoder_initialised) { |
|
wheel_encoder_initialised = true; |
|
for (uint8_t i = 0; i < g2.wheel_encoder.num_sensors(); i++) { |
|
wheel_encoder_last_angle_rad[i] = g2.wheel_encoder.get_delta_angle(i); |
|
wheel_encoder_last_reading_ms[i] = g2.wheel_encoder.get_last_reading_ms(i); |
|
} |
|
return; |
|
} |
|
|
|
// on each iteration send data from alternative wheel encoders |
|
wheel_encoder_last_index_sent++; |
|
if (wheel_encoder_last_index_sent >= g2.wheel_encoder.num_sensors()) { |
|
wheel_encoder_last_index_sent = 0; |
|
} |
|
|
|
// get current time, total delta angle (since startup) and update time from sensor |
|
const float curr_angle_rad = g2.wheel_encoder.get_delta_angle(wheel_encoder_last_index_sent); |
|
const uint32_t sensor_reading_ms = g2.wheel_encoder.get_last_reading_ms(wheel_encoder_last_index_sent); |
|
const uint32_t now_ms = AP_HAL::millis(); |
|
|
|
// calculate angular change (in radians) |
|
#if HAL_NAVEKF3_AVAILABLE |
|
const float delta_angle = curr_angle_rad - wheel_encoder_last_angle_rad[wheel_encoder_last_index_sent]; |
|
#endif |
|
wheel_encoder_last_angle_rad[wheel_encoder_last_index_sent] = curr_angle_rad; |
|
|
|
// calculate delta time using time between sensor readings or time since last send to ekf (whichever is shorter) |
|
uint32_t sensor_diff_ms = sensor_reading_ms - wheel_encoder_last_reading_ms[wheel_encoder_last_index_sent]; |
|
if (sensor_diff_ms == 0 || sensor_diff_ms > 100) { |
|
// if no sensor update or time difference between sensor readings is too long use time since last send to ekf |
|
sensor_diff_ms = now_ms - wheel_encoder_last_reading_ms[wheel_encoder_last_index_sent]; |
|
wheel_encoder_last_reading_ms[wheel_encoder_last_index_sent] = now_ms; |
|
} else { |
|
wheel_encoder_last_reading_ms[wheel_encoder_last_index_sent] = sensor_reading_ms; |
|
} |
|
#if HAL_NAVEKF3_AVAILABLE |
|
const float delta_time = sensor_diff_ms * 0.001f; |
|
|
|
/* delAng is the measured change in angular position from the previous measurement where a positive rotation is produced by forward motion of the vehicle (rad) |
|
* delTime is the time interval for the measurement of delAng (sec) |
|
* timeStamp_ms is the time when the rotation was last measured (msec) |
|
* posOffset is the XYZ body frame position of the wheel hub (m) |
|
*/ |
|
ahrs.EKF3.writeWheelOdom(delta_angle, |
|
delta_time, |
|
wheel_encoder_last_reading_ms[wheel_encoder_last_index_sent], |
|
g2.wheel_encoder.get_pos_offset(wheel_encoder_last_index_sent), |
|
g2.wheel_encoder.get_wheel_radius(wheel_encoder_last_index_sent)); |
|
#endif |
|
} |
|
|
|
// read the rangefinders |
|
void Rover::read_rangefinders(void) |
|
{ |
|
rangefinder.update(); |
|
Log_Write_Depth(); |
|
}
|
|
|