Line data    Source code

       1             : #include "SIM_config.h"
       2             : 
       3             : #if AP_SIM_GPS_ENABLED
       4             : 
       5             : #include "SIM_GPS_SBP_Common.h"
       6             : 
       7             : #include <AP_Math/crc.h>
       8             : 
       9             : using namespace SITL;
      10             : 
      11         720 : void GPS_SBP_Common::sbp_send_message(uint16_t msg_type, uint16_t sender_id, uint8_t len, uint8_t *payload)
      12             : {
      13         720 :     if (len != 0 && payload == nullptr) {
      14           0 :         return; //SBP_NULL_ERROR;
      15             :     }
      16             : 
      17         720 :     uint8_t preamble = 0x55;
      18         720 :     write_to_autopilot((char*)&preamble, 1);
      19         720 :     write_to_autopilot((char*)&msg_type, 2);
      20         720 :     write_to_autopilot((char*)&sender_id, 2);
      21         720 :     write_to_autopilot((char*)&len, 1);
      22         720 :     if (len > 0) {
      23         720 :         write_to_autopilot((char*)payload, len);
      24             :     }
      25             : 
      26             :     uint16_t crc;
      27         720 :     crc = crc16_ccitt((uint8_t*)&(msg_type), 2, 0);
      28         720 :     crc = crc16_ccitt((uint8_t*)&(sender_id), 2, crc);
      29         720 :     crc = crc16_ccitt(&(len), 1, crc);
      30         720 :     crc = crc16_ccitt(payload, len, crc);
      31         720 :     write_to_autopilot((char*)&crc, 2);
      32             : }
      33             : 
      34             : #endif  // AP_SIM_GPS_ENABLED