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 Viewpro gimbal
      17             : */
      18             : 
      19             : #include "SIM_config.h"
      20             : 
      21             : #if AP_SIM_VIEWPRO_ENABLED
      22             : 
      23             : #include "SIM_Viewpro.h"
      24             : #include "SIM_Aircraft.h"
      25             : #include <AP_Math/AP_Math.h>
      26             : #include <errno.h>
      27             : 
      28             : using namespace SITL;
      29             : 
      30             : // scalar from AP_Mount_Viewpro.cpp
      31             : #define VIEWPRO_DEG_TO_OUTPUT  (65536.0f / 360.0f)
      32             : 
      33      249745 : void Viewpro::update(const Aircraft &aircraft)
      34             : {
      35             :     // Drive GimbalSim toward the angles commanded via A1 packets.
      36             :     // The A1 wire encoding is: pitch_raw = -pitch_deg * DEG_TO_OUTPUT,
      37             :     // yaw_raw = yaw_deg * DEG_TO_OUTPUT.  Invert to recover target radians.
      38             :     {
      39             :         static constexpr float OUTPUT_TO_RAD = (360.0f / 65536.0f) * (float)DEG_TO_RAD;
      40      249745 :         const float target_pitch_rad = -_target_pitch_raw * OUTPUT_TO_RAD;
      41      249745 :         const float target_yaw_rad   =  _target_yaw_raw   * OUTPUT_TO_RAD;
      42             : 
      43      249745 :         Vector3f ja;
      44      249745 :         gimbal.get_joint_angles(ja);
      45             : 
      46             :         // P-gain: drives gimbal to commanded angle with ~0.1 s time constant.
      47             :         // joint_angles.y = pitch (negative = down), .z = azimuth.
      48      249745 :         Matrix3f gimbal_dcm;
      49      249745 :         gimbal.get_dcm(gimbal_dcm);
      50      249745 :         const Vector3f vehicle_rate_gimbal = gimbal_dcm.transposed() * aircraft.get_dcm() * aircraft.get_gyro();
      51             : 
      52             :         static constexpr float GAIN = 10.0f;
      53      249745 :         gimbal.set_demanded_rates(Vector3f(
      54      249745 :             vehicle_rate_gimbal.x,
      55      249745 :             vehicle_rate_gimbal.y + (target_pitch_rad - ja.y) * GAIN,
      56      249745 :             vehicle_rate_gimbal.z + (target_yaw_rad   - ja.z) * GAIN));
      57             :     }
      58             : 
      59      249745 :     gimbal.update(aircraft);
      60      249745 :     update_input();
      61             : 
      62             :     // send attitude at 10 Hz
      63      249745 :     const uint32_t now_ms = AP_HAL::millis();
      64      249745 :     if (now_ms - _last_attitude_ms >= 100) {
      65        2080 :         _last_attitude_ms = now_ms;
      66        2080 :         send_t1_f1_b1_d1();
      67             :     }
      68      249745 : }
      69             : 
      70             : /*
      71             :   read bytes from the autopilot and process them byte-by-byte through the
      72             :   Viewpro protocol state machine.
      73             : 
      74             :   The driver discards header bytes without storing them.  After all three
      75             :   header bytes are consumed the internal packet buffer starts fresh:
      76             :     _buf[0]   = length+frame_counter byte  (bits 0-5 = body_length)
      77             :     _buf[1]   = frame_id byte
      78             :     _buf[2+]  = data bytes
      79             :     _buf[N]   = CRC byte (last)
      80             : 
      81             :   body_length = 1(length byte) + 1(frame_id) + data_len + 1(crc)
      82             :   => data_len = body_length - 3
      83             : */
      84      249745 : void Viewpro::update_input()
      85             : {
      86             :     // read new bytes into a scratch buffer; process one at a time
      87             :     uint8_t scratch[128];
      88      249745 :     const ssize_t n = read_from_autopilot((char*)scratch, sizeof(scratch));
      89      249745 :     if (n < 0) {
      90           0 :         if (errno != EAGAIN && errno != EWOULDBLOCK && errno != 0) {
      91           0 :             AP_HAL::panic("Failed to read from autopilot");
      92             :         }
      93           0 :         return;
      94             :     }
      95             : 
      96      390049 :     for (ssize_t i = 0; i < n; i++) {
      97      140304 :         const uint8_t b = scratch[i];
      98             : 
      99      140304 :         switch (_parse_state) {
     100        6094 :         case ParseState::HEADER1:
     101        6094 :             if (b == HEADER1) {
     102        6094 :                 _buflen = 0;
     103        6094 :                 _parse_state = ParseState::HEADER2;
     104             :             }
     105        6094 :             break;
     106             : 
     107        6094 :         case ParseState::HEADER2:
     108        6094 :             if (b == HEADER2) {
     109        6094 :                 _buflen = 0;
     110        6094 :                 _parse_state = ParseState::HEADER3;
     111             :             } else {
     112           0 :                 _buflen = 0;
     113           0 :                 _parse_state = ParseState::HEADER1;
     114             :             }
     115        6094 :             break;
     116             : 
     117        6094 :         case ParseState::HEADER3:
     118        6094 :             if (b == HEADER3) {
     119        6094 :                 _buflen = 0;
     120        6094 :                 _parse_state = ParseState::LENGTH;
     121             :             } else {
     122           0 :                 _buflen = 0;
     123           0 :                 _parse_state = ParseState::HEADER1;
     124             :             }
     125        6094 :             break;
     126             : 
     127        6094 :         case ParseState::LENGTH: {
     128        6094 :             const uint8_t body_length = b & 0x3F;
     129        6094 :             if (body_length < 3) {
     130           0 :                 _buflen = 0;
     131           0 :                 _parse_state = ParseState::HEADER1;
     132           0 :                 break;
     133             :             }
     134        6094 :             _data_len = body_length - 3;
     135        6094 :             _buf[_buflen++] = b;    // _buf[0] = length+fc byte
     136        6094 :             _parse_state = ParseState::FRAMEID;
     137        6094 :             break;
     138             :         }
     139             : 
     140        6094 :         case ParseState::FRAMEID:
     141        6094 :             _data_bytes_received = 0;
     142        6094 :             _buf[_buflen++] = b;    // _buf[1] = frame_id
     143        6094 :             _parse_state = (_data_len > 0) ? ParseState::DATA : ParseState::CHECKSUM;
     144        6094 :             break;
     145             : 
     146      103740 :         case ParseState::DATA:
     147      103740 :             if (_buflen < PACKETLEN_MAX) {
     148      103740 :                 _buf[_buflen++] = b;
     149             :             }
     150      103740 :             _data_bytes_received++;
     151      103740 :             if (_data_bytes_received >= _data_len) {
     152        6094 :                 _parse_state = ParseState::CHECKSUM;
     153             :             }
     154      103740 :             break;
     155             : 
     156        6094 :         case ParseState::CHECKSUM: {
     157        6094 :             if (_buflen < PACKETLEN_MAX) {
     158        6094 :                 _buf[_buflen++] = b;
     159             :             }
     160             :             // verify CRC: XOR of _buf[0.._buflen-2] (all bytes except the crc byte itself)
     161        6094 :             const uint8_t expected_crc = crc_xor_of_bytes(_buf, _buflen - 1);
     162        6094 :             if (expected_crc == b) {
     163        6094 :                 dispatch_packet();
     164             :             }
     165        6094 :             _buflen = 0;
     166        6094 :             _parse_state = ParseState::HEADER1;
     167        6094 :             break;
     168             :         }
     169             :         }
     170             :     }
     171             : }
     172             : 
     173        6094 : void Viewpro::dispatch_packet()
     174             : {
     175             :     // _buf[1] = frame_id; _buf[2+] = data (data_start = 2)
     176        6094 :     const FrameId frame_id = (FrameId)_buf[1];
     177             : 
     178        6094 :     switch (frame_id) {
     179        2056 :     case FrameId::HANDSHAKE:
     180        2056 :         send_t1_f1_b1_d1();
     181        2056 :         break;
     182             : 
     183           2 :     case FrameId::U:
     184             :         // _buf[2] = data[0] = CommConfigCmd
     185           2 :         if (_data_len >= 1) {
     186           2 :             send_v_response((CommConfigCmd)_buf[2]);
     187             :         }
     188           2 :         break;
     189             : 
     190        2056 :     case FrameId::A1:
     191             :         // _buf layout: [0]=length_fc, [1]=frame_id, [2]=servo_status,
     192             :         //              [3..4]=yaw big-endian, [5..6]=pitch big-endian
     193             :         // Store raw int16 values so the driver round-trips them without
     194             :         // any additional float rounding.
     195        2056 :         if (_data_len >= 5) {
     196        2056 :             _target_yaw_raw   = (int16_t)(((uint16_t)_buf[3] << 8) | _buf[4]);
     197        2056 :             _target_pitch_raw = (int16_t)(((uint16_t)_buf[5] << 8) | _buf[6]);
     198             :         }
     199        2056 :         break;
     200        1980 :     case FrameId::C1:
     201             :     case FrameId::C2:
     202             :     case FrameId::E1:
     203             :     case FrameId::E2:
     204             :     case FrameId::M_AHRS:
     205             :     case FrameId::HEARTBEAT:
     206             :         // absorb; gimbal physics is updated via update()
     207        1980 :         break;
     208             : 
     209           0 :     default:
     210           0 :         break;
     211             :     }
     212        6094 : }
     213             : 
     214             : /*
     215             :   send T1_F1_B1_D1 (0x40) packet with current gimbal attitude.
     216             : 
     217             :   Driver reads angles using _msg_buff_data_start=2:
     218             :     data[22]     = tracking status (bits 3-4; 0 = stopped)
     219             :     data[23]     = servo_status(upper 4 bits) | roll_12bit_MSByte(lower 4 bits)
     220             :     data[24]     = roll LSByte   → roll_deg = 12-bit * 180/4095 - 90
     221             :     data[25..26] = yaw int16 big-endian  → yaw_deg = val * 360/65536
     222             :     data[27..28] = pitch int16 big-endian  → pitch_deg = val * 360/65536  (driver negates)
     223             :     data[29]     = image sensor (bits 0-2; stored as val+1; 0→EO1)
     224             :     data[32]     = recording status (bits 0-2; 0=stopped)
     225             :     data[33..34] = rangefinder × 0.1 m
     226             :     data[39..40] = zoom × 0.1
     227             : */
     228        4136 : void Viewpro::send_t1_f1_b1_d1()
     229             : {
     230             :     static constexpr uint8_t DATA_LEN = 41;
     231             :     // databuff: frame_id (1 byte) + data (DATA_LEN bytes)
     232        4136 :     uint8_t databuff[1 + DATA_LEN] {};
     233        4136 :     databuff[0] = (uint8_t)FrameId::T1_F1_B1_D1;
     234             : 
     235        4136 :     uint8_t *data = &databuff[1];
     236             : 
     237             :     // Report actual GimbalSim joint angles.
     238             :     // joint_angles.y = pitch (negative = down), .z = azimuth.
     239             :     // Wire encoding: pitch_out = -pitch_deg * DEG_TO_OUTPUT (driver negates on read).
     240             :     //                yaw_out   =  yaw_deg   * DEG_TO_OUTPUT.
     241             :     // roll: 12-bit value where 2048 (midpoint of 0..4095) ≈ 0 deg (actual = +0.022 deg,
     242             :     //       within the neutral_tol_deg=0.05 used by MountViewPro).
     243             :     // Pitch clamp to ±16380 (±89.978 deg): at exactly ±90 deg (raw ±16384) the float32
     244             :     // from_euler quaternion with the small roll (~0.022 deg) has DCM[2][0] > 1.0 in
     245             :     // float64 (Python), triggering pymavlink gimbal-lock → 180 deg instead of ±90 deg.
     246        4136 :     Vector3f ja;
     247        4136 :     gimbal.get_joint_angles(ja);
     248        4136 :     const int16_t yaw_out   = (int16_t)(degrees(ja.z) * VIEWPRO_DEG_TO_OUTPUT);
     249        4136 :     const int16_t pitch_out = (int16_t)constrain_int16(
     250        4136 :         (int16_t)(-degrees(ja.y) * VIEWPRO_DEG_TO_OUTPUT),
     251             :         -16380, 16380);
     252        4136 :     const uint16_t roll_out = 2048U;
     253             : 
     254             :     // bytes 0..21: T1/F1 fields — leave as zero
     255        4136 :     data[22] = 0x00;    // tracking stopped
     256             : 
     257        4136 :     data[23] = (uint8_t)((roll_out >> 8) & 0x0F);   // lower nibble = roll MSB; upper nibble = 0 (servo status)
     258        4136 :     data[24] = (uint8_t)(roll_out & 0xFF);
     259             : 
     260        4136 :     data[25] = (uint8_t)((uint16_t)yaw_out >> 8);
     261        4136 :     data[26] = (uint8_t)((uint16_t)yaw_out & 0xFF);
     262             : 
     263        4136 :     data[27] = (uint8_t)((uint16_t)pitch_out >> 8);
     264        4136 :     data[28] = (uint8_t)((uint16_t)pitch_out & 0xFF);
     265             : 
     266        4136 :     data[29] = 0x00;    // image sensor bits 0-2 = 0 → driver stores as EO1
     267        4136 :     data[32] = 0x00;    // recording stopped
     268        4136 :     data[33] = 0x00;    // rangefinder MSB
     269        4136 :     data[34] = 0x00;    // rangefinder LSB
     270        4136 :     data[39] = 0x00;    // zoom MSB
     271        4136 :     data[40] = 0x0A;    // zoom LSB: 10 × 0.1 = 1.0× zoom
     272             : 
     273        4136 :     send_packet(databuff, sizeof(databuff));
     274        4136 : }
     275             : 
     276             : /*
     277             :   send V (0x02) response to a U communication config command.
     278             : 
     279             :   Firmware version response (QUERY_FIRMWARE_VER = 0xD0):
     280             :     data[0] = 0xD0 (echo of CommConfigCmd)
     281             :     data[1] = 'S'  prefix
     282             :     data[2..9] = "yyyymmdd" date string
     283             :     driver reads: major=atoi(data[4..5]), minor=atoi(data[6..7]), patch=atoi(data[8..9])
     284             :     Using "20220301": major=22, minor=03, patch=01
     285             : 
     286             :   Model name response (QUERY_MODEL = 0xE4):
     287             :     data[0] = 0xE4
     288             :     data[1+] = model name bytes
     289             :     driver: memcpy(_model_name, &data[1], data_bytes_received-1)
     290             : */
     291           2 : void Viewpro::send_v_response(CommConfigCmd cmd)
     292             : {
     293           2 :     uint8_t databuff[32] {};
     294             :     uint8_t total_len;
     295             : 
     296           2 :     databuff[0] = (uint8_t)FrameId::V;
     297             : 
     298           2 :     switch (cmd) {
     299           1 :     case CommConfigCmd::QUERY_FIRMWARE_VER:
     300           1 :         databuff[1] = (uint8_t)cmd;
     301           1 :         databuff[2] = 'S';          // prefix character
     302           1 :         databuff[3] = 'y';
     303           1 :         databuff[4] = 's';
     304           1 :         databuff[5] = '2';          // major hi: "20"
     305           1 :         databuff[6] = '0';
     306           1 :         databuff[7] = '2';          // minor hi: "22"
     307           1 :         databuff[8] = '2';
     308           1 :         databuff[9] = '0';          // patch hi: "01"
     309           1 :         databuff[10] = '1';
     310           1 :         total_len = 11;
     311           1 :         break;
     312             : 
     313           1 :     case CommConfigCmd::QUERY_MODEL:
     314             :         // datasheet specifies a 10-byte model name field in the response
     315           1 :         databuff[1] = (uint8_t)cmd;
     316           1 :         databuff[2] = 'S';
     317           1 :         databuff[3] = 'I';
     318           1 :         databuff[4] = 'M';
     319           1 :         databuff[5] = '_';
     320           1 :         databuff[6] = 'V';
     321           1 :         databuff[7] = 'P';
     322           1 :         databuff[8] = 0;
     323           1 :         databuff[9] = 0;
     324           1 :         databuff[10] = 0;
     325           1 :         databuff[11] = 0;
     326           1 :         total_len = 12;
     327           1 :         break;
     328             : 
     329           0 :     default:
     330           0 :         return;
     331             :     }
     332             : 
     333           2 :     send_packet(databuff, total_len);
     334             : }
     335             : 
     336             : /*
     337             :   build and write a complete Viewpro packet to the autopilot.
     338             :   databuff[0] = frame_id, databuff[1+] = data payload.
     339             : 
     340             :   Wire format:
     341             :     0x55 0xAA 0xDC [length_fc] [databuff...] [crc]
     342             :   where:
     343             :     length_fc bits 6-7 = frame counter (0-3, increments per packet)
     344             :     length_fc bits 0-5 = databuff_len + 2  (= body_length)
     345             :     crc = XOR of [length_fc .. last_databuff_byte]
     346             : */
     347        4138 : void Viewpro::send_packet(const uint8_t *databuff, uint8_t databuff_len)
     348             : {
     349             :     // total wire bytes: 3 (header) + 1 (length_fc) + databuff_len + 1 (crc)
     350        4138 :     const uint8_t wire_len = 5 + databuff_len;
     351        4138 :     if (wire_len > (uint8_t)(PACKETLEN_MAX + 4)) {
     352           0 :         return;
     353             :     }
     354             : 
     355             :     uint8_t pkt[PACKETLEN_MAX + 4];
     356        4138 :     uint8_t ofs = 0;
     357             : 
     358        4138 :     pkt[ofs++] = HEADER1;
     359        4138 :     pkt[ofs++] = HEADER2;
     360        4138 :     pkt[ofs++] = HEADER3;
     361             : 
     362        4138 :     _frame_counter = (_frame_counter + 1) & 0x03;
     363        4138 :     const uint8_t body_length = (uint8_t)(databuff_len + 2);
     364        4138 :     pkt[ofs++] = (uint8_t)((_frame_counter << 6) | (body_length & 0x3F));
     365             : 
     366      177873 :     for (uint8_t i = 0; i < databuff_len; i++) {
     367      173735 :         pkt[ofs++] = databuff[i];
     368             :     }
     369             : 
     370             :     // CRC: XOR of bytes [3]..[ofs-1] (length_fc through last data byte)
     371        4138 :     pkt[ofs] = crc_xor_of_bytes(&pkt[3], ofs - 3);
     372        4138 :     ofs++;
     373             : 
     374        4138 :     write_to_autopilot((const char*)pkt, ofs);
     375             : }
     376             : 
     377             : 
     378             : #endif  // AP_SIM_VIEWPRO_ENABLED