AC_PrecLand: add override and fix formatting

This commit is contained in:
Randy Mackay 2017-02-03 16:01:03 +09:00
parent 974a6f48c6
commit bdf49c8a01
6 changed files with 34 additions and 34 deletions

View File

@ -26,10 +26,10 @@ 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;
// 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;

View File

@ -53,7 +53,7 @@ float AC_PrecLand_Companion::distance_to_target()
{
return _distance_to_target;
}
void AC_PrecLand_Companion::handle_msg(mavlink_message_t* msg)
{
// parse mavlink message
@ -62,11 +62,11 @@ void AC_PrecLand_Companion::handle_msg(mavlink_message_t* msg)
_timestamp_us = packet.time_usec;
_distance_to_target = packet.distance;
// 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;
}

View File

@ -14,33 +14,33 @@ class AC_PrecLand_Companion : public AC_PrecLand_Backend
public:
// Constructor
AC_PrecLand_Companion(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state);
// perform any required initialisation of backend
void init();
void init() override;
// retrieve updates from sensor
void update();
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);
bool get_los_body(Vector3f& ret) override;
// returns system time in milliseconds of last los measurement
uint32_t los_meas_time_ms();
uint32_t los_meas_time_ms() override;
// return true if there is a valid los measurement available
bool have_los_meas();
bool have_los_meas() override;
// returns distance to target in meters (0 means distance is not known)
float distance_to_target() override;
// parses a mavlink message from the companion computer
void handle_msg(mavlink_message_t* msg);
void handle_msg(mavlink_message_t* msg) override;
private:
uint64_t _timestamp_us; // timestamp from message
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

View File

@ -23,22 +23,22 @@ public:
// Constructor
AC_PrecLand_IRLock(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state);
// perform any required initialisation of backend
void init();
void init() override;
// retrieve updates from sensor
void update();
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);
bool get_los_body(Vector3f& ret) override;
// returns system time in milliseconds of last los measurement
uint32_t los_meas_time_ms();
uint32_t los_meas_time_ms() override;
// return true if there is a valid los measurement available
bool have_los_meas();
bool have_los_meas() override;
private:
AP_IRLock_I2C irlock;

View File

@ -17,20 +17,20 @@ public:
AC_PrecLand_SITL(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state);
// perform any required initialisation of backend
void init();
void init() override;
// retrieve updates from sensor
void update();
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);
bool get_los_body(Vector3f& ret) override;
// returns system time in milliseconds of last los measurement
uint32_t los_meas_time_ms() { return _los_meas_time_ms; }
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();
bool have_los_meas() override;
private:

View File

@ -19,20 +19,20 @@ public:
AC_PrecLand_SITL_Gazebo(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state);
// perform any required initialisation of backend
void init();
void init() override;
// retrieve updates from sensor
void update();
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);
bool get_los_body(Vector3f& ret) override;
// returns system time in milliseconds of last los measurement
uint32_t los_meas_time_ms();
uint32_t los_meas_time_ms() override;
// return true if there is a valid los measurement available
bool have_los_meas();
bool have_los_meas() override;
private:
AP_IRLock_SITL irlock;