mirror of https://github.com/ArduPilot/ardupilot
93 lines
3.4 KiB
C++
93 lines
3.4 KiB
C++
|
/*
|
||
|
|
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
|