Browse Source

AC_PrecLand: return target distance if enabled from SIM_Precland

For rover, we cant measure distance towards target using a rangefinder. Hence, we should must return target distance if enabled through SIM_Precland.
master
Shiv Tyagi 3 years ago committed by Peter Barker
parent
commit
5ff268fa97
  1. 2
      libraries/AC_PrecLand/AC_PrecLand_SITL.cpp
  2. 4
      libraries/AC_PrecLand/AC_PrecLand_SITL.h

2
libraries/AC_PrecLand/AC_PrecLand_SITL.cpp

@ -19,6 +19,7 @@ void AC_PrecLand_SITL::update()
const Vector3d position = _sitl->precland_sim.get_target_position(); const Vector3d position = _sitl->precland_sim.get_target_position();
const Matrix3d body_to_ned = AP::ahrs().get_rotation_body_to_ned().todouble(); const Matrix3d body_to_ned = AP::ahrs().get_rotation_body_to_ned().todouble();
_los_meas_body = body_to_ned.mul_transpose(-position).tofloat(); _los_meas_body = body_to_ned.mul_transpose(-position).tofloat();
_distance_to_target = _sitl->precland_sim.option_enabled(SITL::SIM_Precland::Option::ENABLE_TARGET_DISTANCE) ? _los_meas_body.length() : 0.0f;
_los_meas_body /= _los_meas_body.length(); _los_meas_body /= _los_meas_body.length();
_have_los_meas = true; _have_los_meas = true;
_los_meas_time_ms = _sitl->precland_sim.last_update_ms(); _los_meas_time_ms = _sitl->precland_sim.last_update_ms();
@ -33,7 +34,6 @@ bool AC_PrecLand_SITL::have_los_meas() {
return AP_HAL::millis() - _los_meas_time_ms < 1000; return AP_HAL::millis() - _los_meas_time_ms < 1000;
} }
// provides a unit vector towards the target in body frame // provides a unit vector towards the target in body frame
// returns same as have_los_meas() // returns same as have_los_meas()
bool AC_PrecLand_SITL::get_los_body(Vector3f& ret) { bool AC_PrecLand_SITL::get_los_body(Vector3f& ret) {

4
libraries/AC_PrecLand/AC_PrecLand_SITL.h

@ -32,11 +32,15 @@ public:
// return true if there is a valid los measurement available // return true if there is a valid los measurement available
bool have_los_meas() override; bool have_los_meas() override;
// returns distance to target in meters (0 means distance is not known)
float distance_to_target() override { return _distance_to_target; }
private: private:
SITL::SIM *_sitl; // sitl instance pointer SITL::SIM *_sitl; // sitl instance pointer
Vector3f _los_meas_body; // unit vector in body frame pointing towards target Vector3f _los_meas_body; // unit vector in body frame pointing towards target
uint32_t _los_meas_time_ms; // system time in milliseconds when los was measured uint32_t _los_meas_time_ms; // system time in milliseconds when los was measured
bool _have_los_meas; // true if there is a valid measurement from the camera bool _have_los_meas; // true if there is a valid measurement from the camera
float _distance_to_target; // distance to target in meters
}; };
#endif #endif

Loading…
Cancel
Save