Rover: send wheel encoder data to EKF

APMrover2/Rover.h

@@ -411,6 +411,11 @@ private:
     // 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
+    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
+
     // True when we are doing motor test
     bool motor_test;
 

APMrover2/sensors.cpp

@@ -95,7 +95,63 @@ void Rover::update_visual_odom()
 // 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();
+
+    // initialise on first iteration
+    uint32_t now = AP_HAL::millis();
+    if (wheel_encoder_last_ekf_update_ms == 0) {
+        wheel_encoder_last_ekf_update_ms = now;
+        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);
+        }
+        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
+    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)
+        float curr_angle_rad = g2.wheel_encoder.get_delta_angle(i);
+        float delta_angle = curr_angle_rad - wheel_encoder_last_angle_rad[i];
+        wheel_encoder_last_angle_rad[i] = curr_angle_rad;
+
+        // calculate delta time
+        float delta_time;
+        uint32_t latest_sensor_update_ms = g2.wheel_encoder.get_last_reading_ms(i);
+        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_position(i), g2.wheel_encoder.get_wheel_radius(i));
+    }
+
+    // record system time update for next iteration
+    wheel_encoder_last_ekf_update_ms = now;
 }
 
 // read_battery - reads battery voltage and current and invokes failsafe