ardupilot/libraries/SITL/SIM_SerialRangeFinder.cpp

78 lines
2.8 KiB
C++
Raw Normal View History

/*
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/>.
*/
/*
Base class for serial rangefinders
*/
#include "SIM_SerialRangeFinder.h"
using namespace SITL;
uint16_t SerialRangeFinder::calculate_range_cm(float range_value) const
{
// swiped from sitl_rangefinder.cpp - we should unify them at some stage
const SITL *sitl = AP::sitl();
float altitude = range_value;
if (is_equal(range_value, -1.0f)) { // Use SITL altitude as reading by default
altitude = AP::sitl()->height_agl;
}
// sensor position offset in body frame
const Vector3f relPosSensorBF = sitl->rngfnd_pos_offset;
// adjust altitude for position of the sensor on the vehicle if position offset is non-zero
if (!relPosSensorBF.is_zero()) {
// get a rotation matrix following DCM conventions (body to earth)
Matrix3f rotmat;
sitl->state.quaternion.rotation_matrix(rotmat);
// rotate the offset into earth frame
const Vector3f relPosSensorEF = rotmat * relPosSensorBF;
// correct the altitude at the sensor
altitude -= relPosSensorEF.z;
}
// If the attidude is non reversed for SITL OR we are using rangefinder from external simulator,
// We adjust the reading with noise, glitch and scaler as the reading is on analog port.
if ((fabs(sitl->state.rollDeg) < 90.0 && fabs(sitl->state.pitchDeg) < 90.0) || !is_equal(range_value, -1.0f)) {
if (is_equal(range_value, -1.0f)) { // disable for external reading that already handle this
// adjust for apparent altitude with roll
altitude /= cosf(radians(sitl->state.rollDeg)) * cosf(radians(sitl->state.pitchDeg));
}
// Add some noise on reading
altitude += sitl->sonar_noise * rand_float();
}
return altitude * 100.0f;
}
void SerialRangeFinder::update(float range)
{
// just send a chunk of data at 5Hz:
const uint32_t now = AP_HAL::millis();
if (now - last_sent_ms < reading_interval_ms()) {
return;
}
last_sent_ms = now;
uint8_t data[255];
const uint32_t packetlen = packet_for_alt(calculate_range_cm(range),
data,
ARRAY_SIZE(data));
write_to_autopilot((char*)data, packetlen);
}