// Resets the baro so that it reads zero at the current height
// Resets the EKF height to zero
// Adjusts the EKf origin height so that the EKF height + origin height is the same as before
// Returns true if the height datum reset has been performed
// If using a range finder for height no reset is performed and it returns false
bool AP_AHRS_NavEKF::resetHeightDatum(void)
{
// support locked access functions to AHRS data
WITH_SEMAPHORE(_rsem);
switch (ekf_type()) {
case 2:
default: {
EKF3.resetHeightDatum();
return EKF2.resetHeightDatum();
}
case 3: {
EKF2.resetHeightDatum();
return EKF3.resetHeightDatum();
}
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL:
return false;
#endif
}
return false;
}
// run a full DCM update round
void
AP_AHRS_DCM::update(bool skip_ins_update)
{
// support locked access functions to AHRS data
WITH_SEMAPHORE(_rsem);
float delta_t;
if (_last_startup_ms == 0) {
_last_startup_ms = AP_HAL::millis();
load_watchdog_home();
}
if (!skip_ins_update) {
// tell the IMU to grab some data
AP::ins().update();
}
const AP_InertialSensor &_ins = AP::ins();
// ask the IMU how much time this sensor reading represents
delta_t = _ins.get_delta_time();
// if the update call took more than 0.2 seconds then discard it,
// otherwise we may move too far. This happens when arming motors
// in ArduCopter
if (delta_t > 0.2f) {
memset((void *)&_ra_sum[0], 0, sizeof(_ra_sum));
_ra_deltat = 0;
return;
}
// Integrate the DCM matrix using gyro inputs
matrix_update(delta_t);
// Normalize the DCM matrix
normalize();
// Perform drift correction
drift_correction(delta_t);
// paranoid check for bad values in the DCM matrix
check_matrix();
// Calculate pitch, roll, yaw for stabilization and navigation
euler_angles();
// update trig values including _cos_roll, cos_pitch
update_trig();
// update AOA and SSA
update_AOA_SSA();
backup_attitude();
}
// reset the current gyro drift estimate
// should be called if gyro offsets are recalculated
void AP_AHRS_NavEKF::reset_gyro_drift(void)
{
// support locked access functions to AHRS data
WITH_SEMAPHORE(_rsem);
// update DCM
AP_AHRS_DCM::reset_gyro_drift();
// reset the EKF gyro bias states
EKF2.resetGyroBias();
EKF3.resetGyroBias();
}
// reset the current attitude, used on new IMU calibration
void AP_AHRS_NavEKF::reset_attitude(const float &_roll, const float &_pitch, const float &_yaw)
{
// support locked access functions to AHRS data
WITH_SEMAPHORE(_rsem);
AP_AHRS_DCM::reset_attitude(_roll, _pitch, _yaw);
_dcm_attitude(roll, pitch, yaw);
if (_ekf2_started) {
_ekf2_started = EKF2.InitialiseFilter();
}
if (_ekf3_started) {
_ekf3_started = EKF3.InitialiseFilter();
}
}
void AP_AHRS_NavEKF::reset(bool recover_eulers)
{
// support locked access functions to AHRS data
WITH_SEMAPHORE(_rsem);
AP_AHRS_DCM::reset(recover_eulers);
_dcm_attitude(roll, pitch, yaw);
if (_ekf2_started) {
_ekf2_started = EKF2.InitialiseFilter();
}
if (_ekf3_started) {
_ekf3_started = EKF3.InitialiseFilter();
}
}
// vel is north/east/down!
void AP_Avoidance::add_obstacle(const uint32_t obstacle_timestamp_ms,
const MAV_COLLISION_SRC src,
const uint32_t src_id,
const Location &loc,
const Vector3f &vel_ned)
{
if (! check_startup()) {
return;
}
uint32_t oldest_timestamp = std::numeric_limits<uint32_t>::max();
uint8_t oldest_index = 255; // avoid compiler warning with initialisation
int16_t index = -1;
uint8_t i;
for (i=0; i<_obstacle_count; i++) {
if (_obstacles[i].src_id == src_id &&
_obstacles[i].src == src) {
// pre-existing obstacle found; we will update its information
index = i;
break;
}
if (_obstacles[i].timestamp_ms < oldest_timestamp) {
oldest_timestamp = _obstacles[i].timestamp_ms;
oldest_index = i;
}
}
WITH_SEMAPHORE(_rsem);
if (index == -1) {
// existing obstacle not found. See if we can store it anyway:
if (i <_obstacles_allocated) {
// have room to store more vehicles...
index = _obstacle_count++;
} else if (oldest_timestamp < obstacle_timestamp_ms) {
// replace this very old entry with this new data
index = oldest_index;
} else {
// no room for this (old?!) data
return;
}
_obstacles[index].src = src;
_obstacles[index].src_id = src_id;
}
_obstacles[index]._location = loc;
_obstacles[index]._velocity = vel_ned;
_obstacles[index].timestamp_ms = obstacle_timestamp_ms;
}
void AP_AHRS_NavEKF::update(bool skip_ins_update)
{
// support locked access functions to AHRS data
WITH_SEMAPHORE(_rsem);
// drop back to normal priority if we were boosted by the INS
// calling delay_microseconds_boost()
hal.scheduler->boost_end();
// EKF1 is no longer supported - handle case where it is selected
if (_ekf_type == 1) {
_ekf_type.set(2);
}
update_DCM(skip_ins_update);
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
update_SITL();
#endif
if (_ekf_type == 2) {
// if EK2 is primary then run EKF2 first to give it CPU
// priority
update_EKF2();
update_EKF3();
} else {
// otherwise run EKF3 first
update_EKF3();
update_EKF2();
}
#if AP_MODULE_SUPPORTED
// call AHRS_update hook if any
AP_Module::call_hook_AHRS_update(*this);
#endif
// push gyros if optical flow present
if (hal.opticalflow) {
const Vector3f &exported_gyro_bias = get_gyro_drift();
hal.opticalflow->push_gyro_bias(exported_gyro_bias.x, exported_gyro_bias.y);
}
if (_view != nullptr) {
// update optional alternative attitude view
_view->update(skip_ins_update);
}
}
/*
* reset the DCM matrix and omega. Used on ground start, and on
* extreme errors in the matrix
*/
void
AP_AHRS_DCM::reset(bool recover_eulers)
{
// support locked access functions to AHRS data
WITH_SEMAPHORE(_rsem);
// reset the integration terms
_omega_I.zero();
_omega_P.zero();
_omega_yaw_P.zero();
_omega.zero();
// if the caller wants us to try to recover to the current
// attitude then calculate the dcm matrix from the current
// roll/pitch/yaw values
if (hal.util->was_watchdog_reset() && AP_HAL::millis() < 10000) {
const AP_HAL::Util::PersistentData &pd = hal.util->persistent_data;
roll = pd.roll_rad;
pitch = pd.pitch_rad;
yaw = pd.yaw_rad;
_dcm_matrix.from_euler(roll, pitch, yaw);
gcs().send_text(MAV_SEVERITY_INFO, "Restored watchdog attitude %.0f %.0f %.0f",
degrees(roll), degrees(pitch), degrees(yaw));
} else if (recover_eulers && !isnan(roll) && !isnan(pitch) && !isnan(yaw)) {
_dcm_matrix.from_euler(roll, pitch, yaw);
} else {
// Use the measured accel due to gravity to calculate an initial
// roll and pitch estimate
AP_InertialSensor &_ins = AP::ins();
// Get body frame accel vector
Vector3f initAccVec = _ins.get_accel();
uint8_t counter = 0;
// the first vector may be invalid as the filter starts up
while ((initAccVec.length() < 9.0f || initAccVec.length() > 11) && counter++ < 20) {
_ins.wait_for_sample();
_ins.update();
initAccVec = _ins.get_accel();
}
// normalise the acceleration vector
if (initAccVec.length() > 5.0f) {
// calculate initial pitch angle
pitch = atan2f(initAccVec.x, norm(initAccVec.y, initAccVec.z));
// calculate initial roll angle
roll = atan2f(-initAccVec.y, -initAccVec.z);
} else {
// If we can't use the accel vector, then align flat
roll = 0.0f;
pitch = 0.0f;
}
_dcm_matrix.from_euler(roll, pitch, 0);
}
if (_last_startup_ms == 0) {
load_watchdog_home();
}
_last_startup_ms = AP_HAL::millis();
}
