Line data Source code
1 : /* 2 : This program is free software: you can redistribute it and/or modify 3 : it under the terms of the GNU General Public License as published by 4 : the Free Software Foundation, either version 3 of the License, or 5 : (at your option) any later version. 6 : 7 : This program is distributed in the hope that it will be useful, 8 : but WITHOUT ANY WARRANTY; without even the implied warranty of 9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 10 : GNU General Public License for more details. 11 : 12 : You should have received a copy of the GNU General Public License 13 : along with this program. If not, see <http://www.gnu.org/licenses/>. 14 : */ 15 : /* 16 : Simulator for Siyi ZT30 17 : 18 : ./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --serial5=sim:siyi_zt30 --speedup=1 19 : 20 : param set MNT1_TYPE 8 # siyi 21 : param set CAM1_TYPE 4 # mount 22 : param set SERIAL5_PROTOCOL 8 # gimbal 23 : reboot 24 : 25 : long REQUEST_MESSAGE CAMERA_INFORMATION 26 : status *CAM* 27 : 28 : */ 29 : 30 : #pragma once 31 : 32 : #include "SIM_config.h" 33 : 34 : #if AP_SIM_SIYI_ZT30_ENABLED 35 : 36 : #include "SIM_Siyi.h" 37 : #include "SIM_Gimbal.h" 38 : 39 : namespace SITL { 40 : 41 : class Siyi_ZT30 : public Siyi { 42 : public: 43 : 44 : void update(const Aircraft &aircraft) override; 45 : 46 : protected: 47 : 48 : void handle_message(const PackedMessage<FirmwareVersionRequest> &request) override; 49 : void handle_message(const PackedMessage<HardwareIDRequest> &request) override; 50 : void handle_message(const PackedMessage<GimbalAttitudeRequest> &request) override; 51 : void handle_message(const PackedMessage<ExternalAttitude> &request) override; 52 : void handle_message(const PackedMessage<SetCameraImageType> &request) override; 53 : void handle_message(const PackedMessage<Photo> &request) override; 54 : void handle_message(const PackedMessage<AcquireGimbalConfigInfo> &request) override; 55 : void handle_message(const PackedMessage<GimbalRotation> &request) override; 56 : void handle_message(const PackedMessage<PositionData> &request) override; 57 : void handle_message(const PackedMessage<SetTime> &request) override; 58 : void handle_message(const PackedMessage<ReadRangefinderRequest> &request) override; 59 : 60 : private: 61 : 62 : // the physical gimbal: 63 : Gimbal gimbal; 64 : 65 : class PACKED FirmwareVersion : Siyi::BaseMessage { 66 : public: 67 1 : FirmwareVersion( 68 : uint8_t _camera_major, uint8_t _camera_minor, uint8_t _camera_patch, 69 : uint8_t _gimbal_major, uint8_t _gimbal_minor, uint8_t _gimbal_patch, 70 : uint8_t _zoom_major, uint8_t _zoom_minor, uint8_t _zoom_patch 71 1 : ) : BaseMessage(Siyi::CommandID::ACQUIRE_FIRMWARE_VERSION), 72 1 : camera(_camera_major << 16 | _camera_minor << 8 | _camera_patch), 73 1 : gimbal(_gimbal_major << 16 | _gimbal_minor << 8 | _gimbal_patch), 74 1 : zoom(_zoom_major << 16 | _zoom_minor << 8 | _zoom_patch) 75 1 : { } 76 : 77 : uint32_t camera; 78 : uint32_t gimbal; 79 : uint32_t zoom; 80 : }; 81 : 82 : class PACKED HardwareID : Siyi::BaseMessage { 83 : public: 84 1 : HardwareID(uint8_t _a, uint8_t _b) : 85 : BaseMessage(Siyi::CommandID::HARDWARE_ID), 86 1 : major{_a}, 87 1 : minor{_b} 88 1 : { } 89 : 90 : uint8_t major; 91 : uint8_t minor; 92 : }; 93 : 94 : class PACKED GimbalAttitude : Siyi::BaseMessage { 95 : public: 96 43 : GimbalAttitude(int16_t _yaw_decidegrees, 97 : int16_t _pitch_decidegrees, 98 : int16_t _roll_decidegrees, 99 : int16_t _yaw_rate_decidegrees, 100 : int16_t _pitch_rate_decidegrees, 101 : int16_t _roll_rate_decidegrees 102 43 : ) : 103 : BaseMessage(Siyi::CommandID::ACQUIRE_GIMBAL_ATTITUDE), 104 43 : yaw_decidegrees{_yaw_decidegrees}, 105 43 : pitch_decidegrees{_pitch_decidegrees}, 106 43 : roll_decidegrees{_roll_decidegrees}, 107 43 : yaw_rate_decidegreess{_yaw_rate_decidegrees}, 108 43 : pitch_rate_decidegreess{_pitch_rate_decidegrees}, 109 43 : roll_rate_decidegreess{_roll_rate_decidegrees} 110 43 : { } 111 : 112 : int16_t yaw_decidegrees; 113 : int16_t pitch_decidegrees; 114 : int16_t roll_decidegrees; 115 : 116 : int16_t yaw_rate_decidegreess; 117 : int16_t pitch_rate_decidegreess; 118 : int16_t roll_rate_decidegreess; 119 : }; 120 : 121 : class PACKED GimbalConfigInfo : Siyi::BaseMessage { 122 : public: 123 : GimbalConfigInfo(uint8_t _hdr_status, 124 : uint8_t _record_status, 125 : uint8_t _motion_mode, 126 : uint8_t _mounting_dir, 127 : uint8_t _video_mode 128 : ) : 129 : BaseMessage(Siyi::CommandID::ACQUIRE_GIMBAL_CONFIG_INFO), 130 : hdr_status{_hdr_status}, 131 : record_status{_record_status}, 132 : motion_mode{_motion_mode}, 133 : mounting_dir{_mounting_dir}, 134 : video_mode{_video_mode} 135 : { } 136 : 137 : uint8_t reserved1; 138 : uint8_t hdr_status; 139 : uint8_t reserved3; 140 : uint8_t record_status; 141 : uint8_t motion_mode; 142 : uint8_t mounting_dir; 143 : uint8_t video_mode; 144 : }; 145 : 146 : class PACKED RangefinderDistance : Siyi::BaseMessage { 147 : public: 148 22 : RangefinderDistance(uint16_t _dist_m) : 149 : BaseMessage(Siyi::CommandID::READ_RANGEFINDER), 150 22 : dist_m{_dist_m} 151 22 : { } 152 : uint16_t dist_m; 153 : }; 154 : 155 : class PACKED SetTimeResponse : Siyi::BaseMessage { 156 : public: 157 0 : SetTimeResponse(uint8_t _result) : 158 : BaseMessage(Siyi::CommandID::SET_TIME), 159 0 : result{_result} 160 0 : { } 161 : 162 : uint8_t result; 163 : }; 164 : 165 : static constexpr uint8_t hwid_a { '7' }; 166 : static constexpr uint8_t hwid_b { 'A' }; 167 : 168 : static constexpr uint8_t camera_major { 1 }; 169 : static constexpr uint8_t camera_minor { 2 }; 170 : static constexpr uint8_t camera_patch { 3 }; 171 : 172 : static constexpr uint8_t gimbal_major { 4 }; 173 : static constexpr uint8_t gimbal_minor { 5 }; 174 : static constexpr uint8_t gimbal_patch { 6 }; 175 : 176 : static constexpr uint8_t zoom_major { 7 }; 177 : static constexpr uint8_t zoom_minor { 8 }; 178 : static constexpr uint8_t zoom_patch { 9 }; 179 : 180 : uint64_t epoch_us; // from SET_TIME 181 : 182 : }; // end class Siyi_ZT30 183 : }; // end namespace SITL 184 : 185 : #endif // AP_SIM_SIYI_ZT30_ENABLED
