AP_WheelEncoder: add SITL backend

AP_WheelEncoder: added update function for SITL quadrature encoder

libraries/AP_WheelEncoder/AP_WheelEncoder.cpp

@@ -15,6 +15,7 @@
 
 #include "AP_WheelEncoder.h"
 #include "WheelEncoder_Quadrature.h"
+#include "WheelEncoder_SITL_Quadrature.h"
 #include <AP_Logger/AP_Logger.h>
 
 extern const AP_HAL::HAL& hal;
@@ -24,7 +25,7 @@ const AP_Param::GroupInfo AP_WheelEncoder::var_info[] = {
     // @Param: _TYPE
     // @DisplayName: WheelEncoder type
     // @Description: What type of WheelEncoder is connected
-    // @Values: 0:None,1:Quadrature
+    // @Values: 0:None,1:Quadrature,10:SITL Quadrature
     // @User: Standard
     AP_GROUPINFO_FLAGS("_TYPE", 0, AP_WheelEncoder, _type[0], 0, AP_PARAM_FLAG_ENABLE),
 
@@ -83,7 +84,7 @@ const AP_Param::GroupInfo AP_WheelEncoder::var_info[] = {
     // @Param: 2_TYPE
     // @DisplayName: Second WheelEncoder type
     // @Description: What type of WheelEncoder sensor is connected
-    // @Values: 0:None,1:Quadrature
+    // @Values: 0:None,1:Quadrature,10:SITL Quadrature
     // @User: Standard
     AP_GROUPINFO("2_TYPE",   6, AP_WheelEncoder, _type[1], 0),
 
@@ -155,15 +156,24 @@ void AP_WheelEncoder::init(void)
         return;
     }
     for (uint8_t i=0; i<WHEELENCODER_MAX_INSTANCES; i++) {
-#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
         switch ((WheelEncoder_Type)_type[i].get()) {
+
         case WheelEncoder_TYPE_QUADRATURE:
+#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
             drivers[i] = new AP_WheelEncoder_Quadrature(*this, i, state[i]);
+#endif
             break;
+
+        case WheelEncoder_TYPE_SITL_QUADRATURE:
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+            drivers[i] = new AP_WheelEncoder_SITL_Qaudrature(*this, i, state[i]);
+#endif
+            break;
+            
         case WheelEncoder_TYPE_NONE:
             break;
         }
-#endif
+
 
         if (drivers[i] != nullptr) {
             // we loaded a driver for this instance, so it must be

libraries/AP_WheelEncoder/AP_WheelEncoder.h

@@ -31,6 +31,7 @@ class AP_WheelEncoder
 public:
     friend class AP_WheelEncoder_Backend;
     friend class AP_WheelEncoder_Quadrature;
+    friend class AP_WheelEncoder_SITL_Quadrature;
 
     AP_WheelEncoder(void);
 
@@ -40,8 +41,9 @@ public:
 
     // WheelEncoder driver types
     enum WheelEncoder_Type : uint8_t {
-        WheelEncoder_TYPE_NONE          = 0,
-        WheelEncoder_TYPE_QUADRATURE    = 1
+        WheelEncoder_TYPE_NONE             =   0,
+        WheelEncoder_TYPE_QUADRATURE       =   1,
+        WheelEncoder_TYPE_SITL_QUADRATURE  =  10,
     };
 
     // The WheelEncoder_State structure is filled in by the backend driver

libraries/AP_WheelEncoder/WheelEncoder_SITL_Quadrature.cpp

@@ -0,0 +1,93 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <AP_HAL/AP_HAL.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+
+#include "WheelEncoder_SITL_Quadrature.h"
+#include <GCS_MAVLink/GCS.h>
+
+extern const AP_HAL::HAL& hal;
+
+AP_WheelEncoder_SITL_Qaudrature::AP_WheelEncoder_SITL_Qaudrature(AP_WheelEncoder &frontend, uint8_t instance, AP_WheelEncoder::WheelEncoder_State &state) :
+    AP_WheelEncoder_Backend(frontend, instance, state),
+    _sitl(AP::sitl())
+{
+}
+
+void AP_WheelEncoder_SITL_Qaudrature::update(void)
+{
+    // earth frame velocity of vehicle in vector form
+    const Vector2f ef_velocity(_sitl->state.speedN, _sitl->state.speedE);
+    // store current heading
+    const double current_heading = _sitl->state.yawDeg;
+
+    // transform ef_velocity vector to current_heading frame
+    // helps to calculate direction of movement
+    const Vector2f vf_velocity(ef_velocity.x*cos(radians(current_heading)) + ef_velocity.y*sin(radians(current_heading)),
+                               -ef_velocity.x*sin(radians(current_heading)) + ef_velocity.y*cos(radians(current_heading)));
+
+    // calculate dt
+    const uint32_t time_now = AP_HAL::millis();
+    const double dt = (time_now - _state.last_reading_ms)/1000.0f;
+    if (is_zero(dt)) { // sanity check
+        return;
+    }
+
+    // get current speed and turn rate
+    const double turn_rate = radians(_sitl->state.yawRate);
+    double speed = norm(vf_velocity.x, vf_velocity.y);
+
+    // assign direction to speed value
+    if (vf_velocity.x < 0.0f) {
+        speed *= -1;
+    }
+
+    // distance from center of wheel axis to each wheel
+    const double half_wheelbase = ( fabsf(_frontend.get_pos_offset(0).y) + fabsf(_frontend.get_pos_offset(1).y) )/2.0f;
+    if (is_zero(half_wheelbase)) {
+        gcs().send_text(MAV_SEVERITY_WARNING, "WheelEncoder: wheel offset not set!");
+    }
+
+    if (_state.instance == 0) { 
+        speed = speed - turn_rate * half_wheelbase; // for left wheel
+    } else if (_state.instance == 1) {
+        speed = speed + turn_rate * half_wheelbase; // for right wheel
+    } else {
+        AP_HAL::panic("Invalid wheel encoder instance");
+        return; // invalid instance
+    }
+
+    const double radius = _frontend.get_wheel_radius(_state.instance);
+    if (is_zero(radius)) { // avoid divide by zero
+        gcs().send_text(MAV_SEVERITY_WARNING, "WheelEncoder: wheel radius not set!");
+        return; 
+    }
+
+    // calculate angle turned and corresponding encoder ticks from wheel angular rate
+    const double angle_turned = ( speed/radius ) * dt;
+    const int32_t ticks =  static_cast<int>( ( angle_turned/(2 * M_PI) ) * _frontend.get_counts_per_revolution(_state.instance) );
+
+    // update distance count
+    _distance_count += ticks;
+    // update total count of encoder ticks
+    _total_count += abs(ticks);
+
+    // update previous state to current
+    copy_state_to_frontend(_distance_count, _total_count, 0, time_now);
+}
+
+#endif

libraries/AP_WheelEncoder/WheelEncoder_SITL_Quadrature.h

@@ -0,0 +1,39 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+
+#include "WheelEncoder_Backend.h"
+#include <AP_Math/AP_Math.h>
+#include <SITL/SITL.h>
+
+class AP_WheelEncoder_SITL_Qaudrature : public AP_WheelEncoder_Backend
+{
+public:
+    // constructor
+    AP_WheelEncoder_SITL_Qaudrature(AP_WheelEncoder &frontend, uint8_t instance, AP_WheelEncoder::WheelEncoder_State &state);
+
+    // update state
+    void update(void) override;
+
+private:
+    SITL::SITL *_sitl; // pointer to SITL singleton
+
+    int32_t  _distance_count; // distance count as number of encoder ticks
+    uint32_t _total_count; // total number of encoder ticks
+};
+
+#endif // CONFIG_HAL_BOARD