Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : 
      16             : #include <AP_HAL/AP_HAL.h>
      17             : 
      18             : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
      19             : 
      20             : #include "WheelEncoder_SITL_Quadrature.h"
      21             : #include <GCS_MAVLink/GCS.h>
      22             : 
      23             : extern const AP_HAL::HAL& hal;
      24             : 
      25        5535 : void AP_WheelEncoder_SITL_Quadrature::update(void)
      26             : {
      27        5535 :     const auto *_sitl = AP::sitl();
      28             : 
      29             :     // earth frame velocity of vehicle in vector form
      30        5535 :     const Vector2f ef_velocity(_sitl->state.speedN, _sitl->state.speedE);
      31             :     // store current heading
      32        5535 :     const double current_heading = _sitl->state.yawDeg;
      33             : 
      34             :     // transform ef_velocity vector to current_heading frame
      35             :     // helps to calculate direction of movement
      36        5535 :     const Vector2f vf_velocity(ef_velocity.x*cos(radians(current_heading)) + ef_velocity.y*sin(radians(current_heading)),
      37       11070 :                                -ef_velocity.x*sin(radians(current_heading)) + ef_velocity.y*cos(radians(current_heading)));
      38             : 
      39             :     // calculate dt
      40        5535 :     const uint32_t time_now = AP_HAL::millis();
      41        5535 :     const double dt = (time_now - _state.last_reading_ms)*0.001f;
      42        5535 :     if (is_zero(dt)) { // sanity check
      43           0 :         return;
      44             :     }
      45             : 
      46             :     // get current speed and turn rate
      47        5535 :     const double turn_rate = radians(_sitl->state.yawRate);
      48        5535 :     double speed = norm(vf_velocity.x, vf_velocity.y);
      49             : 
      50             :     // assign direction to speed value
      51        5535 :     if (vf_velocity.x < 0.0f) {
      52          98 :         speed *= -1;
      53             :     }
      54             : 
      55             :     // distance from center of wheel axis to each wheel
      56        5535 :     const double half_wheelbase = ( fabsf(_frontend.get_pos_offset(0).y) + fabsf(_frontend.get_pos_offset(1).y) )/2.0f;
      57        5535 :     if (is_zero(half_wheelbase)) {
      58        2881 :         GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "WheelEncoder: wheel offset not set!");
      59             :     }
      60             : 
      61        5535 :     if (_state.instance == 0) { 
      62        4240 :         speed = speed - turn_rate * half_wheelbase; // for left wheel
      63        1295 :     } else if (_state.instance == 1) {
      64        1295 :         speed = speed + turn_rate * half_wheelbase; // for right wheel
      65             :     } else {
      66           0 :         AP_HAL::panic("Invalid wheel encoder instance");
      67             :         return; // invalid instance
      68             :     }
      69             : 
      70        5535 :     const double radius = _frontend.get_wheel_radius(_state.instance);
      71        5535 :     if (is_zero(radius)) { // avoid divide by zero
      72           0 :         GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "WheelEncoder: wheel radius not set!");
      73           0 :         return; 
      74             :     }
      75             : 
      76             :     // calculate angle turned and corresponding encoder ticks from wheel angular rate
      77        5535 :     const double angle_turned = ( speed/radius ) * dt;
      78        5535 :     const int32_t ticks =  static_cast<int>( ( angle_turned/(2 * M_PI) ) * _frontend.get_counts_per_revolution(_state.instance) );
      79             : 
      80             :     // update distance count
      81        5535 :     _distance_count += ticks;
      82             :     // update total count of encoder ticks
      83        5535 :     _total_count += abs(ticks);
      84             : 
      85             :     // update previous state to current
      86        5535 :     copy_state_to_frontend(_distance_count, _total_count, 0, time_now);
      87             : }
      88             : 
      89             : #endif