Line data Source code
1 : /*
2 : DroneCAN gimbal driver
3 :
4 : Implements gimbal control and attitude feedback using the DroneCAN / DSDL / com / xacti messages
5 : see https://github.com/dronecan/DSDL/tree/master/com/xacti
6 :
7 : */
8 :
9 : #pragma once
10 :
11 : #include "AP_Mount_config.h"
12 :
13 : #if HAL_MOUNT_XACTI_ENABLED
14 :
15 : #include "AP_Mount_Backend.h"
16 :
17 : #include <AP_HAL/AP_HAL.h>
18 : #include <AP_Math/AP_Math.h>
19 : #include <AP_Common/AP_Common.h>
20 : #include <AP_DroneCAN/AP_DroneCAN.h>
21 : #include <AP_HAL/utility/RingBuffer.h>
22 : #include "AP_Mount.h"
23 :
24 : class AP_Mount_Xacti : public AP_Mount_Backend
25 : {
26 :
27 : public:
28 : // Constructor
29 : AP_Mount_Xacti(class AP_Mount &frontend, class AP_Mount_Params ¶ms, uint8_t instance);
30 :
31 : /* Do not allow copies */
32 : CLASS_NO_COPY(AP_Mount_Xacti);
33 :
34 : // init - performs any required initialisation for this instance
35 : void init() override;
36 :
37 : // update mount position - should be called periodically
38 : void update() override;
39 :
40 : // return true if healthy
41 : bool healthy() const override;
42 :
43 : // has_pan_control - returns true if this mount can control its pan (required for multicopters)
44 0 : bool has_pan_control() const override { return yaw_range_valid(); };
45 :
46 : //
47 : // camera controls
48 : //
49 :
50 : // take a picture. returns true on success
51 : bool take_picture() override;
52 :
53 : // start or stop video recording
54 : // set start_recording = true to start record, false to stop recording
55 : bool record_video(bool start_recording) override;
56 :
57 : // set zoom specified as a rate or percentage
58 : bool set_zoom(ZoomType zoom_type, float zoom_value) override;
59 :
60 : // set focus specified as rate, percentage or auto
61 : // focus in = -1, focus hold = 0, focus out = 1
62 : SetFocusResult set_focus(FocusType focus_type, float focus_value) override;
63 :
64 : // set camera lens as a value from 0 to 5
65 : bool set_lens(uint8_t lens) override;
66 :
67 : // set_camera_source is functionally the same as set_lens except primary and secondary lenses are specified by type
68 : // primary and secondary sources use the AP_Camera::CameraSource enum cast to uint8_t
69 : bool set_camera_source(uint8_t primary_source, uint8_t secondary_source) override;
70 :
71 0 : bool has_camera_information() const override { return true; }
72 : // return camera vendor name
73 0 : void get_camera_vendor_name(char *buf, uint8_t buflen) const override { strncpy(buf, "Xacti", buflen); }
74 : // return camera model name
75 0 : void get_camera_model_name(char *buf, uint8_t buflen) const override { strncpy(buf, "CX-GB100", buflen); }
76 : // return camera firmware version
77 0 : uint32_t get_camera_firmware_version() const override { return _firmware_version.received ? _firmware_version.mav_ver : 0; }
78 : // return camera capability flags
79 0 : uint32_t get_camera_cap_flags() const override {
80 : return (CAMERA_CAP_FLAGS_CAPTURE_VIDEO |
81 : CAMERA_CAP_FLAGS_CAPTURE_IMAGE |
82 0 : CAMERA_CAP_FLAGS_HAS_BASIC_FOCUS);
83 : }
84 :
85 : // send camera settings message to GCS
86 : void send_camera_settings(mavlink_channel_t chan) const override;
87 :
88 : // subscribe to Xacti DroneCAN messages
89 : static bool subscribe_msgs(AP_DroneCAN* ap_dronecan);
90 :
91 : // xacti specific message handlers
92 : static void handle_gimbal_attitude_status(AP_DroneCAN* ap_dronecan, const CanardRxTransfer& transfer, const com_xacti_GimbalAttitudeStatus &msg);
93 : static void handle_gnss_status_req(AP_DroneCAN* ap_dronecan, const CanardRxTransfer& transfer, const com_xacti_GnssStatusReq &msg);
94 :
95 : protected:
96 :
97 : // get attitude as a quaternion. returns true on success
98 : bool get_attitude_quaternion(Quaternion& att_quat) override;
99 :
100 : // Xacti can send either rates or angles
101 0 : uint8_t natively_supported_mount_target_types() const override {
102 0 : return NATIVE_ANGLES_AND_RATES_ONLY;
103 : };
104 :
105 : private:
106 :
107 : // send text prefix string to reduce flash cost
108 : static const char* send_text_prefix;
109 :
110 : // Sensor mode enumeration (aka lens)
111 : enum class SensorsMode : uint8_t {
112 : RGB = 0, // RGB (aka "visible)")
113 : IR = 1, // Infrared, aka thermal
114 : PIP = 2, // RGB with IR PIP
115 : NDVI = 3, // NDVI (vegetation greenness)
116 : };
117 : // array of sensor mode enumeration strings for display to user
118 : // if enum above is updated, also update sensor_mode_str definition in cpp
119 : static const char* sensor_mode_str[];
120 :
121 : // send target pitch and yaw rates to gimbal
122 : void send_target_rates(const MountRateTarget &rate_rads) override;
123 :
124 : // send target pitch and yaw angles to gimbal
125 : void send_target_angles(const MountAngleTarget &angle_rad) override;
126 :
127 : // register backend in detected modules array used to map DroneCAN port and node id to backend
128 : void register_backend();
129 :
130 : // find backend associated with the given dronecan port and node_id. also associates backends with zero node ids
131 : // returns pointer to backend on success, nullptr on failure
132 : static AP_Mount_Xacti* get_dronecan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id);
133 :
134 : // DroneCAN parameter handling methods
135 : FUNCTOR_DECLARE(param_int_cb, bool, AP_DroneCAN*, const uint8_t, const char*, int32_t &);
136 : FUNCTOR_DECLARE(param_string_cb, bool, AP_DroneCAN*, const uint8_t, const char*, AP_DroneCAN::string &);
137 : FUNCTOR_DECLARE(param_save_cb, void, AP_DroneCAN*, const uint8_t, bool);
138 : bool handle_param_get_set_response_int(AP_DroneCAN* ap_dronecan, const uint8_t node_id, const char* name, int32_t &value);
139 : bool handle_param_get_set_response_string(AP_DroneCAN* ap_dronecan, const uint8_t node_id, const char* name, AP_DroneCAN::string &value);
140 : void handle_param_save_response(AP_DroneCAN* ap_dronecan, const uint8_t node_id, bool success);
141 :
142 : // param enum. If enum is updated also update _param_names definition in cpp
143 : enum class Param : uint8_t {
144 : SingleShot = 0,
145 : Recording,
146 : FocusMode,
147 : SensorMode,
148 : DigitalZoomMagnification,
149 : FirmwareVersion,
150 : Status,
151 : DateTime,
152 : OpticalZoomMagnification,
153 : LAST = OpticalZoomMagnification, // this should be equal to the final parameter enum
154 : };
155 : static const char* _param_names[]; // array of Xacti parameter strings
156 :
157 : // get parameter name for a particular param enum value
158 : // returns an empty string if not found (which should never happen)
159 : const char* get_param_name_str(Param param) const;
160 :
161 : // helper function to get and set parameters
162 : bool set_param_int32(Param param, int32_t param_value);
163 : bool set_param_string(Param param, const AP_DroneCAN::string& param_value);
164 : bool get_param_string(Param param);
165 :
166 : // process queue of set parameter items. returns true if set-parameter message was sent
167 : bool process_set_param_int32_queue();
168 :
169 : // send gimbal control message via DroneCAN
170 : // mode is 2:angle control or 3:rate control
171 : // pitch_cd is pitch angle in centi-degrees or pitch rate in cds
172 : // yaw_cd is angle in centi-degrees or yaw rate in cds
173 : void send_gimbal_control(uint8_t mode, int16_t pitch_cd, int16_t yaw_cd);
174 :
175 : // send vehicle attitude to gimbal via DroneCAN. now_ms is current system time
176 : // returns true if sent so that we avoid immediately trying to also send other messages
177 : bool send_copter_att_status(uint32_t now_ms);
178 :
179 : // update zoom rate controller. now_ms is current system time
180 : // returns true if sent so that we avoid immediately trying to also send other messages
181 : bool update_zoom_rate_control(uint32_t now_ms);
182 :
183 : // request firmware version. now_ms is current system time
184 : // returns true if sent so that we avoid immediately trying to also send other messages
185 : bool request_firmware_version(uint32_t now_ms);
186 :
187 : // request parameters used to determine camera capabilities. now_ms is current system time
188 : // returns true if a param get/set was sent so that we avoid sending other messages
189 : bool request_capabilities(uint32_t now_ms);
190 :
191 : // set date and time. now_ms is current system time
192 : bool set_datetime(uint32_t now_ms);
193 :
194 : // request status. now_ms is current system time
195 : // returns true if sent so that we avoid immediately trying to also send other messages
196 : bool request_status(uint32_t now_ms);
197 :
198 : // check if safe to send message (if messages sent too often camera will not respond)
199 : // now_ms is current system time
200 : bool is_safe_to_send(uint32_t now_ms) const;
201 :
202 : // internal variables
203 : bool _initialised; // true once the driver has been initialised
204 :
205 : // attitude received from gimbal
206 : Quaternion _current_attitude_quat; // current attitude as a quaternion
207 : uint32_t _last_current_attitude_quat_ms; // system time _current_angle_rad was updated
208 : bool _recording_video; // true if recording video
209 : uint16_t _last_digital_zoom_param_value = 100; // last digital zoom parameter value sent to camera. 100 ~ 1000 (interval 100)
210 : uint16_t _last_optical_zoom_param_value = 100; // last optical zoom parameter value sent to camera. 100 ~ 250 (interval 10)
211 : struct {
212 : bool enabled; // true if zoom rate control is enabled
213 : int8_t dir; // zoom direction (-1 to zoom out, +1 to zoom in)
214 : uint32_t last_update_ms; // system time that zoom rate control last updated zoom
215 : } _zoom_rate_control;
216 :
217 : // firmware version received from gimbal
218 : struct {
219 : uint32_t last_request_ms; // system time of last request for firmware version
220 : bool received; // true once firmware version has been received
221 : char str[12] {}; // firmware version string (11 bytes + 1 null byte)
222 : uint32_t mav_ver; // version formatted for reporting to GCS via CAMERA_INFORMATION message
223 : } _firmware_version;
224 :
225 : // date and time handling
226 : struct {
227 : uint32_t last_request_ms; // system time that date/time was last requested
228 : bool set; // true once date/time has been set
229 : } _datetime;
230 :
231 : // capability handling
232 : enum class Capability : uint8_t {
233 : False = 0,
234 : True = 1,
235 : Unknown = 2,
236 : };
237 : struct {
238 : bool received; // true if we have determined cameras capabilities
239 : uint32_t first_request_ms; // system time of first request for capabilities (used to timeout)
240 : uint32_t last_request_ms; // system time of last capability related parameter check
241 : Capability optical_zoom; // Yes if camera has optical zoom
242 : } capabilities = {false, 0, 0, Capability::Unknown};
243 :
244 : // gimbal status handling
245 : enum class ErrorStatus : uint32_t {
246 : TAKING_PICTURE = 0x04, // currently taking a picture
247 : RECORDING_VIDEO = 0x08, // currently recording video
248 : CANNOT_TAKE_PIC = 0x20,
249 : TIME_NOT_SET = 0x10000,
250 : MEDIA_ERROR = 0x20000,
251 : LENS_ERROR = 0x40000,
252 : MOTOR_INIT_ERROR = 0x100000,
253 : MOTOR_OPERATION_ERROR = 0x200000,
254 : GIMBAL_CONTROL_ERROR = 0x400000,
255 : TEMP_WARNING = 0x1000000
256 : };
257 : struct {
258 : uint32_t error_status; // see ErrorStatus enum
259 : uint32_t video_remain_time; // max seconds of video that may be recorded before SD card is full
260 : uint32_t photo_remain_count; // max number of pics before SD card is full
261 : uint32_t sd_card_size_mb; // SD card size in MB
262 : uint32_t sd_card_free_mb; // SD card remaining size in MB
263 : uint16_t body; // body type
264 : uint16_t cmos; // cmos type
265 : uint16_t gimbal_pitch; // gimbal pitch angle
266 : uint16_t gimbal_roll; // gimbal roll angle
267 : uint16_t gimbal_yaw; // gimbal yaw angle
268 : uint16_t reserved1;
269 : uint8_t date_time[7]; // camera's date and time
270 : uint8_t reserved2;
271 : uint32_t exposure_time_us; // camera's exposure time in us
272 : uint16_t apeture; // cameras' aperture * 100
273 : uint16_t iso_sensitivity; // camera's iso sensitivity
274 : } _status; // latest status received
275 : static_assert(sizeof(_status) == 48, "status must be 48 bytes"); // status should be 48 bytes
276 : struct {
277 : uint32_t last_request_ms; // system time that status was last requested
278 : uint32_t last_error_status; // last error status reported to user
279 : } _status_report;
280 : bool _motor_error; // true if status reports motor or control error (used for health reporting)
281 : bool _camera_error; // true if status reports camera error
282 :
283 : // DroneCAN related variables
284 : static struct DetectedModules {
285 : AP_Mount_Xacti *driver; // pointer to Xacti backends
286 : AP_DroneCAN* ap_dronecan; // DroneCAN interface used by this backend
287 : uint8_t node_id; // DroneCAN node id associated by this backend
288 : } _detected_modules[AP_MOUNT_MAX_INSTANCES];
289 : static HAL_Semaphore _sem_registry; // semaphore protecting access to _detected_modules table
290 : uint32_t last_send_gimbal_control_ms; // system time that send_gimbal_control was last called (used to slow down sends to 5hz)
291 : uint32_t last_send_copter_att_status_ms; // system time that send_copter_att_status was last called (used to slow down sends to 10hz)
292 : uint32_t last_send_getset_param_ms; // system time that a get or set parameter message was sent
293 :
294 : // queue of set parameter int32 items. set-parameter requests to camera are throttled to improve reliability
295 : struct SetParamQueueItem {
296 : Param param; // parameter (name)
297 : int32_t value; // parameter value
298 : };
299 : ObjectArray<SetParamQueueItem> *_set_param_int32_queue; // queue of set-parameter items
300 : };
301 :
302 : #endif // HAL_MOUNT_XACTI_ENABLED
|
