Tools/AP_Periph/msp.cpp

   1 /*
   2   output MSP protocol from AP_Periph for ArduPilot and INAV
   3   Thanks to input from Konstantin Sharlaimov
   4  */
   5
   6 #include <AP_HAL/AP_HAL_Boards.h>
   7 #include "AP_Periph.h"
   8
   9 #ifdef HAL_PERIPH_ENABLE_MSP
  10
  11 void AP_Periph_FW::msp_init(AP_HAL::UARTDriver *_uart)
  12 {
  13     if (_uart) {
  14         msp.port.uart = _uart;
  15         msp.port.msp_version = MSP::MSP_V2_NATIVE;
  16         _uart->begin(115200, 512, 512);
  17     }
  18 }
  19
  20
  21 /*
  22   send a MSP packet
  23  */
  24 void AP_Periph_FW::send_msp_packet(uint16_t cmd, void *p, uint16_t size)
  25 {
  26     uint8_t out_buf[size+16] {};
  27     MSP::msp_packet_t pkt = {
  28         .buf = { .ptr = out_buf, .end = MSP_ARRAYEND(out_buf), },
  29         .cmd = (int16_t)cmd,
  30         .flags = 0,
  31         .result = 0,
  32     };
  33
  34     sbuf_write_data(&pkt.buf, p, size);
  35     sbuf_switch_to_reader(&pkt.buf, &out_buf[0]);
  36
  37     MSP::msp_serial_encode(&msp.port, &pkt, MSP::MSP_V2_NATIVE, true);
  38 }
  39
  40 /*
  41   update MSP sensors
  42  */
  43 void AP_Periph_FW::msp_sensor_update(void)
  44 {
  45     if (msp.port.uart == nullptr) {
  46         return;
  47     }
  48 #if AP_PERIPH_GPS_ENABLED
  49     send_msp_GPS();
  50 #endif
  51 #if AP_PERIPH_BARO_ENABLED
  52     send_msp_baro();
  53 #endif
  54 #if AP_PERIPH_MAG_ENABLED
  55     send_msp_compass();
  56 #endif
  57 #ifdef HAL_PERIPH_ENABLE_AIRSPEED
  58     send_msp_airspeed();
  59 #endif
  60 }
  61
  62
  63 #if AP_PERIPH_GPS_ENABLED
  64 /*
  65   send MSP GPS packet
  66  */
  67 void AP_Periph_FW::send_msp_GPS(void)
  68 {
  69     MSP::msp_gps_data_message_t p;
  70
  71     if (gps.get_type(0) == AP_GPS::GPS_Type::GPS_TYPE_NONE) {
  72         return;
  73     }
  74     if (msp.last_gps_ms == gps.last_message_time_ms(0)) {
  75         return;
  76     }
  77     msp.last_gps_ms = gps.last_message_time_ms(0);
  78
  79     const Location &loc = gps.location(0);
  80     const Vector3f &vel = gps.velocity(0);
  81
  82     p.instance = 0;
  83     p.gps_week = gps.time_week(0);
  84     p.ms_tow = gps.get_itow(0);
  85     p.fix_type = uint8_t(gps.status(0));
  86     p.satellites_in_view = gps.num_sats(0);
  87
  88     float hacc=0, vacc=0, sacc=0;
  89     gps.horizontal_accuracy(0, hacc);
  90     gps.vertical_accuracy(0, vacc);
  91     gps.speed_accuracy(0, sacc);
  92
  93     p.horizontal_vel_accuracy = sacc*100;
  94     p.horizontal_pos_accuracy = hacc*100;
  95     p.vertical_pos_accuracy = vacc*100;
  96     p.hdop = gps.get_hdop(0);
  97     p.longitude = loc.lng;
  98     p.latitude = loc.lat;
  99     p.msl_altitude = loc.alt;
 100     p.ned_vel_north = vel.x*100;
 101     p.ned_vel_east = vel.y*100;
 102     p.ned_vel_down = vel.z*100;
 103     p.ground_course = wrap_360_cd(gps.ground_course(0)*100);
 104     float yaw_deg=0, acc;
 105     uint32_t time_ms;
 106     if (gps.gps_yaw_deg(0, yaw_deg, acc, time_ms)) {
 107         p.true_yaw = wrap_360_cd(yaw_deg*100);
 108     } else {
 109         p.true_yaw = 65535; // unknown
 110     }
 111     uint64_t tepoch_us = gps.time_epoch_usec(0);
 112     time_t utc_sec = tepoch_us / (1000U * 1000U);
 113     struct tm tvd {};
 114     struct tm* tm = gmtime_r(&utc_sec, &tvd);
 115
 116     p.year = tm->tm_year+1900;
 117     p.month = tm->tm_mon;
 118     p.day = tm->tm_mday;
 119     p.hour = tm->tm_hour;
 120     p.min = tm->tm_min;
 121     p.sec = tm->tm_sec;
 122
 123     send_msp_packet(MSP2_SENSOR_GPS, &p, sizeof(p));
 124 }
 125 #endif // AP_PERIPH_GPS_ENABLED
 126
 127
 128 #if AP_PERIPH_BARO_ENABLED
 129 /*
 130   send MSP baro packet
 131  */
 132 void AP_Periph_FW::send_msp_baro(void)
 133 {
 134     MSP::msp_baro_data_message_t p;
 135
 136     if (msp.last_baro_ms == baro.get_last_update(0)) {
 137         return;
 138     }
 139     if (!baro.healthy()) {
 140         // don't send any data
 141         return;
 142     }
 143     msp.last_baro_ms = baro.get_last_update(0);
 144
 145     p.instance = 0;
 146     p.time_ms = msp.last_baro_ms;
 147     p.pressure_pa = baro.get_pressure();
 148     p.temp = baro.get_temperature() * 100;
 149
 150     send_msp_packet(MSP2_SENSOR_BAROMETER, &p, sizeof(p));
 151 }
 152 #endif // AP_PERIPH_BARO_ENABLED
 153
 154 #if AP_PERIPH_MAG_ENABLED
 155 /*
 156   send MSP compass packet
 157  */
 158 void AP_Periph_FW::send_msp_compass(void)
 159 {
 160     MSP::msp_compass_data_message_t p;
 161
 162     if (msp.last_mag_ms == compass.last_update_ms(0)) {
 163         return;
 164     }
 165     if (!compass.healthy()) {
 166         return;
 167     }
 168     msp.last_mag_ms = compass.last_update_ms(0);
 169
 170     const Vector3f &field = compass.get_field(0);
 171     p.instance = 0;
 172     p.time_ms = msp.last_mag_ms;
 173     p.magX = field.x;
 174     p.magY = field.y;
 175     p.magZ = field.z;
 176
 177     send_msp_packet(MSP2_SENSOR_COMPASS, &p, sizeof(p));
 178 }
 179 #endif // AP_PERIPH_MAG_ENABLED
 180
 181 #ifdef HAL_PERIPH_ENABLE_AIRSPEED
 182 /*
 183   send MSP airspeed packet
 184  */
 185 void AP_Periph_FW::send_msp_airspeed(void)
 186 {
 187     MSP::msp_airspeed_data_message_t p;
 188
 189     const uint32_t last_update_ms = airspeed.last_update_ms();
 190     if (msp.last_airspeed_ms == last_update_ms) {
 191         return;
 192     }
 193     if (!airspeed.healthy()) {
 194         // we don't report at all for an unhealthy sensor. This maps
 195         // to unhealthy in the flight controller driver
 196         return;
 197     }
 198     msp.last_airspeed_ms = last_update_ms;
 199
 200     p.instance = 0;
 201     p.time_ms = msp.last_airspeed_ms;
 202     p.pressure = airspeed.get_corrected_pressure();
 203     float temp;
 204     if (!airspeed.get_temperature(temp)) {
 205         p.temp = INT16_MIN; //invalid temperature
 206     } else {
 207         p.temp = temp * 100;
 208     }
 209
 210     send_msp_packet(MSP2_SENSOR_AIRSPEED, &p, sizeof(p));
 211 }
 212 #endif // HAL_PERIPH_ENABLE_AIRSPEED
 213
 214
 215 #endif // HAL_PERIPH_ENABLE_MSP