aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Rover: wheel encoder sends alternate sensors on each update

Browse Source
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++) { for (uint8_t i = 0; i < g2.wheel_encoder.num_sensors(); i++) {
distances[i] = wheel_encoder_last_distance_m[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. // Store the time the last GPS message was received.
uint32_t last_gps_msg_ms{0}; 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_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_reading_ms[WHEELENCODER_MAX_INSTANCES]; // system time of last ping from each encoder
uint32_t wheel_encoder_last_update_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
uint32_t wheel_encoder_last_ekf_update_ms; // system time of last encoder data push to EKF
// True when we are doing motor test // True when we are doing motor test
bool motor_test; bool motor_test;

76
APMrover2/sensors.cpp
View File

@ -43,57 +43,59 @@ void Rover::update_wheel_encoder()
// update encoders // update encoders
g2.wheel_encoder.update(); 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 // initialise on first iteration
const uint32_t now = AP_HAL::millis(); if (!wheel_encoder_initialised) {
if (wheel_encoder_last_ekf_update_ms == 0) { wheel_encoder_initialised = true;
wheel_encoder_last_ekf_update_ms = now;
for (uint8_t i = 0; i < g2.wheel_encoder.num_sensors(); i++) { 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_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; return;
} }
// calculate delta angle and delta time and send to EKF // on each iteration send data from alternative wheel encoders
// use time of last ping from wheel encoder wheel_encoder_last_index_sent++;
// send delta time (time between this wheel encoder time and previous wheel encoder time) if (wheel_encoder_last_index_sent >= g2.wheel_encoder.num_sensors()) {
// in case where wheel hasn't moved, count = 0 (cap the delta time at 50ms - or system time) wheel_encoder_last_index_sent = 0;
// 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;
} }
// 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) /* 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) * delTime is the time interval for the measurement of delAng (sec)
* timeStamp_ms is the time when the rotation was last measured (msec) * timeStamp_ms is the time when the rotation was last measured (msec)
* posOffset is the XYZ body frame position of the wheel hub (m) * 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)); EKF3.writeWheelOdom(delta_angle,
} delta_time,
wheel_encoder_last_reading_ms[wheel_encoder_last_index_sent],
// record system time update for next iteration g2.wheel_encoder.get_pos_offset(wheel_encoder_last_index_sent),
wheel_encoder_last_ekf_update_ms = now; g2.wheel_encoder.get_wheel_radius(wheel_encoder_last_index_sent));
} }
// Accel calibration // Accel calibration