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             : #pragma once
      17             : 
      18             : #include "AP_VideoTX_config.h"
      19             : 
      20             : #if AP_SMARTAUDIO_ENABLED
      21             : 
      22             : #include <AP_Param/AP_Param.h>
      23             : #include <AP_HAL/utility/RingBuffer.h>
      24             : #include "AP_VideoTX.h"
      25             : 
      26             : #define SMARTAUDIO_BUFFER_CAPACITY 5
      27             : 
      28             : // SmartAudio Serial Protocol
      29             : #define AP_SMARTAUDIO_UART_BAUD            4800
      30             : #define AP_SMARTAUDIO_SMARTBAUD_MIN        4560     // -5%
      31             : #define AP_SMARTAUDIO_SMARTBAUD_MAX        5040     // +5%
      32             : #define AP_SMARTAUDIO_SMARTBAUD_STEP       60
      33             : #define AP_SMARTAUDIO_UART_BUFSIZE_RX      16
      34             : #define AP_SMARTAUDIO_UART_BUFSIZE_TX      16
      35             : #define AP_SMARTAUDIO_MAX_PACKET_SIZE      32
      36             : 
      37             : #define SMARTAUDIO_SYNC_BYTE            0xAA
      38             : #define SMARTAUDIO_HEADER_BYTE          0x55
      39             : #define SMARTAUDIO_START_CODE           SMARTAUDIO_SYNC_BYTE + SMARTAUDIO_HEADER_BYTE
      40             : #define SMARTAUDIO_GET_PITMODE_FREQ     (1 << 14)
      41             : #define SMARTAUDIO_SET_PITMODE_FREQ     (1 << 15)
      42             : #define SMARTAUDIO_FREQUENCY_MASK       0x3FFF
      43             : 
      44             : #define SMARTAUDIO_CMD_GET_SETTINGS     0x03
      45             : #define SMARTAUDIO_CMD_SET_POWER        0x05
      46             : #define SMARTAUDIO_CMD_SET_CHANNEL      0x07
      47             : #define SMARTAUDIO_CMD_SET_FREQUENCY    0x09
      48             : #define SMARTAUDIO_CMD_SET_MODE         0x0B
      49             : 
      50             : #define SMARTAUDIO_COMMAND_FRAME_SIZE   (sizeof(FrameHeader) + 1)
      51             : #define SMARTAUDIO_U8_COMMAND_FRAME_SIZE   (sizeof(FrameHeader) + 2)
      52             : #define SMARTAUDIO_U16_COMMAND_FRAME_SIZE   (sizeof(FrameHeader) + 3)
      53             : 
      54             : #define SMARTAUDIO_RSP_GET_SETTINGS_V1  SMARTAUDIO_CMD_GET_SETTINGS >> 1
      55             : #define SMARTAUDIO_RSP_GET_SETTINGS_V2  (SMARTAUDIO_CMD_GET_SETTINGS >> 1) | 0x08
      56             : #define SMARTAUDIO_RSP_GET_SETTINGS_V21 (SMARTAUDIO_CMD_GET_SETTINGS >> 1) | 0x10
      57             : #define SMARTAUDIO_RSP_SET_POWER        SMARTAUDIO_CMD_SET_POWER >> 1
      58             : #define SMARTAUDIO_RSP_SET_CHANNEL      SMARTAUDIO_CMD_SET_CHANNEL >> 1
      59             : #define SMARTAUDIO_RSP_SET_FREQUENCY    SMARTAUDIO_CMD_SET_FREQUENCY >> 1
      60             : #define SMARTAUDIO_RSP_SET_MODE         SMARTAUDIO_CMD_SET_MODE >> 1
      61             : 
      62             : #define SMARTAUDIO_BANDCHAN_TO_INDEX(band, channel) (band * VTX_MAX_CHANNELS + (channel))
      63             : 
      64             : //#define SA_DEBUG
      65             : 
      66             : class AP_SmartAudio
      67             : {
      68             : public:
      69             :     enum ProtocolVersion {
      70             :         SMARTAUDIO_SPEC_PROTOCOL_v1 = 0,
      71             :         SMARTAUDIO_SPEC_PROTOCOL_v2 = 1,
      72             :         SMARTAUDIO_SPEC_PROTOCOL_v21 = 2
      73             :     };
      74             : 
      75             :     struct Settings {
      76             :         uint8_t  version;
      77             :         uint8_t  mode;
      78             :         uint8_t  channel;
      79             :         uint8_t  power;
      80             :         uint16_t frequency;
      81             :         uint8_t  band;
      82             : 
      83             :         uint8_t num_power_levels;
      84             :         uint8_t power_levels[8];
      85             :         uint8_t  power_in_dbm;
      86             : 
      87             :         uint16_t pitmodeFrequency;
      88             :         bool userFrequencyMode;     // user is setting freq
      89             :         bool initialized;
      90             :     };
      91             : 
      92             :     struct FrameHeader {
      93             :         uint8_t syncByte;
      94             :         uint8_t headerByte;
      95             :         uint8_t command;
      96             :         uint8_t length;
      97             : 
      98           0 :         void init(uint8_t cmd, uint8_t payloadLength) {
      99           0 :             syncByte = SMARTAUDIO_SYNC_BYTE;
     100           0 :             headerByte= SMARTAUDIO_HEADER_BYTE;
     101           0 :             length = payloadLength;
     102           0 :             command = cmd;
     103           0 :         }
     104             :     } PACKED;
     105             : 
     106             :     struct Frame {
     107             :         FrameHeader header;
     108             :         uint8_t payload[3];
     109             :     } PACKED;
     110             : 
     111             :     struct U8ResponseFrame {
     112             :         FrameHeader header;
     113             :         uint8_t payload;
     114             :         //uint8_t reserved;
     115             :         //uint8_t crc;
     116             :     } PACKED;
     117             : 
     118             :     struct U16ResponseFrame {
     119             :         FrameHeader header;
     120             :         uint16_t payload;
     121             :         //uint8_t reserved;
     122             :         //uint8_t crc;
     123             :     } PACKED;
     124             : 
     125             :     struct SettingsResponseFrame {
     126             :         FrameHeader header;
     127             :         uint8_t channel;
     128             :         uint8_t power;
     129             :         uint8_t operationMode;
     130             :         uint16_t frequency;
     131             :         //uint8_t crc;
     132             :     } PACKED;
     133             : 
     134             :     struct SettingsExtendedResponseFrame {
     135             :         SettingsResponseFrame settings;
     136             :         uint8_t power_dbm;  // current power
     137             :         uint8_t num_power_levels;
     138             :         uint8_t power_levels[8];   // first in the list of dbm levels
     139             :         //uint8_t crc;
     140             :     } PACKED;
     141             : 
     142             :     // v 2.1 additions to response frame
     143             :     //0x0E (current power in dBm) 0x03 (amount of power levels) 0x00(dBm level 1) 0x0E (dBm level 2) 0x14 (dBm level 3) 0x1A (dBm level 4) 0x01(CRC8)
     144             : 
     145             :     // request packet to be processed
     146             :     struct Packet {
     147             :         Frame frame;
     148             :         uint8_t frame_size;
     149             :     } PACKED;
     150             : 
     151             :     AP_SmartAudio();
     152             : 
     153             :     static AP_SmartAudio *get_singleton(void)
     154             :     {
     155             :         return _singleton;
     156             :     }
     157             : 
     158             :     /* Do not allow copies */
     159             :     CLASS_NO_COPY(AP_SmartAudio);
     160             : 
     161             :     // init threads and lookup for io uart.
     162             :     bool init();
     163             : 
     164             : private:
     165             :     // serial interface
     166             :     AP_HAL::UARTDriver *_port;                  // UART used to send data to SmartAudio VTX
     167             : 
     168             :     //Pointer to singleton
     169             :     static AP_SmartAudio* _singleton;
     170             : 
     171             :     // response buffer length, permit splitted responses
     172             :     uint8_t _inline_buffer_length;
     173             :     // expected packet size
     174             :     uint8_t _packet_size;
     175             : 
     176             :     ProtocolVersion _protocol_version;
     177             :     // statistics
     178             :     uint16_t _packets_sent;
     179             :     uint16_t _packets_rcvd;
     180             : 
     181             :     bool _vtx_freq_change_pending; // a vtx command has been issued but not confirmed by a vtx broadcast frame
     182             :     bool _vtx_power_change_pending;
     183             :     bool _vtx_options_change_pending;
     184             :     bool _vtx_changes_pending;
     185             :     bool _vtx_use_set_freq; // should frequency set by band/channel or frequency
     186             : 
     187             :     // value for current baud adjust
     188             :     int32_t _smartbaud = AP_SMARTAUDIO_UART_BAUD;
     189             :     int32_t _smartbaud_direction = 1;
     190             : 
     191             :     // hw vtx state control with 2 elements array use methods _push _peek
     192             :     uint8_t _vtx_state_idx;
     193             :     Settings _vtx_states_buffer[2];
     194             :     Settings *_vtx_current_state;
     195             : 
     196             :     // ready to go
     197             :     volatile bool _initialised;
     198             : 
     199             :     // RingBuffer to store outgoing request.
     200             :     ObjectBuffer<Packet> requests_queue{SMARTAUDIO_BUFFER_CAPACITY};
     201             : 
     202             :     // time the last_request is process
     203             :     uint32_t _last_request_sent_ms;
     204             : 
     205             :     // loops is waiting a response after a request
     206             :     bool _is_waiting_response;
     207             : 
     208             : #ifdef SA_DEBUG
     209             :     // utility method for debugging.
     210             :     void print_bytes_to_hex_string(const char* msg, const uint8_t buf[], uint8_t length);
     211             : #endif
     212             :     void print_settings(const Settings* settings);
     213             : 
     214             :     void update_vtx_params();
     215             :     void update_vtx_settings(const Settings& settings);
     216             : 
     217           0 :     bool ignore_crc() const { return AP::vtx().has_option(AP_VideoTX::VideoOptions::VTX_SA_IGNORE_CRC); }
     218             : 
     219             :     // looping over requests
     220             :     void loop();
     221             :     // send a frame over the wire
     222             :     void send_request(const Frame& requestFrame, uint8_t size);
     223             :     // receive a frame response over the wire
     224             :     bool read_response(uint8_t *response_buffer);
     225             :     // parses the response and updates the vtx settings
     226             :     bool parse_frame_response(const uint8_t *buffer);
     227             :     bool parse_response_buffer(const uint8_t *buffer, uint8_t buffer_length);
     228             :     // get last reading from the fifo queue
     229             :     bool get_readings(AP_VideoTX *vtx_dest);
     230             : 
     231             :     // command functions
     232             :     // request settings
     233             :     void request_settings();
     234             :     // change the mode
     235             :     void set_operation_mode(uint8_t mode);
     236             :     // change the frequency
     237             :     void set_frequency(uint16_t frequency, bool isPitModeFreq);
     238             :     // change the channel
     239             :     void set_channel(uint8_t chan);
     240             :     // get the pitmode frequency
     241             :     void request_pit_mode_frequency();
     242             :     // set the power
     243             :     void set_power(uint16_t power_mw, uint16_t dbm);
     244             :     // set the power using power_level, spec versions 1 and 2 or dbm value for spec version 2.1
     245             :     void set_power(uint8_t power_level);
     246             :     // set the band and channel
     247             :     void set_band_channel(const uint8_t band, const uint8_t channel);
     248             : 
     249             :     // command functions
     250             :     void push_command_only_frame(uint8_t command);
     251             :     void push_uint8_command_frame(uint8_t command, uint8_t data);
     252             :     void push_uint16_command_frame(uint8_t command, uint16_t data);
     253             : 
     254             :     static void unpack_frequency(Settings *settings, const uint16_t frequency);
     255             :     static void unpack_settings(Settings *settings, const SettingsResponseFrame *frame);
     256             :     static void unpack_settings(Settings *settings, const SettingsExtendedResponseFrame *frame);
     257             : 
     258             :     // change baud automatically when request-response fails many times
     259             :     void update_baud_rate();
     260             : 
     261           0 :     void set_configuration_pending(bool pending) { _vtx_changes_pending = pending; }
     262           0 :     bool is_configuration_pending(){ return _vtx_changes_pending;}
     263             : };
     264             : #endif