diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 94e7a24a1ca14e..c07a5bce409c18 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -15,6 +15,7 @@ #include "AP_Mount_Scripting.h" #include "AP_Mount_Xacti.h" #include "AP_Mount_Viewpro.h" +#include "AP_Mount_Topotek.h" #include #include #include @@ -156,6 +157,16 @@ void AP_Mount::init() serial_instance++; break; #endif // HAL_MOUNT_VIEWPRO_ENABLED + +#if HAL_MOUNT_TOPOTEK_ENABLED + // check for Topotek gimbal + case Type::Topotek: + _backends[instance] = new AP_Mount_Topotek(*this, _params[instance], instance, serial_instance); + _num_instances++; + serial_instance++; + break; +#endif // HAL_MOUNT_TOPOTEK_ENABLED + } // init new instance diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index 110267bcc4496f..bfada56e1d27a8 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -47,6 +47,8 @@ class AP_Mount_Siyi; class AP_Mount_Scripting; class AP_Mount_Xacti; class AP_Mount_Viewpro; +class AP_Mount_Topotek; + /* This is a workaround to allow the MAVLink backend access to the @@ -67,6 +69,7 @@ class AP_Mount friend class AP_Mount_Scripting; friend class AP_Mount_Xacti; friend class AP_Mount_Viewpro; + friend class AP_Mount_Topotek; public: AP_Mount(); @@ -114,6 +117,9 @@ class AP_Mount #endif #if HAL_MOUNT_VIEWPRO_ENABLED Viewpro = 11, /// Viewpro gimbal using a custom serial protocol +#endif +#if HAL_MOUNT_TOPOTEK_ENABLED + Topotek = 12, /// Topotek gimbal using a custom serial protocol #endif }; diff --git a/libraries/AP_Mount/AP_Mount_Topotek.cpp b/libraries/AP_Mount/AP_Mount_Topotek.cpp new file mode 100755 index 00000000000000..e0d8854558ec1f --- /dev/null +++ b/libraries/AP_Mount/AP_Mount_Topotek.cpp @@ -0,0 +1,1250 @@ + +#include "AP_Mount_Topotek.h" + +#if HAL_MOUNT_TOPOTEK_ENABLED + +#include +#include +#include +#include + +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_firmware) { + request_gimbal_firmware(); + } + break; + case 1: + case 6: + // request tracking info + if (_is_tracking) { + request_track_status(); + } + 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 7: + // 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) { + cancel_tracking(); + } else { + // image tracking is active so we do not send attitude targets + if (_is_tracking_info_recv) { + 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::STOP_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; + // enable the fuzzy click function of the gimbal when in point selection mode + // disable the fuzzy click function of the gimbal when in box selection mode + if (tracking_type == TrackingType::TRK_POINT) { + // enable gimbal fuzzy click function + databuff[9] = 9; + } else { + // disable gimbal fuzzy click function + databuff[9] = 1; + } + + // 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)); + + _is_tracking |= res; + 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; + } + // put the gimbal in the stop tracking state + uint8_t track_set = 0; + // when the gimbal tracking status is tracking, send a command to position the gimbal in the ready tracking state + if (_last_tracking_state == TrackingStatus::TRACKING_IN_PROGRESS) { + // put the gimbal in the ready tracking state + track_set = 1; + } else { + // when the tracking status is in the waiting tracking state, set the tracking status manually to stop, + // because at this time, the gimbal cannot receive the tracking status feedback + _last_tracking_state = TrackingStatus::STOP_TRACKING; + _is_tracking = false; + } + + // send tracking command + if (send_fixedlen_packet(AddressByte::SYSTEM_AND_IMAGE, AP_MOUNT_TOPOTEK_ID3CHAR_TRACKING, true, track_set)) { + return true; + } + return false; +} + +// 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] {}; + + if (_got_gimbal_model_name) { + strcpy((char*)model_name, (const char*)_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 firmware version number +void AP_Mount_Topotek::request_gimbal_firmware() +{ + // 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::STOP_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 waited", 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_info_recv = true; + break; + } +} + +// gimbal firmware version message analysis +void AP_Mount_Topotek::gimbal_firmware_ver_analyse() +{ + // version[0] is the major version number, version[1] is the minor version number, and version[2] is the revision version number + uint8_t version[3] = ""; + + // extract firmware version + const uint8_t data_buf_len = char_to_hex(_msg_buff[5]); + if (data_buf_len <= 7) { + 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]); + } + } else { + int count = 0; + uint8_t num = 0; + + for (int i = 0; i < data_buf_len; i++) { + if (_msg_buff[10 + i] == '.') { + version[count++] = num; + num = 0; + if (count == 3) { + break; + } + } else if (_msg_buff[10 + i] >= '0' && _msg_buff[10 + i] <= '9') { + num = num * 10 + (_msg_buff[10 + i] - '0'); + } + } + version[count] = num; + } + _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], + version[1], + version[2]); + + _got_gimbal_firmware = 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, "Mount: Topotek model name %s", _model_name); + + _got_gimbal_model_name = true; +} + +// 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; +} + +// 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= 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 diff --git a/libraries/AP_Mount/AP_Mount_Topotek.h b/libraries/AP_Mount/AP_Mount_Topotek.h new file mode 100755 index 00000000000000..ae14270c4cce15 --- /dev/null +++ b/libraries/AP_Mount/AP_Mount_Topotek.h @@ -0,0 +1,283 @@ +/* + Topotek gimbal driver using custom serial protocol + + Packet format (courtesy of Topotek's SDK document) + + ------------------------------------------------------------------------------------------- + Field Index Bytes Description + ------------------------------------------------------------------------------------------- + Frame Header 0 3 type of command + Address Bit 3 2 the source address comes first, and the destination address comes last + Data_Len 5 1 data length + Control Bit 6 1 r -> query w -> setup and control + Identification Bit 7 3 identification function + Data 10 Data_Len + Check Bit 2 the frame header is converted to HEX before reaching the check bit, + the sum is done, and the result is converted to ASC-II. Two bytes, the high one first + */ + +#pragma once + +#include "AP_Mount_Backend_Serial.h" + +#if HAL_MOUNT_TOPOTEK_ENABLED + +#include +#include +#include + +#define AP_MOUNT_TOPOTEK_PACKETLEN_MAX 36 // maximum number of bytes in a packet sent to or received from the gimbal +#define AP_MOUNT_RECV_GIMBAL_CMD_CATEGORIES_NUM 7 // parse the number of gimbal command types + +class AP_Mount_Topotek : public AP_Mount_Backend_Serial +{ + +public: + // Constructor + using AP_Mount_Backend_Serial::AP_Mount_Backend_Serial; + + // Do not allow copies + CLASS_NO_COPY(AP_Mount_Topotek); + + // update mount position - should be called periodically + void update() override; + + // return true if healthy + bool healthy() const override; + + // has_pan_control - returns true if this mount can control its pan (required for multicopters) + bool has_pan_control() const override { return yaw_range_valid(); }; + + // + // camera controls for gimbals + // + + // take a picture. returns true on success + bool take_picture() override; + + // start or stop video recording + // set start_recording = true to start record, false to stop recording + bool record_video(bool start_recording) override; + + // set zoom specified as a rate + bool set_zoom(ZoomType zoom_type, float zoom_value) override; + + // set focus specified as rate or auto + // focus in = -1, focus hold = 0, focus out = 1 + SetFocusResult set_focus(FocusType focus_type, float focus_value) override; + + // 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 set_tracking(TrackingType tracking_type, const Vector2f& p1, const Vector2f& p2) override; + + // send command to gimbal to cancel tracking (if necessary) + // returns true on success, false on failure to send message + bool cancel_tracking(); + + // set camera picture-in-picture mode + bool set_lens(uint8_t lens) override; + + // 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 set_camera_source(uint8_t primary_source, uint8_t secondary_source) override; + + // send camera information message to GCS + void send_camera_information(mavlink_channel_t chan) const override; + + // send camera settings message to GCS + void send_camera_settings(mavlink_channel_t chan) const override; + + // + // rangefinder + // + + // get rangefinder distance. Returns true on success + bool get_rangefinder_distance(float& distance_m) const override; + + // enable/disable rangefinder. Returns true on success + bool set_rangefinder_enable(bool enable) override; + +protected: + + // get attitude as a quaternion. returns true on success + bool get_attitude_quaternion(Quaternion& att_quat) override; + +private: + + // header type (fixed or variable length) + // first three bytes of packet determined by this value + enum class HeaderType : uint8_t { + FIXED_LEN = 0x00, // #TP will be sent + VARIABLE_LEN = 0x01, // #tp will be sent + }; + + // address (2nd and 3rd bytes of packet) + // first byte is always U followed by one of the other options + enum class AddressByte : uint8_t { + SYSTEM_AND_IMAGE = 68, // 'D' + AUXILIARY_EQUIPMENT = 69, // 'E' + GIMBAL = 71, // 'G' + LENS = 77, // 'M' + NETWORK = 80, // 'P' + UART = 85, // 'U' + }; + + // control byte (read or write) + // sent as 7th byte of packet + enum class ControlByte : uint8_t { + READ = 114, // 'r' + WRITE = 119, // 'w' + }; + + // parsing state + enum class ParseState : uint8_t { + WAITING_FOR_HEADER1 = 0,// # + WAITING_FOR_HEADER2, // T or t + WAITING_FOR_HEADER3, // P or p + WAITING_FOR_ADDR1, // normally U + WAITING_FOR_ADDR2, // M, D, E, P, G + WAITING_FOR_DATALEN, + WAITING_FOR_CONTROL, // r or w + WAITING_FOR_ID1, // e.g. 'G' + WAITING_FOR_ID2, // e.g. 'A' + WAITING_FOR_ID3, // e.g. 'C' + WAITING_FOR_DATA, // normally hex numbers in char form (e.g. '0A') + WAITING_FOR_CRC_LOW, + WAITING_FOR_CRC_HIGH, + }; + + // tracking status + enum class TrackingStatus : uint8_t { + STOP_TRACKING = 0x30, // not tracking + WAITING_FOR_TRACKING = 0x31, // wait to track command status + TRACKING_IN_PROGRESS = 0x32 // the status is being tracked. + }; + + // identifier bytes + typedef char Identifier[3]; + + // send text prefix string + static const char* send_message_prefix; + + // reading incoming packets from gimbal and confirm they are of the correct format + void read_incoming_packets(); + + // request gimbal attitude + void request_gimbal_attitude(); + + // request gimbal memory card information + void request_gimbal_sdcard_info(); + + // request gimbal tracking status + void request_track_status(); + + // request gimbal firmware version number + void request_gimbal_firmware(); + + // request gimbal model name. + void request_gimbal_model_name(); + + // send angle target in radians to gimbal + void send_angle_target(const MountTarget& angle_rad); + + // send rate target in rad/s to gimbal + void send_rate_target(const MountTarget& rate_rads); + + // send time and date to gimbal + bool send_time_to_gimbal(); + + // send GPS-related information to the gimbal + bool send_location_info(); + + // attitude information analysis of gimbal + void gimbal_angle_analyse(); + + // gimbal video information analysis + void gimbal_record_analyse(); + + // information analysis of gimbal storage card + void gimbal_sdcard_analyse(); + + // gimbal tracking information analysis + void gimbal_track_analyse(); + + // gimbal firmware version message analysis + void gimbal_firmware_ver_analyse(); + + // gimbal model name message analysis + void gimbal_model_name_analyse(); + + // gimbal distance information analysis + void gimbal_dist_info_analyse(); + + // calculate checksum + uint8_t calculate_crc(const uint8_t *cmd, uint8_t len) const; + + // hexadecimal to character conversion + uint8_t hex2char(uint8_t data) const; + + // 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 hexchar4_to_int16(char high, char mid_high, char mid_low, char low) const; + + // send a fixed length packet to gimbal + // returns true on success, false if serial port initialization failed + bool send_fixedlen_packet(AddressByte address, const Identifier id, bool write, uint8_t value); + + // send a variable length packet to gimbal + // returns true on success, false if serial port initialization failed + bool send_variablelen_packet(HeaderType header, AddressByte address, const Identifier id, bool write, const uint8_t* databuff, uint8_t databuff_len); + + // 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 set_gimbal_lock(bool lock); + + // members + bool _recording; // recording status (received from gimbal) + bool _is_tracking; // whether to enable the tracking state + bool _is_tracking_info_recv; // status of receiving tracking information flag + TrackingStatus _last_tracking_state = TrackingStatus::STOP_TRACKING; // last tracking state received from gimbal + uint8_t _last_mode; // mode during latest update, used to detect mode changes and cancel tracking + bool _sdcard_status; // memory card status (received from gimbal) + bool _last_lock; // last lock mode sent to gimbal + bool _got_gimbal_firmware; // true if gimbal's firmware version has been received + bool _got_gimbal_model_name; // true if gimbal's model name has been received + bool _last_zoom_stop; // true if zoom has been stopped (used to re-send in order to handle lost packets) + bool _last_focus_stop; // true if focus has been stopped (used to re-sent in order to handle lost packets) + uint8_t _model_name[16]; // gimbal type + uint8_t _sent_time_count; // count of current time messages sent to gimbal + uint32_t _firmware_ver; // firmware version + Vector3f _current_angle_rad; // current angles in radians received from gimbal (x=roll, y=pitch, z=yaw) + uint32_t _last_current_angle_ms; // system time (in milliseconds) that angle information received from the gimbal + uint32_t _last_req_current_info_ms; // system time that this driver last requested current gimbal infomation + uint8_t _last_req_step; // 10hz request loop step (different requests are sent at various steps) + uint8_t _stop_order_count; // number of stop commands sent since target rates became zero + float _measure_dist_m = -1.0f; // latest rangefinder distance (in meters) + uint8_t _msg_buff[AP_MOUNT_TOPOTEK_PACKETLEN_MAX]; // buffer holding bytes from latest packet received. only used to calculate crc + uint8_t _msg_buff_len; // number of bytes in the msg buffer + struct { + ParseState state; // parser state + uint8_t data_len; // expected number of data bytes + } _parser; + + // mapping from received message key to member function pointer to consume the message + typedef struct { + uint8_t uart_cmd_key[4]; // gimbal message key; + void (AP_Mount_Topotek::*func)(void); // member function to consume messager + } UartCmdFunctionHandler; + + // stores command ID and corresponding member functions that are compared with the command received by the gimbal + UartCmdFunctionHandler uart_recv_cmd_compare_list[AP_MOUNT_RECV_GIMBAL_CMD_CATEGORIES_NUM] = { + {{"GAC"}, &AP_Mount_Topotek::gimbal_angle_analyse}, + {{"REC"}, &AP_Mount_Topotek::gimbal_record_analyse}, + {{"SDC"}, &AP_Mount_Topotek::gimbal_sdcard_analyse}, + {{"LRF"}, &AP_Mount_Topotek::gimbal_dist_info_analyse}, + {{"TRC"}, &AP_Mount_Topotek::gimbal_track_analyse}, + {{"VSN"}, &AP_Mount_Topotek::gimbal_firmware_ver_analyse}, + {{"PA2"}, &AP_Mount_Topotek::gimbal_model_name_analyse} + }; +}; + +#endif // HAL_MOUNT_TOPOTEK_ENABLED diff --git a/libraries/AP_Mount/AP_Mount_config.h b/libraries/AP_Mount/AP_Mount_config.h index 0a6d38671cb037..bd60bebffb5052 100644 --- a/libraries/AP_Mount/AP_Mount_config.h +++ b/libraries/AP_Mount/AP_Mount_config.h @@ -50,11 +50,15 @@ #define HAL_MOUNT_VIEWPRO_ENABLED AP_MOUNT_BACKEND_DEFAULT_ENABLED && BOARD_FLASH_SIZE > 1024 #endif +#ifndef HAL_MOUNT_TOPOTEK_ENABLED +#define HAL_MOUNT_TOPOTEK_ENABLED AP_MOUNT_BACKEND_DEFAULT_ENABLED +#endif + #ifndef AP_MOUNT_POI_TO_LATLONALT_ENABLED #define AP_MOUNT_POI_TO_LATLONALT_ENABLED HAL_MOUNT_ENABLED && AP_TERRAIN_AVAILABLE && BOARD_FLASH_SIZE > 1024 #endif // set camera source is supported on gimbals that may have more than one lens #ifndef HAL_MOUNT_SET_CAMERA_SOURCE_ENABLED -#define HAL_MOUNT_SET_CAMERA_SOURCE_ENABLED HAL_MOUNT_SIYI_ENABLED || HAL_MOUNT_XACTI_ENABLED || HAL_MOUNT_VIEWPRO_ENABLED +#define HAL_MOUNT_SET_CAMERA_SOURCE_ENABLED HAL_MOUNT_SIYI_ENABLED || HAL_MOUNT_XACTI_ENABLED || HAL_MOUNT_VIEWPRO_ENABLED || HAL_MOUNT_TOPOTEK_ENABLED #endif