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

1236 lines
46 KiB
C++
Executable File
Raw Blame History

#include "AP_Mount_Topotek.h"
#if HAL_MOUNT_TOPOTEK_ENABLED
#include <AP_HAL/AP_HAL.h>
#include <AP_AHRS/AP_AHRS.h>
#include <GCS_MAVLink/GCS.h>
#include <AP_RTC/AP_RTC.h>
extern const AP_HAL::HAL& hal;
#define ANGULAR_VELOCITY_CONVERSION 1.220740379 // gimbal angular velocity conversion ratio
#define TRACK_TOTAL_WIDTH 1920 // track the maximum width of the image range
#define TRACK_TOTAL_HEIGHT 1080 // track the maximum height of the image range
#define TRACK_RANGE 60 // the size of the image at point tracking
#define AP_MOUNT_TOPOTEK_UPDATE_INTERVAL_MS 100 // resend angle or rate targets to gimbal at this interval
#define AP_MOUNT_TOPOTEK_HEALTH_TIMEOUT_MS 1000 // timeout for health and rangefinder readings
#define AP_MOUNT_TOPOTEK_PACKETLEN_MIN 12 // packet length not including the data segment
#define AP_MOUNT_TOPOTEK_DATALEN_MAX (AP_MOUNT_TOPOTEK_PACKETLEN_MAX - AP_MOUNT_TOPOTEK_PACKETLEN_MIN) // data segment lens can be no more tha this
// 3 character identifiers
# define AP_MOUNT_TOPOTEK_ID3CHAR_CAPTURE "CAP" // take picture, data bytes: 01:RGB + thermal, 02:RGB, 03:thermal, 05:RGB + thermal (with temp measurement)
# define AP_MOUNT_TOPOTEK_ID3CHAR_RECORD_VIDEO "REC" // record video, data bytes: 00:stop, 01:start, 0A:toggle start/stop
# define AP_MOUNT_TOPOTEK_ID3CHAR_CONTROL_ZOOM "ZMC" // control zoom, data bytes: 00:stop, 01:zoom out, 02:zoom in
# define AP_MOUNT_TOPOTEK_ID3CHAR_GET_ZOOM "ZOM" // get zoom, no data bytes
# define AP_MOUNT_TOPOTEK_ID3CHAR_CONTROL_FOCUS "FCC" // control focus, data bytes: 00:stop, 01:focus+, 02:focus-, 0x10:auto focus, 0x11:manual focus, 0x12:manu focus (save), 0x13:auto focus (save)
# define AP_MOUNT_TOPOTEK_ID3CHAR_GET_FOCUS "FOC" // get focus, no data bytes
# define AP_MOUNT_TOPOTEK_ID3CHAR_SET_ZOOM_AND_FOCU "ZFP" // set zoom and focus
# define AP_MOUNT_TOPOTEK_ID3CHAR_DAY_NIGHT_SWITCHING "IRC" // set day/night setting, data bytes: 00:day, 01:night, 0A:toggle state
# define AP_MOUNT_TOPOTEK_ID3CHAR_TRACKING "TRC" // get/set image tracking, data bytes: 00:get status (use with "r"), 01:stop (use with "w")
# define AP_MOUNT_TOPOTEK_ID3CHAR_START_TRACKING "LOC" // start image tracking
# define AP_MOUNT_TOPOTEK_ID3CHAR_LRF "LRF" // laser rangefinder control, data bytes: 00:ranging stop, 01:ranging start, 02:single measurement, 03:continuous measurement
# define AP_MOUNT_TOPOTEK_ID3CHAR_PIP "PIP" // set picture-in-picture setting, data bytes: // 00:main only, 01:main+sub, 02:sub+main, 03:sub only, 0A:next
# define AP_MOUNT_TOPOTEK_ID3CHAR_GIMBAL_ATT "GAA" // get gimbal attitude, data bytes: 00:stop, 01:start
# define AP_MOUNT_TOPOTEK_ID3CHAR_SD_CARD "SDC" // get SD card state, data bytes: 00:get remaining capacity, 01:get total capacity
# define AP_MOUNT_TOPOTEK_ID3CHAR_TIME "UTC" // set time and date, data bytes: HHMMSSDDMMYY
# define AP_MOUNT_TOPOTEK_ID3CHAR_GET_VERSION "VSN" // get firmware version, data bytes always 00
# define AP_MOUNT_TOPOTEK_ID3CHAR_GET_MODEL_NAME "PA2" // get model name, data bytes always 00
# define AP_MOUNT_TOPOTEK_ID3CHAR_GIMBAL_MODE "PTZ" // set gimbal mode, data bytes: 00:stop, 01:up, 02:down, 03:left, 04:right, 05:home position, 06:lock, 07:follow, 08:lock/follow toggle, 09:calibration, 0A:one button down
# define AP_MOUNT_TOPOTEK_ID3CHAR_YPR_RATE "YPR" // set the rate yaw, pitch and roll targets of the gimbal yaw in range -99 ~ +99
# define AP_MOUNT_TOPOTEK_ID3CHAR_YAW_ANGLE "GIY" // set the yaw angle target in the range -150 ~ 150, speed 0 ~ 99 (0.1deg/sec)
# define AP_MOUNT_TOPOTEK_ID3CHAR_YAW_ANGLE_BF "GAY" // set the yaw angle target in body-frame in the range -150 ~ 150, speed 0 ~ 99 (0.1deg/sec)
# define AP_MOUNT_TOPOTEK_ID3CHAR_PITCH_ANGLE "GIP" // set the pitch angle target in the range -90 ~ 90, speed 0 ~ 99 (0.1deg/sec)
# define AP_MOUNT_TOPOTEK_ID3CHAR_ROLL_ANGLE "GIR" // set the roll angle target in the range -90 ~ 90, speed 0 ~ 99 (0.1deg/sec)
# define AP_MOUNT_TOPOTEK_ID3CHAR_SET_LAT "LAT" // set the gimbal's latitude
# define AP_MOUNT_TOPOTEK_ID3CHAR_SET_LON "LON" // set the gimbal's longitude
# define AP_MOUNT_TOPOTEK_ID3CHAR_SET_ALT "ALT" // set the gimbal's altitude
# define AP_MOUNT_TOPOTEK_ID3CHAR_SET_AZIMUTH "AZI" // set the gimbal's yaw (aka azimuth)
#define AP_MOUNT_TOPOTEK_DEBUG 0
#define debug(fmt, args ...) do { if (AP_MOUNT_TOPOTEK_DEBUG) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Topotek: " fmt, ## args); } } while (0)
const char* AP_Mount_Topotek::send_message_prefix = "Mount: Topotek";
// update mount position - should be called periodically
void AP_Mount_Topotek::update()
{
// exit immediately if not initialised
if (!_initialised) {
return;
}
// reading incoming packets from gimbal
read_incoming_packets();
// everything below updates at 10hz
uint32_t now_ms = AP_HAL::millis();
if ((now_ms - _last_req_current_info_ms) < 100) {
return;
}
_last_req_current_info_ms = now_ms;
// re-send the stop zoom command a second time to prevent data transmission errors.
if (_last_zoom_stop) {
_last_zoom_stop = false;
send_fixedlen_packet(AddressByte::LENS, AP_MOUNT_TOPOTEK_ID3CHAR_CONTROL_ZOOM, true, 0);
}
// re-send the stop focus command a second time to prevent data transmission errors.
if (_last_focus_stop) {
_last_focus_stop = false;
send_fixedlen_packet(AddressByte::LENS, AP_MOUNT_TOPOTEK_ID3CHAR_CONTROL_FOCUS, true, 0);
}
// send GPS-related information to the gimbal
send_location_info();
// calls below here called at 1hz
_last_req_step++;
if (_last_req_step >= 10) {
_last_req_step = 0;
}
switch (_last_req_step) {
case 0:
// get gimbal version
if (!_got_gimbal_version) {
request_gimbal_version();
}
break;
case 2:
// request gimbal attitude at 1hz
// gimbal will continue to send attitude information during the next period
request_gimbal_attitude();
break;
case 4:
// request memory card information
request_gimbal_sdcard_info();
break;
case 6:
// request tracking info
request_track_status();
break;
case 8:
// get gimbal model name
if (!_got_gimbal_model_name) {
request_gimbal_model_name();
}
break;
}
// change to RC_TARGETING mode if RC input has changed
set_rctargeting_on_rcinput_change();
// handle tracking state
if (_is_tracking) {
// cancel tracking if mode has changed
if (_last_mode != _mode) {
_last_mode = _mode;
cancel_tracking();
}
return;
}
_last_mode = _mode;
// update based on mount mode
switch (get_mode()) {
// move mount to a "retracted" position. To-Do: remove support and replace with a relaxed mode?
case MAV_MOUNT_MODE_RETRACT: {
const Vector3f &angle_bf_target = _params.retract_angles.get();
mnt_target.target_type = MountTargetType::ANGLE;
mnt_target.angle_rad.set(angle_bf_target*DEG_TO_RAD, false);
break;
}
// move mount to a neutral position, typically pointing forward
case MAV_MOUNT_MODE_NEUTRAL: {
const Vector3f &angle_bf_target = _params.neutral_angles.get();
mnt_target.target_type = MountTargetType::ANGLE;
mnt_target.angle_rad.set(angle_bf_target*DEG_TO_RAD, false);
break;
}
// point to the angles given by a mavlink message
case MAV_MOUNT_MODE_MAVLINK_TARGETING:
// mavlink targets are stored while handling the incoming message
break;
// RC radio manual angle control, but with stabilization from the AHRS
case MAV_MOUNT_MODE_RC_TARGETING: {
// update targets using pilot's RC inputs
MountTarget rc_target;
get_rc_target(mnt_target.target_type, rc_target);
switch (mnt_target.target_type) {
case MountTargetType::ANGLE:
mnt_target.angle_rad = rc_target;
break;
case MountTargetType::RATE:
mnt_target.rate_rads = rc_target;
break;
}
break;
}
// point mount to a GPS point given by the mission planner
case MAV_MOUNT_MODE_GPS_POINT:
if (get_angle_target_to_roi(mnt_target.angle_rad)) {
mnt_target.target_type = MountTargetType::ANGLE;
}
break;
// point mount to Home location
case MAV_MOUNT_MODE_HOME_LOCATION:
if (get_angle_target_to_home(mnt_target.angle_rad)) {
mnt_target.target_type = MountTargetType::ANGLE;
}
break;
// point mount to another vehicle
case MAV_MOUNT_MODE_SYSID_TARGET:
if (get_angle_target_to_sysid(mnt_target.angle_rad)) {
mnt_target.target_type = MountTargetType::ANGLE;
}
break;
default:
// we do not know this mode so raise internal error
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
break;
}
// send target angles or rates depending on the target type
switch (mnt_target.target_type) {
case MountTargetType::ANGLE:
send_angle_target(mnt_target.angle_rad);
break;
case MountTargetType::RATE:
send_rate_target(mnt_target.rate_rads);
break;
}
}
// return true if healthy
bool AP_Mount_Topotek::healthy() const
{
// exit immediately if not initialised
if (!_initialised) {
return false;
}
// unhealthy if attitude information not received recently
const uint32_t last_current_angle_ms = _last_current_angle_ms;
return (AP_HAL::millis() - last_current_angle_ms < AP_MOUNT_TOPOTEK_HEALTH_TIMEOUT_MS);
}
// take a picture. returns true on success
bool AP_Mount_Topotek::take_picture()
{
// exit immediately if not initialised
if (!_initialised) {
return false;
}
// exit immediately if the memory card is abnormal
if (!_sdcard_status) {
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "%s SD card error", send_message_prefix);
return false;
}
// sample command: #TPUD2wCAP01
return send_fixedlen_packet(AddressByte::SYSTEM_AND_IMAGE, AP_MOUNT_TOPOTEK_ID3CHAR_CAPTURE, true, 1);
}
// start or stop video recording. returns true on success
// set start_recording = true to start record, false to stop recording
bool AP_Mount_Topotek::record_video(bool start_recording)
{
// exit immediately if not initialised
if (!_initialised) {
return false;
}
// exit immediately if the memory card is abnormal
if (!_sdcard_status) {
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "%s SD card error", send_message_prefix);
return false;
}
// sample command: #TPUD2wREC01
return send_fixedlen_packet(AddressByte::SYSTEM_AND_IMAGE, AP_MOUNT_TOPOTEK_ID3CHAR_RECORD_VIDEO, true, start_recording ? 1 : 0);
}
// set zoom specified as a rate
bool AP_Mount_Topotek::set_zoom(ZoomType zoom_type, float zoom_value)
{
// exit immediately if not initialised
if (!_initialised) {
return false;
}
// zoom rate
if (zoom_type == ZoomType::RATE) {
uint8_t zoom_cmd;
if (is_zero(zoom_value)) {
// stop zoom
zoom_cmd = 0;
_last_zoom_stop = true;
} else if (zoom_value < 0) {
// zoom out
zoom_cmd = 1;
} else {
// zoom in
zoom_cmd = 2;
}
// sample command: #TPUM2wZMC00
return send_fixedlen_packet(AddressByte::LENS, AP_MOUNT_TOPOTEK_ID3CHAR_CONTROL_ZOOM, true, zoom_cmd);
}
// unsupported zoom type
return false;
}
// set focus specified as rate, percentage or auto
// focus in = -1, focus hold = 0, focus out = 1
SetFocusResult AP_Mount_Topotek::set_focus(FocusType focus_type, float focus_value)
{
// exit immediately if not initialised
if (!_initialised) {
return SetFocusResult::FAILED;
}
switch (focus_type) {
case FocusType::RATE: {
// focus stop
uint8_t focus_cmd;
if (is_zero(focus_value)) {
focus_cmd = 0;
_last_focus_stop = true;
} else if (focus_value < 0) {
// focus-
focus_cmd = 2;
} else {
// focus+
focus_cmd = 1;
}
// send focus command and switch to manual focus
// sample command: #TPUM2wFCC00
if (send_fixedlen_packet(AddressByte::LENS, AP_MOUNT_TOPOTEK_ID3CHAR_CONTROL_FOCUS, true, focus_cmd) &&
send_fixedlen_packet(AddressByte::LENS, AP_MOUNT_TOPOTEK_ID3CHAR_CONTROL_FOCUS, true, 0x11)) {
return SetFocusResult::ACCEPTED;
}
return SetFocusResult::FAILED;
}
case FocusType::PCT:
// not supported
return SetFocusResult::INVALID_PARAMETERS;
case FocusType::AUTO:
// auto focus
if (send_fixedlen_packet(AddressByte::LENS, AP_MOUNT_TOPOTEK_ID3CHAR_CONTROL_FOCUS, true, 0x10)) {
return SetFocusResult::ACCEPTED;
}
return SetFocusResult::FAILED;
}
// unsupported focus type
return SetFocusResult::INVALID_PARAMETERS;
}
// set tracking to none, point or rectangle (see TrackingType enum)
// if POINT only p1 is used, if RECTANGLE then p1 is top-left, p2 is bottom-right
// p1,p2 are in range 0 to 1. 0 is left or top, 1 is right or bottom
bool AP_Mount_Topotek::set_tracking(TrackingType tracking_type, const Vector2f& p1, const Vector2f& p2)
{
// exit immediately if not initialised
if (!_initialised) {
return false;
}
// local variables holding tracker center and width
int16_t track_center_x, track_center_y, track_width, track_height;
bool send_tracking_cmd = false;
switch (tracking_type) {
case TrackingType::TRK_NONE:
return cancel_tracking();
case TrackingType::TRK_POINT: {
// calculate tracking center, width and height
track_center_x = (int16_t)((p1.x*TRACK_TOTAL_WIDTH - 960) / 0.96);
track_center_y = (int16_t)((p1.y*TRACK_TOTAL_HEIGHT - 540) / 0.54);
track_width = (int16_t)(TRACK_RANGE / 0.96);
track_height = (int16_t)(TRACK_RANGE / 0.54);
send_tracking_cmd = true;
break;
}
case TrackingType::TRK_RECTANGLE:
// calculate upper left and bottom right points
// handle case where p1 and p2 are in an unexpected order
int16_t upper_leftx = (int16_t)(MIN(p1.x, p2.x)*TRACK_TOTAL_WIDTH);
int16_t upper_lefty = (int16_t)(MIN(p1.y, p2.y)*TRACK_TOTAL_HEIGHT);
int16_t bottom_rightx = (int16_t)(MAX(p1.x, p2.x)*TRACK_TOTAL_WIDTH);
int16_t bottom_righty = (int16_t)(MAX(p1.y, p2.y)*TRACK_TOTAL_HEIGHT);
// calculated width and height and sanity check
const int16_t frame_selection_width = bottom_rightx - upper_leftx;
const int16_t frame_selection_height = bottom_righty - upper_lefty;
if (frame_selection_width <= 0 || frame_selection_height <= 0) {
return false;
}
// calculate tracking center
track_center_x = (int16_t)((((upper_leftx + bottom_rightx) * 0.5) - 960) / 0.96);
track_center_y = (int16_t)((((upper_lefty + bottom_righty) * 0.5) - 540) / 0.54);
// tracking range after conversion
track_width = (int16_t)(frame_selection_width / 0.96);
track_height = (int16_t)(frame_selection_height / 0.54);
send_tracking_cmd = true;
break;
}
if (send_tracking_cmd) {
// set the gimbal to the ready-to-track state when the gimbal tracking status is stopped
if (_last_tracking_state == TrackingStatus::STOPPED_TRACKING) {
send_fixedlen_packet(AddressByte::SYSTEM_AND_IMAGE, AP_MOUNT_TOPOTEK_ID3CHAR_TRACKING, true, 2);
}
// prepare data bytes
uint8_t databuff[10];
databuff[0] = HIGHBYTE(track_center_x);
databuff[1] = LOWBYTE(track_center_x);
databuff[2] = HIGHBYTE(track_center_y);
databuff[3] = LOWBYTE(track_center_y);
databuff[4] = HIGHBYTE(track_width);
databuff[5] = LOWBYTE(track_width);
databuff[6] = HIGHBYTE(track_height);
databuff[7] = LOWBYTE(track_height);
databuff[8] = 0;
databuff[9] = (tracking_type == TrackingType::TRK_POINT) ? 9 : 1; // when tracking point, enable fuzzy click function
// send tracking command
bool res = send_variablelen_packet(HeaderType::VARIABLE_LEN,
AddressByte::SYSTEM_AND_IMAGE,
AP_MOUNT_TOPOTEK_ID3CHAR_START_TRACKING,
true,
(uint8_t*)databuff, ARRAY_SIZE(databuff));
// display error message on failure
if (!res) {
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s tracking failed", send_message_prefix);
}
return res;
}
// should never reach here
return false;
}
// send command to gimbal to cancel tracking (if necessary)
// returns true on success, false on failure to send message
bool AP_Mount_Topotek::cancel_tracking()
{
// exit immediately if not initialised
if (!_initialised) {
return false;
}
// if gimbal is tracking-in-progress change to waiting state, otherwise stop
const uint8_t track_set = _last_tracking_state == TrackingStatus::TRACKING_IN_PROGRESS ? 1 : 0;
// send tracking command
return send_fixedlen_packet(AddressByte::SYSTEM_AND_IMAGE, AP_MOUNT_TOPOTEK_ID3CHAR_TRACKING, true, track_set);
}
// set camera picture-in-picture mode
bool AP_Mount_Topotek::set_lens(uint8_t lens)
{
// exit immediately if not initialised
if (!_initialised) {
return false;
}
// sanity check lens number
// 00:main only, 01:main+sub, 02:sub+main, 03:sub only, 0A:next
// sample command: #TPUD2wPIP0A
if (lens > 3) {
return false;
}
// send pip command
return send_fixedlen_packet(AddressByte::SYSTEM_AND_IMAGE, AP_MOUNT_TOPOTEK_ID3CHAR_PIP, true, lens);
}
// set_camera_source is functionally the same as set_lens except primary and secondary lenses are specified by type
// primary and secondary sources use the AP_Camera::CameraSource enum cast to uint8_t
bool AP_Mount_Topotek::set_camera_source(uint8_t primary_source, uint8_t secondary_source)
{
// exit immediately if not initialised
if (!_initialised) {
return false;
}
// maps primary and secondary source to pip setting
// pip settings 00:main only, 01:main+sub, 02:sub+main, 03:sub only, 0A:next
// sample command: #TPUD2wPIP0A
uint8_t pip_setting = 0;
switch (primary_source) {
case 0: // Default (RGB)
FALLTHROUGH;
case 1: // RGB
switch (secondary_source) {
case 0: // RGB + Default (None)
pip_setting = 0; // main only
break;
case 2: // PIP RGB+IR
pip_setting = 1; // main+sub
break;
default:
return false;
}
break;
case 2: // IR
switch (secondary_source) {
case 0: // IR + Default (None)
pip_setting = 3; // sub only
break;
case 1: // IR+RGB
pip_setting = 2; // sub+main
break;
default:
return false;
}
break;
default:
return false;
}
// send pip command
return send_fixedlen_packet(AddressByte::SYSTEM_AND_IMAGE, AP_MOUNT_TOPOTEK_ID3CHAR_PIP, true, pip_setting);
}
// send camera information message to GCS
void AP_Mount_Topotek::send_camera_information(mavlink_channel_t chan) const
{
// exit immediately if not initialised
if (!_initialised) {
return;
}
static const uint8_t vendor_name[32] = "Topotek";
static uint8_t model_name[32] {};
const char cam_definition_uri[140] {};
// copy model name if available
if (_got_gimbal_model_name) {
strncpy((char*)model_name, (const char*)_model_name, ARRAY_SIZE(model_name));
}
// capability flags
const uint32_t flags = CAMERA_CAP_FLAGS_CAPTURE_VIDEO |
CAMERA_CAP_FLAGS_CAPTURE_IMAGE |
CAMERA_CAP_FLAGS_HAS_BASIC_ZOOM |
CAMERA_CAP_FLAGS_HAS_BASIC_FOCUS |
CAMERA_CAP_FLAGS_HAS_TRACKING_POINT |
CAMERA_CAP_FLAGS_HAS_TRACKING_RECTANGLE;
// send CAMERA_INFORMATION message
mavlink_msg_camera_information_send(
chan,
AP_HAL::millis(), // time_boot_ms
vendor_name, // vendor_name uint8_t[32]
model_name, // model_name uint8_t[32]
_firmware_ver, // firmware version uint32_t
0, // focal_length float (mm)
0, // sensor_size_h float (mm)
0, // sensor_size_v float (mm)
0, // resolution_h uint16_t (pix)
0, // resolution_v uint16_t (pix)
0, // lens_id uint8_t
flags, // flags uint32_t (CAMERA_CAP_FLAGS)
0, // cam_definition_version uint16_t
cam_definition_uri, // cam_definition_uri char[140]
_instance + 1); // gimbal_device_id uint8_t
}
// send camera settings message to GCS
void AP_Mount_Topotek::send_camera_settings(mavlink_channel_t chan) const
{
// exit immediately if not initialised
if (!_initialised) {
return;
}
const float NaN = nanf("0x4152");
// send CAMERA_SETTINGS message
mavlink_msg_camera_settings_send(
chan,
AP_HAL::millis(), // time_boot_ms
_recording ? CAMERA_MODE_VIDEO : CAMERA_MODE_IMAGE, // camera mode (0:image, 1:video, 2:image survey)
NaN, // zoomLevel float, percentage from 0 to 100, NaN if unknown
NaN); // focusLevel float, percentage from 0 to 100, NaN if unknown
}
// get rangefinder distance. Returns true on success
bool AP_Mount_Topotek::get_rangefinder_distance(float& distance_m) const
{
// if not healthy or negative distance return false
// healthy() checks attitude timeout which is in same message as rangefinder distance
if (!healthy() || (_measure_dist_m < 0)) {
return false;
}
distance_m = _measure_dist_m;
return true;
}
// enable/disable rangefinder. Returns true on success
bool AP_Mount_Topotek::set_rangefinder_enable(bool enable)
{
// exit immediately if not initialised
if (!_initialised) {
return false;
}
// 00:ranging stop, 01:ranging start, 02:single measurement, 03:continuous measurement
// sample command: #TPUM2wLRF00
return send_fixedlen_packet(AddressByte::LENS, AP_MOUNT_TOPOTEK_ID3CHAR_LRF, true, enable ? 3 : 0);
}
// get attitude as a quaternion. returns true on success
bool AP_Mount_Topotek::get_attitude_quaternion(Quaternion& att_quat)
{
att_quat.from_euler(_current_angle_rad.x, _current_angle_rad.y, _current_angle_rad.z);
return true;
}
// reading incoming packets from gimbal and confirm they are of the correct format
void AP_Mount_Topotek::read_incoming_packets()
{
// check for bytes on the serial port
int16_t nbytes = MIN(_uart->available(), 1024U);
if (nbytes <= 0 ) {
return;
}
// flag to allow cases below to reset parser state
bool reset_parser = false;
// process bytes received
for (int16_t i = 0; i < nbytes; i++) {
uint8_t b;
if (!_uart->read(b)) {
continue;
}
// add latest byte to buffer
_msg_buff[_msg_buff_len++] = b;
// protect against overly long messages
if (_msg_buff_len >= AP_MOUNT_TOPOTEK_PACKETLEN_MAX) {
reset_parser = true;
}
// process byte depending upon current state
switch (_parser.state) {
case ParseState::WAITING_FOR_HEADER1:
if (b == '#') {
_parser.state = ParseState::WAITING_FOR_HEADER2;
break;
}
reset_parser = true;
break;
case ParseState::WAITING_FOR_HEADER2:
if (b == 't' || b == 'T') {
_parser.state = ParseState::WAITING_FOR_HEADER3;
break;
}
reset_parser = true;
break;
case ParseState::WAITING_FOR_HEADER3:
if (b == 'p' || b == 'P') {
_parser.state = ParseState::WAITING_FOR_ADDR1;
break;
}
reset_parser = true;
break;
case ParseState::WAITING_FOR_ADDR1:
case ParseState::WAITING_FOR_ADDR2:
if (b == 'U' || b =='M' || b == 'D' || b =='E' || b =='P' || b =='G') {
// advance to next state
_parser.state = (ParseState)((uint8_t)_parser.state+1);
break;
}
reset_parser = true;
break;
case ParseState::WAITING_FOR_DATALEN:
// sanity check data length
_parser.data_len = (uint8_t)char_to_hex(b);
if (_parser.data_len <= AP_MOUNT_TOPOTEK_DATALEN_MAX) {
_parser.state = ParseState::WAITING_FOR_CONTROL;
break;
}
reset_parser = true;
break;
case ParseState::WAITING_FOR_CONTROL:
// r or w
if (b == 'r' || b == 'w') {
_parser.state = ParseState::WAITING_FOR_ID1;
break;
}
reset_parser = true;
break;
case ParseState::WAITING_FOR_ID1:
case ParseState::WAITING_FOR_ID2:
case ParseState::WAITING_FOR_ID3:
// check all uppercase letters and numbers. eg 'GAC'
if ((b >= 'A' && b <= 'Z') || (b >= '0' && b <= '9')) {
// advance to next state
_parser.state = (ParseState)((uint8_t)_parser.state+1);
break;
}
reset_parser = true;
break;
case ParseState::WAITING_FOR_DATA: {
// normally hex numbers in char form (e.g. '0A')
const uint8_t data_bytes_received = _msg_buff_len - (AP_MOUNT_TOPOTEK_PACKETLEN_MIN - 2);
// sanity check to protect against programming errors
if (data_bytes_received > AP_MOUNT_TOPOTEK_DATALEN_MAX) {
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
reset_parser = true;
break;
}
// advance parser state once expected number of bytes have been received
if (data_bytes_received == _parser.data_len) {
_parser.state = ParseState::WAITING_FOR_CRC_LOW;
}
break;
}
case ParseState::WAITING_FOR_CRC_LOW:
_parser.state = ParseState::WAITING_FOR_CRC_HIGH;
break;
case ParseState::WAITING_FOR_CRC_HIGH:
// this is the last byte in the message so reset the parser
reset_parser = true;
// sanity check to protect against programming errors
if (_msg_buff_len < AP_MOUNT_TOPOTEK_PACKETLEN_MIN) {
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
break;
}
// calculate and check CRC
const uint8_t crc_value = calculate_crc(_msg_buff, _msg_buff_len - 2);
const char crc_char1 = hex2char((crc_value >> 4) & 0x0f);
const char crc_char2 = hex2char((crc_value) & 0x0f);
if (crc_char1 != _msg_buff[_msg_buff_len - 2] || crc_char2 != _msg_buff[_msg_buff_len-1]) {
debug("CRC expected:%x got:%c%c", (int)crc_value, crc_char1, crc_char2);
break;
}
// CRC is OK, call function to process the message
for (uint8_t count = 0; count < AP_MOUNT_RECV_GIMBAL_CMD_CATEGORIES_NUM; count++) {
if (strncmp((const char*)_msg_buff + 7, (const char*)(uart_recv_cmd_compare_list[count].uart_cmd_key), 3) == 0) {
(this->*(uart_recv_cmd_compare_list[count].func))();
break;
}
}
}
// handle reset of parser
if (reset_parser) {
_parser.state = ParseState::WAITING_FOR_HEADER1;
_msg_buff_len = 0;
reset_parser = false;
}
}
}
// request gimbal attitude
void AP_Mount_Topotek::request_gimbal_attitude()
{
// sample command: #TPUG2wGAA01
send_fixedlen_packet(AddressByte::GIMBAL, AP_MOUNT_TOPOTEK_ID3CHAR_GIMBAL_ATT, true, 1);
}
// request gimbal memory card information
void AP_Mount_Topotek::request_gimbal_sdcard_info()
{
// request remaining capacity
// sample command including CRC: #TPUD2rSDC003E
// 00:get remaining capacity, 01:get total capacity
send_fixedlen_packet(AddressByte::SYSTEM_AND_IMAGE, AP_MOUNT_TOPOTEK_ID3CHAR_SD_CARD, false, 0);
}
// request gimbal tracking status
void AP_Mount_Topotek::request_track_status()
{
// 00:get status (use with "r"), 01:stop (use with "w")
// sample command: #TPUD2rTRC00
send_fixedlen_packet(AddressByte::SYSTEM_AND_IMAGE, AP_MOUNT_TOPOTEK_ID3CHAR_TRACKING, false, 0);
}
// request gimbal version
void AP_Mount_Topotek::request_gimbal_version()
{
// sample command: #TPUD2rVSN00
send_fixedlen_packet(AddressByte::SYSTEM_AND_IMAGE, AP_MOUNT_TOPOTEK_ID3CHAR_GET_VERSION, false, 0);
}
// request gimbal model name
void AP_Mount_Topotek::request_gimbal_model_name()
{
// sample command: #TPUG2rPA200
send_fixedlen_packet(AddressByte::GIMBAL, AP_MOUNT_TOPOTEK_ID3CHAR_GET_MODEL_NAME, false, 0);
}
// send angle target in radians to gimbal
void AP_Mount_Topotek::send_angle_target(const MountTarget& angle_rad)
{
// gimbal's earth-frame angle control drifts so always use body frame
// set gimbal's lock state if it has changed
if (!set_gimbal_lock(false)) {
return;
}
// calculate and send yaw target
// sample command #tpUG6wGIY
const char* format_str = "%04x%02x";
const uint8_t speed = 99;
const uint16_t yaw_angle_cd = (uint16_t)constrain_int16(degrees(angle_rad.get_bf_yaw()) * 100, MAX(-18000, _params.yaw_angle_min * 100), MIN(18000, _params.yaw_angle_max * 100));
uint8_t databuff[7];
hal.util->snprintf((char *)databuff, ARRAY_SIZE(databuff), format_str, yaw_angle_cd, speed);
send_variablelen_packet(HeaderType::VARIABLE_LEN,
AddressByte::GIMBAL,
AP_MOUNT_TOPOTEK_ID3CHAR_YAW_ANGLE_BF,
true,
(uint8_t*)databuff, ARRAY_SIZE(databuff)-1);
// send pitch target
// sample command: #tpUG6wGIP
const uint16_t pitch_angle_cd = (uint16_t)constrain_int16(-degrees(angle_rad.pitch) * 100, -9000, 9000);
hal.util->snprintf((char *)databuff, ARRAY_SIZE(databuff), format_str, pitch_angle_cd, speed);
send_variablelen_packet(HeaderType::VARIABLE_LEN,
AddressByte::GIMBAL,
AP_MOUNT_TOPOTEK_ID3CHAR_PITCH_ANGLE,
true,
(uint8_t*)databuff, ARRAY_SIZE(databuff)-1);
// send roll target
// sample command: #tpUG6wGIR
const uint16_t roll_angle_cd = (uint16_t)constrain_int16(degrees(angle_rad.roll) * 100, -18000, 18000);
hal.util->snprintf((char *)databuff, ARRAY_SIZE(databuff), format_str, roll_angle_cd, speed);
send_variablelen_packet(HeaderType::VARIABLE_LEN,
AddressByte::GIMBAL,
AP_MOUNT_TOPOTEK_ID3CHAR_ROLL_ANGLE,
true,
(uint8_t*)databuff, ARRAY_SIZE(databuff)-1);
}
// send rate target in rad/s to gimbal
void AP_Mount_Topotek::send_rate_target(const MountTarget& rate_rads)
{
// set gimbal's lock state if it has changed
if (!set_gimbal_lock(rate_rads.yaw_is_ef)) {
return;
}
// convert and constrain rates
const uint8_t roll_angle_speed = constrain_int16(degrees(rate_rads.roll) * ANGULAR_VELOCITY_CONVERSION, -99, 99);
const uint8_t pitch_angle_speed = constrain_int16(degrees(rate_rads.pitch) * ANGULAR_VELOCITY_CONVERSION, -99, 99);
const uint8_t yaw_angle_speed = constrain_int16(degrees(rate_rads.yaw) * ANGULAR_VELOCITY_CONVERSION, -99, 99);
// send stop rotation command three times if target roll, pitch and yaw are zero
if (roll_angle_speed == 0 && pitch_angle_speed == 0 && yaw_angle_speed == 0) {
if (_stop_order_count < 3) {
// sample command: #TPUG2wPTZ00
if (send_fixedlen_packet(AddressByte::GIMBAL, AP_MOUNT_TOPOTEK_ID3CHAR_GIMBAL_MODE, true, 0)) {
_stop_order_count++;
}
}
return;
}
_stop_order_count = 0;
// prepare and send command
// sample command: #tpUG6wYPR
uint8_t databuff[7];
hal.util->snprintf((char *)databuff, ARRAY_SIZE(databuff), "%02x%02x%02x", yaw_angle_speed, pitch_angle_speed, roll_angle_speed);
send_variablelen_packet(HeaderType::VARIABLE_LEN, AddressByte::GIMBAL, AP_MOUNT_TOPOTEK_ID3CHAR_YPR_RATE, true, databuff, ARRAY_SIZE(databuff)-1);
}
// send time and date to gimbal
bool AP_Mount_Topotek::send_time_to_gimbal()
{
#if AP_RTC_ENABLED
// get date and time
// year is the regular Gregorian year, month is 0~11, day is 1~31, hour is 0~23, minute is 0~59, second is 0~60 (1 leap second), ms is 0~999
uint16_t year, ms;
uint8_t month, day, hour, min, sec;
if (!AP::rtc().get_date_and_time_utc(year, month, day, hour, min, sec, ms)) {
return false;
}
// sample command: #tpUDCwUTCHHMMSSDDMMYY
uint8_t databuff[13];
hal.util->snprintf((char*)databuff, ARRAY_SIZE(databuff), "%02d%02d%02d%02d%02d%02d", hour, min, sec, day, month + 1, year - 2000);
return send_variablelen_packet(HeaderType::VARIABLE_LEN, AddressByte::SYSTEM_AND_IMAGE, AP_MOUNT_TOPOTEK_ID3CHAR_TIME, true, (uint8_t*)databuff, ARRAY_SIZE(databuff)-1);
#else
return false;
#endif
}
// send GPS-related information to the gimbal
bool AP_Mount_Topotek::send_location_info()
{
// get current location
Location loc;
int32_t alt_amsl_cm = 0;
if (!AP::ahrs().get_location(loc) || !loc.get_alt_cm(Location::AltFrame::ABSOLUTE, alt_amsl_cm)) {
return false;
}
// convert latitude and longitude to positive angles in degrees
const double latitude = labs(loc.lat) * 1e-7;
const double longitude = labs(loc.lng) * 1e-7;
// get the degree part
const int16_t lat_deg = (int16_t)latitude;
const int16_t lng_deg = (int16_t)longitude;
// get the minute part
const double lat_min = (latitude - lat_deg) * 60.0;
const double lng_min = (longitude - lng_deg) * 60.0;
// prepare and send latitude
// first byte is N or S, followed by GPS coordinates in degree division format, in the format of ddmm.mmmm
// first byte is zero and will also be transmitted. same as the data format in $GPGGA
// sample command: #tpUDAwLATNddmm.mmmm
uint8_t databuff_lat[11];
hal.util->snprintf((char*)databuff_lat, ARRAY_SIZE(databuff_lat), "%c%02d%07.4f", loc.lat > 0 ? 'N':'S', lat_deg, lat_min);
if (!send_variablelen_packet(HeaderType::VARIABLE_LEN, AddressByte::SYSTEM_AND_IMAGE, AP_MOUNT_TOPOTEK_ID3CHAR_SET_LAT, true, (uint8_t*)databuff_lat, ARRAY_SIZE(databuff_lat)-1)) {
return false;
}
// prepare and send longitude
// sample command: #tpUDBwLONEdddmm.mmmm
uint8_t databuff_lon[12];
hal.util->snprintf((char*)databuff_lon, ARRAY_SIZE(databuff_lon), "%c%03d%07.4f", loc.lng > 0 ? 'E':'W', lng_deg, lng_min);
if (!send_variablelen_packet(HeaderType::VARIABLE_LEN, AddressByte::SYSTEM_AND_IMAGE, AP_MOUNT_TOPOTEK_ID3CHAR_SET_LON, true, (uint8_t*)databuff_lon, ARRAY_SIZE(databuff_lon)-1)) {
return false;
}
// get the height in meters
float alt_amsl_m = alt_amsl_cm * 0.01;
// prepare and send vehicle altitude
// sample command: #tpUD8wALT000000.0, similar format to $GPGGA
uint8_t databuff_alt[9];
hal.util->snprintf((char*)databuff_alt, ARRAY_SIZE(databuff_alt), "%08.1f", alt_amsl_m);
if (!send_variablelen_packet(HeaderType::VARIABLE_LEN, AddressByte::SYSTEM_AND_IMAGE, AP_MOUNT_TOPOTEK_ID3CHAR_SET_ALT, true, (uint8_t*)databuff_alt, ARRAY_SIZE(databuff_alt)-1)) {
return false;
}
// prepare and send vehicle yaw
// sample command: #tpUD5wAZI359.9, similar format to $GPRMC
const float veh_yaw_deg = wrap_360(degrees(AP::ahrs().get_yaw()));
uint8_t databuff_azimuth[6];
hal.util->snprintf((char*)databuff_azimuth, ARRAY_SIZE(databuff_azimuth), "%05.1f", veh_yaw_deg);
if (!send_variablelen_packet(HeaderType::VARIABLE_LEN, AddressByte::SYSTEM_AND_IMAGE, AP_MOUNT_TOPOTEK_ID3CHAR_SET_AZIMUTH, true, (uint8_t*)databuff_azimuth, ARRAY_SIZE(databuff_azimuth)-1)) {
return false;
}
return true;
}
// attitude information analysis of gimbal
void AP_Mount_Topotek::gimbal_angle_analyse()
{
// consume current angles
int16_t yaw_angle_cd = wrap_180_cd(hexchar4_to_int16(_msg_buff[10], _msg_buff[11], _msg_buff[12], _msg_buff[13]));
int16_t pitch_angle_cd = -hexchar4_to_int16(_msg_buff[14], _msg_buff[15], _msg_buff[16], _msg_buff[17]);
int16_t roll_angle_cd = hexchar4_to_int16(_msg_buff[18], _msg_buff[19], _msg_buff[20], _msg_buff[21]);
// convert cd to radians
_current_angle_rad.x = radians(roll_angle_cd * 0.01);
_current_angle_rad.y = radians(pitch_angle_cd * 0.01);
_current_angle_rad.z = radians(yaw_angle_cd * 0.01);
_last_current_angle_ms = AP_HAL::millis();
return;
}
// gimbal video information analysis
void AP_Mount_Topotek::gimbal_record_analyse()
{
_recording = (_msg_buff[10] == '1' || _msg_buff[11] == '1');
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s recording %s", send_message_prefix, _recording ? "ON" : "OFF");
}
// information analysis of gimbal storage card
void AP_Mount_Topotek::gimbal_sdcard_analyse()
{
if (('N' == _msg_buff[10]) && ('N' == _msg_buff[11]) && ('N' == _msg_buff[12]) && ('N' == _msg_buff[13])) {
// memory card exception
_sdcard_status = false;
return;
}
_sdcard_status = true;
// send UTC time to the camera
if (_sent_time_count < 7) {
if (send_time_to_gimbal()) {
_sent_time_count++;
}
}
return;
}
// gimbal tracking information analysis
void AP_Mount_Topotek::gimbal_track_analyse()
{
// ignore tracking state if unchanged
TrackingStatus tracking_state = (TrackingStatus)_msg_buff[11];
if (tracking_state == _last_tracking_state) {
return;
}
_last_tracking_state = tracking_state;
// inform user
const char* tracking_str = "tracking";
switch (tracking_state) {
case TrackingStatus::STOPPED_TRACKING:
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s %s stopped", send_message_prefix, tracking_str);
_is_tracking = false;
break;
case TrackingStatus::WAITING_FOR_TRACKING:
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s %s waiting", send_message_prefix, tracking_str);
_is_tracking = false;
break;
case TrackingStatus::TRACKING_IN_PROGRESS:
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s %s started", send_message_prefix, tracking_str);
_is_tracking = true;
break;
}
}
// gimbal distance information analysis
void AP_Mount_Topotek::gimbal_dist_info_analyse()
{
if ('E' == _msg_buff[10] && 'R' == _msg_buff[11] && 'R' ==_msg_buff[12]) {
_measure_dist_m = -1.0f;
return;
}
// distance is in meters in the format, "12345.6" where each digit is in decimal
_measure_dist_m = char_to_hex(_msg_buff[10]) * 10000.0 +
char_to_hex(_msg_buff[11]) * 1000.0 +
char_to_hex(_msg_buff[12]) * 100.0 +
char_to_hex(_msg_buff[13]) * 10.0 +
char_to_hex(_msg_buff[14]) +
char_to_hex(_msg_buff[16]) * 0.1;
}
// gimbal basic information analysis
void AP_Mount_Topotek::gimbal_version_analyse()
{
// version array with index 0=major, 1=minor, 2=patch
uint8_t version[3] {};
// extract firmware version
// the version can be in the format "1.2.3" or "123"
const uint8_t data_buf_len = char_to_hex(_msg_buff[5]);
// check for "."
bool contains_period = false;
for (uint8_t i = 0; i < data_buf_len; i++) {
contains_period |= _msg_buff[10 + i] == '.';
}
// if contains period, extract version number
uint32_t ver_num = 0;
uint8_t ver_count = 0;
if (contains_period) {
for (uint8_t i = 0; i < data_buf_len; i++) {
if (_msg_buff[10 + i] != '.') {
ver_num = ver_num * 10 + char_to_hex(_msg_buff[10 + i]);
} else {
version[ver_count++] = ver_num;
ver_num = 0;
}
if (ver_count >= ARRAY_SIZE(version)) {
break;
}
}
} else {
if (data_buf_len >= 1) {
version[0] = char_to_hex(_msg_buff[10]);
}
if (data_buf_len >= 2) {
version[1] = char_to_hex(_msg_buff[11]);
}
if (data_buf_len >= 3) {
version[2] = char_to_hex(_msg_buff[12]);
}
}
_firmware_ver = (version[2] << 16) | (version[1] << 8) | (version[0]);
// display gimbal model and firmware version to user
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s v%u.%u.%u",
send_message_prefix,
version[0], // major version
version[1], // minor version
version[2]); // patch version
_got_gimbal_version = true;
}
// gimbal model name message analysis
void AP_Mount_Topotek::gimbal_model_name_analyse()
{
strncpy((char *)_model_name, (const char *)_msg_buff + 10, char_to_hex(_msg_buff[5]));
// display gimbal model name to user
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s %s", send_message_prefix, _model_name);
_got_gimbal_model_name = true;
}
// calculate checksum
uint8_t AP_Mount_Topotek::calculate_crc(const uint8_t *cmd, uint8_t len) const
{
uint8_t crc = 0;
for (uint16_t i = 0; i<len; i++) {
crc += cmd[i];
}
return(crc);
}
// hexadecimal to character conversion
uint8_t AP_Mount_Topotek::hex2char(uint8_t data) const
{
if ((9 >= data)) {
return (data + '0');
} else {
return (data - 10 + 'A');
}
}
// convert a 4 character hex number to an integer
// the characters are in the format "1234" where the most significant digit is first
int16_t AP_Mount_Topotek::hexchar4_to_int16(char high, char mid_high, char mid_low, char low) const
{
const int16_t value = (char_to_hex(high) << 12) |
(char_to_hex(mid_high) << 8) |
(char_to_hex(mid_low) << 4) |
(char_to_hex(low));
return value;
}
// send a fixed length packet
bool AP_Mount_Topotek::send_fixedlen_packet(AddressByte address, const Identifier id, bool write, uint8_t value)
{
uint8_t databuff[3];
hal.util->snprintf((char *)databuff, ARRAY_SIZE(databuff), "%02x", value);
return send_variablelen_packet(HeaderType::FIXED_LEN, address, id, write, databuff, ARRAY_SIZE(databuff)-1);
}
// send variable length packet
bool AP_Mount_Topotek::send_variablelen_packet(HeaderType header, AddressByte address, const Identifier id, bool write, const uint8_t* databuff, uint8_t databuff_len)
{
// exit immediately if not initialised
if (!_initialised) {
return false;
}
// calculate and sanity check packet size
const uint16_t packet_size = AP_MOUNT_TOPOTEK_PACKETLEN_MIN + databuff_len;
if (packet_size > AP_MOUNT_TOPOTEK_PACKETLEN_MAX) {
debug("send_packet data buff too large");
return false;
}
// check for sufficient space in outgoing buffer
if (_uart->txspace() < packet_size) {
debug("tx buffer full");
return false;
}
// create buffer for holding outgoing packet
uint8_t send_buff[packet_size];
uint8_t send_buff_ofs = 0;
// packet header (bytes 0 ~ 2)
send_buff[send_buff_ofs++] = '#';
send_buff[send_buff_ofs++] = (header == HeaderType::FIXED_LEN) ? 'T' : 't';
send_buff[send_buff_ofs++] = (header == HeaderType::FIXED_LEN) ? 'P' : 'p';
// address (bytes 3, 4)
send_buff[send_buff_ofs++] = (uint8_t)AddressByte::UART;
send_buff[send_buff_ofs++] = (uint8_t)address;
// data length (byte 5)
send_buff[send_buff_ofs++] = hex2char(databuff_len);
// control byte (byte 6)
send_buff[send_buff_ofs++] = write ? (uint8_t)ControlByte::WRITE : (uint8_t)ControlByte::READ;
// identified (bytes 7 ~ 9)
send_buff[send_buff_ofs++] = id[0];
send_buff[send_buff_ofs++] = id[1];
send_buff[send_buff_ofs++] = id[2];
// data
if (databuff_len != 0) {
memcpy(&send_buff[send_buff_ofs], databuff, databuff_len);
send_buff_ofs += databuff_len;
}
// crc
uint8_t crc = calculate_crc(send_buff, send_buff_ofs);
send_buff[send_buff_ofs++] = hex2char((crc >> 4) & 0x0f);
send_buff[send_buff_ofs++] = hex2char(crc & 0x0f);
// send packet
_uart->write(send_buff, send_buff_ofs);
return true;
}
// set gimbal's lock vs follow mode
// lock should be true if gimbal should maintain an earth-frame target
// lock is false to follow / maintain a body-frame target
bool AP_Mount_Topotek::set_gimbal_lock(bool lock)
{
if (_last_lock == lock) {
return true;
}
// send message and update lock state
if (send_fixedlen_packet(AddressByte::GIMBAL, AP_MOUNT_TOPOTEK_ID3CHAR_GIMBAL_MODE, true, lock ? 6 : 7)) {
_last_lock = lock;
return true;
}
return false;
}
#endif // HAL_MOUNT_TOPOTEK_ENABLED
Reference in New Issue View Git Blame Copy Permalink