2015-08-28 05:11:52 -03:00
|
|
|
#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"
|
2015-02-16 00:37:13 -04:00
|
|
|
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
|
2015-10-25 14:03:46 -03:00
|
|
|
const AP_Param::GroupInfo AC_PrecLand::var_info[] = {
|
2016-01-07 04:35:18 -04:00
|
|
|
// @Param: ENABLED
|
2015-02-16 00:37:13 -04:00
|
|
|
// @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
|
2016-08-04 15:29:45 -03:00
|
|
|
AP_GROUPINFO_FLAGS("ENABLED", 0, AC_PrecLand, _enabled, 0, AP_PARAM_FLAG_ENABLE),
|
2015-02-16 00:37:13 -04:00
|
|
|
|
|
|
|
// @Param: TYPE
|
|
|
|
// @DisplayName: Precision Land Type
|
|
|
|
// @Description: Precision Land Type
|
|
|
|
// @Values: 0:None, 1:CompanionComputer, 2:IRLock
|
|
|
|
// @User: Advanced
|
|
|
|
AP_GROUPINFO("TYPE", 1, AC_PrecLand, _type, 0),
|
|
|
|
|
2016-07-15 14:30:21 -03:00
|
|
|
// @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),
|
|
|
|
|
2015-02-16 00:37:13 -04:00
|
|
|
AP_GROUPEND
|
|
|
|
};
|
|
|
|
|
|
|
|
// Default constructor.
|
|
|
|
// Note that the Vector/Matrix constructors already implicitly zero
|
|
|
|
// their values.
|
|
|
|
//
|
2016-05-05 23:03:59 -03:00
|
|
|
AC_PrecLand::AC_PrecLand(const AP_AHRS& ahrs, const AP_InertialNav& inav) :
|
2015-02-16 00:37:13 -04:00
|
|
|
_ahrs(ahrs),
|
|
|
|
_inav(inav),
|
2016-06-16 14:10:48 -03:00
|
|
|
_last_update_ms(0),
|
2016-08-08 01:11:20 -03:00
|
|
|
_last_backend_los_meas_ms(0),
|
2015-02-16 00:37:13 -04:00
|
|
|
_backend(NULL)
|
|
|
|
{
|
|
|
|
// 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 != NULL) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
// default health to false
|
|
|
|
_backend = NULL;
|
|
|
|
_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
|
2016-09-14 20:31:16 -03:00
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN || CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
2015-02-16 00:37:13 -04:00
|
|
|
case PRECLAND_TYPE_IRLOCK:
|
|
|
|
_backend = new AC_PrecLand_IRLock(*this, _backend_state);
|
|
|
|
break;
|
2015-09-01 21:10:04 -03:00
|
|
|
#endif
|
2015-02-16 00:37:13 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
// init backend
|
|
|
|
if (_backend != NULL) {
|
|
|
|
_backend->init();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
// update - give chance to driver to get updates from sensor
|
2016-07-15 14:30:21 -03:00
|
|
|
void AC_PrecLand::update(float rangefinder_alt_cm, bool rangefinder_alt_valid)
|
2015-02-16 00:37:13 -04:00
|
|
|
{
|
2016-07-15 14:30:21 -03:00
|
|
|
_attitude_history.push_back(_ahrs.get_rotation_body_to_ned());
|
|
|
|
|
2015-02-16 00:37:13 -04:00
|
|
|
// run backend update
|
2016-07-08 15:47:17 -03:00
|
|
|
if (_backend != NULL && _enabled) {
|
2015-02-16 00:37:13 -04:00
|
|
|
// read from sensor
|
|
|
|
_backend->update();
|
2016-07-15 14:30:21 -03:00
|
|
|
|
|
|
|
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);
|
|
|
|
}
|
|
|
|
|
2016-07-05 16:37:17 -03:00
|
|
|
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);
|
2016-07-15 14:30:21 -03:00
|
|
|
|
|
|
|
// 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) {
|
2016-10-11 17:26:18 -03:00
|
|
|
_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));
|
2016-07-15 14:30:21 -03:00
|
|
|
} 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++;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
2016-07-05 16:37:17 -03:00
|
|
|
}
|
2015-02-16 00:37:13 -04:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2016-07-15 14:30:21 -03:00
|
|
|
bool AC_PrecLand::target_acquired() const
|
2015-02-16 00:37:13 -04:00
|
|
|
{
|
2016-07-15 14:30:21 -03:00
|
|
|
return (AP_HAL::millis()-_last_update_ms) < 2000;
|
2016-06-16 14:10:48 -03:00
|
|
|
}
|
2015-02-16 00:37:13 -04:00
|
|
|
|
2016-07-15 14:30:21 -03:00
|
|
|
bool AC_PrecLand::get_target_position_cm(Vector2f& ret) const
|
2016-06-16 14:10:48 -03:00
|
|
|
{
|
|
|
|
if (!target_acquired()) {
|
|
|
|
return false;
|
2015-02-16 00:37:13 -04:00
|
|
|
}
|
|
|
|
|
2016-07-15 14:30:21 -03:00
|
|
|
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;
|
2016-06-16 14:10:48 -03:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2016-07-15 14:30:21 -03:00
|
|
|
bool AC_PrecLand::get_target_position_relative_cm(Vector2f& ret) const
|
2016-06-16 14:10:48 -03:00
|
|
|
{
|
|
|
|
if (!target_acquired()) {
|
|
|
|
return false;
|
|
|
|
}
|
2015-02-16 00:37:13 -04:00
|
|
|
|
2016-07-15 14:30:21 -03:00
|
|
|
Vector3f land_ofs_ned_cm = _ahrs.get_rotation_body_to_ned() * Vector3f(_land_ofs_cm_x,_land_ofs_cm_y,0);
|
2015-02-16 00:37:13 -04:00
|
|
|
|
2016-07-15 14:30:21 -03:00
|
|
|
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;
|
2015-02-16 00:37:13 -04:00
|
|
|
}
|
|
|
|
|
2016-07-15 14:30:21 -03:00
|
|
|
bool AC_PrecLand::get_target_velocity_relative_cms(Vector2f& ret) const
|
2015-02-16 00:37:13 -04:00
|
|
|
{
|
2016-07-15 14:30:21 -03:00
|
|
|
if (!target_acquired()) {
|
|
|
|
return false;
|
2016-06-16 14:10:48 -03:00
|
|
|
}
|
2016-07-15 14:30:21 -03:00
|
|
|
ret.x = _ekf_x.getVel()*100.0f;
|
|
|
|
ret.y = _ekf_y.getVel()*100.0f;
|
|
|
|
return true;
|
2015-02-16 00:37:13 -04:00
|
|
|
}
|
2015-09-11 08:00:18 -03:00
|
|
|
|
|
|
|
// handle_msg - Process a LANDING_TARGET mavlink message
|
|
|
|
void AC_PrecLand::handle_msg(mavlink_message_t* msg)
|
|
|
|
{
|
|
|
|
// run backend update
|
|
|
|
if (_backend != NULL) {
|
|
|
|
_backend->handle_msg(msg);
|
|
|
|
}
|
|
|
|
}
|