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

AC_PrecLand: reduce code duplication, move common functionally to the base class

This commit is contained in:
Dr.-Ing. Amilcar do Carmo Lucas 2024-02-22 15:08:29 +01:00 committed by Andrew Tridgell
parent 381fb42023
commit f0fc447aed
9 changed files with 16 additions and 108 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/AC_PrecLand/AC_PrecLand_Backend.h
View File

@ -28,16 +28,22 @@ public:
// 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;
bool get_los_body(Vector3f& dir_body) {
if (have_los_meas()) {
dir_body = _los_meas_body;
return true;
}
return false;
};
// returns system time in milliseconds of last los measurement
virtual uint32_t los_meas_time_ms() = 0;
uint32_t los_meas_time_ms() { return _los_meas_time_ms; };
// return true if there is a valid los measurement available
virtual bool have_los_meas() = 0;
bool have_los_meas() { return _have_los_meas; };
// returns distance to target in meters (0 means distance is not known)
virtual float distance_to_target() { return 0.0f; };
float distance_to_target() { return _distance_to_target; };
// parses a mavlink message from the companion computer
virtual void handle_msg(const mavlink_landing_target_t &packet, uint32_t timestamp_ms) {};
@ -48,6 +54,11 @@ public:
protected:
const AC_PrecLand& _frontend; // reference to precision landing front end
AC_PrecLand::precland_state &_state; // reference to this instances state
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 sensor
float _distance_to_target; // distance from the sensor to landing target in meters
};
#endif // AC_PRECLAND_ENABLED

11
libraries/AC_PrecLand/AC_PrecLand_Companion.cpp
View File

@ -11,7 +11,6 @@ void AC_PrecLand_Companion::init()
{
// set healthy
_state.healthy = true;
_have_los_meas = false;
}
// retrieve updates from sensor
@ -20,16 +19,6 @@ void AC_PrecLand_Companion::update()
_have_los_meas = _have_los_meas && 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_Companion::get_los_body(Vector3f& ret) {
if (have_los_meas()) {
ret = _los_meas_body;
return true;
}
return false;
}
void AC_PrecLand_Companion::handle_msg(const mavlink_landing_target_t &packet, uint32_t timestamp_ms)
{
_distance_to_target = packet.distance;

18
libraries/AC_PrecLand/AC_PrecLand_Companion.h
View File

@ -26,28 +26,10 @@ public:
// retrieve updates from sensor
void update() override;
// provides a unit vector towards the target in body frame
// returns same as have_los_meas()
bool get_los_body(Vector3f& ret) override;
// returns system time in milliseconds of last los measurement
uint32_t los_meas_time_ms() override { return _los_meas_time_ms; }
// return true if there is a valid los measurement available
bool have_los_meas() override { return _have_los_meas; }
// returns distance to target in meters (0 means distance is not known)
float distance_to_target() override { return _distance_to_target; }
// parses a mavlink message from the companion computer
void handle_msg(const mavlink_landing_target_t &packet, uint32_t timestamp_ms) override;
private:
float _distance_to_target; // distance from the camera to target in meters
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
bool _wrong_frame_msg_sent;
};

10
libraries/AC_PrecLand/AC_PrecLand_IRLock.cpp
View File

@ -35,14 +35,4 @@ void AC_PrecLand_IRLock::update()
_have_los_meas = _have_los_meas && 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_IRLock::get_los_body(Vector3f& ret) {
if (have_los_meas()) {
ret = _los_meas_body;
return true;
}
return false;
}
#endif // AC_PRECLAND_IRLOCK_ENABLED

15
libraries/AC_PrecLand/AC_PrecLand_IRLock.h
View File

@ -15,7 +15,7 @@
/*
* AC_PrecLand_IRLock - implements precision landing using target vectors provided
* by a companion computer (i.e. Odroid) communicating via MAVLink
* by an IRLock
*/
class AC_PrecLand_IRLock : public AC_PrecLand_Backend
@ -31,25 +31,12 @@ public:
// retrieve updates from sensor
void update() override;
// provides a unit vector towards the target in body frame
// returns same as have_los_meas()
bool get_los_body(Vector3f& ret) override;
// returns system time in milliseconds of last los measurement
uint32_t los_meas_time_ms() override { return _los_meas_time_ms; }
// return true if there is a valid los measurement available
bool have_los_meas() override { return _have_los_meas; }
private:
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
AP_IRLock_SITL irlock;
#else
AP_IRLock_I2C irlock;
#endif
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 // AC_PRECLAND_IRLOCK_ENABLED

10
libraries/AC_PrecLand/AC_PrecLand_SITL.cpp
View File

@ -39,14 +39,4 @@ void AC_PrecLand_SITL::update()
_have_los_meas = _have_los_meas && 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) {
if (have_los_meas()) {
ret = _los_meas_body;
return true;
}
return false;
}
#endif // AC_PRECLAND_SITL_ENABLED

17
libraries/AC_PrecLand/AC_PrecLand_SITL.h
View File

@ -25,25 +25,8 @@ public:
// retrieve updates from sensor
void update() override;
// provides a unit vector towards the target in body frame
// returns same as have_los_meas()
bool get_los_body(Vector3f& ret) override;
// returns system time in milliseconds of last los measurement
uint32_t los_meas_time_ms() override { return _los_meas_time_ms; }
// return true if there is a valid los measurement available
bool have_los_meas() override { return _have_los_meas; }
// 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 // AC_PRECLAND_SITL_ENABLED

10
libraries/AC_PrecLand/AC_PrecLand_SITL_Gazebo.cpp
View File

@ -35,14 +35,4 @@ void AC_PrecLand_SITL_Gazebo::update()
_have_los_meas = _have_los_meas && 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_Gazebo::get_los_body(Vector3f& ret) {
if (have_los_meas()) {
ret = _los_meas_body;
return true;
}
return false;
}
#endif // AC_PRECLAND_SITL_GAZEBO_ENABLED

14
libraries/AC_PrecLand/AC_PrecLand_SITL_Gazebo.h
View File

@ -26,22 +26,8 @@ public:
// retrieve updates from sensor
void update() override;
// provides a unit vector towards the target in body frame
// returns same as have_los_meas()
bool get_los_body(Vector3f& ret) override;
// returns system time in milliseconds of last los measurement
uint32_t los_meas_time_ms() override { return _los_meas_time_ms; }
// return true if there is a valid los measurement available
bool have_los_meas() override { return _have_los_meas; }
private:
AP_IRLock_SITL_Gazebo 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 // AC_PRECLAND_SITL_GAZEBO_ENABLED