Line data    Source code

       1             : #include "Rover.h"
       2             : 
       3             : #include <AP_RangeFinder/AP_RangeFinder_Backend.h>
       4             : 
       5             : // check for new compass data - 10Hz
       6      125720 : void Rover::update_compass(void)
       7             : {
       8      125720 :     compass.read();
       9      125720 : }
      10             : 
      11             : // update wheel encoders
      12      629048 : void Rover::update_wheel_encoder()
      13             : {
      14             :     // exit immediately if not enabled
      15      629048 :     if (g2.wheel_encoder.num_sensors() == 0) {
      16      624691 :         return;
      17             :     }
      18             : 
      19             :     // update encoders
      20        4357 :     g2.wheel_encoder.update();
      21             : 
      22             :     // save cumulative distances at current time (in meters) for reporting to GCS
      23       10068 :     for (uint8_t i = 0; i < g2.wheel_encoder.num_sensors(); i++) {
      24        5711 :         wheel_encoder_last_distance_m[i] = g2.wheel_encoder.get_distance(i);
      25             :     }
      26             : 
      27             :     // send wheel encoder delta angle and delta time to EKF
      28             :     // this should not be done at more than 50hz
      29             :     // initialise on first iteration
      30        4357 :     if (!wheel_encoder_initialised) {
      31           4 :         wheel_encoder_initialised = true;
      32          10 :         for (uint8_t i = 0; i < g2.wheel_encoder.num_sensors(); i++) {
      33           6 :             wheel_encoder_last_angle_rad[i] = g2.wheel_encoder.get_delta_angle(i);
      34           6 :             wheel_encoder_last_reading_ms[i] = g2.wheel_encoder.get_last_reading_ms(i);
      35             :         }
      36           4 :         return;
      37             :     }
      38             : 
      39             :     // on each iteration send data from alternative wheel encoders
      40        4353 :     wheel_encoder_last_index_sent++;
      41        4353 :     if (wheel_encoder_last_index_sent >= g2.wheel_encoder.num_sensors()) {
      42        3676 :         wheel_encoder_last_index_sent = 0;
      43             :     }
      44             : 
      45             :     // get current time, total delta angle (since startup) and update time from sensor
      46        4353 :     const float curr_angle_rad = g2.wheel_encoder.get_delta_angle(wheel_encoder_last_index_sent);
      47        4353 :     const uint32_t sensor_reading_ms = g2.wheel_encoder.get_last_reading_ms(wheel_encoder_last_index_sent);
      48        4353 :     const uint32_t now_ms = AP_HAL::millis();
      49             : 
      50             :     // calculate angular change (in radians)
      51             : #if HAL_NAVEKF3_AVAILABLE
      52        4353 :     const float delta_angle = curr_angle_rad - wheel_encoder_last_angle_rad[wheel_encoder_last_index_sent];
      53             : #endif
      54        4353 :     wheel_encoder_last_angle_rad[wheel_encoder_last_index_sent] = curr_angle_rad;
      55             : 
      56             :     // calculate delta time using time between sensor readings or time since last send to ekf (whichever is shorter)
      57        4353 :     uint32_t sensor_diff_ms = sensor_reading_ms - wheel_encoder_last_reading_ms[wheel_encoder_last_index_sent];
      58        4353 :     if (sensor_diff_ms == 0 || sensor_diff_ms > 100) {
      59             :         // if no sensor update or time difference between sensor readings is too long use time since last send to ekf
      60         116 :         sensor_diff_ms = now_ms - wheel_encoder_last_reading_ms[wheel_encoder_last_index_sent];
      61         116 :         wheel_encoder_last_reading_ms[wheel_encoder_last_index_sent] = now_ms;
      62             :     } else {
      63        4237 :         wheel_encoder_last_reading_ms[wheel_encoder_last_index_sent] = sensor_reading_ms;
      64             :     }
      65             : #if HAL_NAVEKF3_AVAILABLE
      66        4353 :     const float delta_time = sensor_diff_ms * 0.001f;
      67             : 
      68             :     /* 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)
      69             :      * delTime is the time interval for the measurement of delAng (sec)
      70             :      * timeStamp_ms is the time when the rotation was last measured (msec)
      71             :      * posOffset is the XYZ body frame position of the wheel hub (m)
      72             :      */
      73       13059 :     ahrs.ekf3.EKF3.writeWheelOdom(delta_angle,
      74             :                         delta_time,
      75        4353 :                         wheel_encoder_last_reading_ms[wheel_encoder_last_index_sent],
      76        4353 :                         g2.wheel_encoder.get_pos_offset(wheel_encoder_last_index_sent),
      77        4353 :                         g2.wheel_encoder.get_wheel_radius(wheel_encoder_last_index_sent));
      78             : #endif
      79             : }
      80             : 
      81             : #if AP_RANGEFINDER_ENABLED
      82             : // read the rangefinders
      83      629048 : void Rover::read_rangefinders(void)
      84             : {
      85      629048 :     rangefinder.update();
      86             : #if HAL_LOGGING_ENABLED
      87      629048 :     Log_Write_Depth();
      88             : #endif
      89      629048 : }
      90             : #endif