aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Mount: Gremsy uses _link.send_message(id, buffer) to send set-attitude

Browse Source
This commit is contained in:
Peter Barker 2023-02-24 16:09:59 +11:00 committed by Peter Barker
parent adb364cd98
commit 44bf0ec1a7
1 changed files with 45 additions and 63 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

108
libraries/AP_Mount/AP_Mount_Gremsy.cpp
View File

@ -234,19 +234,22 @@ void AP_Mount_Gremsy::request_gimbal_device_information() const
if (_link == nullptr) {
return;
}
const mavlink_channel_t chan = _link->get_chan();
// check we have space for the message
if (!HAVE_PAYLOAD_SPACE(chan, COMMAND_LONG)) {
return;
}
mavlink_msg_command_long_send(
chan,
const mavlink_command_long_t pkt {
MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION, // param1
0, // param2
0, // param3
0, // param4
0, // param5
0, // param6
0, // param7
MAV_CMD_REQUEST_MESSAGE,
_sysid,
_compid,
MAV_CMD_REQUEST_MESSAGE,
0, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION, 0, 0, 0, 0, 0, 0);
0 // confirmation
};
_link->send_message(MAVLINK_MSG_ID_COMMAND_LONG, (const char*)&pkt);
}
// start sending ATTITUDE and AUTOPILOT_STATE_FOR_GIMBAL_DEVICE to gimbal
@ -266,51 +269,37 @@ bool AP_Mount_Gremsy::start_sending_attitude_to_gimbal()
// send GIMBAL_DEVICE_SET_ATTITUDE to gimbal to command gimbal to retract (aka relax)
void AP_Mount_Gremsy::send_gimbal_device_retract() const
{
if (_link == nullptr) {
return;
}
const mavlink_channel_t chan = _link->get_chan();
const mavlink_gimbal_device_set_attitude_t pkt {
{NAN, NAN, NAN, NAN}, // attitude
0, // angular velocity x
0, // angular velocity y
0, // angular velocity z
GIMBAL_DEVICE_FLAGS_RETRACT, // flags
_sysid,
_compid
};
// check we have space for the message
if (!HAVE_PAYLOAD_SPACE(chan, GIMBAL_DEVICE_SET_ATTITUDE)) {
return;
}
// send command_long command containing a do_mount_control command
const float quat_array[4] = {NAN, NAN, NAN, NAN};
mavlink_msg_gimbal_device_set_attitude_send(chan,
_sysid, // target system
_compid, // target component
GIMBAL_DEVICE_FLAGS_RETRACT, // gimbal device flags
quat_array, // attitude as a quaternion
0, 0, 0); // angular velocities
_link->send_message(MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE, (const char*)&pkt);
}
// send GIMBAL_DEVICE_SET_ATTITUDE to gimbal to control rate
// earth_frame should be true if yaw_rads target is an earth frame rate, false if body_frame
void AP_Mount_Gremsy::send_gimbal_device_set_rate(float roll_rads, float pitch_rads, float yaw_rads, bool earth_frame) const
{
if (_link == nullptr) {
return;
}
const mavlink_channel_t chan = _link->get_chan();
// check we have space for the message
if (!HAVE_PAYLOAD_SPACE(chan, GIMBAL_DEVICE_SET_ATTITUDE)) {
return;
}
// prepare flags
const uint16_t flags = earth_frame ? (GIMBAL_DEVICE_FLAGS_ROLL_LOCK | GIMBAL_DEVICE_FLAGS_PITCH_LOCK | GIMBAL_DEVICE_FLAGS_YAW_LOCK) : 0;
const float quat_array[4] = {NAN, NAN, NAN, NAN};
// send command_long command containing a do_mount_control command
mavlink_msg_gimbal_device_set_attitude_send(chan,
_sysid, // target system
_compid, // target component
flags, // gimbal device flags
quat_array, // attitude as a quaternion
roll_rads, pitch_rads, yaw_rads); // angular velocities
const mavlink_gimbal_device_set_attitude_t pkt {
{NAN, NAN, NAN, NAN}, // attitude
roll_rads, // angular velocity x
pitch_rads, // angular velocity y
yaw_rads, // angular velocity z
flags,
_sysid,
_compid
};
_link->send_message(MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE, (const char*)&pkt);
}
// send GIMBAL_DEVICE_SET_ATTITUDE to gimbal to control attitude
@ -322,31 +311,24 @@ void AP_Mount_Gremsy::send_gimbal_device_set_attitude(float roll_rad, float pitc
return;
}
if (_link == nullptr) {
return;
}
const mavlink_channel_t chan = _link->get_chan();
// check we have space for the message
if (!HAVE_PAYLOAD_SPACE(chan, GIMBAL_DEVICE_SET_ATTITUDE)) {
return;
}
// prepare flags
const uint16_t flags = earth_frame ? (GIMBAL_DEVICE_FLAGS_ROLL_LOCK | GIMBAL_DEVICE_FLAGS_PITCH_LOCK | GIMBAL_DEVICE_FLAGS_YAW_LOCK) : 0;
// convert euler angles to quaternion
Quaternion q;
q.from_euler(roll_rad, pitch_rad, yaw_rad);
const float quat_array[4] = {q.q1, q.q2, q.q3, q.q4};
// send command_long command containing a do_mount_control command
mavlink_msg_gimbal_device_set_attitude_send(chan,
_sysid, // target system
_compid, // target component
flags, // gimbal device flags
quat_array, // attitude as a quaternion
NAN, NAN, NAN); // angular velocities
const mavlink_gimbal_device_set_attitude_t pkt {
{q.q1, q.q2, q.q3, q.q4},
NAN, // angular velocity x
NAN, // angular velocity y
NAN, // angular velocity z
flags,
_sysid,
_compid
};
_link->send_message(MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE, (const char*)&pkt);
}
#endif // HAL_MOUNT_GREMSY_ENABLED