Line data    Source code

       1             : #include "AP_Mount_config.h"
       2             : 
       3             : #if HAL_MOUNT_STORM32SERIAL_ENABLED
       4             : 
       5             : #include "AP_Mount_SToRM32_serial.h"
       6             : 
       7             : #include <AP_HAL/AP_HAL.h>
       8             : #include <GCS_MAVLink/GCS_MAVLink.h>
       9             : #include <GCS_MAVLink/include/mavlink/v2.0/checksum.h>
      10             : 
      11             : // update mount position - should be called periodically
      12           0 : void AP_Mount_SToRM32_serial::update()
      13             : {
      14           0 :     AP_Mount_Backend::update();
      15             : 
      16             :     // exit immediately if not initialised
      17           0 :     if (!_initialised) {
      18           0 :         return;
      19             :     }
      20             : 
      21           0 :     read_incoming(); // read the incoming messages from the gimbal
      22             : 
      23           0 :     AP_Mount_Backend::update_mnt_target();
      24             : 
      25             :     // send target angles (which may be derived from other target types)
      26           0 :     AP_Mount_Backend::send_target_to_gimbal();
      27             : 
      28           0 :     if ((AP_HAL::millis() - _last_send) > AP_MOUNT_STORM32_SERIAL_TIMEOUT_MS) {
      29           0 :         _reply_type = ReplyType_UNKNOWN;
      30             :     }
      31             : }
      32             : 
      33             : // get attitude as a quaternion.  returns true on success
      34           0 : bool AP_Mount_SToRM32_serial::get_attitude_quaternion(Quaternion& att_quat)
      35             : {
      36           0 :     att_quat.from_euler(cd_to_rad(_current_angle.x), cd_to_rad(_current_angle.y), cd_to_rad(_current_angle.z));
      37           0 :     return true;
      38             : }
      39             : 
      40           0 : bool AP_Mount_SToRM32_serial::can_send() {
      41           0 :     uint16_t required_tx = 1;
      42           0 :     required_tx += sizeof(AP_Mount_SToRM32_serial::cmd_set_angles_struct);
      43           0 :     return (_reply_type == ReplyType_UNKNOWN) && (_uart->txspace() >= required_tx);
      44             : }
      45             : 
      46             : 
      47             : // send_target_angles
      48           0 : void AP_Mount_SToRM32_serial::send_target_angles(const MountAngleTarget& angle_target_rad)
      49             : {
      50           0 :     if (!can_send()) {
      51           0 :         return;
      52             :     }
      53             : 
      54             :     static cmd_set_angles_struct cmd_set_angles_data = {
      55             :         0xFA,
      56             :         0x0E,
      57             :         0x11,
      58             :         0, // pitch
      59             :         0, // roll
      60             :         0, // yaw
      61             :         0, // flags
      62             :         0, // type
      63             :         0, // crc
      64             :     };
      65             : 
      66             :     // exit immediately if not initialised
      67           0 :     if (!_initialised) {
      68           0 :         return;
      69             :     }
      70             : 
      71           0 :     if ((size_t)_uart->txspace() < sizeof(cmd_set_angles_data)) {
      72           0 :         return;
      73             :     }
      74             : 
      75             :     // send CMD_SETANGLE (Note: reversed pitch and yaw)
      76           0 :     cmd_set_angles_data.pitch = -degrees(angle_target_rad.pitch);
      77           0 :     cmd_set_angles_data.roll = degrees(angle_target_rad.roll);
      78           0 :     cmd_set_angles_data.yaw = -degrees(angle_target_rad.get_bf_yaw());
      79             : 
      80           0 :     uint8_t* buf = (uint8_t*)&cmd_set_angles_data;
      81             : 
      82           0 :     cmd_set_angles_data.crc = crc_calculate(&buf[1], sizeof(cmd_set_angles_data)-3);
      83             : 
      84           0 :     for (uint8_t i = 0;  i != sizeof(cmd_set_angles_data) ; i++) {
      85           0 :         _uart->write(buf[i]);
      86             :     }
      87             : 
      88             :     // store time of send
      89           0 :     _last_send = AP_HAL::millis();
      90             : 
      91             :     // we pipeline commands.  We have sent in a command to set angles,
      92             :     // now fetch data from the device:
      93           0 :     get_angles();
      94             :     // we expect an ACK back for the set-angles command.  A state
      95             :     // machine in read_incoming will move us to ReplyType_DATA once
      96             :     // the ACK has been received.
      97           0 :     _reply_type = ReplyType_ACK;
      98           0 :     _reply_counter = 0;
      99           0 :     _reply_length = get_reply_size(_reply_type);
     100             : 
     101             : }
     102             : 
     103           0 : void AP_Mount_SToRM32_serial::get_angles() {
     104             :     // exit immediately if not initialised
     105           0 :     if (!_initialised) {
     106           0 :         return;
     107             :     }
     108             : 
     109           0 :     if (_uart->txspace() < 1) {
     110           0 :         return;
     111             :     }
     112             : 
     113           0 :     _uart->write('d');
     114             : };
     115             : 
     116             : 
     117           0 : uint8_t AP_Mount_SToRM32_serial::get_reply_size(ReplyType reply_type) {
     118           0 :     switch (reply_type) {
     119           0 :         case ReplyType_DATA:
     120           0 :             return sizeof(SToRM32_reply_data_struct);
     121             :             break;
     122           0 :         case ReplyType_ACK:
     123           0 :             return sizeof(SToRM32_reply_ack_struct);
     124             :             break;
     125           0 :         default:
     126           0 :             return 0;
     127             :     }
     128             : }
     129             : 
     130             : 
     131           0 : void AP_Mount_SToRM32_serial::read_incoming() {
     132             :     uint8_t data;
     133             :     int16_t numc;
     134             : 
     135           0 :     numc = _uart->available();
     136             : 
     137           0 :     if (numc < 0 ) {
     138           0 :         return;
     139             :     }
     140             : 
     141           0 :     for (int16_t i = 0; i < numc; i++) {        // Process bytes received
     142           0 :         data = _uart->read();
     143           0 :         if (_reply_type == ReplyType_UNKNOWN) {
     144           0 :             continue;
     145             :         }
     146             : 
     147           0 :         _buffer[_reply_counter++] = data;
     148           0 :         if (_reply_counter == _reply_length) {
     149           0 :             parse_reply();
     150             : 
     151           0 :             switch (_reply_type) {
     152           0 :                 case ReplyType_ACK:
     153           0 :                     _reply_type = ReplyType_DATA;
     154           0 :                     _reply_length = get_reply_size(_reply_type);
     155           0 :                     _reply_counter = 0;
     156           0 :                     break;
     157           0 :                 case ReplyType_DATA:
     158           0 :                     _reply_type = ReplyType_UNKNOWN;
     159           0 :                     _reply_length = get_reply_size(_reply_type);
     160           0 :                     _reply_counter = 0;
     161           0 :                     break;
     162           0 :                 default:
     163           0 :                     _reply_length = get_reply_size(_reply_type);
     164           0 :                     _reply_counter = 0;
     165           0 :                     break;
     166             :             }
     167             :         }
     168             :     }
     169             : }
     170             : 
     171           0 : void AP_Mount_SToRM32_serial::parse_reply() {
     172             :     uint16_t crc;
     173             :     bool crc_ok;
     174             : 
     175           0 :     switch (_reply_type) {
     176           0 :         case ReplyType_DATA:
     177           0 :             crc = crc_calculate(&_buffer[0], sizeof(_buffer.data) - 3);
     178           0 :             crc_ok = crc == _buffer.data.crc;
     179           0 :             if (!crc_ok) {
     180           0 :                 break;
     181             :             }
     182             : 
     183             :             // Parse angles (Note: reversed pitch and yaw) to match ardupilot coordinate system
     184           0 :             _current_angle.x = _buffer.data.imu1_roll;
     185           0 :             _current_angle.y = -_buffer.data.imu1_pitch;
     186           0 :             _current_angle.z = -_buffer.data.imu1_yaw;
     187           0 :             break;
     188           0 :         default:
     189           0 :             break;
     190             :     }
     191           0 : }
     192             : #endif // HAL_MOUNT_STORM32SERIAL_ENABLED