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             :   Base class for Siyi camera simulators
      17             : */
      18             : 
      19             : #pragma once
      20             : 
      21             : #include "SIM_config.h"
      22             : 
      23             : #if AP_SIM_SIYI_ENABLED
      24             : 
      25             : #include "SIM_Mount.h"
      26             : 
      27             : #include <AP_Math/crc.h>
      28             : #include <AP_Common/Location.h>
      29             : #include <AP_InternalError/AP_InternalError.h>
      30             : 
      31             : namespace SITL {
      32             : 
      33             : class Siyi : public Mount {
      34             : public:
      35             : 
      36             :     void update(const class Aircraft &aircraft) override;
      37             : 
      38             : protected:
      39             : 
      40             :     static constexpr uint8_t HEADER1 { 0x55 };
      41             :     static constexpr uint8_t HEADER2 { 0x66 };
      42             : 
      43             :     template <typename T>
      44             :     class PACKED PackedMessage {
      45             :     public:
      46          67 :         PackedMessage(T _msg) :
      47          67 :             msg(_msg)
      48             :         {
      49          67 :             update_checksum();
      50          67 :         }
      51             :         uint8_t header1 { HEADER1 };
      52             :         uint8_t header2 { HEADER2 };
      53             :         uint8_t ctrl;
      54             :         uint16_t datalen {sizeof(T)-1 };  // -1 to exclude command ID
      55             :         uint16_t seq;
      56             :         T msg;
      57             :         uint16_t checksum;
      58             : 
      59         287 :         uint16_t calculate_checksum(uint16_t len) const WARN_IF_UNUSED {
      60         287 :             return crc16_ccitt((uint8_t*)this, len, 0);
      61             :         }
      62          67 :         uint16_t calculate_checksum() const WARN_IF_UNUSED {
      63          67 :             return calculate_checksum(sizeof(*this)-2);
      64             :         }
      65             : 
      66          67 :         void update_checksum() {
      67          67 :             checksum = calculate_checksum();
      68          67 :         }
      69             : 
      70          45 :         bool response_requested() const { return (ctrl & 0x1) == 0x1; }
      71             :     };
      72             : 
      73             :     // message ids
      74             :     enum class CommandID : uint8_t {
      75             :         ACQUIRE_FIRMWARE_VERSION = 0x01,
      76             :         HARDWARE_ID = 0x02,
      77             :         // AUTO_FOCUS = 0x04,
      78             :         // MANUAL_ZOOM_AND_AUTO_FOCUS = 0x05,
      79             :         // MANUAL_FOCUS = 0x06,
      80             :         GIMBAL_ROTATION = 0x07,
      81             :         // CENTER = 0x08,
      82             :         ACQUIRE_GIMBAL_CONFIG_INFO = 0x0A,
      83             :         // FUNCTION_FEEDBACK_INFO = 0x0B,
      84             :         PHOTO = 0x0C,
      85             :         ACQUIRE_GIMBAL_ATTITUDE = 0x0D,
      86             :         // ABSOLUTE_ZOOM = 0x0F,
      87             :         SET_CAMERA_IMAGE_TYPE = 0x11,
      88             :         GET_TEMP_FULL_IMAGE = 0x14,
      89             :         READ_RANGEFINDER = 0x15,
      90             :         EXTERNAL_ATTITUDE = 0x22,
      91             :         SET_TIME = 0x30,
      92             :         POSITION_DATA = 0x3e,
      93             :     };
      94             : 
      95             :     class PACKED BaseMessage {
      96             :     public:
      97          67 :         BaseMessage(CommandID _cmdid) :
      98          67 :             cmdid{uint8_t(_cmdid)}
      99          67 :         { }
     100             :         uint8_t cmdid;
     101             :     };
     102             : 
     103             :     class PACKED FirmwareVersionRequest : public BaseMessage { };
     104             :     virtual void handle_message(const PackedMessage<FirmwareVersionRequest> &request) = 0;
     105             : 
     106             :     class PACKED HardwareIDRequest : public BaseMessage { };
     107             :     virtual void handle_message(const PackedMessage<HardwareIDRequest> &request) = 0;
     108             : 
     109             :     class PACKED GimbalAttitudeRequest : public BaseMessage { };
     110             :     virtual void handle_message(const PackedMessage<GimbalAttitudeRequest> &request) = 0;
     111             : 
     112             :     class PACKED ExternalAttitude : public BaseMessage {
     113             :     public:
     114             :         uint32_t time_boot_ms;
     115             :         float roll, pitch, yaw;
     116             :         float rollspeed, pitchspeed, yawspeed;
     117             :     };
     118           0 :     virtual void handle_message(const PackedMessage<ExternalAttitude> &request) {
     119           0 :         AP_HAL::panic("Unhandled ExternalAttitude message");
     120             :     }
     121             : 
     122             :     class PACKED SetCameraImageType : public BaseMessage { };
     123           0 :     virtual void handle_message(const PackedMessage<SetCameraImageType> &request) {
     124           0 :         AP_HAL::panic("Unhandled SetCameraImageType message");
     125             :     }
     126             : 
     127             :     class PACKED Photo : public BaseMessage {
     128             :     public:
     129             :         uint8_t command;
     130             :     };
     131           0 :     virtual void handle_message(const PackedMessage<Photo> &request) {
     132           0 :         AP_HAL::panic("Unhandled Photo message");
     133             :     }
     134             : 
     135             :     class PACKED AcquireGimbalConfigInfo : public BaseMessage {
     136             :     public:
     137             :     };
     138           0 :     virtual void handle_message(const PackedMessage<AcquireGimbalConfigInfo> &request) {
     139           0 :         AP_HAL::panic("Unhandled AcquireGimbalConfigInfo message");
     140             :     }
     141             : 
     142             :     class PACKED GetTempFullImageRequest : public BaseMessage {
     143             :     public:
     144             :         uint8_t mode;
     145             :     };
     146           1 :     virtual void handle_message(const PackedMessage<GetTempFullImageRequest> &request) { }
     147             : 
     148             :     class PACKED ReadRangefinderRequest : public BaseMessage { };
     149           0 :     virtual void handle_message(const PackedMessage<ReadRangefinderRequest> &request) { }
     150             : 
     151             :     class PACKED GimbalRotation : public BaseMessage {
     152             :     public:
     153             :         uint8_t yaw_rate_pct;
     154             :         uint8_t pitch_rate_pct;
     155             :     };
     156           0 :     virtual void handle_message(const PackedMessage<GimbalRotation> &request) {
     157           0 :         AP_HAL::panic("Unhandled GimbalRotation message");
     158             :     }
     159             : 
     160             :     class PACKED PositionData : public BaseMessage {
     161             :     public:
     162             :         uint32_t time_boot_ms;
     163             :         int32_t lat, lon;
     164             :         int32_t alt_msl, alt_ellipsoid;
     165             :         int32_t velocity_ned_int32[3];
     166             :     };
     167           0 :     virtual void handle_message(const PackedMessage<PositionData> &request) {
     168           0 :         AP_HAL::panic("Unhandled PositionData message");
     169             :     }
     170             : 
     171             :     class PACKED SetTime : public BaseMessage {
     172             :     public:
     173             :         uint64_t epoch_us;
     174             :     };
     175           0 :     virtual void handle_message(const PackedMessage<SetTime> &request) {
     176           0 :         AP_HAL::panic("Unhandled SetTime message");
     177             :     }
     178             : 
     179             :     /*
     180             :      *  Input Handling
     181             :      */
     182             :     void update_input();
     183             :     void handle_received_message();
     184             :     uint32_t expected_message_length(CommandID id) const;
     185             : 
     186             :     union u {
     187           1 :         u() {}
     188             :         uint8_t buffer[256]; // from-autopilot
     189             :         PackedMessage<BaseMessage> packed_empty;
     190             :         PackedMessage<FirmwareVersionRequest> packed_firmwareversionrequest;
     191             :         PackedMessage<HardwareIDRequest> packed_hardwareidrequest;
     192             :         PackedMessage<GimbalAttitudeRequest> packed_gimbalattituderequest;
     193             :         PackedMessage<ExternalAttitude> packed_externalattitude;
     194             :         PackedMessage<SetCameraImageType> packed_setcameraimgetype;
     195             :         PackedMessage<Photo> packed_photo;
     196             :         PackedMessage<AcquireGimbalConfigInfo> packed_acquiregimbalconfiginfo;
     197             :         PackedMessage<GetTempFullImageRequest> packed_gettempfullimage;
     198             :         PackedMessage<ReadRangefinderRequest> packed_readrangefinder;
     199             :         PackedMessage<GimbalRotation> packed_gimbalrotation;
     200             :         PackedMessage<PositionData> packed_positiondata;
     201             :         PackedMessage<SetTime> packed_settime;
     202             :     } msg;
     203             :     uint8_t buflen;
     204             : 
     205             :     void move_preamble_in_buffer(uint8_t search_start_pos=0);
     206             : 
     207             :     /*
     208             :      *  Output Handling
     209             :      */
     210             :     void update_output(const Location &location);
     211             : 
     212             :     uint16_t expected_seq;
     213             : 
     214             :     static constexpr bool strict_parsing = true;
     215             : 
     216             : };  // end class Siyi
     217             : 
     218             : };  // end namespace SITL
     219             : 
     220             : #endif  // AP_SIM_SIYI_ENABLED