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 for Siyi camera simulaors
      17             : */
      18             : 
      19             : #include "SIM_config.h"
      20             : 
      21             : #if AP_SIM_SIYI_ENABLED
      22             : 
      23             : #include "SIM_Siyi.h"
      24             : 
      25             : #include <AP_HAL/utility/sparse-endian.h>
      26             : #include <stdio.h>
      27             : #include <errno.h>
      28             : 
      29             : using namespace SITL;
      30             : 
      31         220 : void Siyi::move_preamble_in_buffer(uint8_t search_start_pos)
      32             : {
      33             :     uint8_t i;
      34         220 :     for (i=search_start_pos; i<buflen; i++) {
      35          91 :         if ((uint8_t)msg.buffer[i] == HEADER1) {
      36          91 :             break;
      37             :         }
      38             :     }
      39         220 :     if (i == 0) {
      40           0 :         return;
      41             :     }
      42         220 :     memmove(msg.buffer, &msg.buffer[i], buflen-i);
      43         220 :     buflen = buflen - i;
      44             : }
      45             : 
      46         220 : uint32_t Siyi::expected_message_length(Siyi::CommandID id) const
      47             : {
      48         220 :     switch (id) {
      49           1 :     case CommandID::ACQUIRE_FIRMWARE_VERSION:
      50           1 :         return sizeof(FirmwareVersionRequest);
      51           1 :     case CommandID::HARDWARE_ID:
      52           1 :         return sizeof(HardwareIDRequest);
      53          43 :        case CommandID::ACQUIRE_GIMBAL_ATTITUDE:
      54          43 :         return sizeof(GimbalAttitudeRequest);
      55          22 :     case CommandID::EXTERNAL_ATTITUDE:
      56          22 :         return sizeof(ExternalAttitude);
      57           0 :     case CommandID::SET_CAMERA_IMAGE_TYPE:
      58           0 :         return sizeof(SetCameraImageType);
      59           1 :     case CommandID::GET_TEMP_FULL_IMAGE:
      60           1 :         return sizeof(GetTempFullImageRequest);
      61          22 :     case CommandID::READ_RANGEFINDER:
      62          22 :         return sizeof(ReadRangefinderRequest);
      63           1 :     case CommandID::PHOTO:
      64           1 :         return sizeof(Photo);
      65           2 :     case CommandID::ACQUIRE_GIMBAL_CONFIG_INFO:
      66           2 :         return sizeof(AcquireGimbalConfigInfo);
      67         127 :     case CommandID::GIMBAL_ROTATION:
      68         127 :         return sizeof(GimbalRotation);
      69           0 :     case CommandID::POSITION_DATA:
      70           0 :         return sizeof(PositionData);
      71           0 :     case CommandID::SET_TIME:
      72           0 :         return sizeof(SetTime);
      73             :     }
      74           0 :     AP_HAL::panic("Unknown command received (0x%02x)", unsigned(CommandID(msg.packed_empty.msg.cmdid)));
      75             : }
      76             : 
      77         220 : void Siyi::handle_received_message()
      78             : {
      79         220 :     const CommandID id { CommandID(msg.packed_empty.msg.cmdid) };
      80             : 
      81         220 :     const uint16_t want_datalen = expected_message_length(id) - 1;
      82         220 :     const uint16_t got_datalen = le16toh(msg.packed_empty.datalen);
      83         220 :     if (want_datalen != got_datalen) {
      84             :         if (strict_parsing) {
      85           0 :             AP_HAL::panic("Bad datalen for %02x, got=%u want=%u", (unsigned)id, got_datalen, want_datalen);
      86             :         }
      87             :         return;
      88             :     }
      89             : 
      90         220 :     switch (id) {
      91           1 :     case CommandID::ACQUIRE_FIRMWARE_VERSION:
      92           1 :         handle_message(msg.packed_firmwareversionrequest);
      93           1 :         break;
      94           1 :     case CommandID::HARDWARE_ID:
      95           1 :         handle_message(msg.packed_hardwareidrequest);
      96           1 :         break;
      97          43 :     case CommandID::ACQUIRE_GIMBAL_ATTITUDE:
      98          43 :         handle_message(msg.packed_gimbalattituderequest);
      99          43 :         break;
     100          22 :     case CommandID::EXTERNAL_ATTITUDE:
     101          22 :         handle_message(msg.packed_externalattitude);
     102          22 :         break;
     103           0 :     case CommandID::SET_CAMERA_IMAGE_TYPE:
     104           0 :         handle_message(msg.packed_setcameraimgetype);
     105           0 :         break;
     106           1 :     case CommandID::PHOTO:
     107           1 :         handle_message(msg.packed_photo);
     108           1 :         break;
     109           2 :     case CommandID::ACQUIRE_GIMBAL_CONFIG_INFO:
     110           2 :         handle_message(msg.packed_acquiregimbalconfiginfo);
     111           2 :         break;
     112           1 :     case CommandID::GET_TEMP_FULL_IMAGE:
     113           1 :         handle_message(msg.packed_gettempfullimage);
     114           1 :         break;
     115          22 :     case CommandID::READ_RANGEFINDER:
     116          22 :         handle_message(msg.packed_readrangefinder);
     117          22 :         break;
     118         127 :     case CommandID::GIMBAL_ROTATION:
     119         127 :         handle_message(msg.packed_gimbalrotation);
     120         127 :         break;
     121           0 :     case CommandID::POSITION_DATA:
     122           0 :         handle_message(msg.packed_positiondata);
     123           0 :         break;
     124           0 :     case CommandID::SET_TIME:
     125           0 :         handle_message(msg.packed_settime);
     126           0 :         break;
     127           0 :     default:
     128             :         if (strict_parsing) {
     129           0 :             AP_HAL::panic("Unknown command received (%02x)", unsigned(CommandID(msg.packed_empty.msg.cmdid)));
     130             :         }
     131             :     }
     132             : }
     133             : 
     134        6045 : void Siyi::update_input()
     135             : {
     136        6045 :     const ssize_t n = read_from_autopilot((char*)&msg.buffer[buflen], ARRAY_SIZE(msg.buffer) - buflen - 1);
     137        6045 :     if (n < 0) {
     138             :         // TODO: do better here
     139           0 :         if (errno != EAGAIN && errno != EWOULDBLOCK && errno != 0) {
     140           0 :             AP_HAL::panic("Failed to read from autopilot");
     141             :         }
     142             :     } else {
     143        6045 :         buflen += n;
     144             :     }
     145             : 
     146        6045 :     switch (buflen) {
     147         220 :     default:
     148             :     case 7:  // validate sequence number
     149             :     case 6:
     150             :         FALLTHROUGH;
     151             :     case 5:  // validate data length
     152             :         if (strict_parsing) {
     153         220 :             const uint16_t received_length = le16toh(msg.packed_empty.datalen);
     154         220 :             if (received_length > 60) {
     155             :                 // the number is arbitrary and based off our current
     156             :                 // driver; if you're adding support for longer messages
     157             :                 // just change it.
     158           0 :                 AP_HAL::panic("Bad length %u", received_length);
     159             :             }
     160             :         }
     161             :         FALLTHROUGH;
     162             :     case 4:
     163             :     case 3:  // validate control field
     164         220 :         if (msg.packed_empty.ctrl != 1) {
     165             :             if (strict_parsing) {
     166           0 :                 AP_HAL::panic("Bad control");
     167             :             }
     168             :             move_preamble_in_buffer(3);
     169             :             return;
     170             :         }
     171             :         FALLTHROUGH;
     172             :     case 2:  // validate HEADER2
     173         220 :         if (msg.packed_empty.header2 != HEADER2) {
     174             :             if (strict_parsing) {
     175           0 :                 AP_HAL::panic("bad HEADER2");
     176             :             }
     177             :             move_preamble_in_buffer(1);
     178             :             return;
     179             :         }
     180             :         FALLTHROUGH;
     181             :     case 1:  // validate HEADER1
     182         220 :         if (msg.packed_empty.header1 != HEADER1) {
     183             :             if (strict_parsing) {
     184           0 :                 AP_HAL::panic("bad HEADER1");
     185             :             }
     186             :             move_preamble_in_buffer(1);
     187             :             return;
     188             :         }
     189         220 :         break;
     190        5825 :     case 0:
     191        5825 :         return;
     192             :     }
     193             : 
     194             :     // this sanity check so we don't use the length fields when
     195             :     // they're not valid:
     196         220 :     if (buflen < sizeof(msg.packed_empty)) {
     197           0 :         return;
     198             :     }
     199             : 
     200         220 :     const uint16_t datalen = le16toh(msg.packed_empty.datalen);
     201         220 :     const uint16_t totallen = sizeof(msg.packed_empty) + datalen;
     202         220 :     if (totallen > sizeof(msg.buffer)) {
     203             :         if (strict_parsing) {
     204           0 :             AP_HAL::panic("bad datalen");
     205             :         }
     206             :         move_preamble_in_buffer(1);
     207             :         return;
     208             :     }
     209             : 
     210         220 :     if (buflen < totallen) {
     211             :         if (strict_parsing) {
     212           0 :             AP_HAL::panic("bad datalen");
     213             :         }
     214             :         move_preamble_in_buffer(1);
     215             :         return;
     216             :     }
     217             : 
     218         220 :     const uint16_t calculated_checksum = msg.packed_empty.calculate_checksum(totallen-2);  // -2 to omit checksum bytes
     219         220 :     const uint16_t received_checksum = UINT16_VALUE(msg.buffer[totallen-1], msg.buffer[totallen-2]);
     220         220 :     if (calculated_checksum != received_checksum) {
     221             :         if (strict_parsing) {
     222           0 :             AP_HAL::panic("bad checksum");
     223             :         }
     224             :         move_preamble_in_buffer(1);
     225             :         return;
     226             :     }
     227             : 
     228             :     if (strict_parsing) {
     229         220 :         const uint16_t received_seq = le16toh(msg.packed_empty.seq);
     230             :         // printf("received_seq=%u\n", unsigned(received_seq));
     231         220 :         if (received_seq != expected_seq) {
     232           0 :             AP_HAL::panic("Bad sequence (got=%u vs want=%u)", msg.packed_empty.seq, expected_seq+1);
     233             :         }
     234         220 :         expected_seq = received_seq + 1;
     235             :     }
     236             : 
     237         220 :     handle_received_message();
     238             : 
     239         220 :     move_preamble_in_buffer(totallen);
     240             : }
     241             : 
     242        6045 : void Siyi::update(const class Aircraft &aircraft)
     243             : {
     244        6045 :     update_input();
     245             :     // update_output(location);
     246        6045 : }
     247             : 
     248             : #endif  // AP_SIM_SIYI_ENABLED