Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-21 16:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_PrecLand: return target distance if enabled from SIM_Precland

Browse Source 
For rover, we cant measure distance towards target using a rangefinder. Hence, we should must return target distance if enabled through SIM_Precland.
This commit is contained in:
Shiv Tyagi 2022-06-24 18:37:34 +05:30 committed by Peter Barker
parent 8411b2ec03
commit 5ff268fa97
2 changed files with 5 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AC_PrecLand/AC_PrecLand_SITL.cpp
View File

@ -19,6 +19,7 @@ void AC_PrecLand_SITL::update()
const Vector3d position = _sitl->precland_sim.get_target_position();
const Matrix3d body_to_ned = AP::ahrs().get_rotation_body_to_ned().todouble();
_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();
_have_los_meas = true;
_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;
}
// provides a unit vector towards the target in body frame
// returns same as have_los_meas()
bool AC_PrecLand_SITL::get_los_body(Vector3f& ret) {

4
libraries/AC_PrecLand/AC_PrecLand_SITL.h
View File

@ -32,11 +32,15 @@ public:
// return true if there is a valid los measurement available
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:
SITL::SIM *_sitl; // sitl instance pointer
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
bool _have_los_meas; // true if there is a valid measurement from the camera
float _distance_to_target; // distance to target in meters
};
#endif