mirror of https://github.com/ArduPilot/ardupilot
78 lines
2.8 KiB
C++
78 lines
2.8 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/>.
|
|
*/
|
|
/*
|
|
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);
|
|
}
|