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             :   Simulator for Siyi ZT30
      16             : 
      17             :  */
      18             : 
      19             : #include "SIM_config.h"
      20             : 
      21             : #if AP_SIM_SIYI_ZT30_ENABLED
      22             : 
      23             : #include "SIM_Siyi_ZT30.h"
      24             : 
      25             : using namespace SITL;
      26             : 
      27        6045 : void Siyi_ZT30::update(const Aircraft &aircraft)
      28             : {
      29        6045 :     gimbal.update(aircraft);
      30             : 
      31             :     // base class update function:
      32        6045 :     Siyi::update(aircraft);
      33        6045 : }
      34             : 
      35           1 : void Siyi_ZT30::handle_message(const PackedMessage<FirmwareVersionRequest> &request)
      36             : {
      37           1 :     if (!request.response_requested()) {
      38             :         // weird
      39           0 :         AP_HAL::panic("response not requested?");
      40             :     }
      41             :     const PackedMessage<FirmwareVersion> response{
      42             :         FirmwareVersion{
      43             :             camera_major, camera_minor, camera_patch,
      44             :             gimbal_major, gimbal_minor, gimbal_patch,
      45             :             zoom_major, zoom_minor, zoom_patch
      46             :         },
      47           1 :     };
      48           1 :     write_to_autopilot((char*)&response, sizeof(response));
      49           1 : }
      50             : 
      51           1 : void Siyi_ZT30::handle_message(const PackedMessage<HardwareIDRequest> &request)
      52             : {
      53           1 :     if (!request.response_requested()) {
      54             :         // weird
      55           0 :         AP_HAL::panic("response not requested?");
      56             :     }
      57             :     const PackedMessage<HardwareID> response{
      58             :         HardwareID{
      59             :             hwid_a, hwid_b
      60             :         },
      61           1 :     };
      62           1 :     write_to_autopilot((char*)&response, sizeof(response));
      63           1 : }
      64             : 
      65          43 : void Siyi_ZT30::handle_message(const PackedMessage<GimbalAttitudeRequest> &request)
      66             : {
      67          43 :     if (!request.response_requested()) {
      68             :         // weird
      69           0 :         AP_HAL::panic("response not requested?");
      70             :     }
      71             : 
      72             :     uint32_t delta_time_us;
      73          43 :     Vector3f delta_angles;
      74          43 :     Vector3f delta_velocities;
      75          43 :     gimbal.get_deltas(delta_angles, delta_velocities, delta_time_us);
      76             :     const Vector3<int16_t> rates_decidegrees(
      77          43 :         (delta_angles.x * RAD_TO_DEG * 1e6 / delta_time_us),
      78          43 :         delta_angles.y * RAD_TO_DEG * 1e6 / delta_time_us,
      79          43 :         delta_angles.z * RAD_TO_DEG * 1e6 / delta_time_us
      80          43 :     );
      81             : 
      82          43 :     Vector3f joint_angles;
      83          43 :     gimbal.get_joint_angles(joint_angles);
      84             :     const Vector3<int16_t> joint_angles_decidegrees(
      85          43 :         joint_angles.x * 10 * RAD_TO_DEG,
      86          43 :         joint_angles.y * 10 * RAD_TO_DEG,
      87          43 :         joint_angles.z * 10 * RAD_TO_DEG
      88          43 :     );
      89             : 
      90             :     const PackedMessage<GimbalAttitude> response{
      91             :         GimbalAttitude{
      92          43 :             joint_angles_decidegrees.x, joint_angles_decidegrees.y, joint_angles_decidegrees.z,
      93          43 :             rates_decidegrees.x, rates_decidegrees.y, rates_decidegrees.z
      94             :         },
      95          43 :     };
      96          43 :     write_to_autopilot((char*)&response, sizeof(response));
      97          43 : }
      98             : 
      99          22 : void Siyi_ZT30::handle_message(const PackedMessage<ExternalAttitude> &request)
     100             : {
     101             :     // MAVProxy and the ArduPilot driver both request an ACK but the
     102             :     // cameras don't seem to supply it.
     103             : 
     104             :     // if (request.ctrl & 0x1) {
     105             :     //     AP_HAL::panic("Response requested for ExternalAttitude?");
     106             :     // }
     107          22 : }
     108             : 
     109           0 : void Siyi_ZT30::handle_message(const PackedMessage<SetCameraImageType> &request)
     110             : {
     111             :     // MAVProxy and the ArduPilot driver both request an ACK but the
     112             :     // cameras don't seem to supply it.
     113             : 
     114             :     // if (request.ctrl & 0x1) {
     115             :     //     AP_HAL::panic("Response requested for SetCameraImageType?");
     116             :     // }
     117           0 : }
     118             : 
     119           1 : void Siyi_ZT30::handle_message(const PackedMessage<Photo> &request)
     120             : {
     121             :     // MAVProxy and the ArduPilot driver both request an ACK but the
     122             :     // cameras don't seem to supply it.
     123             : 
     124             :     // if (request.ctrl & 0x1) {
     125             :     //     AP_HAL::panic("Response requested for SetCameraImageType?");
     126             :     // }
     127           1 : }
     128             : 
     129           2 : void Siyi_ZT30::handle_message(const PackedMessage<AcquireGimbalConfigInfo> &request)
     130             : {
     131             :     // MAVProxy and the ArduPilot driver both request an ACK but the
     132             :     // cameras don't seem to supply it.
     133             : 
     134             :     // if (request.ctrl & 0x1) {
     135             :     //     AP_HAL::panic("Response requested for AcquireGimbalConfigInfo?");
     136             :     // }
     137           2 : }
     138             : 
     139         127 : void Siyi_ZT30::handle_message(const PackedMessage<GimbalRotation> &request)
     140             : {
     141         127 :     const float max_demanded_yaw_rate_rad = 2*M_PI;
     142         127 :     const float max_demanded_pitch_rate_rad = 2*M_PI;
     143             : 
     144             :     const Vector3f demanded_rates{
     145         127 :         max_demanded_yaw_rate_rad * request.msg.yaw_rate_pct * 0.01f,
     146         127 :         max_demanded_pitch_rate_rad * request.msg.pitch_rate_pct * 0.01f,
     147             :         0
     148         127 :     };
     149         127 :     gimbal.set_demanded_rates(demanded_rates);
     150             : 
     151             :     // MAVProxy and the ArduPilot driver both request an ACK but the
     152             :     // cameras don't seem to supply it.
     153             : 
     154             :     // if (request.ctrl & 0x1) {
     155             :     //     AP_HAL::panic("Response requested for GimbalRotation?");
     156             :     // }
     157         127 : }
     158             : 
     159           0 : void Siyi_ZT30::handle_message(const PackedMessage<PositionData> &request)
     160             : {
     161             :     // MAVProxy and the ArduPilot driver both request an ACK but the
     162             :     // cameras don't seem to supply it.
     163             : 
     164             :     // if (request.ctrl & 0x1) {
     165             :     //     AP_HAL::panic("Response requested for GimbalRotation?");
     166             :     // }
     167           0 : }
     168             : 
     169          22 : void Siyi_ZT30::handle_message(const PackedMessage<ReadRangefinderRequest> &request)
     170             : {
     171             :     const PackedMessage<RangefinderDistance> response{
     172             :         RangefinderDistance{50},  // 50m simulated distance
     173          22 :     };
     174          22 :     write_to_autopilot((char*)&response, sizeof(response));
     175          22 : }
     176             : 
     177           0 : void Siyi_ZT30::handle_message(const PackedMessage<SetTime> &request)
     178             : {
     179           0 :     epoch_us = request.msg.epoch_us;
     180             : 
     181           0 :     if (!request.response_requested()) {
     182             :         // weird
     183           0 :         return;
     184             :     }
     185             : 
     186             :     const PackedMessage<SetTimeResponse> response{
     187             :         SetTimeResponse{1},
     188           0 :     };
     189           0 :     write_to_autopilot((char*)&response, sizeof(response));
     190             : }
     191             : 
     192             : #endif  // AP_SIM_SIYI_ZT30_ENABLED