bool AC_PrecLand::get_target_velocity_relative_cms(Vector2f& ret)
{
if (!target_acquired()) {
return false;
}
ret = _target_vel_rel_out_NE*100.0f;
return true;
}
bool AC_PrecLand::get_target_position_relative_cm(Vector2f& ret)
{
if (!target_acquired()) {
return false;
}
ret = _target_pos_rel_out_NE*100.0f;
return true;
}
bool AC_PrecLand::get_target_velocity_relative_cms(Vector2f& ret) const
{
if (!target_acquired()) {
return false;
}
ret.x = _ekf_x.getVel()*100.0f;
ret.y = _ekf_y.getVel()*100.0f;
return true;
}
bool AC_PrecLand::get_target_position_relative_cm(Vector2f& ret) const
{
if (!target_acquired()) {
return false;
}
Vector3f land_ofs_ned_cm = _ahrs.get_rotation_body_to_ned() * Vector3f(_land_ofs_cm_x,_land_ofs_cm_y,0);
ret.x = _ekf_x.getPos()*100.0f + land_ofs_ned_cm.x;
ret.y = _ekf_y.getPos()*100.0f + land_ofs_ned_cm.y;
return true;
}
bool AC_PrecLand::get_target_position_cm(Vector2f& ret)
{
if (!target_acquired()) {
return false;
}
Vector2f curr_pos;
if (!AP::ahrs().get_relative_position_NE_origin(curr_pos)) {
return false;
}
ret.x = (_target_pos_rel_out_NE.x + curr_pos.x) * 100.0f; // m to cm
ret.y = (_target_pos_rel_out_NE.y + curr_pos.y) * 100.0f; // m to cm
return true;
}
void AC_PrecLand::run_estimator(float rangefinder_alt_m, bool rangefinder_alt_valid)
{
const struct inertial_data_frame_s *inertial_data_delayed = (*_inertial_history)[0];
switch (_estimator_type) {
case ESTIMATOR_TYPE_RAW_SENSOR: {
// Return if there's any invalid velocity data
for (uint8_t i=0; i<_inertial_history->available(); i++) {
const struct inertial_data_frame_s *inertial_data = (*_inertial_history)[i];
if (!inertial_data->inertialNavVelocityValid) {
_target_acquired = false;
return;
}
}
// Predict
if (target_acquired()) {
_target_pos_rel_est_NE.x -= inertial_data_delayed->inertialNavVelocity.x * inertial_data_delayed->dt;
_target_pos_rel_est_NE.y -= inertial_data_delayed->inertialNavVelocity.y * inertial_data_delayed->dt;
_target_vel_rel_est_NE.x = -inertial_data_delayed->inertialNavVelocity.x;
_target_vel_rel_est_NE.y = -inertial_data_delayed->inertialNavVelocity.y;
}
// Update if a new Line-Of-Sight measurement is available
if (construct_pos_meas_using_rangefinder(rangefinder_alt_m, rangefinder_alt_valid)) {
_target_pos_rel_est_NE.x = _target_pos_rel_meas_NED.x;
_target_pos_rel_est_NE.y = _target_pos_rel_meas_NED.y;
_target_vel_rel_est_NE.x = -inertial_data_delayed->inertialNavVelocity.x;
_target_vel_rel_est_NE.y = -inertial_data_delayed->inertialNavVelocity.y;
_last_update_ms = AP_HAL::millis();
_target_acquired = true;
}
// Output prediction
if (target_acquired()) {
run_output_prediction();
}
break;
}
case ESTIMATOR_TYPE_KALMAN_FILTER: {
// Predict
if (target_acquired()) {
const float& dt = inertial_data_delayed->dt;
const Vector3f& vehicleDelVel = inertial_data_delayed->correctedVehicleDeltaVelocityNED;
_ekf_x.predict(dt, -vehicleDelVel.x, _accel_noise*dt);
_ekf_y.predict(dt, -vehicleDelVel.y, _accel_noise*dt);
}
// Update if a new Line-Of-Sight measurement is available
if (construct_pos_meas_using_rangefinder(rangefinder_alt_m, rangefinder_alt_valid)) {
float xy_pos_var = sq(_target_pos_rel_meas_NED.z*(0.01f + 0.01f*AP::ahrs().get_gyro().length()) + 0.02f);
if (!target_acquired()) {
// reset filter state
if (inertial_data_delayed->inertialNavVelocityValid) {
_ekf_x.init(_target_pos_rel_meas_NED.x, xy_pos_var, -inertial_data_delayed->inertialNavVelocity.x, sq(2.0f));
_ekf_y.init(_target_pos_rel_meas_NED.y, xy_pos_var, -inertial_data_delayed->inertialNavVelocity.y, sq(2.0f));
} else {
_ekf_x.init(_target_pos_rel_meas_NED.x, xy_pos_var, 0.0f, sq(10.0f));
_ekf_y.init(_target_pos_rel_meas_NED.y, xy_pos_var, 0.0f, sq(10.0f));
}
_last_update_ms = AP_HAL::millis();
_target_acquired = true;
} else {
float NIS_x = _ekf_x.getPosNIS(_target_pos_rel_meas_NED.x, xy_pos_var);
float NIS_y = _ekf_y.getPosNIS(_target_pos_rel_meas_NED.y, xy_pos_var);
if (MAX(NIS_x, NIS_y) < 3.0f || _outlier_reject_count >= 3) {
_outlier_reject_count = 0;
_ekf_x.fusePos(_target_pos_rel_meas_NED.x, xy_pos_var);
_ekf_y.fusePos(_target_pos_rel_meas_NED.y, xy_pos_var);
_last_update_ms = AP_HAL::millis();
_target_acquired = true;
} else {
_outlier_reject_count++;
}
}
}
// Output prediction
if (target_acquired()) {
_target_pos_rel_est_NE.x = _ekf_x.getPos();
_target_pos_rel_est_NE.y = _ekf_y.getPos();
_target_vel_rel_est_NE.x = _ekf_x.getVel();
_target_vel_rel_est_NE.y = _ekf_y.getVel();
run_output_prediction();
}
break;
}
}
}
// update - give chance to driver to get updates from sensor
void AC_PrecLand::update(float rangefinder_alt_cm, bool rangefinder_alt_valid)
{
_attitude_history.push_back(_ahrs.get_rotation_body_to_ned());
// run backend update
if (_backend != nullptr && _enabled) {
// read from sensor
_backend->update();
Vector3f vehicleVelocityNED = _inav.get_velocity()*0.01f;
vehicleVelocityNED.z = -vehicleVelocityNED.z;
if (target_acquired()) {
// EKF prediction step
float dt;
Vector3f targetDelVel;
_ahrs.getCorrectedDeltaVelocityNED(targetDelVel, dt);
targetDelVel = -targetDelVel;
_ekf_x.predict(dt, targetDelVel.x, 0.5f*dt);
_ekf_y.predict(dt, targetDelVel.y, 0.5f*dt);
}
if (_backend->have_los_meas() && _backend->los_meas_time_ms() != _last_backend_los_meas_ms) {
// we have a new, unique los measurement
_last_backend_los_meas_ms = _backend->los_meas_time_ms();
Vector3f target_vec_unit_body;
_backend->get_los_body(target_vec_unit_body);
// Apply sensor yaw alignment rotation
float sin_yaw_align = sinf(radians(_yaw_align*0.01f));
float cos_yaw_align = cosf(radians(_yaw_align*0.01f));
Matrix3f Rz = Matrix3f(
cos_yaw_align, -sin_yaw_align, 0,
sin_yaw_align, cos_yaw_align, 0,
0, 0, 1
);
Vector3f target_vec_unit_ned = _attitude_history.front() * Rz * target_vec_unit_body;
bool target_vec_valid = target_vec_unit_ned.z > 0.0f;
if (target_vec_valid && rangefinder_alt_valid && rangefinder_alt_cm > 0.0f) {
float alt = MAX(rangefinder_alt_cm*0.01f, 0.0f);
float dist = alt/target_vec_unit_ned.z;
Vector3f targetPosRelMeasNED = Vector3f(target_vec_unit_ned.x*dist, target_vec_unit_ned.y*dist, alt);
float xy_pos_var = sq(targetPosRelMeasNED.z*(0.01f + 0.01f*_ahrs.get_gyro().length()) + 0.02f);
if (!target_acquired()) {
// reset filter state
if (_inav.get_filter_status().flags.horiz_pos_rel) {
_ekf_x.init(targetPosRelMeasNED.x, xy_pos_var, -vehicleVelocityNED.x, sq(2.0f));
_ekf_y.init(targetPosRelMeasNED.y, xy_pos_var, -vehicleVelocityNED.y, sq(2.0f));
} else {
_ekf_x.init(targetPosRelMeasNED.x, xy_pos_var, 0.0f, sq(10.0f));
_ekf_y.init(targetPosRelMeasNED.y, xy_pos_var, 0.0f, sq(10.0f));
}
_last_update_ms = AP_HAL::millis();
} else {
float NIS_x = _ekf_x.getPosNIS(targetPosRelMeasNED.x, xy_pos_var);
float NIS_y = _ekf_y.getPosNIS(targetPosRelMeasNED.y, xy_pos_var);
if (MAX(NIS_x, NIS_y) < 3.0f || _outlier_reject_count >= 3) {
_outlier_reject_count = 0;
_ekf_x.fusePos(targetPosRelMeasNED.x, xy_pos_var);
_ekf_y.fusePos(targetPosRelMeasNED.y, xy_pos_var);
_last_update_ms = AP_HAL::millis();
} else {
_outlier_reject_count++;
}
}
}
}
}
}
