Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Mount: Xacti gets reduced call to AP_HAL::millis()

This commit is contained in:
Randy Mackay 2023-09-16 14:12:58 +09:00 committed by Andrew Tridgell
parent fc24b45d56
commit b7d44c75a6
2 changed files with 26 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
libraries/AP_Mount/AP_Mount_Xacti.cpp
View File

@ -58,28 +58,29 @@ void AP_Mount_Xacti::update()
} }
// return immediately if any message sent is unlikely to be processed // return immediately if any message sent is unlikely to be processed
if (!is_safe_to_send()) { const uint32_t now_ms = AP_HAL::millis();
if (!is_safe_to_send(now_ms)) {
return; return;
} }
// get firmware version // get firmware version
if (request_firmware_version()) { if (request_firmware_version(now_ms)) {
return; return;
} }
// request status // request status
if (request_status()) { if (request_status(now_ms)) {
return; return;
} }
// periodically send copter attitude and GPS status // periodically send copter attitude and GPS status
if (send_copter_att_status()) { if (send_copter_att_status(now_ms)) {
// if message sent avoid sending other messages // if message sent avoid sending other messages
return; return;
} }
// update zoom rate control // update zoom rate control
if (update_zoom_rate_control()) { if (update_zoom_rate_control(now_ms)) {
// if message sent avoid sending other messages // if message sent avoid sending other messages
return; return;
} }
@ -665,9 +666,9 @@ void AP_Mount_Xacti::send_gimbal_control(uint8_t mode, int16_t pitch_cd, int16_t
_detected_modules[_instance].ap_dronecan->xacti_gimbal_control_data.broadcast(gimbal_control_data_msg); _detected_modules[_instance].ap_dronecan->xacti_gimbal_control_data.broadcast(gimbal_control_data_msg);
} }
// send copter attitude status message to gimbal // send copter attitude status message to gimbal. now_ms is current system time
// returns true if sent so that we avoid immediately trying to also send other messages // returns true if sent so that we avoid immediately trying to also send other messages
bool AP_Mount_Xacti::send_copter_att_status() bool AP_Mount_Xacti::send_copter_att_status(uint32_t now_ms)
{ {
// exit immediately if no DroneCAN port // exit immediately if no DroneCAN port
if (_detected_modules[_instance].ap_dronecan == nullptr) { if (_detected_modules[_instance].ap_dronecan == nullptr) {
@ -675,7 +676,6 @@ bool AP_Mount_Xacti::send_copter_att_status()
} }
// send at no faster than 5hz // send at no faster than 5hz
const uint32_t now_ms = AP_HAL::millis();
if (now_ms - last_send_copter_att_status_ms < 100) { if (now_ms - last_send_copter_att_status_ms < 100) {
return false; return false;
} }
@ -699,9 +699,9 @@ bool AP_Mount_Xacti::send_copter_att_status()
return true; return true;
} }
// update zoom rate controller // update zoom rate controller. now_ms is current system time
// returns true if sent so that we avoid immediately trying to also send other messages // returns true if sent so that we avoid immediately trying to also send other messages
bool AP_Mount_Xacti::update_zoom_rate_control() bool AP_Mount_Xacti::update_zoom_rate_control(uint32_t now_ms)
{ {
// return immediately if zoom rate control is not enabled // return immediately if zoom rate control is not enabled
if (!_zoom_rate_control.enabled) { if (!_zoom_rate_control.enabled) {
@ -709,7 +709,6 @@ bool AP_Mount_Xacti::update_zoom_rate_control()
} }
// update only every 0.5 sec // update only every 0.5 sec
const uint32_t now_ms = AP_HAL::millis();
if (now_ms - _zoom_rate_control.last_update_ms < XACTI_ZOOM_RATE_UPDATE_INTERVAL_MS) { if (now_ms - _zoom_rate_control.last_update_ms < XACTI_ZOOM_RATE_UPDATE_INTERVAL_MS) {
return false; return false;
} }
@ -728,9 +727,9 @@ bool AP_Mount_Xacti::update_zoom_rate_control()
return set_param_int32(XACTI_PARAM_DIGITALZOOM, zoom_value); return set_param_int32(XACTI_PARAM_DIGITALZOOM, zoom_value);
} }
// request firmware version // request firmware version. now_ms should be current system time (reduces calls to AP_HAL::millis)
// returns true if sent so that we avoid immediately trying to also send other messages // returns true if sent so that we avoid immediately trying to also send other messages
bool AP_Mount_Xacti::request_firmware_version() bool AP_Mount_Xacti::request_firmware_version(uint32_t now_ms)
{ {
// return immediately if already have version or no dronecan // return immediately if already have version or no dronecan
if (_firmware_version.received) { if (_firmware_version.received) {
@ -738,7 +737,6 @@ bool AP_Mount_Xacti::request_firmware_version()
} }
// send request once per second until received // send request once per second until received
const uint32_t now_ms = AP_HAL::millis();
if (now_ms - _firmware_version.last_request_ms < 1000) { if (now_ms - _firmware_version.last_request_ms < 1000) {
return false; return false;
} }
@ -746,12 +744,11 @@ bool AP_Mount_Xacti::request_firmware_version()
return get_param_string(XACTI_PARAM_FIRMWAREVERSION); return get_param_string(XACTI_PARAM_FIRMWAREVERSION);
} }
// request status // request status. now_ms should be current system time (reduces calls to AP_HAL::millis)
// returns true if sent so that we avoid immediately trying to also send other messages // returns true if sent so that we avoid immediately trying to also send other messages
bool AP_Mount_Xacti::request_status() bool AP_Mount_Xacti::request_status(uint32_t now_ms)
{ {
// return immediately if 3 seconds has not passed // return immediately if 3 seconds has not passed
const uint32_t now_ms = AP_HAL::millis();
if (now_ms - _status_report.last_request_ms < XACTI_STATUS_REQ_INTERVAL_MS) { if (now_ms - _status_report.last_request_ms < XACTI_STATUS_REQ_INTERVAL_MS) {
return false; return false;
} }
@ -761,10 +758,9 @@ bool AP_Mount_Xacti::request_status()
} }
// check if safe to send message (if messages sent too often camera will not respond) // check if safe to send message (if messages sent too often camera will not respond)
bool AP_Mount_Xacti::is_safe_to_send() const // now_ms should be current system time (reduces calls to AP_HAL::millis)
bool AP_Mount_Xacti::is_safe_to_send(uint32_t now_ms) const
{ {
const uint32_t now_ms = AP_HAL::millis();
// check time since last attitude sent // check time since last attitude sent
if (now_ms - last_send_copter_att_status_ms < XACTI_MSG_SEND_MIN_MS) { if (now_ms - last_send_copter_att_status_ms < XACTI_MSG_SEND_MIN_MS) {
return false; return false;

19
libraries/AP_Mount/AP_Mount_Xacti.h
View File

@ -128,24 +128,25 @@ private:
// yaw_cd is angle in centi-degrees or yaw rate in cds // yaw_cd is angle in centi-degrees or yaw rate in cds
void send_gimbal_control(uint8_t mode, int16_t pitch_cd, int16_t yaw_cd); void send_gimbal_control(uint8_t mode, int16_t pitch_cd, int16_t yaw_cd);
// send vehicle attitude to gimbal via DroneCAN // send vehicle attitude to gimbal via DroneCAN. now_ms is current system time
// returns true if sent so that we avoid immediately trying to also send other messages // returns true if sent so that we avoid immediately trying to also send other messages
bool send_copter_att_status(); bool send_copter_att_status(uint32_t now_ms);
// update zoom rate controller // update zoom rate controller. now_ms is current system time
// returns true if sent so that we avoid immediately trying to also send other messages // returns true if sent so that we avoid immediately trying to also send other messages
bool update_zoom_rate_control(); bool update_zoom_rate_control(uint32_t now_ms);
// request firmware version // request firmware version. now_ms is current system time
// returns true if sent so that we avoid immediately trying to also send other messages // returns true if sent so that we avoid immediately trying to also send other messages
bool request_firmware_version(); bool request_firmware_version(uint32_t now_ms);
// request status // request status. now_ms is current system time
// returns true if sent so that we avoid immediately trying to also send other messages // returns true if sent so that we avoid immediately trying to also send other messages
bool request_status(); bool request_status(uint32_t now_ms);
// check if safe to send message (if messages sent too often camera will not respond) // check if safe to send message (if messages sent too often camera will not respond)
bool is_safe_to_send() const; // now_ms is current system time
bool is_safe_to_send(uint32_t now_ms) const;
// internal variables // internal variables
bool _initialised; // true once the driver has been initialised bool _initialised; // true once the driver has been initialised