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;
|
2015-04-22 22:19:44 -03:00
|
|
|
|
2017-01-09 03:31:26 -04:00
|
|
|
motors->set_air_density_ratio(barometer.get_air_density_ratio());
|
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
|
|
|
{
|
2016-04-27 08:37:04 -03:00
|
|
|
#if RANGEFINDER_ENABLED == 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);
|
2016-04-28 00:30:41 -03:00
|
|
|
rangefinder_state.alt_cm_filt.set_cutoff_frequency(RANGEFINDER_WPNAV_FILT_HZ);
|
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
|
|
|
|
rangefinder_up_state.alt_cm_filt.set_cutoff_frequency(RANGEFINDER_WPNAV_FILT_HZ);
|
|
|
|
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
|
|
|
{
|
2016-04-27 07:55:35 -03:00
|
|
|
#if RANGEFINDER_ENABLED == ENABLED
|
2016-04-27 08:37:04 -03:00
|
|
|
rangefinder.update();
|
2014-06-27 01:23:56 -03:00
|
|
|
|
2019-08-23 22:59:22 -03:00
|
|
|
#if RANGEFINDER_TILT_CORRECTION == ENABLED
|
|
|
|
const float tilt_correction = MAX(0.707f, ahrs.get_rotation_body_to_ned().c.z);
|
|
|
|
#else
|
2019-10-31 19:34:33 -03:00
|
|
|
const float tilt_correction = 1.0f;
|
2019-08-23 22:59:22 -03:00
|
|
|
#endif
|
2019-06-25 23:16:30 -03:00
|
|
|
|
2019-08-23 22:59:22 -03:00
|
|
|
// iterate through downward and upward facing lidar
|
|
|
|
struct {
|
|
|
|
RangeFinderState &state;
|
|
|
|
enum Rotation orientation;
|
2020-02-16 22:42:25 -04:00
|
|
|
} rngfnd[2] = {{rangefinder_state, ROTATION_PITCH_270}, {rangefinder_up_state, ROTATION_PITCH_90}};
|
2019-08-23 22:59:22 -03:00
|
|
|
|
|
|
|
for (uint8_t i=0; i < ARRAY_SIZE(rngfnd); i++) {
|
|
|
|
// local variables to make accessing simpler
|
|
|
|
RangeFinderState &rf_state = rngfnd[i].state;
|
|
|
|
enum Rotation rf_orient = rngfnd[i].orientation;
|
|
|
|
|
|
|
|
// update health
|
2019-11-01 02:13:55 -03:00
|
|
|
rf_state.alt_healthy = ((rangefinder.status_orient(rf_orient) == RangeFinder::Status::Good) &&
|
2019-08-23 22:59:22 -03:00
|
|
|
(rangefinder.range_valid_count_orient(rf_orient) >= RANGEFINDER_HEALTH_MAX));
|
|
|
|
|
|
|
|
// tilt corrected but unfiltered, not glitch protected alt
|
|
|
|
rf_state.alt_cm = tilt_correction * rangefinder.distance_cm_orient(rf_orient);
|
|
|
|
|
2020-02-18 18:22:31 -04:00
|
|
|
// remember inertial alt to allow us to interpolate rangefinder
|
|
|
|
rf_state.inertial_alt_cm = inertial_nav.get_altitude();
|
|
|
|
|
2019-08-23 22:59:22 -03:00
|
|
|
// glitch handling. rangefinder readings more than RANGEFINDER_GLITCH_ALT_CM from the last good reading
|
|
|
|
// are considered a glitch and glitch_count becomes non-zero
|
|
|
|
// glitches clear after RANGEFINDER_GLITCH_NUM_SAMPLES samples in a row.
|
|
|
|
// glitch_cleared_ms is set so surface tracking (or other consumers) can trigger a target reset
|
|
|
|
const int32_t glitch_cm = rf_state.alt_cm - rf_state.alt_cm_glitch_protected;
|
|
|
|
if (glitch_cm >= RANGEFINDER_GLITCH_ALT_CM) {
|
|
|
|
rf_state.glitch_count = MAX(rf_state.glitch_count+1, 1);
|
|
|
|
} else if (glitch_cm <= -RANGEFINDER_GLITCH_ALT_CM) {
|
|
|
|
rf_state.glitch_count = MIN(rf_state.glitch_count-1, -1);
|
2016-04-28 00:30:41 -03:00
|
|
|
} else {
|
2019-08-23 22:59:22 -03:00
|
|
|
rf_state.glitch_count = 0;
|
|
|
|
rf_state.alt_cm_glitch_protected = rf_state.alt_cm;
|
|
|
|
}
|
|
|
|
if (abs(rf_state.glitch_count) >= RANGEFINDER_GLITCH_NUM_SAMPLES) {
|
|
|
|
// clear glitch and record time so consumers (i.e. surface tracking) can reset their target altitudes
|
|
|
|
rf_state.glitch_count = 0;
|
|
|
|
rf_state.alt_cm_glitch_protected = rf_state.alt_cm;
|
|
|
|
rf_state.glitch_cleared_ms = AP_HAL::millis();
|
2016-04-28 00:30:41 -03:00
|
|
|
}
|
|
|
|
|
2019-08-23 22:59:22 -03:00
|
|
|
// filter rangefinder altitude
|
|
|
|
uint32_t now = AP_HAL::millis();
|
|
|
|
const bool timed_out = now - rf_state.last_healthy_ms > RANGEFINDER_TIMEOUT_MS;
|
|
|
|
if (rf_state.alt_healthy) {
|
|
|
|
if (timed_out) {
|
|
|
|
// reset filter if we haven't used it within the last second
|
|
|
|
rf_state.alt_cm_filt.reset(rf_state.alt_cm);
|
|
|
|
} else {
|
2019-09-27 22:01:38 -03:00
|
|
|
rf_state.alt_cm_filt.apply(rf_state.alt_cm, 0.05f);
|
2019-08-23 22:59:22 -03:00
|
|
|
}
|
|
|
|
rf_state.last_healthy_ms = now;
|
|
|
|
}
|
|
|
|
|
2021-02-19 14:56:00 -04:00
|
|
|
// send downward facing lidar altitude and health to the libraries that require it
|
2019-08-23 22:59:22 -03:00
|
|
|
if (rf_orient == ROTATION_PITCH_270) {
|
|
|
|
if (rangefinder_state.alt_healthy || timed_out) {
|
|
|
|
wp_nav->set_rangefinder_alt(rangefinder_state.enabled, rangefinder_state.alt_healthy, rangefinder_state.alt_cm_filt.get());
|
2020-10-01 19:30:30 -03:00
|
|
|
#if MODE_CIRCLE_ENABLED
|
2020-04-09 02:09:06 -03:00
|
|
|
circle_nav->set_rangefinder_alt(rangefinder_state.enabled && wp_nav->rangefinder_used(), rangefinder_state.alt_healthy, rangefinder_state.alt_cm_filt.get());
|
2021-02-19 14:56:00 -04:00
|
|
|
#endif
|
|
|
|
#if PROXIMITY_ENABLED == ENABLED
|
|
|
|
g2.proximity.set_rangefinder_alt(rangefinder_state.enabled, rangefinder_state.alt_healthy, rangefinder_state.alt_cm_filt.get());
|
2020-10-01 19:30:30 -03:00
|
|
|
#endif
|
2019-08-23 22:59:22 -03:00
|
|
|
}
|
|
|
|
}
|
2019-06-25 23:16:30 -03:00
|
|
|
}
|
2016-04-28 00:30:41 -03:00
|
|
|
|
2012-12-21 01:03:47 -04:00
|
|
|
#else
|
2019-08-23 22:59:22 -03:00
|
|
|
// downward facing rangefinder
|
2016-05-11 03:31:58 -03:00
|
|
|
rangefinder_state.enabled = false;
|
2016-05-07 04:55:40 -03:00
|
|
|
rangefinder_state.alt_healthy = false;
|
|
|
|
rangefinder_state.alt_cm = 0;
|
2019-08-23 22:59:22 -03:00
|
|
|
|
|
|
|
// upward facing rangefinder
|
|
|
|
rangefinder_up_state.enabled = false;
|
|
|
|
rangefinder_up_state.alt_healthy = false;
|
|
|
|
rangefinder_up_state.alt_cm = 0;
|
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
|
|
|
{
|
2016-05-07 04:55:40 -03:00
|
|
|
return (rangefinder_state.enabled && rangefinder_state.alt_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
|
|
|
{
|
|
|
|
return (rangefinder_up_state.enabled && rangefinder_up_state.alt_healthy);
|
|
|
|
}
|
|
|
|
|
2020-02-18 18:22:31 -04:00
|
|
|
/*
|
|
|
|
get inertially interpolated rangefinder height. Inertial height is
|
|
|
|
recorded whenever we update the rangefinder height, then we use the
|
|
|
|
difference between the inertial height at that time and the current
|
|
|
|
inertial height to give us interpolation of height from rangefinder
|
|
|
|
*/
|
|
|
|
bool Copter::get_rangefinder_height_interpolated_cm(int32_t& ret)
|
|
|
|
{
|
|
|
|
if (!rangefinder_alt_ok()) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
ret = rangefinder_state.alt_cm_filt.get();
|
|
|
|
float inertial_alt_cm = inertial_nav.get_altitude();
|
|
|
|
ret += inertial_alt_cm - rangefinder_state.inertial_alt_cm;
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
2015-08-07 02:34:56 -03:00
|
|
|
/*
|
|
|
|
update RPM sensors
|
|
|
|
*/
|
|
|
|
void Copter::rpm_update(void)
|
|
|
|
{
|
2018-03-29 10:49:27 -03:00
|
|
|
#if RPM_ENABLED == ENABLED
|
2015-08-07 02:34:56 -03:00
|
|
|
rpm_sensor.update();
|
2015-11-19 22:06:14 -04:00
|
|
|
if (rpm_sensor.enabled(0) || rpm_sensor.enabled(1)) {
|
2015-08-07 07:34:27 -03:00
|
|
|
if (should_log(MASK_LOG_RCIN)) {
|
2019-01-18 00:24:08 -04:00
|
|
|
logger.Write_RPM(rpm_sensor);
|
2015-08-07 07:34:27 -03:00
|
|
|
}
|
|
|
|
}
|
2018-03-29 10:49:27 -03:00
|
|
|
#endif
|
2015-08-07 02:34:56 -03:00
|
|
|
}
|
|
|
|
|
2014-10-31 08:40:24 -03:00
|
|
|
// initialise optical flow sensor
|
2015-05-29 23:12:49 -03:00
|
|
|
void Copter::init_optflow()
|
2011-12-29 23:06:31 -04:00
|
|
|
{
|
2012-11-18 12:16:07 -04:00
|
|
|
#if OPTFLOW == ENABLED
|
2015-04-23 04:24:31 -03:00
|
|
|
// initialise optical flow sensor
|
2018-09-02 09:25:52 -03:00
|
|
|
optflow.init(MASK_LOG_OPTFLOW);
|
2012-11-18 12:16:07 -04:00
|
|
|
#endif // OPTFLOW == ENABLED
|
2011-12-29 23:06:31 -04:00
|
|
|
}
|
|
|
|
|
2015-07-01 16:36:53 -03:00
|
|
|
void Copter::compass_cal_update()
|
|
|
|
{
|
2019-03-25 06:22:28 -03:00
|
|
|
compass.cal_update();
|
2016-09-27 17:33:56 -03:00
|
|
|
|
2019-03-25 06:22:28 -03:00
|
|
|
if (hal.util->get_soft_armed()) {
|
|
|
|
return;
|
2015-07-01 16:36:53 -03:00
|
|
|
}
|
2016-09-27 17:33:56 -03:00
|
|
|
|
2019-03-25 06:22:28 -03:00
|
|
|
static uint32_t compass_cal_stick_gesture_begin = 0;
|
|
|
|
|
2016-09-27 17:33:56 -03:00
|
|
|
if (compass.is_calibrating()) {
|
|
|
|
if (channel_yaw->get_control_in() < -4000 && channel_throttle->get_control_in() > 900) {
|
|
|
|
compass.cancel_calibration_all();
|
|
|
|
}
|
|
|
|
} else {
|
2017-01-09 03:31:26 -04:00
|
|
|
bool stick_gesture_detected = compass_cal_stick_gesture_begin != 0 && !motors->armed() && channel_yaw->get_control_in() > 4000 && channel_throttle->get_control_in() > 900;
|
2016-09-27 17:33:56 -03:00
|
|
|
uint32_t tnow = millis();
|
|
|
|
|
|
|
|
if (!stick_gesture_detected) {
|
|
|
|
compass_cal_stick_gesture_begin = tnow;
|
|
|
|
} else if (tnow-compass_cal_stick_gesture_begin > 1000*COMPASS_CAL_STICK_GESTURE_TIME) {
|
2016-10-25 02:19:18 -03:00
|
|
|
#ifdef CAL_ALWAYS_REBOOT
|
|
|
|
compass.start_calibration_all(true,true,COMPASS_CAL_STICK_DELAY,true);
|
|
|
|
#else
|
2016-09-27 17:33:56 -03:00
|
|
|
compass.start_calibration_all(true,true,COMPASS_CAL_STICK_DELAY,false);
|
2016-10-25 02:19:18 -03:00
|
|
|
#endif
|
2016-09-27 17:33:56 -03:00
|
|
|
}
|
|
|
|
}
|
2015-07-01 16:36:53 -03:00
|
|
|
}
|
|
|
|
|
2015-10-16 19:05:53 -03:00
|
|
|
void Copter::accel_cal_update()
|
|
|
|
{
|
|
|
|
if (hal.util->get_soft_armed()) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
ins.acal_update();
|
|
|
|
// check if new trim values, and set them
|
|
|
|
float trim_roll, trim_pitch;
|
|
|
|
if(ins.get_new_trim(trim_roll, trim_pitch)) {
|
|
|
|
ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
|
|
|
|
}
|
2016-01-05 20:23:16 -04:00
|
|
|
|
|
|
|
#ifdef CAL_ALWAYS_REBOOT
|
|
|
|
if (ins.accel_cal_requires_reboot()) {
|
|
|
|
hal.scheduler->delay(1000);
|
|
|
|
hal.scheduler->reboot(false);
|
|
|
|
}
|
|
|
|
#endif
|
2015-10-16 19:05:53 -03:00
|
|
|
}
|
|
|
|
|
2016-08-15 03:53:45 -03:00
|
|
|
// initialise proximity sensor
|
|
|
|
void Copter::init_proximity(void)
|
|
|
|
{
|
|
|
|
#if PROXIMITY_ENABLED == ENABLED
|
2016-10-14 02:02:29 -03:00
|
|
|
g2.proximity.init();
|
2016-08-15 03:53:45 -03:00
|
|
|
#endif
|
|
|
|
}
|