Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_PrecLand: use new irlock interface, refactor backend interface

This commit is contained in:
Jonathan Challinger 2016-07-05 12:37:17 -07:00 committed by Randy Mackay
parent 6254608c45
commit e84d1581ab
7 changed files with 137 additions and 142 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

63
libraries/AC_PrecLand/AC_PrecLand.cpp
View File

@ -86,9 +86,16 @@ void AC_PrecLand::update(float alt_above_terrain_cm)
if (_backend != NULL) {
// read from sensor
_backend->update();
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();
// calculate angles to target and position estimate
calc_angles_and_pos(alt_above_terrain_cm);
Vector3f target_vec_unit_body;
_backend->get_los_body(target_vec_unit_body);
calc_angles_and_pos(target_vec_unit_body, alt_above_terrain_cm);
}
}
}
@ -126,55 +133,23 @@ bool AC_PrecLand::get_target_velocity_relative(Vector3f& ret)
// raw sensor angles stored in _angle_to_target (might be in earth frame, or maybe body frame)
// earth-frame angles stored in _ef_angle_to_target
// position estimate is stored in _target_pos
void AC_PrecLand::calc_angles_and_pos(float alt_above_terrain_cm)
void AC_PrecLand::calc_angles_and_pos(const Vector3f& target_vec_unit_body, float alt_above_terrain_cm)
{
// exit immediately if not enabled
if (_backend == NULL) {
return;
}
// get angles to target from backend
if (!_backend->get_angle_to_target(_angle_to_target.x, _angle_to_target.y)) {
return;
}
_angle_to_target.x = -_angle_to_target.x;
_angle_to_target.y = -_angle_to_target.y;
// compensate for delay
_angle_to_target.x -= _ahrs.get_gyro().x*4.0e-2f;
_angle_to_target.y -= _ahrs.get_gyro().y*4.0e-2f;
Vector3f target_vec_ned;
if (_angle_to_target.is_zero()) {
target_vec_ned = Vector3f(0.0f,0.0f,1.0f);
} else {
float theta = _angle_to_target.length();
Vector3f axis = Vector3f(_angle_to_target.x, _angle_to_target.y, 0.0f)/theta;
float sin_theta = sinf(theta);
float cos_theta = cosf(theta);
target_vec_ned.x = axis.y*sin_theta;
target_vec_ned.y = -axis.x*sin_theta;
target_vec_ned.z = cos_theta;
}
// rotate into NED frame
target_vec_ned = _ahrs.get_rotation_body_to_ned()*target_vec_ned;
Vector3f target_vec_unit_ned = _ahrs.get_rotation_body_to_ned()*target_vec_unit_body;
// extract the angles to target (logging only)
_ef_angle_to_target.x = atan2f(target_vec_ned.z,target_vec_ned.x);
_ef_angle_to_target.y = atan2f(target_vec_ned.z,target_vec_ned.y);
_angle_to_target.x = atan2f(-target_vec_unit_body.y, target_vec_unit_body.z);
_angle_to_target.y = atan2f( target_vec_unit_body.x, target_vec_unit_body.z);
_ef_angle_to_target.x = atan2f(-target_vec_unit_ned.y, target_vec_unit_ned.z);
_ef_angle_to_target.y = atan2f( target_vec_unit_ned.x, target_vec_unit_ned.z);
// ensure that the angle to target is no more than 70 degrees
if (target_vec_ned.z > 0.26f) {
if (target_vec_unit_ned.z > 0.0f) {
// get current altitude (constrained to be positive)
float alt = MAX(alt_above_terrain_cm, 0.0f);
float dist = alt/target_vec_ned.z;
//
_target_pos_rel.x = target_vec_ned.x*dist;
_target_pos_rel.y = target_vec_ned.y*dist;
float dist = alt/target_vec_unit_ned.z;
_target_pos_rel.x = target_vec_unit_ned.x*dist;
_target_pos_rel.y = target_vec_unit_ned.y*dist;
_target_pos_rel.z = alt; // not used
_target_pos = _inav.get_position()+_target_pos_rel;

3
libraries/AC_PrecLand/AC_PrecLand.h
View File

@ -83,7 +83,7 @@ private:
// angles stored in _angle_to_target
// earth-frame angles stored in _ef_angle_to_target
// position estimate is stored in _target_pos
void calc_angles_and_pos(float alt_above_terrain_cm);
void calc_angles_and_pos(const Vector3f& target_vec_unit_body, float alt_above_terrain_cm);
// returns enabled parameter as an behaviour
enum PrecLandBehaviour get_behaviour() const { return (enum PrecLandBehaviour)(_enabled.get()); }
@ -97,6 +97,7 @@ private:
AP_Int8 _type; // precision landing controller type
uint32_t _last_update_ms; // epoch time in millisecond when update is called
uint32_t _last_backend_los_meas_ms;
// output from sensor (stored for logging)
Vector2f _angle_to_target; // last raw sensor angle to target

29
libraries/AC_PrecLand/AC_PrecLand_Backend.h
View File

@ -10,7 +10,6 @@
class AC_PrecLand_Backend
{
public:
// Constructor
AC_PrecLand_Backend(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state) :
_frontend(frontend),
@ -19,26 +18,26 @@ public:
// destructor
virtual ~AC_PrecLand_Backend() {}
// init - perform any required initialisation of backend controller
// perform any required initialisation of backend
virtual void init() = 0;
// update - give chance to driver to get updates from sensor
// returns true if new data available
virtual bool update() = 0;
// what frame of reference is our sensor reporting in?
virtual MAV_FRAME get_frame_of_reference() = 0;
// retrieve updates from sensor
virtual void update() = 0;
// get_angle_to_target - returns angles (in radians) to target
// returns true if angles are available, false if not (i.e. no target)
// x_angle_rad : roll direction, positive = target is to right (looking down)
// y_angle_rad : pitch direction, postiive = target is forward (looking down)
virtual bool get_angle_to_target(float &x_angle_rad, float &y_angle_rad) = 0;
// handle_msg - parses a mavlink message from the companion computer
// provides a unit vector towards the target in body frame
// returns same as have_los_meas()
virtual bool get_los_body(Vector3f& dir_body) = 0;
// returns system time in milliseconds of last los measurement
virtual uint32_t los_meas_time_ms() = 0;
// return true if there is a valid los measurement available
virtual bool have_los_meas() = 0;
// parses a mavlink message from the companion computer
virtual void handle_msg(mavlink_message_t* msg) = 0;
protected:
const AC_PrecLand& _frontend; // reference to precision landing front end
AC_PrecLand::precland_state &_state; // reference to this instances state
};

61
libraries/AC_PrecLand/AC_PrecLand_Companion.cpp
View File

@ -7,52 +7,47 @@ extern const AP_HAL::HAL& hal;
// Constructor
AC_PrecLand_Companion::AC_PrecLand_Companion(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state)
: AC_PrecLand_Backend(frontend, state),
_frame(MAV_FRAME_BODY_NED),
_distance_to_target(0.0f),
_timestamp_us(0),
_new_estimate(false)
_have_los_meas(false)
{
}
// init - perform initialisation of this backend
// perform any required initialisation of backend
void AC_PrecLand_Companion::init()
{
// set healthy
_state.healthy = true;
_new_estimate = false;
_have_los_meas = false;
}
// update - give chance to driver to get updates from sensor
// returns true if new data available
bool AC_PrecLand_Companion::update()
// retrieve updates from sensor
void AC_PrecLand_Companion::update()
{
// Mavlink commands are received asynchronous so all new data is processed by handle_msg()
return _new_estimate;
_have_los_meas = _have_los_meas && AP_HAL::millis()-_los_meas_time_ms <= 1000;
}
MAV_FRAME AC_PrecLand_Companion::get_frame_of_reference()
{
return _frame;
}
// get_angle_to_target - returns angles (in radians) to target
// returns true if angles are available, false if not (i.e. no target)
// x_angle_rad : roll direction, positive = target is to right (looking down)
// y_angle_rad : pitch direction, postiive = target is forward (looking down)
bool AC_PrecLand_Companion::get_angle_to_target(float &x_angle_rad, float &y_angle_rad)
{
if (_new_estimate){
x_angle_rad = _angle_to_target.x;
y_angle_rad = _angle_to_target.y;
// reset and wait for new data
_new_estimate = false;
// provides a unit vector towards the target in body frame
// returns same as have_los_meas()
bool AC_PrecLand_Companion::get_los_body(Vector3f& ret) {
if (have_los_meas()) {
ret = _los_meas_body;
return true;
}
return false;
}
// returns system time in milliseconds of last los measurement
uint32_t AC_PrecLand_Companion::los_meas_time_ms() {
return _los_meas_time_ms;
}
// return true if there is a valid los measurement available
bool AC_PrecLand_Companion::have_los_meas()
{
return _have_los_meas;
}
void AC_PrecLand_Companion::handle_msg(mavlink_message_t* msg)
{
// parse mavlink message
@ -60,10 +55,12 @@ void AC_PrecLand_Companion::handle_msg(mavlink_message_t* msg)
mavlink_msg_landing_target_decode(msg, &packet);
_timestamp_us = packet.time_usec;
_frame = (MAV_FRAME) packet.frame;
_angle_to_target.x = packet.angle_x;
_angle_to_target.y = packet.angle_y;
_distance_to_target = packet.distance;
_state.healthy = true;
_new_estimate = true;
// compute unit vector towards target
_los_meas_body = Vector3f(-tanf(packet.angle_y), tanf(packet.angle_x), 1.0f);
_los_meas_body /= _los_meas_body.length();
_los_meas_time_ms = AP_HAL::millis();
_have_los_meas = true;
}

41
libraries/AC_PrecLand/AC_PrecLand_Companion.h
View File

@ -13,34 +13,33 @@
class AC_PrecLand_Companion : public AC_PrecLand_Backend
{
public:
// Constructor
AC_PrecLand_Companion(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state);
// init - perform any required initialisation of backend controller
// perform any required initialisation of backend
void init();
// update - give chance to driver to get updates from sensor
// returns true if new data available
bool update();
// what frame of reference is our sensor reporting in?
MAV_FRAME get_frame_of_reference();
// retrieve updates from sensor
void update();
// get_angle_to_target - returns angles (in radians) to target
// returns true if angles are available, false if not (i.e. no target)
// x_angle_rad : roll direction, positive = target is to right (looking down)
// y_angle_rad : pitch direction, postiive = target is forward (looking down)
bool get_angle_to_target(float &x_angle_rad, float &y_angle_rad);
// handle_msg - parses a mavlink message from the companion computer
// provides a unit vector towards the target in body frame
// returns same as have_los_meas()
bool get_los_body(Vector3f& ret);
// returns system time in milliseconds of last los measurement
uint32_t los_meas_time_ms();
// return true if there is a valid los measurement available
bool have_los_meas();
// parses a mavlink message from the companion computer
void handle_msg(mavlink_message_t* msg);
private:
// output from camera
MAV_FRAME _frame; // what frame of reference is our sensor reporting in?
Vector2f _angle_to_target; // last angle to target
uint64_t _timestamp_us; // timestamp from message
float _distance_to_target; // distance from the camera to target in meters
uint64_t _timestamp_us; // timestamp when the image was captured(synced via UAVCAN)
bool _new_estimate; // true if new data from the camera has been received
Vector3f _los_meas_body; // unit vector in body frame pointing towards target
bool _have_los_meas; // true if there is a valid measurement from the camera
uint32_t _los_meas_time_ms; // system time in milliseconds when los was measured
};

46
libraries/AC_PrecLand/AC_PrecLand_IRLock.cpp
View File

@ -10,7 +10,9 @@ extern const AP_HAL::HAL& hal;
// Constructor
AC_PrecLand_IRLock::AC_PrecLand_IRLock(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state)
: AC_PrecLand_Backend(frontend, state),
irlock()
irlock(),
_have_los_meas(false),
_los_meas_time_ms(0)
{
}
@ -18,25 +20,43 @@ AC_PrecLand_IRLock::AC_PrecLand_IRLock(const AC_PrecLand& frontend, AC_PrecLand:
void AC_PrecLand_IRLock::init()
{
irlock.init();
// set healthy
_state.healthy = irlock.healthy();
}
// update - give chance to driver to get updates from sensor
bool AC_PrecLand_IRLock::update()
void AC_PrecLand_IRLock::update()
{
// update health
_state.healthy = irlock.healthy();
// get new sensor data
return (irlock.update());
irlock.update();
if (irlock.num_targets() > 0 && irlock.last_update_ms() != _los_meas_time_ms) {
irlock.get_unit_vector_body(_los_meas_body);
_have_los_meas = true;
_los_meas_time_ms = irlock.last_update_ms();
}
_have_los_meas = _have_los_meas && AP_HAL::millis()-_los_meas_time_ms <= 1000;
}
// get_angle_to_target - returns body frame angles (in radians) to target
// returns true if angles are available, false if not (i.e. no target)
// x_angle_rad : body-frame roll direction, positive = target is to right (looking down)
// y_angle_rad : body-frame pitch direction, postiive = target is forward (looking down)
bool AC_PrecLand_IRLock::get_angle_to_target(float &x_angle_rad, float &y_angle_rad)
{
return irlock.get_angle_to_target(x_angle_rad, y_angle_rad);
// provides a unit vector towards the target in body frame
// returns same as have_los_meas()
bool AC_PrecLand_IRLock::get_los_body(Vector3f& ret) {
if (have_los_meas()) {
ret = _los_meas_body;
return true;
}
return false;
}
// returns system time in milliseconds of last los measurement
uint32_t AC_PrecLand_IRLock::los_meas_time_ms() {
return _los_meas_time_ms;
}
// return true if there is a valid los measurement available
bool AC_PrecLand_IRLock::have_los_meas() {
return _have_los_meas;
}
#endif // PX4

36
libraries/AC_PrecLand/AC_PrecLand_IRLock.h
View File

@ -20,27 +20,31 @@ public:
// Constructor
AC_PrecLand_IRLock(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state);
// init - perform any required initialisation of backend controller
// perform any required initialisation of backend
void init();
// update - give chance to driver to get updates from sensor
// returns true if new data available
bool update();
// IRLock is hard-mounted to the frame of the vehicle, so it will always be in body-frame
MAV_FRAME get_frame_of_reference() { return MAV_FRAME_BODY_NED; }
// retrieve updates from sensor
void update();
// get_angle_to_target - returns body frame angles (in radians) to target
// returns true if angles are available, false if not (i.e. no target)
// x_angle_rad : body-frame roll direction, positive = target is to right (looking down)
// y_angle_rad : body-frame pitch direction, postiive = target is forward (looking down)
bool get_angle_to_target(float &x_angle_rad, float &y_angle_rad);
// handle_msg - parses a mavlink message from the companion computer
void handle_msg(mavlink_message_t* msg) { /* do nothing */ }
// provides a unit vector towards the target in body frame
// returns same as have_los_meas()
bool get_los_body(Vector3f& ret);
// returns system time in milliseconds of last los measurement
uint32_t los_meas_time_ms();
// return true if there is a valid los measurement available
bool have_los_meas();
// parses a mavlink message from the companion computer
void handle_msg(mavlink_message_t* msg) {};
private:
AP_IRLock_PX4 irlock;
Vector3f _los_meas_body; // unit vector in body frame pointing towards target
bool _have_los_meas; // true if there is a valid measurement from the camera
uint32_t _los_meas_time_ms; // system time in milliseconds when los was measured
};
#endif