mirror of
https://github.com/ArduPilot/ardupilot.git
synced 2026-10-02 10:23:25 +08:00
Peter Barker
1b7026ae61
the truncation is not going to be a problem on a real vehicle, but the SITL-only sanity check where we go to use it in change_alt_frame is a problem.
42 lines
1.5 KiB
C++
42 lines
1.5 KiB
C++
#include "Copter.h"
|
|
|
|
// read_inertia - read inertia in from accelerometers
|
|
void Copter::read_inertia()
|
|
{
|
|
// inertial altitude estimates. Use barometer climb rate during high vibrations
|
|
pos_control->update_estimates(vibration_check.high_vibes);
|
|
#if MODE_FOLLOW_ENABLED
|
|
g2.follow.update_estimates();
|
|
#endif
|
|
|
|
// pull position from ahrs
|
|
Location loc;
|
|
// AHRS provides a best-guess in case of failure
|
|
UNUSED_RESULT(ahrs.get_location(loc));
|
|
current_loc.lat = loc.lat;
|
|
current_loc.lng = loc.lng;
|
|
|
|
// exit immediately if we do not have an altitude estimate
|
|
float pos_d_m;
|
|
if (!AP::ahrs().get_relative_position_D_origin_float(pos_d_m)) {
|
|
return;
|
|
}
|
|
|
|
// current_loc.alt is alt-above-home, converted from AHRS's alt-above-ekf-origin
|
|
const float alt_above_origin_m = -pos_d_m;
|
|
current_loc.set_alt_m(alt_above_origin_m, Location::AltFrame::ABOVE_ORIGIN);
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
|
if (!current_loc.initialised()) {
|
|
// this can happen if we don't have lat/lng and our altitude
|
|
// is *very* close the origin i.e. lat=0, lng=0 and
|
|
// int32_t(abs(pos_d_m)*100)==0
|
|
return;
|
|
}
|
|
#endif // CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
|
|
|
if (!ahrs.home_is_set() || !current_loc.change_alt_frame(Location::AltFrame::ABOVE_HOME)) {
|
|
// if home has not been set yet we treat alt-above-origin as alt-above-home
|
|
current_loc.set_alt_m(alt_above_origin_m, Location::AltFrame::ABOVE_HOME);
|
|
}
|
|
}
|