diff --git a/libraries/AC_PrecLand/AC_PrecLand.cpp b/libraries/AC_PrecLand/AC_PrecLand.cpp index 1ba2a8ec18..6a6d718bc2 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand.cpp @@ -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; diff --git a/libraries/AC_PrecLand/AC_PrecLand.h b/libraries/AC_PrecLand/AC_PrecLand.h index 12ab889167..5be6d4390c 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.h +++ b/libraries/AC_PrecLand/AC_PrecLand.h @@ -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 diff --git a/libraries/AC_PrecLand/AC_PrecLand_Backend.h b/libraries/AC_PrecLand/AC_PrecLand_Backend.h index d8b7aeefd1..6cdc422417 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_Backend.h +++ b/libraries/AC_PrecLand/AC_PrecLand_Backend.h @@ -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 }; diff --git a/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp b/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp index 1498dd7fd6..61f3accec4 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp @@ -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; } diff --git a/libraries/AC_PrecLand/AC_PrecLand_Companion.h b/libraries/AC_PrecLand/AC_PrecLand_Companion.h index 7048d5605b..9c108f90e1 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_Companion.h +++ b/libraries/AC_PrecLand/AC_PrecLand_Companion.h @@ -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 }; diff --git a/libraries/AC_PrecLand/AC_PrecLand_IRLock.cpp b/libraries/AC_PrecLand/AC_PrecLand_IRLock.cpp index 9666aa44cf..47124e10be 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_IRLock.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand_IRLock.cpp @@ -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 diff --git a/libraries/AC_PrecLand/AC_PrecLand_IRLock.h b/libraries/AC_PrecLand/AC_PrecLand_IRLock.h index 62e1fbddc1..2d792bf86a 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_IRLock.h +++ b/libraries/AC_PrecLand/AC_PrecLand_IRLock.h @@ -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