Rover: integrate wheel encoder

APMrover2/APMrover2.cpp

@@ -54,6 +54,7 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = {
     SCHED_TASK(update_alt,             10,   3400),
     SCHED_TASK(update_beacon,          50,     50),
     SCHED_TASK(update_visual_odom,     50,     50),
+    SCHED_TASK(update_wheel_encoder,   50,     50),
     SCHED_TASK(navigate,               10,   1600),
     SCHED_TASK(update_compass,         10,   2000),
     SCHED_TASK(update_commands,        10,   1000),
@@ -283,6 +284,7 @@ void Rover::update_logging2(void)
 
     if (should_log(MASK_LOG_RC)) {
         Log_Write_RC();
+        Log_Write_WheelEncoder();
     }
 
     if (should_log(MASK_LOG_IMU)) {

APMrover2/Log.cpp

@@ -446,6 +446,33 @@ void Rover::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_targ
     DataFlash.WriteBlock(&pkt, sizeof(pkt));
 }
 
+// wheel encoder packet
+struct PACKED log_WheelEncoder {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    float distance_0;
+    uint8_t quality_0;
+    float distance_1;
+    uint8_t quality_1;
+};
+
+// log wheel encoder information
+void Rover::Log_Write_WheelEncoder()
+{
+    // return immediately if no wheel encoders are enabled
+    if (!g2.wheel_encoder.enabled(0) && !g2.wheel_encoder.enabled(1)) {
+        return;
+    }
+    struct log_WheelEncoder pkt = {
+        LOG_PACKET_HEADER_INIT(LOG_WHEELENCODER_MSG),
+        time_us     : AP_HAL::micros64(),
+        distance_0  : g2.wheel_encoder.get_distance(0),
+        quality_0   : (uint8_t)constrain_float(g2.wheel_encoder.get_signal_quality(0),0.0f,100.0f),
+        distance_1  : g2.wheel_encoder.get_distance(1),
+        quality_1   : (uint8_t)constrain_float(g2.wheel_encoder.get_signal_quality(1),0.0f,100.0f)
+    };
+    DataFlash.WriteBlock(&pkt, sizeof(pkt));
+}
 
 const LogStructure Rover::log_structure[] = {
     LOG_COMMON_STRUCTURES,
@@ -467,6 +494,8 @@ const LogStructure Rover::log_structure[] = {
       "GUID",  "QBffffff",    "TimeUS,Type,pX,pY,pZ,vX,vY,vZ" },
     { LOG_ERROR_MSG, sizeof(log_Error),
       "ERR",   "QBB",         "TimeUS,Subsys,ECode" },
+    { LOG_WHEELENCODER_MSG, sizeof(log_WheelEncoder),
+      "WENC",  "Qfbfb",       "TimeUS,Dist0,Qual0,Dist1,Qual1" },
 };
 
 void Rover::log_init(void)
@@ -532,5 +561,6 @@ void Rover::Log_Write_Baro(void) {}
 void Rover::Log_Arm_Disarm() {}
 void Rover::Log_Write_Error(uint8_t sub_system, uint8_t error_code) {}
 void Rover::Log_Write_Steering() {}
+void Rover::Log_Write_WheelEncoder() {}
 
 #endif  // LOGGING_ENABLED

APMrover2/Parameters.cpp

@@ -546,6 +546,10 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @Path: MotorsUGV.cpp
     AP_SUBGROUPINFO(motors, "MOT_", 8, ParametersG2, AP_MotorsUGV),
 
+    // @Group: WENC_
+    // @Path: ../libraries/AP_WheelEncoder/AP_WheelEncoder.cpp
+    AP_SUBGROUPINFO(wheel_encoder, "WENC_", 9, ParametersG2, AP_WheelEncoder),
+
     AP_GROUPEND
 };
 

APMrover2/Parameters.h

@@ -324,6 +324,9 @@ public:
 
     // Motor library
     AP_MotorsUGV motors;
+
+    // wheel encoders
+    AP_WheelEncoder wheel_encoder;
 };
 
 extern const AP_Param::Info var_info[];

APMrover2/Rover.h

@@ -76,6 +76,7 @@
 #include <AP_Stats/AP_Stats.h>                      // statistics library
 #include <AP_Beacon/AP_Beacon.h>
 #include <AP_VisualOdom/AP_VisualOdom.h>
+#include <AP_WheelEncoder/AP_WheelEncoder.h>
 
 // Configuration
 #include "config.h"
@@ -469,6 +470,7 @@ private:
     void Log_Write_Baro(void);
     void Log_Write_Home_And_Origin();
     void Log_Write_Vehicle_Startup_Messages();
+    void Log_Write_WheelEncoder();
     void Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page);
     void log_init(void);
     void start_logging();
@@ -524,6 +526,7 @@ private:
     void update_beacon();
     void init_visual_odom();
     void update_visual_odom();
+    void update_wheel_encoder();
     void read_battery(void);
     void read_receiver_rssi(void);
     void read_sonars(void);

APMrover2/defines.h

@@ -60,6 +60,7 @@ enum GuidedMode {
 #define LOG_ARM_DISARM_MSG      0x08
 #define LOG_STEERING_MSG        0x0D
 #define LOG_GUIDEDTARGET_MSG    0x0E
+#define LOG_WHEELENCODER_MSG    0x0F
 #define LOG_ERROR_MSG           0x13
 
 #define TYPE_AIRSTART_MSG       0x00

APMrover2/sensors.cpp

@@ -92,6 +92,12 @@ void Rover::update_visual_odom()
     }
 }
 
+// update wheel encoders
+void Rover::update_wheel_encoder()
+{
+    g2.wheel_encoder.update();
+}
+
 // read_battery - reads battery voltage and current and invokes failsafe
 // should be called at 10hz
 void Rover::read_battery(void)

APMrover2/system.cpp

@@ -181,6 +181,9 @@ void Rover::init_ardupilot()
     g2.motors.init();        // init motors including setting servo out channels ranges
     init_rc_out();           // enable output
 
+    // init wheel encoders
+    g2.wheel_encoder.init();
+
     relay.init();
 
 #if MOUNT == ENABLED