mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-23 06:36:45 +08:00
vmount: fix POI for gimbal v2
This includes several changes to fix POI when used with MAVLink gimbal v2 input: - Correctly set capability flag that POI is supported. - Keep lat/lon and calculate attitude on each cycle, instead of only once on init. - Always publish gimbal manager information, with or withoug gimbal v2 device.
This commit is contained in:
Julian Oes
Daniel Agar
@@ -54,8 +54,6 @@
|
||||
#include <math.h>
|
||||
#include <matrix/matrix/math.hpp>
|
||||
|
||||
using matrix::wrap_pi;
|
||||
|
||||
namespace vmount
|
||||
{
|
||||
|
||||
@@ -121,6 +119,7 @@ int InputMavlinkROI::update_impl(unsigned int timeout_ms, ControlData **control_
|
||||
|
||||
_control_data.type = ControlData::Type::Neutral;
|
||||
*control_data = &_control_data;
|
||||
_cur_roi_mode = vehicle_roi.mode;
|
||||
|
||||
} else if (vehicle_roi.mode == vehicle_roi_s::ROI_WPNEXT) {
|
||||
_control_data.type = ControlData::Type::LonLat;
|
||||
@@ -132,17 +131,17 @@ int InputMavlinkROI::update_impl(unsigned int timeout_ms, ControlData **control_
|
||||
_control_data.type_data.lonlat.yaw_angle_offset = vehicle_roi.yaw_offset;
|
||||
|
||||
*control_data = &_control_data;
|
||||
_cur_roi_mode = vehicle_roi.mode;
|
||||
|
||||
} else if (vehicle_roi.mode == vehicle_roi_s::ROI_LOCATION) {
|
||||
control_data_set_lon_lat(vehicle_roi.lon, vehicle_roi.lat, vehicle_roi.alt);
|
||||
|
||||
*control_data = &_control_data;
|
||||
_cur_roi_mode = vehicle_roi.mode;
|
||||
|
||||
} else if (vehicle_roi.mode == vehicle_roi_s::ROI_TARGET) {
|
||||
//TODO is this even suported?
|
||||
}
|
||||
|
||||
_cur_roi_mode = vehicle_roi.mode;
|
||||
}
|
||||
|
||||
// check whether the position setpoint got updated
|
||||
@@ -371,17 +370,14 @@ void InputMavlinkCmdMount::print_status()
|
||||
PX4_INFO("Input: Mavlink (CMD_MOUNT)");
|
||||
}
|
||||
|
||||
InputMavlinkGimbalV2::InputMavlinkGimbalV2(bool has_v2_gimbal_device, uint8_t mav_sys_id, uint8_t mav_comp_id,
|
||||
InputMavlinkGimbalV2::InputMavlinkGimbalV2(uint8_t mav_sys_id, uint8_t mav_comp_id,
|
||||
float &mnt_rate_pitch, float &mnt_rate_yaw) :
|
||||
_mav_sys_id(mav_sys_id),
|
||||
_mav_comp_id(mav_comp_id),
|
||||
_mnt_rate_pitch(mnt_rate_pitch),
|
||||
_mnt_rate_yaw(mnt_rate_yaw)
|
||||
{
|
||||
if (!has_v2_gimbal_device) {
|
||||
/* dumb gimbal or MAVLink v1 protocol gimbal: fake GIMBAL_DEVICE_INFORMATION */
|
||||
_stream_gimbal_manager_information();
|
||||
}
|
||||
_stream_gimbal_manager_information();
|
||||
}
|
||||
|
||||
InputMavlinkGimbalV2::~InputMavlinkGimbalV2()
|
||||
@@ -564,29 +560,24 @@ int InputMavlinkGimbalV2::update_impl(unsigned int timeout_ms, ControlData **con
|
||||
|
||||
_control_data.type = ControlData::Type::Neutral;
|
||||
*control_data = &_control_data;
|
||||
_is_roi_set = false;
|
||||
_cur_roi_mode = vehicle_roi.mode;
|
||||
|
||||
} else if (vehicle_roi.mode == vehicle_roi_s::ROI_WPNEXT) {
|
||||
double lat, lon, alt = 0.;
|
||||
_read_lat_lon_alt_from_position_setpoint_sub(lon, lat, alt);
|
||||
_control_data.type = ControlData::Type::LonLat;
|
||||
_read_control_data_from_position_setpoint_sub();
|
||||
_control_data.type_data.lonlat.pitch_fixed_angle = -10.f;
|
||||
|
||||
_control_data.type_data.lonlat.roll_angle = vehicle_roi.roll_offset;
|
||||
_control_data.type_data.lonlat.pitch_angle_offset = vehicle_roi.pitch_offset;
|
||||
_control_data.type_data.lonlat.yaw_angle_offset = vehicle_roi.yaw_offset;
|
||||
|
||||
_transform_lon_lat_to_angle(lon, lat, alt);
|
||||
|
||||
*control_data = &_control_data;
|
||||
_is_roi_set = true;
|
||||
_cur_roi_mode = vehicle_roi.mode;
|
||||
|
||||
} else if (vehicle_roi.mode == vehicle_roi_s::ROI_LOCATION) {
|
||||
control_data_set_lon_lat(vehicle_roi.lon, vehicle_roi.lat, vehicle_roi.alt);
|
||||
|
||||
_transform_lon_lat_to_angle(vehicle_roi.lon, vehicle_roi.lat, (double)vehicle_roi.alt);
|
||||
*control_data = &_control_data;
|
||||
_is_roi_set = true;
|
||||
_cur_roi_mode = vehicle_roi.mode;
|
||||
|
||||
} else if (vehicle_roi.mode == vehicle_roi_s::ROI_TARGET) {
|
||||
@@ -601,9 +592,7 @@ int InputMavlinkGimbalV2::update_impl(unsigned int timeout_ms, ControlData **con
|
||||
// check whether the position setpoint got updated
|
||||
if (polls[2].revents & POLLIN) {
|
||||
if (_cur_roi_mode == vehicle_roi_s::ROI_WPNEXT) {
|
||||
double lat, lon, alt = 0.;
|
||||
_read_lat_lon_alt_from_position_setpoint_sub(lon, lat, alt);
|
||||
_transform_lon_lat_to_angle(lon, lat, alt);
|
||||
_read_control_data_from_position_setpoint_sub();
|
||||
*control_data = &_control_data;
|
||||
|
||||
} else { // must do an orb_copy() in *every* case
|
||||
@@ -908,71 +897,6 @@ int InputMavlinkGimbalV2::update_impl(unsigned int timeout_ms, ControlData **con
|
||||
return 0;
|
||||
}
|
||||
|
||||
void InputMavlinkGimbalV2::_transform_lon_lat_to_angle(const double roi_lon, const double roi_lat,
|
||||
const double roi_alt)
|
||||
{
|
||||
vehicle_global_position_s vehicle_global_position;
|
||||
_vehicle_global_position_sub.copy(&vehicle_global_position);
|
||||
const double &vlat = vehicle_global_position.lat;
|
||||
const double &vlon = vehicle_global_position.lon;
|
||||
|
||||
_control_data.type = ControlData::Type::Angle;
|
||||
_control_data.type_data.angle.frames[0] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
|
||||
_control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
|
||||
_control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
|
||||
|
||||
const float roll = 0.0f;
|
||||
float pitch = 0.0f;
|
||||
float yaw = 0.0f;
|
||||
|
||||
// interface: use fixed pitch value > -pi otherwise consider ROI altitude
|
||||
if (_control_data.type_data.lonlat.pitch_fixed_angle >= -M_PI_F) {
|
||||
pitch = _control_data.type_data.lonlat.pitch_fixed_angle;
|
||||
|
||||
} else {
|
||||
pitch = _calculate_pitch(roi_lon, roi_lat, roi_alt, vehicle_global_position);
|
||||
}
|
||||
|
||||
yaw = get_bearing_to_next_waypoint(vlat, vlon, roi_lat, roi_lon) - vehicle_local_position.yaw;
|
||||
|
||||
// add offsets from VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET
|
||||
pitch += _control_data.type_data.lonlat.pitch_angle_offset;
|
||||
yaw += _control_data.type_data.lonlat.yaw_angle_offset;
|
||||
|
||||
// make sure yaw is wrapped correctly for the output
|
||||
yaw = wrap_pi(yaw);
|
||||
|
||||
matrix::Eulerf euler(roll, pitch, yaw);
|
||||
matrix::Quatf q(euler);
|
||||
q.copyTo(_control_data.type_data.angle.q);
|
||||
}
|
||||
|
||||
float InputMavlinkGimbalV2::_calculate_pitch(double lon, double lat, float altitude,
|
||||
const vehicle_global_position_s &global_position)
|
||||
{
|
||||
if (!map_projection_initialized(&_projection_reference)) {
|
||||
map_projection_init(&_projection_reference, global_position.lat, global_position.lon);
|
||||
}
|
||||
|
||||
float x1, y1, x2, y2;
|
||||
map_projection_project(&_projection_reference, lat, lon, &x1, &y1);
|
||||
map_projection_project(&_projection_reference, global_position.lat, global_position.lon, &x2, &y2);
|
||||
float dx = x1 - x2, dy = y1 - y2;
|
||||
float target_distance = sqrtf(dx * dx + dy * dy);
|
||||
float z = altitude - global_position.alt;
|
||||
|
||||
return atan2f(z, target_distance);
|
||||
}
|
||||
|
||||
void InputMavlinkGimbalV2::_read_lat_lon_alt_from_position_setpoint_sub(double &lon_sp, double &lat_sp, double &alt_sp)
|
||||
{
|
||||
position_setpoint_triplet_s position_setpoint_triplet;
|
||||
orb_copy(ORB_ID(position_setpoint_triplet), _position_setpoint_triplet_sub, &position_setpoint_triplet);
|
||||
lon_sp = position_setpoint_triplet.current.lon;
|
||||
lat_sp = position_setpoint_triplet.current.lat;
|
||||
alt_sp = (double)position_setpoint_triplet.current.alt;
|
||||
}
|
||||
|
||||
void InputMavlinkGimbalV2::_set_control_data_from_set_attitude(const uint32_t flags, const matrix::Quatf &q,
|
||||
const matrix::Vector3f &angular_velocity)
|
||||
{
|
||||
@@ -1022,4 +946,13 @@ void InputMavlinkGimbalV2::_ack_vehicle_command(vehicle_command_s *cmd, uint8_t
|
||||
cmd_ack_pub.publish(vehicle_command_ack);
|
||||
}
|
||||
|
||||
void InputMavlinkGimbalV2::_read_control_data_from_position_setpoint_sub()
|
||||
{
|
||||
position_setpoint_triplet_s position_setpoint_triplet;
|
||||
orb_copy(ORB_ID(position_setpoint_triplet), _position_setpoint_triplet_sub, &position_setpoint_triplet);
|
||||
_control_data.type_data.lonlat.lon = position_setpoint_triplet.current.lon;
|
||||
_control_data.type_data.lonlat.lat = position_setpoint_triplet.current.lat;
|
||||
_control_data.type_data.lonlat.altitude = position_setpoint_triplet.current.alt;
|
||||
}
|
||||
|
||||
} /* namespace vmount */
|
||||
|
||||
@@ -109,8 +109,7 @@ private:
|
||||
class InputMavlinkGimbalV2 : public InputBase
|
||||
{
|
||||
public:
|
||||
InputMavlinkGimbalV2(bool has_v2_gimbal_device, uint8_t mav_sys_id, uint8_t mav_comp_id, float &mnt_rate_pitch,
|
||||
float &mnt_rate_yaw);
|
||||
InputMavlinkGimbalV2(uint8_t mav_sys_id, uint8_t mav_comp_id, float &mnt_rate_pitch, float &mnt_rate_yaw);
|
||||
virtual ~InputMavlinkGimbalV2();
|
||||
|
||||
virtual void print_status();
|
||||
@@ -120,16 +119,12 @@ protected:
|
||||
virtual int initialize();
|
||||
|
||||
private:
|
||||
|
||||
void _transform_lon_lat_to_angle(const double roi_lon, const double roi_lat, const double roi_alt);
|
||||
float _calculate_pitch(double lon, double lat, float altitude,
|
||||
const vehicle_global_position_s &global_position);
|
||||
void _read_lat_lon_alt_from_position_setpoint_sub(double &lon_sp, double &lat_sp, double &alt_sp);
|
||||
void _set_control_data_from_set_attitude(const uint32_t flags, const matrix::Quatf &q,
|
||||
const matrix::Vector3f &angular_velocity);
|
||||
void _ack_vehicle_command(vehicle_command_s *cmd, uint8_t result);
|
||||
void _stream_gimbal_manager_information();
|
||||
void _stream_gimbal_manager_status();
|
||||
void _read_control_data_from_position_setpoint_sub();
|
||||
|
||||
int _vehicle_roi_sub = -1;
|
||||
int _gimbal_manager_set_attitude_sub = -1;
|
||||
@@ -146,13 +141,10 @@ private:
|
||||
float &_mnt_rate_pitch;
|
||||
float &_mnt_rate_yaw;
|
||||
|
||||
bool _is_roi_set{false};
|
||||
|
||||
uORB::Subscription _gimbal_device_attitude_status_sub{ORB_ID(gimbal_device_attitude_status)};
|
||||
uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
|
||||
uORB::Publication<gimbal_manager_information_s> _gimbal_manager_info_pub{ORB_ID(gimbal_manager_information)};
|
||||
uORB::Publication<gimbal_manager_status_s> _gimbal_manager_status_pub{ORB_ID(gimbal_manager_status)};
|
||||
map_projection_reference_s _projection_reference = {}; ///< reference to convert (lon, lat) to local [m]
|
||||
uint8_t _cur_roi_mode = vehicle_roi_s::ROI_NONE;
|
||||
};
|
||||
|
||||
|
||||
@@ -51,8 +51,6 @@
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <matrix/math.hpp>
|
||||
|
||||
using matrix::wrap_pi;
|
||||
|
||||
namespace vmount
|
||||
{
|
||||
|
||||
@@ -102,17 +100,10 @@ void OutputBase::_set_angle_setpoints(const ControlData *control_data)
|
||||
{
|
||||
_cur_control_data = control_data;
|
||||
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
_angle_speeds[i] = 0.f;
|
||||
}
|
||||
|
||||
switch (control_data->type) {
|
||||
case ControlData::Type::Angle:
|
||||
|
||||
{
|
||||
matrix::Quatf q(control_data->type_data.angle.q);
|
||||
matrix::Eulerf euler(q);
|
||||
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
switch (control_data->type_data.angle.frames[i]) {
|
||||
case ControlData::TypeData::TypeAngle::Frame::AngularRate:
|
||||
@@ -127,8 +118,11 @@ void OutputBase::_set_angle_setpoints(const ControlData *control_data)
|
||||
break;
|
||||
}
|
||||
|
||||
_angle_speeds[i] = control_data->type_data.angle.angular_velocity[i];
|
||||
_angle_setpoints[i] = euler(i);
|
||||
_angle_velocity[i] = control_data->type_data.angle.angular_velocity[i];
|
||||
}
|
||||
|
||||
for (int i = 0; i < 4; ++i) {
|
||||
_q_setpoint[i] = control_data->type_data.angle.q[i];
|
||||
}
|
||||
}
|
||||
|
||||
@@ -139,9 +133,13 @@ void OutputBase::_set_angle_setpoints(const ControlData *control_data)
|
||||
break;
|
||||
|
||||
case ControlData::Type::Neutral:
|
||||
_angle_setpoints[0] = 0.f;
|
||||
_angle_setpoints[1] = 0.f;
|
||||
_angle_setpoints[2] = 0.f;
|
||||
_q_setpoint[0] = 1.f;
|
||||
_q_setpoint[1] = 0.f;
|
||||
_q_setpoint[2] = 0.f;
|
||||
_q_setpoint[3] = 0.f;
|
||||
_angle_velocity[0] = NAN;
|
||||
_angle_velocity[1] = NAN;
|
||||
_angle_velocity[2] = NAN;
|
||||
break;
|
||||
}
|
||||
}
|
||||
@@ -176,31 +174,31 @@ void OutputBase::_handle_position_update(bool force_update)
|
||||
const double &lon = _cur_control_data->type_data.lonlat.lon;
|
||||
const float &alt = _cur_control_data->type_data.lonlat.altitude;
|
||||
|
||||
_angle_setpoints[0] = _cur_control_data->type_data.lonlat.roll_angle;
|
||||
float roll = _cur_control_data->type_data.lonlat.roll_angle;
|
||||
|
||||
// interface: use fixed pitch value > -pi otherwise consider ROI altitude
|
||||
if (_cur_control_data->type_data.lonlat.pitch_fixed_angle >= -M_PI_F) {
|
||||
_angle_setpoints[1] = _cur_control_data->type_data.lonlat.pitch_fixed_angle;
|
||||
float pitch = (_cur_control_data->type_data.lonlat.pitch_fixed_angle >= -M_PI_F) ?
|
||||
_cur_control_data->type_data.lonlat.pitch_fixed_angle :
|
||||
_calculate_pitch(lon, lat, alt, vehicle_global_position);
|
||||
|
||||
} else {
|
||||
_angle_setpoints[1] = _calculate_pitch(lon, lat, alt, vehicle_global_position);
|
||||
}
|
||||
|
||||
_angle_setpoints[2] = get_bearing_to_next_waypoint(vlat, vlon, lat, lon) - vehicle_local_position.heading;
|
||||
float yaw = get_bearing_to_next_waypoint(vlat, vlon, lat, lon) - vehicle_local_position.heading;
|
||||
|
||||
// add offsets from VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET
|
||||
_angle_setpoints[1] += _cur_control_data->type_data.lonlat.pitch_angle_offset;
|
||||
_angle_setpoints[2] += _cur_control_data->type_data.lonlat.yaw_angle_offset;
|
||||
pitch += _cur_control_data->type_data.lonlat.pitch_angle_offset;
|
||||
yaw += _cur_control_data->type_data.lonlat.yaw_angle_offset;
|
||||
|
||||
// make sure yaw is wrapped correctly for the output
|
||||
_angle_setpoints[2] = wrap_pi(_angle_setpoints[2]);
|
||||
matrix::Quatf(matrix::Eulerf(roll, pitch, yaw)).copyTo(_q_setpoint);
|
||||
|
||||
_angle_velocity[0] = NAN;
|
||||
_angle_velocity[1] = NAN;
|
||||
_angle_velocity[2] = NAN;
|
||||
}
|
||||
|
||||
void OutputBase::_calculate_output_angles(const hrt_abstime &t)
|
||||
void OutputBase::_calculate_angle_output(const hrt_abstime &t)
|
||||
{
|
||||
//get the output angles and stabilize if necessary
|
||||
vehicle_attitude_s vehicle_attitude{};
|
||||
matrix::Eulerf euler;
|
||||
matrix::Eulerf euler_vehicle;
|
||||
|
||||
// We only need to apply additional compensation if the required angle is
|
||||
// absolute (world frame) as well as the gimbal is not capable of doing that
|
||||
@@ -214,29 +212,31 @@ void OutputBase::_calculate_output_angles(const hrt_abstime &t)
|
||||
|
||||
if (compensate[0] || compensate[1] || compensate[2]) {
|
||||
_vehicle_attitude_sub.copy(&vehicle_attitude);
|
||||
euler = matrix::Quatf(vehicle_attitude.q);
|
||||
euler_vehicle = matrix::Quatf(vehicle_attitude.q);
|
||||
}
|
||||
|
||||
float dt = (t - _last_update) / 1.e6f;
|
||||
|
||||
matrix::Eulerf euler_gimbal = matrix::Quatf(_q_setpoint);
|
||||
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
|
||||
if (PX4_ISFINITE(_angle_setpoints[i])) {
|
||||
_angle_outputs[i] = _angle_setpoints[i];
|
||||
if (PX4_ISFINITE(euler_gimbal(i))) {
|
||||
_angle_outputs[i] = euler_gimbal(i);
|
||||
}
|
||||
|
||||
if (PX4_ISFINITE(_angle_speeds[i])) {
|
||||
_angle_outputs[i] += dt * _angle_speeds[i];
|
||||
if (PX4_ISFINITE(_angle_velocity[i])) {
|
||||
_angle_outputs[i] += dt * _angle_velocity[i];
|
||||
}
|
||||
|
||||
if (compensate[i]) {
|
||||
_angle_outputs[i] -= euler(i);
|
||||
_angle_outputs[i] -= euler_vehicle(i);
|
||||
}
|
||||
|
||||
//bring angles into proper range [-pi, pi]
|
||||
while (_angle_outputs[i] > M_PI_F) { _angle_outputs[i] -= 2.f * M_PI_F; }
|
||||
|
||||
while (_angle_outputs[i] < -M_PI_F) { _angle_outputs[i] += 2.f * M_PI_F; }
|
||||
if (PX4_ISFINITE(_angle_outputs[i])) {
|
||||
//bring angles into proper range [-pi, pi]
|
||||
_angle_outputs[i] = matrix::wrap_pi(_angle_outputs[i]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -114,8 +114,10 @@ protected:
|
||||
void _handle_position_update(bool force_update = false);
|
||||
|
||||
const ControlData *_cur_control_data = nullptr;
|
||||
float _angle_setpoints[3] = { 0.f, 0.f, 0.f }; ///< [rad], can be NAN if not specifically set
|
||||
float _angle_speeds[3] = { 0.f, 0.f, 0.f }; ///< [rad/s], can be NAN if not specifically set
|
||||
|
||||
float _q_setpoint[4] = { NAN, NAN, NAN, NAN }; ///< can be NAN if not specifically set
|
||||
float _angle_velocity[3] = { NAN, NAN, NAN }; //< [rad/s], can be NAN if not specifically set
|
||||
|
||||
bool _stabilize[3] = { false, false, false };
|
||||
|
||||
// Pitch and role are by default aligned with the horizon.
|
||||
@@ -123,7 +125,7 @@ protected:
|
||||
bool _absolute_angle[3] = {true, true, false };
|
||||
|
||||
/** calculate the _angle_outputs (with speed) and stabilize if needed */
|
||||
void _calculate_output_angles(const hrt_abstime &t);
|
||||
void _calculate_angle_output(const hrt_abstime &t);
|
||||
|
||||
float _angle_outputs[3] = { 0.f, 0.f, 0.f }; ///< calculated output angles (roll, pitch, yaw) [rad]
|
||||
hrt_abstime _last_update;
|
||||
|
||||
@@ -94,7 +94,7 @@ int OutputMavlinkV1::update(const ControlData *control_data)
|
||||
_handle_position_update();
|
||||
|
||||
hrt_abstime t = hrt_absolute_time();
|
||||
_calculate_output_angles(t);
|
||||
_calculate_angle_output(t);
|
||||
|
||||
vehicle_command.timestamp = t;
|
||||
vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL;
|
||||
@@ -136,17 +136,13 @@ int OutputMavlinkV2::update(const ControlData *control_data)
|
||||
_last_gimbal_device_checked = t;
|
||||
|
||||
} else {
|
||||
// Only start sending attitude setpoints once a device has been discovered.
|
||||
|
||||
if (control_data) {
|
||||
//got new command
|
||||
_set_angle_setpoints(control_data);
|
||||
_publish_gimbal_device_set_attitude(control_data);
|
||||
}
|
||||
|
||||
_handle_position_update();
|
||||
|
||||
_calculate_output_angles(t);
|
||||
_publish_gimbal_device_set_attitude();
|
||||
_last_update = t;
|
||||
}
|
||||
|
||||
@@ -185,51 +181,31 @@ void OutputMavlinkV2::print_status()
|
||||
PX4_INFO("Output: MAVLink gimbal protocol v2");
|
||||
}
|
||||
|
||||
void OutputMavlinkV2::_publish_gimbal_device_set_attitude(const ControlData *control_data)
|
||||
void OutputMavlinkV2::_publish_gimbal_device_set_attitude()
|
||||
{
|
||||
gimbal_device_set_attitude_s set_attitude{};
|
||||
set_attitude.timestamp = hrt_absolute_time();
|
||||
set_attitude.target_system = (uint8_t)_mav_sys_id;
|
||||
set_attitude.target_component = _gimbal_device_compid;
|
||||
|
||||
switch (control_data->type) {
|
||||
case ControlData::Type::Neutral:
|
||||
set_attitude.flags |= gimbal_device_set_attitude_s::GIMBAL_DEVICE_FLAGS_NEUTRAL;
|
||||
set_attitude.angular_velocity_x = NAN;
|
||||
set_attitude.angular_velocity_y = NAN;
|
||||
set_attitude.angular_velocity_z = NAN;
|
||||
set_attitude.q[0] = NAN;
|
||||
set_attitude.q[1] = NAN;
|
||||
set_attitude.q[2] = NAN;
|
||||
set_attitude.q[3] = NAN;
|
||||
break;
|
||||
set_attitude.angular_velocity_x = _angle_velocity[0];
|
||||
set_attitude.angular_velocity_y = _angle_velocity[1];
|
||||
set_attitude.angular_velocity_z = _angle_velocity[2];
|
||||
set_attitude.q[0] = _q_setpoint[0];
|
||||
set_attitude.q[1] = _q_setpoint[1];
|
||||
set_attitude.q[2] = _q_setpoint[2];
|
||||
set_attitude.q[3] = _q_setpoint[3];
|
||||
|
||||
case ControlData::Type::Angle:
|
||||
set_attitude.angular_velocity_x = control_data->type_data.angle.angular_velocity[0];
|
||||
set_attitude.angular_velocity_y = control_data->type_data.angle.angular_velocity[1];
|
||||
set_attitude.angular_velocity_z = control_data->type_data.angle.angular_velocity[2];
|
||||
set_attitude.q[0] = control_data->type_data.angle.q[0];
|
||||
set_attitude.q[1] = control_data->type_data.angle.q[1];
|
||||
set_attitude.q[2] = control_data->type_data.angle.q[2];
|
||||
set_attitude.q[3] = control_data->type_data.angle.q[3];
|
||||
if (_absolute_angle[0]) {
|
||||
set_attitude.flags |= gimbal_device_set_attitude_s::GIMBAL_DEVICE_FLAGS_ROLL_LOCK;
|
||||
}
|
||||
|
||||
if (control_data->type_data.angle.frames[0] == ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame) {
|
||||
set_attitude.flags |= gimbal_device_set_attitude_s::GIMBAL_DEVICE_FLAGS_ROLL_LOCK;
|
||||
}
|
||||
if (_absolute_angle[1]) {
|
||||
set_attitude.flags |= gimbal_device_set_attitude_s::GIMBAL_DEVICE_FLAGS_PITCH_LOCK;
|
||||
}
|
||||
|
||||
if (control_data->type_data.angle.frames[1] == ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame) {
|
||||
set_attitude.flags |= gimbal_device_set_attitude_s::GIMBAL_DEVICE_FLAGS_PITCH_LOCK;
|
||||
}
|
||||
|
||||
if (control_data->type_data.angle.frames[2] == ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame) {
|
||||
set_attitude.flags |= gimbal_device_set_attitude_s::GIMBAL_DEVICE_FLAGS_YAW_LOCK;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case ControlData::Type::LonLat:
|
||||
// FIXME: needs conversion to angle.
|
||||
break;
|
||||
if (_absolute_angle[2]) {
|
||||
set_attitude.flags |= gimbal_device_set_attitude_s::GIMBAL_DEVICE_FLAGS_YAW_LOCK;
|
||||
}
|
||||
|
||||
_gimbal_device_set_attitude_pub.publish(set_attitude);
|
||||
|
||||
@@ -78,7 +78,7 @@ public:
|
||||
virtual void print_status();
|
||||
|
||||
private:
|
||||
void _publish_gimbal_device_set_attitude(const ControlData *control_data);
|
||||
void _publish_gimbal_device_set_attitude();
|
||||
void _request_gimbal_device_information();
|
||||
void _check_for_gimbal_device_information();
|
||||
|
||||
|
||||
@@ -63,7 +63,7 @@ int OutputRC::update(const ControlData *control_data)
|
||||
_handle_position_update();
|
||||
|
||||
hrt_abstime t = hrt_absolute_time();
|
||||
_calculate_output_angles(t);
|
||||
_calculate_angle_output(t);
|
||||
|
||||
actuator_controls_s actuator_controls{};
|
||||
actuator_controls.timestamp = hrt_absolute_time();
|
||||
|
||||
@@ -281,11 +281,11 @@ static int vmount_thread_main(int argc, char *argv[])
|
||||
break;
|
||||
|
||||
case 4: //MAVLINK_V2
|
||||
thread_data.input_objs[0] = new InputMavlinkGimbalV2(params.mnt_mode_out == 2,
|
||||
params.mav_sys_id,
|
||||
params.mav_comp_id,
|
||||
params.mnt_rate_pitch,
|
||||
params.mnt_rate_yaw);
|
||||
thread_data.input_objs[0] = new InputMavlinkGimbalV2(
|
||||
params.mav_sys_id,
|
||||
params.mav_comp_id,
|
||||
params.mnt_rate_pitch,
|
||||
params.mnt_rate_yaw);
|
||||
break;
|
||||
|
||||
default:
|
||||
|
||||
Reference in New Issue
Block a user