You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
93 lines
3.4 KiB
93 lines
3.4 KiB
/* |
|
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 |