Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-18 14:48:28 -04:00
Code Issues 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) { if (_link == nullptr) {
return; return;
} }
const mavlink_channel_t chan = _link->get_chan();
// check we have space for the message const mavlink_command_long_t pkt {
if (!HAVE_PAYLOAD_SPACE(chan, COMMAND_LONG)) { MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION, // param1
return; 0, // param2
} 0, // param3
0, // param4
mavlink_msg_command_long_send( 0, // param5
chan, 0, // param6
0, // param7
MAV_CMD_REQUEST_MESSAGE,
_sysid, _sysid,
_compid, _compid,
MAV_CMD_REQUEST_MESSAGE, 0 // confirmation
0, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION, 0, 0, 0, 0, 0, 0); };
_link->send_message(MAVLINK_MSG_ID_COMMAND_LONG, (const char*)&pkt);
} }
// start sending ATTITUDE and AUTOPILOT_STATE_FOR_GIMBAL_DEVICE to gimbal // 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) // send GIMBAL_DEVICE_SET_ATTITUDE to gimbal to command gimbal to retract (aka relax)
void AP_Mount_Gremsy::send_gimbal_device_retract() const void AP_Mount_Gremsy::send_gimbal_device_retract() const
{ {
if (_link == nullptr) { const mavlink_gimbal_device_set_attitude_t pkt {
return; {NAN, NAN, NAN, NAN}, // attitude
} 0, // angular velocity x
const mavlink_channel_t chan = _link->get_chan(); 0, // angular velocity y
0, // angular velocity z
GIMBAL_DEVICE_FLAGS_RETRACT, // flags
_sysid,
_compid
};
// check we have space for the message _link->send_message(MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE, (const char*)&pkt);
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
} }
// send GIMBAL_DEVICE_SET_ATTITUDE to gimbal to control rate // 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 // 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 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 // 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 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 const mavlink_gimbal_device_set_attitude_t pkt {
mavlink_msg_gimbal_device_set_attitude_send(chan, {NAN, NAN, NAN, NAN}, // attitude
_sysid, // target system roll_rads, // angular velocity x
_compid, // target component pitch_rads, // angular velocity y
flags, // gimbal device flags yaw_rads, // angular velocity z
quat_array, // attitude as a quaternion flags,
roll_rads, pitch_rads, yaw_rads); // angular velocities _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 // 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; 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 // 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 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 // convert euler angles to quaternion
Quaternion q; Quaternion q;
q.from_euler(roll_rad, pitch_rad, yaw_rad); 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 const mavlink_gimbal_device_set_attitude_t pkt {
mavlink_msg_gimbal_device_set_attitude_send(chan, {q.q1, q.q2, q.q3, q.q4},
_sysid, // target system NAN, // angular velocity x
_compid, // target component NAN, // angular velocity y
flags, // gimbal device flags NAN, // angular velocity z
quat_array, // attitude as a quaternion flags,
NAN, NAN, NAN); // angular velocities _sysid,
_compid
};
_link->send_message(MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE, (const char*)&pkt);
} }
#endif // HAL_MOUNT_GREMSY_ENABLED #endif // HAL_MOUNT_GREMSY_ENABLED