2015-05-29 23:12:49 -03:00
|
|
|
#include "Copter.h"
|
|
|
|
|
2012-06-20 02:17:15 -03:00
|
|
|
// return barometric altitude in centimeters
|
2015-05-29 23:12:49 -03:00
|
|
|
void Copter::read_barometer(void)
|
2011-02-21 00:30:56 -04:00
|
|
|
{
|
2015-01-05 07:28:00 -04:00
|
|
|
barometer.update();
|
2018-04-11 09:50:53 -03:00
|
|
|
|
2014-10-22 04:07:10 -03:00
|
|
|
baro_alt = barometer.get_altitude() * 100.0f;
|
2010-12-19 12:40:33 -04:00
|
|
|
}
|
|
|
|
|
2016-04-27 08:37:04 -03:00
|
|
|
void Copter::init_rangefinder(void)
|
2014-10-31 08:40:24 -03:00
|
|
|
{
|
2024-05-01 09:50:58 -03:00
|
|
|
#if RANGEFINDER_ENABLED == ENABLED && AP_RANGEFINDER_ENABLED
|
2019-04-05 06:19:58 -03:00
|
|
|
rangefinder.set_log_rfnd_bit(MASK_LOG_CTUN);
|
2019-04-05 06:14:36 -03:00
|
|
|
rangefinder.init(ROTATION_PITCH_270);
|
2021-06-14 00:38:37 -03:00
|
|
|
rangefinder_state.alt_cm_filt.set_cutoff_frequency(g2.rangefinder_filt);
|
2017-02-09 07:30:59 -04:00
|
|
|
rangefinder_state.enabled = rangefinder.has_orientation(ROTATION_PITCH_270);
|
2019-08-23 22:59:22 -03:00
|
|
|
|
|
|
|
// upward facing range finder
|
2021-06-14 00:38:37 -03:00
|
|
|
rangefinder_up_state.alt_cm_filt.set_cutoff_frequency(g2.rangefinder_filt);
|
2019-08-23 22:59:22 -03:00
|
|
|
rangefinder_up_state.enabled = rangefinder.has_orientation(ROTATION_PITCH_90);
|
2014-10-31 08:40:24 -03:00
|
|
|
#endif
|
2016-04-27 08:37:04 -03:00
|
|
|
}
|
2014-10-31 08:40:24 -03:00
|
|
|
|
2016-04-27 08:37:04 -03:00
|
|
|
// return rangefinder altitude in centimeters
|
2016-04-27 09:20:03 -03:00
|
|
|
void Copter::read_rangefinder(void)
|
2012-12-21 01:03:47 -04:00
|
|
|
{
|
2024-05-01 09:50:58 -03:00
|
|
|
#if RANGEFINDER_ENABLED == ENABLED && AP_RANGEFINDER_ENABLED
|
2016-04-27 08:37:04 -03:00
|
|
|
rangefinder.update();
|
2014-06-27 01:23:56 -03:00
|
|
|
|
2024-05-01 09:50:58 -03:00
|
|
|
rangefinder_state.update();
|
|
|
|
rangefinder_up_state.update();
|
2023-03-05 19:53:58 -04:00
|
|
|
|
2023-03-03 21:01:34 -04:00
|
|
|
#if HAL_PROXIMITY_ENABLED
|
2024-05-01 09:50:58 -03:00
|
|
|
if (rangefinder_state.enabled_and_healthy() || rangefinder_state.data_stale()) {
|
|
|
|
g2.proximity.set_rangefinder_alt(rangefinder_state.enabled, rangefinder_state.alt_healthy, rangefinder_state.alt_cm_filt.get());
|
2019-06-25 23:16:30 -03:00
|
|
|
}
|
2024-05-01 09:50:58 -03:00
|
|
|
#endif
|
2016-04-28 00:30:41 -03:00
|
|
|
|
2012-12-21 01:03:47 -04:00
|
|
|
#endif
|
|
|
|
}
|
|
|
|
|
2016-04-27 09:18:35 -03:00
|
|
|
// return true if rangefinder_alt can be used
|
2021-02-01 12:26:21 -04:00
|
|
|
bool Copter::rangefinder_alt_ok() const
|
2016-04-27 09:18:35 -03:00
|
|
|
{
|
2024-05-01 09:50:58 -03:00
|
|
|
return rangefinder_state.enabled_and_healthy();
|
2016-04-27 09:18:35 -03:00
|
|
|
}
|
|
|
|
|
2019-08-23 22:59:22 -03:00
|
|
|
// return true if rangefinder_alt can be used
|
2021-02-01 12:26:21 -04:00
|
|
|
bool Copter::rangefinder_up_ok() const
|
2019-08-23 22:59:22 -03:00
|
|
|
{
|
2024-05-01 09:50:58 -03:00
|
|
|
return rangefinder_up_state.enabled_and_healthy();
|
2019-08-23 22:59:22 -03:00
|
|
|
}
|
|
|
|
|
2023-03-03 21:01:34 -04:00
|
|
|
// update rangefinder based terrain offset
|
|
|
|
// terrain offset is the terrain's height above the EKF origin
|
|
|
|
void Copter::update_rangefinder_terrain_offset()
|
|
|
|
{
|
|
|
|
float terrain_offset_cm = rangefinder_state.inertial_alt_cm - rangefinder_state.alt_cm_glitch_protected;
|
|
|
|
rangefinder_state.terrain_offset_cm += (terrain_offset_cm - rangefinder_state.terrain_offset_cm) * (copter.G_Dt / MAX(copter.g2.surftrak_tc, copter.G_Dt));
|
|
|
|
|
|
|
|
terrain_offset_cm = rangefinder_up_state.inertial_alt_cm + rangefinder_up_state.alt_cm_glitch_protected;
|
|
|
|
rangefinder_up_state.terrain_offset_cm += (terrain_offset_cm - rangefinder_up_state.terrain_offset_cm) * (copter.G_Dt / MAX(copter.g2.surftrak_tc, copter.G_Dt));
|
|
|
|
|
2024-05-01 09:50:58 -03:00
|
|
|
if (rangefinder_state.alt_healthy || rangefinder_state.data_stale()) {
|
2023-03-03 21:01:34 -04:00
|
|
|
wp_nav->set_rangefinder_terrain_offset(rangefinder_state.enabled, rangefinder_state.alt_healthy, rangefinder_state.terrain_offset_cm);
|
|
|
|
#if MODE_CIRCLE_ENABLED
|
|
|
|
circle_nav->set_rangefinder_terrain_offset(rangefinder_state.enabled && wp_nav->rangefinder_used(), rangefinder_state.alt_healthy, rangefinder_state.terrain_offset_cm);
|
|
|
|
#endif
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2024-05-01 09:50:58 -03:00
|
|
|
// helper function to get inertially interpolated rangefinder height.
|
2022-06-15 00:24:26 -03:00
|
|
|
bool Copter::get_rangefinder_height_interpolated_cm(int32_t& ret) const
|
2020-02-18 18:22:31 -04:00
|
|
|
{
|
2024-05-01 09:50:58 -03:00
|
|
|
#if RANGEFINDER_ENABLED == ENABLED && AP_RANGEFINDER_ENABLED
|
|
|
|
return rangefinder_state.get_rangefinder_height_interpolated_cm(ret);
|
|
|
|
#else
|
|
|
|
return false;
|
|
|
|
#endif
|
2020-02-18 18:22:31 -04:00
|
|
|
}
|