Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Rover: wheel encoder sends alternate sensors on each update

This commit is contained in:
Randy Mackay 2019-10-17 20:03:18 +09:00
parent b4a8691a9e
commit a7eea2a4e3
3 changed files with 50 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
APMrover2/GCS_Mavlink.cpp
View File

@ -289,7 +289,7 @@ void Rover::send_wheel_encoder_distance(const mavlink_channel_t chan)
for (uint8_t i = 0; i < g2.wheel_encoder.num_sensors(); i++) {
distances[i] = wheel_encoder_last_distance_m[i];
}
mavlink_msg_wheel_distance_send(chan, 1000UL * wheel_encoder_last_ekf_update_ms, g2.wheel_encoder.num_sensors(), distances);
mavlink_msg_wheel_distance_send(chan, 1000UL * AP_HAL::millis(), g2.wheel_encoder.num_sensors(), distances);
}
}

9
APMrover2/Rover.h
View File

@ -267,11 +267,12 @@ private:
// Store the time the last GPS message was received.
uint32_t last_gps_msg_ms{0};
// last wheel encoder update times
// latest wheel encoder values
float wheel_encoder_last_distance_m[WHEELENCODER_MAX_INSTANCES]; // total distance recorded by wheel encoder (for reporting to GCS)
bool wheel_encoder_initialised; // true once arrays below have been initialised to sensors initial values
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)
uint32_t wheel_encoder_last_update_ms[WHEELENCODER_MAX_INSTANCES]; // system time of last ping from each encoder
uint32_t wheel_encoder_last_ekf_update_ms; // system time of last encoder data push to EKF
uint32_t wheel_encoder_last_reading_ms[WHEELENCODER_MAX_INSTANCES]; // system time of last ping from each encoder
uint8_t wheel_encoder_last_index_sent; // index of the last wheel encoder sent to the EKF
// True when we are doing motor test
bool motor_test;

86
APMrover2/sensors.cpp
View File

@ -43,57 +43,59 @@ void Rover::update_wheel_encoder()
// 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
const uint32_t now = AP_HAL::millis();
if (wheel_encoder_last_ekf_update_ms == 0) {
wheel_encoder_last_ekf_update_ms = now;
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_update_ms[i] = g2.wheel_encoder.get_last_reading_ms(i);
wheel_encoder_last_reading_ms[i] = g2.wheel_encoder.get_last_reading_ms(i);
}
return;
}
// calculate delta angle and delta time and send to EKF
// use time of last ping from wheel encoder
// send delta time (time between this wheel encoder time and previous wheel encoder time)
// in case where wheel hasn't moved, count = 0 (cap the delta time at 50ms - or system time)
// use system clock of last update instead of time of last ping
const float system_dt = (now - wheel_encoder_last_ekf_update_ms) / 1000.0f;
for (uint8_t i = 0; i < g2.wheel_encoder.num_sensors(); i++) {
// calculate angular change (in radians)
const float curr_angle_rad = g2.wheel_encoder.get_delta_angle(i);
const float delta_angle = curr_angle_rad - wheel_encoder_last_angle_rad[i];
wheel_encoder_last_angle_rad[i] = curr_angle_rad;
// save cumulative distances at current time (in meters)
wheel_encoder_last_distance_m[i] = g2.wheel_encoder.get_distance(i);
// calculate delta time
float delta_time;
const uint32_t latest_sensor_update_ms = g2.wheel_encoder.get_last_reading_ms(i);
const uint32_t sensor_diff_ms = latest_sensor_update_ms - wheel_encoder_last_update_ms[i];
// if we have not received any sensor updates, or time difference is too high then use time since last update to the ekf
// check for old or insane sensor update times
if (sensor_diff_ms == 0 || sensor_diff_ms > 100) {
delta_time = system_dt;
wheel_encoder_last_update_ms[i] = wheel_encoder_last_ekf_update_ms;
} else {
delta_time = sensor_diff_ms / 1000.0f;
wheel_encoder_last_update_ms[i] = latest_sensor_update_ms;
}
/* 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)
*/
EKF3.writeWheelOdom(delta_angle, delta_time, wheel_encoder_last_update_ms[i], g2.wheel_encoder.get_pos_offset(i), g2.wheel_encoder.get_wheel_radius(i));
// 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;
}
// record system time update for next iteration
wheel_encoder_last_ekf_update_ms = now;
// 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)
const float delta_angle = curr_angle_rad - wheel_encoder_last_angle_rad[wheel_encoder_last_index_sent];
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;
}
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)
*/
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));
}
// Accel calibration