ardupilot/ArduCopter/sensors.cpp

230 lines
7.1 KiB
C++

#include "Copter.h"
// return barometric altitude in centimeters
void Copter::read_barometer(void)
{
barometer.update();
baro_alt = barometer.get_altitude() * 100.0f;
motors->set_air_density_ratio(barometer.get_air_density_ratio());
}
void Copter::init_rangefinder(void)
{
#if RANGEFINDER_ENABLED == ENABLED
rangefinder.set_log_rfnd_bit(MASK_LOG_CTUN);
rangefinder.init(ROTATION_PITCH_270);
rangefinder_state.alt_cm_filt.set_cutoff_frequency(RANGEFINDER_WPNAV_FILT_HZ);
rangefinder_state.enabled = rangefinder.has_orientation(ROTATION_PITCH_270);
// 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);
#endif
}
// return rangefinder altitude in centimeters
void Copter::read_rangefinder(void)
{
#if RANGEFINDER_ENABLED == ENABLED
rangefinder.update();
#if RANGEFINDER_TILT_CORRECTION == ENABLED
const float tilt_correction = MAX(0.707f, ahrs.get_rotation_body_to_ned().c.z);
#else
const float tilt_correction = 1.0f;
#endif
// iterate through downward and upward facing lidar
struct {
RangeFinderState &state;
enum Rotation orientation;
} rngfnd[2] = { {rangefinder_state, ROTATION_PITCH_270}, {rangefinder_up_state, ROTATION_PITCH_90}};
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
rf_state.alt_healthy = ((rangefinder.status_orient(rf_orient) == RangeFinder::Status::Good) &&
(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);
// 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);
} else {
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();
}
// 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 {
rf_state.alt_cm_filt.apply(rf_state.alt_cm, 0.05f);
}
rf_state.last_healthy_ms = now;
}
// send downward facing lidar altitude and health to waypoint navigation library
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());
}
}
}
#else
// downward facing rangefinder
rangefinder_state.enabled = false;
rangefinder_state.alt_healthy = false;
rangefinder_state.alt_cm = 0;
// upward facing rangefinder
rangefinder_up_state.enabled = false;
rangefinder_up_state.alt_healthy = false;
rangefinder_up_state.alt_cm = 0;
#endif
}
// return true if rangefinder_alt can be used
bool Copter::rangefinder_alt_ok()
{
return (rangefinder_state.enabled && rangefinder_state.alt_healthy);
}
// return true if rangefinder_alt can be used
bool Copter::rangefinder_up_ok()
{
return (rangefinder_up_state.enabled && rangefinder_up_state.alt_healthy);
}
/*
update RPM sensors
*/
void Copter::rpm_update(void)
{
#if RPM_ENABLED == ENABLED
rpm_sensor.update();
if (rpm_sensor.enabled(0) || rpm_sensor.enabled(1)) {
if (should_log(MASK_LOG_RCIN)) {
logger.Write_RPM(rpm_sensor);
}
}
#endif
}
// initialise optical flow sensor
void Copter::init_optflow()
{
#if OPTFLOW == ENABLED
// initialise optical flow sensor
optflow.init(MASK_LOG_OPTFLOW);
#endif // OPTFLOW == ENABLED
}
void Copter::compass_cal_update()
{
compass.cal_update();
if (hal.util->get_soft_armed()) {
return;
}
static uint32_t compass_cal_stick_gesture_begin = 0;
if (compass.is_calibrating()) {
if (channel_yaw->get_control_in() < -4000 && channel_throttle->get_control_in() > 900) {
compass.cancel_calibration_all();
}
} else {
bool stick_gesture_detected = compass_cal_stick_gesture_begin != 0 && !motors->armed() && channel_yaw->get_control_in() > 4000 && channel_throttle->get_control_in() > 900;
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) {
#ifdef CAL_ALWAYS_REBOOT
compass.start_calibration_all(true,true,COMPASS_CAL_STICK_DELAY,true);
#else
compass.start_calibration_all(true,true,COMPASS_CAL_STICK_DELAY,false);
#endif
}
}
}
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));
}
#ifdef CAL_ALWAYS_REBOOT
if (ins.accel_cal_requires_reboot()) {
hal.scheduler->delay(1000);
hal.scheduler->reboot(false);
}
#endif
}
// initialise proximity sensor
void Copter::init_proximity(void)
{
#if PROXIMITY_ENABLED == ENABLED
g2.proximity.init();
#endif
}
// init visual odometry sensor
void Copter::init_visual_odom()
{
#if VISUAL_ODOMETRY_ENABLED == ENABLED
g2.visual_odom.init();
#endif
}
// winch and wheel encoder initialisation
void Copter::winch_init()
{
#if WINCH_ENABLED == ENABLED
g2.wheel_encoder.init();
g2.winch.init(&g2.wheel_encoder);
#endif
}
// winch and wheel encoder update
void Copter::winch_update()
{
#if WINCH_ENABLED == ENABLED
g2.wheel_encoder.update();
g2.winch.update();
#endif
}