mirror of https://github.com/ArduPilot/ardupilot
67 lines
2.0 KiB
C++
67 lines
2.0 KiB
C++
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
#include <AP_HAL/AP_HAL.h>
|
|
#include "AC_PrecLand_Companion.h"
|
|
|
|
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)
|
|
{
|
|
}
|
|
|
|
// init - perform initialisation of this backend
|
|
void AC_PrecLand_Companion::init()
|
|
{
|
|
// set healthy
|
|
_state.healthy = true;
|
|
_new_estimate = false;
|
|
_frame = MAV_FRAME_BODY_NED; // assume body until we get our first message that says otherwise
|
|
}
|
|
|
|
// update - give chance to driver to get updates from sensor
|
|
// returns true if new data available
|
|
bool AC_PrecLand_Companion::update()
|
|
{
|
|
// Mavlink commands are received asynchronous so all new data is processed by handle_msg()
|
|
return _new_estimate;
|
|
}
|
|
|
|
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;
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
void AC_PrecLand_Companion::handle_msg(mavlink_message_t* msg)
|
|
{
|
|
// parse mavlink message
|
|
__mavlink_landing_target_t packet;
|
|
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;
|
|
}
|