Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-07 08:23:56 -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) // 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) 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; const GimbalMotionMode mode = yaw_is_ef ? GimbalMotionMode::LOCK : GimbalMotionMode::FOLLOW;
if (mode != _config_info.motion_mode) { if (!set_motion_mode(mode)) {
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}; 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)); 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 // FOLLOW: roll and pitch are in earth-frame, yaw is in body-frame
// LOCK: roll, pitch and yaw are all in earth-frame // LOCK: roll, pitch and yaw are all in earth-frame
// FPV: roll, pitch and yaw are all in body-frame // FPV: roll, pitch and yaw are all in body-frame
// Returns true if mode successfully sent to Gimbal // 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; PhotoFunction data = PhotoFunction::LOCK_MODE;
switch (mode) { switch (mode) {
case GimbalMotionMode::LOCK: data = PhotoFunction::LOCK_MODE; break; case GimbalMotionMode::LOCK: data = PhotoFunction::LOCK_MODE; break;
case GimbalMotionMode::FOLLOW: data = PhotoFunction::FOLLOW_MODE; break; case GimbalMotionMode::FOLLOW: data = PhotoFunction::FOLLOW_MODE; break;
case GimbalMotionMode::FPV: data = PhotoFunction::FPV_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 // 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) // 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); 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 // FOLLOW: roll and pitch are in earth-frame, yaw is in body-frame
// LOCK: roll, pitch and yaw are all in earth-frame // LOCK: roll, pitch and yaw are all in earth-frame
// FPV: roll, pitch and yaw are all in body-frame // FPV: roll, pitch and yaw are all in body-frame
// Returns true if message successfully sent to Gimbal // 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 // 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 // yaw_is_ef should be true if yaw_rads target is an earth frame rate, false if body_frame