forked from Archive/PX4-Autopilot
gimbal: implement gimbal_device_id
It turns out that I had omitted implementing the gimbal_device_id which is the component ID of the gimbal device that the gimbal manager (in this case PX4) is responsible for. Signed-off-by: Julian Oes <julian@oes.ch>
This commit is contained in:
parent
b23eee40f0
commit
3f13a6e787
|
@ -82,6 +82,7 @@ struct ControlData {
|
||||||
uint8_t compid_primary_control = 0; // The MAVLink component ID selected to be in control, 0 for no one.
|
uint8_t compid_primary_control = 0; // The MAVLink component ID selected to be in control, 0 for no one.
|
||||||
// uint8_t sysid_secondary_control = 0; // The MAVLink system ID selected for additional input, not implemented yet.
|
// uint8_t sysid_secondary_control = 0; // The MAVLink system ID selected for additional input, not implemented yet.
|
||||||
// uint8_t compid_secondary_control = 0; // The MAVLink component ID selected for additional input, not implemented yet.
|
// uint8_t compid_secondary_control = 0; // The MAVLink component ID selected for additional input, not implemented yet.
|
||||||
|
uint8_t device_compid = 0;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -211,6 +211,7 @@ static int gimbal_thread_main(int argc, char *argv[])
|
||||||
// Reset control as no one is active anymore, or yet.
|
// Reset control as no one is active anymore, or yet.
|
||||||
thread_data.control_data.sysid_primary_control = 0;
|
thread_data.control_data.sysid_primary_control = 0;
|
||||||
thread_data.control_data.compid_primary_control = 0;
|
thread_data.control_data.compid_primary_control = 0;
|
||||||
|
thread_data.control_data.device_compid = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
InputBase::UpdateResult update_result = InputBase::UpdateResult::NoUpdate;
|
InputBase::UpdateResult update_result = InputBase::UpdateResult::NoUpdate;
|
||||||
|
|
|
@ -375,7 +375,6 @@ void InputMavlinkCmdMount::print_status() const
|
||||||
InputMavlinkGimbalV2::InputMavlinkGimbalV2(Parameters ¶meters) :
|
InputMavlinkGimbalV2::InputMavlinkGimbalV2(Parameters ¶meters) :
|
||||||
InputBase(parameters)
|
InputBase(parameters)
|
||||||
{
|
{
|
||||||
_stream_gimbal_manager_information();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
InputMavlinkGimbalV2::~InputMavlinkGimbalV2()
|
InputMavlinkGimbalV2::~InputMavlinkGimbalV2()
|
||||||
|
@ -453,7 +452,7 @@ void InputMavlinkGimbalV2::_stream_gimbal_manager_status(const ControlData &cont
|
||||||
gimbal_manager_status_s gimbal_manager_status{};
|
gimbal_manager_status_s gimbal_manager_status{};
|
||||||
gimbal_manager_status.timestamp = hrt_absolute_time();
|
gimbal_manager_status.timestamp = hrt_absolute_time();
|
||||||
gimbal_manager_status.flags = gimbal_device_attitude_status.device_flags;
|
gimbal_manager_status.flags = gimbal_device_attitude_status.device_flags;
|
||||||
gimbal_manager_status.gimbal_device_id = 0;
|
gimbal_manager_status.gimbal_device_id = control_data.device_compid;
|
||||||
gimbal_manager_status.primary_control_sysid = control_data.sysid_primary_control;
|
gimbal_manager_status.primary_control_sysid = control_data.sysid_primary_control;
|
||||||
gimbal_manager_status.primary_control_compid = control_data.compid_primary_control;
|
gimbal_manager_status.primary_control_compid = control_data.compid_primary_control;
|
||||||
gimbal_manager_status.secondary_control_sysid = 0; // TODO: support secondary control
|
gimbal_manager_status.secondary_control_sysid = 0; // TODO: support secondary control
|
||||||
|
@ -462,7 +461,7 @@ void InputMavlinkGimbalV2::_stream_gimbal_manager_status(const ControlData &cont
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void InputMavlinkGimbalV2::_stream_gimbal_manager_information()
|
void InputMavlinkGimbalV2::_stream_gimbal_manager_information(const ControlData &control_data)
|
||||||
{
|
{
|
||||||
// TODO: Take gimbal_device_information into account.
|
// TODO: Take gimbal_device_information into account.
|
||||||
|
|
||||||
|
@ -483,6 +482,8 @@ void InputMavlinkGimbalV2::_stream_gimbal_manager_information()
|
||||||
gimbal_manager_info.yaw_max = M_PI_F;
|
gimbal_manager_info.yaw_max = M_PI_F;
|
||||||
gimbal_manager_info.yaw_min = -M_PI_F;
|
gimbal_manager_info.yaw_min = -M_PI_F;
|
||||||
|
|
||||||
|
gimbal_manager_info.gimbal_device_id = control_data.device_compid;
|
||||||
|
|
||||||
_gimbal_manager_info_pub.publish(gimbal_manager_info);
|
_gimbal_manager_info_pub.publish(gimbal_manager_info);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -578,6 +579,11 @@ InputMavlinkGimbalV2::update(unsigned int timeout_ms, ControlData &control_data,
|
||||||
|
|
||||||
_stream_gimbal_manager_status(control_data);
|
_stream_gimbal_manager_status(control_data);
|
||||||
|
|
||||||
|
if (_last_device_compid != control_data.device_compid) {
|
||||||
|
_last_device_compid = control_data.device_compid;
|
||||||
|
_stream_gimbal_manager_information(control_data);
|
||||||
|
}
|
||||||
|
|
||||||
return update_result;
|
return update_result;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -114,7 +114,7 @@ private:
|
||||||
void _set_control_data_from_set_attitude(ControlData &control_data, const uint32_t flags, const matrix::Quatf &q,
|
void _set_control_data_from_set_attitude(ControlData &control_data, const uint32_t flags, const matrix::Quatf &q,
|
||||||
const matrix::Vector3f &angular_velocity);
|
const matrix::Vector3f &angular_velocity);
|
||||||
void _ack_vehicle_command(const vehicle_command_s &cmd, uint8_t result);
|
void _ack_vehicle_command(const vehicle_command_s &cmd, uint8_t result);
|
||||||
void _stream_gimbal_manager_information();
|
void _stream_gimbal_manager_information(const ControlData &control_data);
|
||||||
void _stream_gimbal_manager_status(const ControlData &control_data);
|
void _stream_gimbal_manager_status(const ControlData &control_data);
|
||||||
void _read_control_data_from_position_setpoint_sub(ControlData &control_data);
|
void _read_control_data_from_position_setpoint_sub(ControlData &control_data);
|
||||||
|
|
||||||
|
@ -128,6 +128,8 @@ private:
|
||||||
uORB::Publication<gimbal_manager_information_s> _gimbal_manager_info_pub{ORB_ID(gimbal_manager_information)};
|
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)};
|
uORB::Publication<gimbal_manager_status_s> _gimbal_manager_status_pub{ORB_ID(gimbal_manager_status)};
|
||||||
uint8_t _cur_roi_mode = vehicle_roi_s::ROI_NONE;
|
uint8_t _cur_roi_mode = vehicle_roi_s::ROI_NONE;
|
||||||
|
|
||||||
|
uint8_t _last_device_compid = 0;
|
||||||
};
|
};
|
||||||
|
|
||||||
} /* namespace gimbal */
|
} /* namespace gimbal */
|
||||||
|
|
|
@ -55,7 +55,7 @@ public:
|
||||||
explicit OutputBase(const Parameters ¶meters);
|
explicit OutputBase(const Parameters ¶meters);
|
||||||
virtual ~OutputBase() = default;
|
virtual ~OutputBase() = default;
|
||||||
|
|
||||||
virtual void update(const ControlData &control_data, bool new_setpoints) = 0;
|
virtual void update(ControlData &control_data, bool new_setpoints) = 0;
|
||||||
|
|
||||||
virtual void print_status() const = 0;
|
virtual void print_status() const = 0;
|
||||||
|
|
||||||
|
|
|
@ -46,7 +46,7 @@ OutputMavlinkV1::OutputMavlinkV1(const Parameters ¶meters)
|
||||||
: OutputBase(parameters)
|
: OutputBase(parameters)
|
||||||
{}
|
{}
|
||||||
|
|
||||||
void OutputMavlinkV1::update(const ControlData &control_data, bool new_setpoints)
|
void OutputMavlinkV1::update(ControlData &control_data, bool new_setpoints)
|
||||||
{
|
{
|
||||||
vehicle_command_s vehicle_command{};
|
vehicle_command_s vehicle_command{};
|
||||||
vehicle_command.timestamp = hrt_absolute_time();
|
vehicle_command.timestamp = hrt_absolute_time();
|
||||||
|
@ -142,7 +142,7 @@ OutputMavlinkV2::OutputMavlinkV2(const Parameters ¶meters)
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|
||||||
void OutputMavlinkV2::update(const ControlData &control_data, bool new_setpoints)
|
void OutputMavlinkV2::update(ControlData &control_data, bool new_setpoints)
|
||||||
{
|
{
|
||||||
_check_for_gimbal_device_information();
|
_check_for_gimbal_device_information();
|
||||||
|
|
||||||
|
@ -162,6 +162,8 @@ void OutputMavlinkV2::update(const ControlData &control_data, bool new_setpoints
|
||||||
_last_update = t;
|
_last_update = t;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
control_data.device_compid = _gimbal_device_found ? _gimbal_device_compid : 0;
|
||||||
|
|
||||||
_publish_gimbal_device_set_attitude();
|
_publish_gimbal_device_set_attitude();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -206,6 +208,13 @@ void OutputMavlinkV2::print_status() const
|
||||||
(double)_angle_velocity[0],
|
(double)_angle_velocity[0],
|
||||||
(double)_angle_velocity[1],
|
(double)_angle_velocity[1],
|
||||||
(double)_angle_velocity[2]);
|
(double)_angle_velocity[2]);
|
||||||
|
|
||||||
|
if (_gimbal_device_found) {
|
||||||
|
PX4_INFO_RAW(" gimbal device compid found: %d\n", _gimbal_device_compid);
|
||||||
|
|
||||||
|
} else {
|
||||||
|
PX4_INFO_RAW(" gimbal device compid not found\n");
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void OutputMavlinkV2::_publish_gimbal_device_set_attitude()
|
void OutputMavlinkV2::_publish_gimbal_device_set_attitude()
|
||||||
|
|
|
@ -51,7 +51,7 @@ public:
|
||||||
OutputMavlinkV1(const Parameters ¶meters);
|
OutputMavlinkV1(const Parameters ¶meters);
|
||||||
virtual ~OutputMavlinkV1() = default;
|
virtual ~OutputMavlinkV1() = default;
|
||||||
|
|
||||||
virtual void update(const ControlData &control_data, bool new_setpoints);
|
virtual void update(ControlData &control_data, bool new_setpoints);
|
||||||
|
|
||||||
virtual void print_status() const;
|
virtual void print_status() const;
|
||||||
|
|
||||||
|
@ -69,7 +69,7 @@ public:
|
||||||
OutputMavlinkV2(const Parameters ¶meters);
|
OutputMavlinkV2(const Parameters ¶meters);
|
||||||
virtual ~OutputMavlinkV2() = default;
|
virtual ~OutputMavlinkV2() = default;
|
||||||
|
|
||||||
virtual void update(const ControlData &control_data, bool new_setpoints);
|
virtual void update(ControlData &control_data, bool new_setpoints);
|
||||||
|
|
||||||
virtual void print_status() const;
|
virtual void print_status() const;
|
||||||
|
|
||||||
|
|
|
@ -48,7 +48,7 @@ OutputRC::OutputRC(const Parameters ¶meters)
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|
||||||
void OutputRC::update(const ControlData &control_data, bool new_setpoints)
|
void OutputRC::update(ControlData &control_data, bool new_setpoints)
|
||||||
{
|
{
|
||||||
if (new_setpoints) {
|
if (new_setpoints) {
|
||||||
_set_angle_setpoints(control_data);
|
_set_angle_setpoints(control_data);
|
||||||
|
@ -61,6 +61,9 @@ void OutputRC::update(const ControlData &control_data, bool new_setpoints)
|
||||||
|
|
||||||
_stream_device_attitude_status();
|
_stream_device_attitude_status();
|
||||||
|
|
||||||
|
// If the output is RC, then it means we are also the gimbal device.
|
||||||
|
control_data.device_compid = (uint8_t)_parameters.mnt_mav_compid_v1;
|
||||||
|
|
||||||
// _angle_outputs are in radians, gimbal_controls are in [-1, 1]
|
// _angle_outputs are in radians, gimbal_controls are in [-1, 1]
|
||||||
gimbal_controls_s gimbal_controls{};
|
gimbal_controls_s gimbal_controls{};
|
||||||
gimbal_controls.control[gimbal_controls_s::INDEX_ROLL] = constrain(
|
gimbal_controls.control[gimbal_controls_s::INDEX_ROLL] = constrain(
|
||||||
|
|
|
@ -50,7 +50,7 @@ public:
|
||||||
explicit OutputRC(const Parameters ¶meters);
|
explicit OutputRC(const Parameters ¶meters);
|
||||||
virtual ~OutputRC() = default;
|
virtual ~OutputRC() = default;
|
||||||
|
|
||||||
virtual void update(const ControlData &control_data, bool new_setpoints);
|
virtual void update(ControlData &control_data, bool new_setpoints);
|
||||||
virtual void print_status() const;
|
virtual void print_status() const;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
|
@ -71,7 +71,7 @@ private:
|
||||||
|
|
||||||
msg.time_boot_ms = gimbal_manager_information.timestamp / 1000;
|
msg.time_boot_ms = gimbal_manager_information.timestamp / 1000;
|
||||||
msg.cap_flags = gimbal_manager_information.cap_flags;
|
msg.cap_flags = gimbal_manager_information.cap_flags;
|
||||||
msg.gimbal_device_id = 0;
|
msg.gimbal_device_id = gimbal_manager_information.gimbal_device_id;
|
||||||
msg.roll_min = gimbal_manager_information.roll_min;
|
msg.roll_min = gimbal_manager_information.roll_min;
|
||||||
msg.roll_max = gimbal_manager_information.roll_max;
|
msg.roll_max = gimbal_manager_information.roll_max;
|
||||||
msg.pitch_min = gimbal_manager_information.pitch_min;
|
msg.pitch_min = gimbal_manager_information.pitch_min;
|
||||||
|
|
Loading…
Reference in New Issue