Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-30 20:48:33 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Mount: Make Siyi set_motion_mode() only send on change

Browse Source
This commit is contained in:
Nick Exton 2023-09-07 10:31:08 +10:00 committed by Randy Mackay
parent 62858ff91f
commit d3358ff3af
2 changed files with 19 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
libraries/AP_Mount/AP_Mount_Siyi.cpp
View File

@ -604,30 +604,41 @@ bool AP_Mount_Siyi::send_1byte_packet(SiyiCommandId cmd_id, uint8_t data_byte)
// yaw_is_ef should be true if gimbal should maintain an earth-frame target (aka lock)
void AP_Mount_Siyi::rotate_gimbal(int8_t pitch_scalar, int8_t yaw_scalar, bool yaw_is_ef)
{
// send lock/follow value if it has changed
// send lock/follow value
const GimbalMotionMode mode = yaw_is_ef ? GimbalMotionMode::LOCK : GimbalMotionMode::FOLLOW;
if (mode != _config_info.motion_mode) {
set_motion_mode(mode);
if (!set_motion_mode(mode)) {
// couldn't set mode, so don't send rotation
return;
}
const uint8_t yaw_and_pitch_rates[] {(uint8_t)yaw_scalar, (uint8_t)pitch_scalar};
send_packet(SiyiCommandId::GIMBAL_ROTATION, yaw_and_pitch_rates, ARRAY_SIZE(yaw_and_pitch_rates));
}
// Set gimbal's motion mode
// Set gimbal's motion mode if it has changed. Use force=true to always send.
// FOLLOW: roll and pitch are in earth-frame, yaw is in body-frame
// LOCK: roll, pitch and yaw are all in earth-frame
// FPV: roll, pitch and yaw are all in body-frame
// Returns true if mode successfully sent to Gimbal
bool AP_Mount_Siyi::set_motion_mode(const GimbalMotionMode mode)
bool AP_Mount_Siyi::set_motion_mode(const GimbalMotionMode mode, const bool force)
{
if (!force && (mode == _config_info.motion_mode)) {
// we're already in the right mode...
return true;
}
PhotoFunction data = PhotoFunction::LOCK_MODE;
switch (mode) {
case GimbalMotionMode::LOCK: data = PhotoFunction::LOCK_MODE; break;
case GimbalMotionMode::FOLLOW: data = PhotoFunction::FOLLOW_MODE; break;
case GimbalMotionMode::FPV: data = PhotoFunction::FPV_MODE; break;
}
return send_1byte_packet(SiyiCommandId::PHOTO, (uint8_t)data);
bool sent = send_1byte_packet(SiyiCommandId::PHOTO, (uint8_t)data);
if (sent) {
// assume the mode is set correctly until told otherwise
_config_info.motion_mode = mode;
}
return sent;
}
// send target pitch and yaw rates to gimbal

4
libraries/AP_Mount/AP_Mount_Siyi.h
View File

@ -247,12 +247,12 @@ private:
// yaw_is_ef should be true if gimbal should maintain an earth-frame target (aka lock)
void rotate_gimbal(int8_t pitch_scalar, int8_t yaw_scalar, bool yaw_is_ef);
// Set gimbal's motion mode
// Set gimbal's motion mode if it has changed. Use force=true to always send.
// FOLLOW: roll and pitch are in earth-frame, yaw is in body-frame
// LOCK: roll, pitch and yaw are all in earth-frame
// FPV: roll, pitch and yaw are all in body-frame
// Returns true if message successfully sent to Gimbal
bool set_motion_mode(const GimbalMotionMode mode);
bool set_motion_mode(const GimbalMotionMode mode, const bool force=false);
// send target pitch and yaw rates to gimbal
// yaw_is_ef should be true if yaw_rads target is an earth frame rate, false if body_frame