ardupilot/libraries/AC_PrecLand/AC_PrecLand.cpp

248 lines
8.3 KiB
C++

#include <AP_HAL/AP_HAL.h>
#include "AC_PrecLand.h"
#include "AC_PrecLand_Backend.h"
#include "AC_PrecLand_Companion.h"
#include "AC_PrecLand_IRLock.h"
#include "AC_PrecLand_SITL_Gazebo.h"
#include "AC_PrecLand_SITL.h"
extern const AP_HAL::HAL& hal;
const AP_Param::GroupInfo AC_PrecLand::var_info[] = {
// @Param: ENABLED
// @DisplayName: Precision Land enabled/disabled and behaviour
// @Description: Precision Land enabled/disabled and behaviour
// @Values: 0:Disabled, 1:Enabled Always Land, 2:Enabled Strict
// @User: Advanced
AP_GROUPINFO_FLAGS("ENABLED", 0, AC_PrecLand, _enabled, 0, AP_PARAM_FLAG_ENABLE),
// @Param: TYPE
// @DisplayName: Precision Land Type
// @Description: Precision Land Type
// @Values: 0:None, 1:CompanionComputer, 2:IRLock, 3:SITL_Gazebo, 4:SITL
// @User: Advanced
AP_GROUPINFO("TYPE", 1, AC_PrecLand, _type, 0),
// @Param: YAW_ALIGN
// @DisplayName: Sensor yaw alignment
// @Description: Yaw angle from body x-axis to sensor x-axis.
// @Range: 0 360
// @Increment: 1
// @User: Advanced
// @Units: Centi-degrees
AP_GROUPINFO("YAW_ALIGN", 2, AC_PrecLand, _yaw_align, 0),
// @Param: LAND_OFS_X
// @DisplayName: Land offset forward
// @Description: Desired landing position of the camera forward of the target in vehicle body frame
// @Range: -20 20
// @Increment: 1
// @User: Advanced
// @Units: Centimeters
AP_GROUPINFO("LAND_OFS_X", 3, AC_PrecLand, _land_ofs_cm_x, 0),
// @Param: LAND_OFS_Y
// @DisplayName: Land offset right
// @Description: desired landing position of the camera right of the target in vehicle body frame
// @Range: -20 20
// @Increment: 1
// @User: Advanced
// @Units: Centimeters
AP_GROUPINFO("LAND_OFS_Y", 4, AC_PrecLand, _land_ofs_cm_y, 0),
// 5 RESERVED for EKF_TYPE
// 6 RESERVED for ACC_NSE
AP_GROUPEND
};
// Default constructor.
// Note that the Vector/Matrix constructors already implicitly zero
// their values.
//
AC_PrecLand::AC_PrecLand(const AP_AHRS& ahrs, const AP_InertialNav& inav) :
_ahrs(ahrs),
_inav(inav),
_last_update_ms(0),
_last_backend_los_meas_ms(0),
_backend(nullptr)
{
// set parameters to defaults
AP_Param::setup_object_defaults(this, var_info);
// other initialisation
_backend_state.healthy = false;
}
// init - perform any required initialisation of backends
void AC_PrecLand::init()
{
// exit immediately if init has already been run
if (_backend != nullptr) {
return;
}
// default health to false
_backend = nullptr;
_backend_state.healthy = false;
// instantiate backend based on type parameter
switch ((enum PrecLandType)(_type.get())) {
// no type defined
case PRECLAND_TYPE_NONE:
default:
return;
// companion computer
case PRECLAND_TYPE_COMPANION:
_backend = new AC_PrecLand_Companion(*this, _backend_state);
break;
// IR Lock
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
case PRECLAND_TYPE_IRLOCK:
_backend = new AC_PrecLand_IRLock(*this, _backend_state);
break;
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case PRECLAND_TYPE_SITL_GAZEBO:
_backend = new AC_PrecLand_SITL_Gazebo(*this, _backend_state);
break;
case PRECLAND_TYPE_SITL:
_backend = new AC_PrecLand_SITL(*this, _backend_state);
break;
#endif
}
// init backend
if (_backend != nullptr) {
_backend->init();
}
}
// 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++;
}
}
}
}
}
}
bool AC_PrecLand::target_acquired() const
{
return (AP_HAL::millis()-_last_update_ms) < 2000;
}
bool AC_PrecLand::get_target_position_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 + _inav.get_position().x + land_ofs_ned_cm.x;
ret.y = _ekf_y.getPos()*100.0f + _inav.get_position().y + land_ofs_ned_cm.y;
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_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;
}
// handle_msg - Process a LANDING_TARGET mavlink message
void AC_PrecLand::handle_msg(mavlink_message_t* msg)
{
// run backend update
if (_backend != nullptr) {
_backend->handle_msg(msg);
}
}