From 63e8c5abcb6d6f4650550632095b3345fea7ce77 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 5 Sep 2019 12:59:48 +1000 Subject: [PATCH] AP_WheelEncoder: add SITL backend AP_WheelEncoder: added update function for SITL quadrature encoder --- libraries/AP_WheelEncoder/AP_WheelEncoder.cpp | 18 +++- libraries/AP_WheelEncoder/AP_WheelEncoder.h | 6 +- .../WheelEncoder_SITL_Quadrature.cpp | 93 +++++++++++++++++++ .../WheelEncoder_SITL_Quadrature.h | 39 ++++++++ 4 files changed, 150 insertions(+), 6 deletions(-) create mode 100644 libraries/AP_WheelEncoder/WheelEncoder_SITL_Quadrature.cpp create mode 100644 libraries/AP_WheelEncoder/WheelEncoder_SITL_Quadrature.h diff --git a/libraries/AP_WheelEncoder/AP_WheelEncoder.cpp b/libraries/AP_WheelEncoder/AP_WheelEncoder.cpp index 08c9486df5..210e50cf81 100644 --- a/libraries/AP_WheelEncoder/AP_WheelEncoder.cpp +++ b/libraries/AP_WheelEncoder/AP_WheelEncoder.cpp @@ -15,6 +15,7 @@ #include "AP_WheelEncoder.h" #include "WheelEncoder_Quadrature.h" +#include "WheelEncoder_SITL_Quadrature.h" #include 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. + */ + +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + +#include "WheelEncoder_SITL_Quadrature.h" +#include + +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( ( 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 \ No newline at end of file diff --git a/libraries/AP_WheelEncoder/WheelEncoder_SITL_Quadrature.h b/libraries/AP_WheelEncoder/WheelEncoder_SITL_Quadrature.h new file mode 100644 index 0000000000..9f65a43690 --- /dev/null +++ b/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 . + */ +#pragma once + +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + +#include "WheelEncoder_Backend.h" +#include +#include + +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 \ No newline at end of file