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 Topotek gimbal
      17             : 
      18             : ./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --serial5=sim:topotek --speedup=1
      19             : 
      20             : param set MNT1_TYPE 12        # topotek
      21             : param set SERIAL5_PROTOCOL 8  # gimbal
      22             : reboot
      23             : 
      24             : The gimbal can also be simulated as being connected to one of the
      25             : autopilot's network ports rather than to one of its serial ports:
      26             : 
      27             : ./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --net-device=topotek:15005 --speedup=1
      28             : 
      29             : param set MNT1_TYPE 12        # topotek
      30             : param set NET_ENABLE 1
      31             : param set NET_P1_TYPE 3       # TCP client
      32             : param set NET_P1_PROTOCOL 8   # gimbal
      33             : param set NET_P1_IP0 127
      34             : param set NET_P1_IP1 0
      35             : param set NET_P1_IP2 0
      36             : param set NET_P1_IP3 1
      37             : param set NET_P1_PORT 15005
      38             : reboot
      39             : 
      40             : */
      41             : 
      42             : #pragma once
      43             : 
      44             : #include "SIM_config.h"
      45             : 
      46             : #if AP_SIM_TOPOTEK_ENABLED
      47             : 
      48             : #include "SIM_Mount.h"
      49             : #include "SIM_Gimbal.h"
      50             : 
      51             : namespace SITL {
      52             : 
      53             : class Topotek : public Mount {
      54             : public:
      55             : 
      56             :     void update(const Aircraft &aircraft) override;
      57             : 
      58             : private:
      59             : 
      60             :     // the physical gimbal:
      61             :     Gimbal gimbal;
      62             : 
      63             :     // input accumulation buffer; also used as working buffer by handle_packet()
      64             :     static constexpr uint8_t PACKETLEN_MAX = 36;
      65             :     uint8_t _buf[PACKETLEN_MAX];
      66             :     uint8_t _buflen;
      67             : 
      68             :     uint32_t _last_attitude_ms;     // time of last attitude packet sent
      69             : 
      70             :     // last commanded angles from GIP/GIY packets (wire centidegrees, same sign as sent by driver)
      71             :     int16_t _commanded_pitch_cd;
      72             :     int16_t _commanded_yaw_cd;
      73             : 
      74             :     // The gimbal sends each reply out of the interface named by the
      75             :     // source address in the request (the 4th byte of the packet), not
      76             :     // out of the interface the request arrived on.  If the autopilot
      77             :     // says it is on our UART when it is really on our network port then
      78             :     // our replies go out of the UART and are lost.
      79             :     static constexpr uint8_t ADDRESS_UART = 'U';
      80             :     static constexpr uint8_t ADDRESS_NETWORK = 'P';
      81             : 
      82             :     // address of the interface the autopilot is really connected to
      83        2761 :     uint8_t connected_interface() const {
      84        2761 :         return is_network_attached() ? ADDRESS_NETWORK : ADDRESS_UART;
      85             :     }
      86             : 
      87             :     // address the gimbal is currently sending its replies to; updated
      88             :     // from the source address of each packet received.  Defaults to the
      89             :     // UART as that is where the gimbal talks before being told otherwise
      90             :     uint8_t _reply_address = ADDRESS_UART;
      91             : 
      92             :     // read and dispatch incoming packets from autopilot
      93             :     void update_input();
      94             : 
      95             :     // scan forward from search_start_pos for '#' and move it to _buf[0]
      96             :     void move_preamble_in_buffer(uint8_t search_start_pos);
      97             : 
      98             :     // send gimbal attitude packet to the driver
      99             :     void send_attitude();
     100             : 
     101             :     // dispatch a complete packet beginning at _buf[0], data_len data bytes
     102             :     void handle_packet(uint8_t data_len);
     103             : 
     104             :     // build and send a response packet
     105             :     void send_packet(char addr2, const char id[3], bool write, const uint8_t *data, uint8_t len);
     106             : 
     107             :     // encode a uint16 as 4 uppercase ASCII hex chars
     108             :     static void uint16_to_hex4(uint16_t val, uint8_t buf[4]);
     109             : 
     110             :     // convert a nibble (0-15) to an uppercase ASCII hex character
     111       65879 :     static uint8_t hex2char(uint8_t nibble) {
     112       65879 :         return nibble < 10 ? ('0' + nibble) : ('A' + nibble - 10);
     113             :     }
     114             : };
     115             : 
     116             : }  // namespace SITL
     117             : 
     118             : #endif  // AP_SIM_TOPOTEK_ENABLED