mirror of https://github.com/ArduPilot/ardupilot
AP_Periph: support output of MSP sensor data
This commit is contained in:
parent
5647aadbcf
commit
9c24415f86
|
@ -141,6 +141,10 @@ void AP_Periph_FW::init()
|
|||
hwesc_telem.init(hal.uartB);
|
||||
#endif
|
||||
|
||||
#ifdef HAL_PERIPH_ENABLE_MSP
|
||||
msp_init(hal.uartD);
|
||||
#endif
|
||||
|
||||
start_ms = AP_HAL::millis();
|
||||
}
|
||||
|
||||
|
|
|
@ -5,6 +5,8 @@
|
|||
#include <AP_Baro/AP_Baro.h>
|
||||
#include <AP_Airspeed/AP_Airspeed.h>
|
||||
#include <AP_RangeFinder/AP_RangeFinder.h>
|
||||
#include <AP_MSP/AP_MSP.h>
|
||||
#include <AP_MSP/msp.h>
|
||||
#include "../AP_Bootloader/app_comms.h"
|
||||
#include "hwing_esc.h"
|
||||
|
||||
|
@ -51,6 +53,22 @@ public:
|
|||
AP_Baro baro;
|
||||
#endif
|
||||
|
||||
#ifdef HAL_PERIPH_ENABLE_MSP
|
||||
struct {
|
||||
AP_MSP msp;
|
||||
MSP::msp_port_t port;
|
||||
uint32_t last_gps_ms;
|
||||
uint32_t last_baro_ms;
|
||||
uint32_t last_mag_ms;
|
||||
} msp;
|
||||
void msp_init(AP_HAL::UARTDriver *_uart);
|
||||
void msp_sensor_update(void);
|
||||
void send_msp_packet(uint16_t cmd, void *p, uint16_t size);
|
||||
void send_msp_GPS(void);
|
||||
void send_msp_compass(void);
|
||||
void send_msp_baro(void);
|
||||
#endif
|
||||
|
||||
#ifdef HAL_PERIPH_ENABLE_ADSB
|
||||
void adsb_init();
|
||||
void adsb_update();
|
||||
|
|
|
@ -1180,6 +1180,9 @@ void AP_Periph_FW::can_update()
|
|||
#ifdef HAL_PERIPH_ENABLE_HWESC
|
||||
hwesc_telem_update();
|
||||
#endif
|
||||
#ifdef HAL_PERIPH_ENABLE_MSP
|
||||
msp_sensor_update();
|
||||
#endif
|
||||
|
||||
processTx();
|
||||
processRx();
|
||||
|
|
|
@ -0,0 +1,163 @@
|
|||
/*
|
||||
output MSP protocol from AP_Periph for ArduPilot and INAV
|
||||
Thanks to input from Konstantin Sharlaimov
|
||||
*/
|
||||
|
||||
#include "AP_Periph.h"
|
||||
|
||||
#ifdef HAL_PERIPH_ENABLE_MSP
|
||||
|
||||
void AP_Periph_FW::msp_init(AP_HAL::UARTDriver *_uart)
|
||||
{
|
||||
msp.port.uart = _uart;
|
||||
msp.port.msp_version = MSP::MSP_V2_NATIVE;
|
||||
_uart->begin(115200, 512, 512);
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
send a MSP packet
|
||||
*/
|
||||
void AP_Periph_FW::send_msp_packet(uint16_t cmd, void *p, uint16_t size)
|
||||
{
|
||||
uint8_t out_buf[size+16] {};
|
||||
MSP::msp_packet_t pkt = {
|
||||
.buf = { .ptr = out_buf, .end = MSP_ARRAYEND(out_buf), },
|
||||
.cmd = (int16_t)cmd,
|
||||
.flags = 0,
|
||||
.result = 0,
|
||||
};
|
||||
|
||||
sbuf_write_data(&pkt.buf, p, size);
|
||||
sbuf_switch_to_reader(&pkt.buf, &out_buf[0]);
|
||||
|
||||
MSP::msp_serial_encode(&msp.port, &pkt, MSP::MSP_V2_NATIVE, true);
|
||||
}
|
||||
|
||||
/*
|
||||
update MSP sensors
|
||||
*/
|
||||
void AP_Periph_FW::msp_sensor_update(void)
|
||||
{
|
||||
#ifdef HAL_PERIPH_ENABLE_GPS
|
||||
send_msp_GPS();
|
||||
#endif
|
||||
#ifdef HAL_PERIPH_ENABLE_BARO
|
||||
send_msp_baro();
|
||||
#endif
|
||||
#ifdef HAL_PERIPH_ENABLE_MAG
|
||||
send_msp_compass();
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
#ifdef HAL_PERIPH_ENABLE_GPS
|
||||
/*
|
||||
send MSP GPS packet
|
||||
*/
|
||||
void AP_Periph_FW::send_msp_GPS(void)
|
||||
{
|
||||
MSP::msp_gps_data_message_t p;
|
||||
|
||||
if (gps.get_type(0) == AP_GPS::GPS_Type::GPS_TYPE_NONE) {
|
||||
return;
|
||||
}
|
||||
if (msp.last_gps_ms == gps.last_message_time_ms(0)) {
|
||||
return;
|
||||
}
|
||||
msp.last_gps_ms = gps.last_message_time_ms(0);
|
||||
|
||||
const Location &loc = gps.location(0);
|
||||
const Vector3f &vel = gps.velocity(0);
|
||||
|
||||
p.instance = 0;
|
||||
p.gps_week = gps.time_week(0);
|
||||
p.ms_tow = gps.get_itow(0);
|
||||
p.fix_type = uint8_t(gps.status(0));
|
||||
p.satellites_in_view = gps.num_sats(0);
|
||||
|
||||
float hacc=0, vacc=0, sacc=0;
|
||||
gps.horizontal_accuracy(0, hacc);
|
||||
gps.vertical_accuracy(0, vacc);
|
||||
gps.speed_accuracy(0, sacc);
|
||||
|
||||
p.horizontal_vel_accuracy = sacc*100;
|
||||
p.horizontal_pos_accuracy = hacc*100;
|
||||
p.vertical_pos_accuracy = vacc*100;
|
||||
p.hdop = gps.get_hdop(0);
|
||||
p.longitude = loc.lng;
|
||||
p.latitude = loc.lat;
|
||||
p.msl_altitude = loc.alt;
|
||||
p.ned_vel_north = vel.x*100;
|
||||
p.ned_vel_east = vel.y*100;
|
||||
p.ned_vel_down = vel.z*100;
|
||||
p.ground_course = wrap_360_cd(gps.ground_course(0)*100);
|
||||
float yaw_deg=0, acc;
|
||||
if (gps.gps_yaw_deg(0, yaw_deg, acc)) {
|
||||
p.true_yaw = wrap_360_cd(yaw_deg*100);
|
||||
} else {
|
||||
p.true_yaw = 65535; // unknown
|
||||
}
|
||||
uint64_t tepoch_us = gps.time_epoch_usec(0);
|
||||
time_t utc_sec = tepoch_us / (1000U * 1000U);
|
||||
struct tm* tm = gmtime(&utc_sec);
|
||||
|
||||
p.year = tm->tm_year+1900;
|
||||
p.month = tm->tm_mon;
|
||||
p.day = tm->tm_mday;
|
||||
p.hour = tm->tm_hour;
|
||||
p.min = tm->tm_min;
|
||||
p.sec = tm->tm_sec;
|
||||
|
||||
send_msp_packet(MSP2_SENSOR_GPS, &p, sizeof(p));
|
||||
}
|
||||
#endif // HAL_PERIPH_ENABLE_GPS
|
||||
|
||||
|
||||
#ifdef HAL_PERIPH_ENABLE_BARO
|
||||
/*
|
||||
send MSP baro packet
|
||||
*/
|
||||
void AP_Periph_FW::send_msp_baro(void)
|
||||
{
|
||||
MSP::msp_baro_data_message_t p;
|
||||
|
||||
if (msp.last_baro_ms == baro.get_last_update(0)) {
|
||||
return;
|
||||
}
|
||||
msp.last_baro_ms = baro.get_last_update(0);
|
||||
|
||||
p.instance = 0;
|
||||
p.time_ms = msp.last_baro_ms;
|
||||
p.pressure_pa = baro.get_pressure();
|
||||
p.temp = baro.get_temperature() * 100;
|
||||
|
||||
send_msp_packet(MSP2_SENSOR_BAROMETER, &p, sizeof(p));
|
||||
}
|
||||
#endif // HAL_PERIPH_ENABLE_BARO
|
||||
|
||||
#ifdef HAL_PERIPH_ENABLE_MAG
|
||||
/*
|
||||
send MSP compass packet
|
||||
*/
|
||||
void AP_Periph_FW::send_msp_compass(void)
|
||||
{
|
||||
MSP::msp_compass_data_message_t p;
|
||||
|
||||
if (msp.last_mag_ms == compass.last_update_ms(0)) {
|
||||
return;
|
||||
}
|
||||
msp.last_mag_ms = compass.last_update_ms(0);
|
||||
|
||||
const Vector3f &field = compass.get_field(0);
|
||||
p.instance = 0;
|
||||
p.time_ms = msp.last_mag_ms;
|
||||
p.magX = field.x;
|
||||
p.magY = field.y;
|
||||
p.magZ = field.z;
|
||||
|
||||
send_msp_packet(MSP2_SENSOR_COMPASS, &p, sizeof(p));
|
||||
}
|
||||
#endif // HAL_PERIPH_ENABLE_MAG
|
||||
|
||||
#endif // HAL_PERIPH_ENABLE_MSP
|
|
@ -34,6 +34,7 @@ def build(bld):
|
|||
'AP_Airspeed',
|
||||
'AP_RangeFinder',
|
||||
'AP_ROMFS',
|
||||
'AP_MSP',
|
||||
],
|
||||
exclude_src=[
|
||||
'libraries/AP_HAL_ChibiOS/Storage.cpp'
|
||||
|
|
|
@ -0,0 +1,59 @@
|
|||
#include <stdint.h>
|
||||
|
||||
namespace MSP
|
||||
{
|
||||
// inav/src/main/io/rangefinder_msp.c
|
||||
typedef struct PACKED {
|
||||
uint8_t quality; // [0;255]
|
||||
int32_t distance_mm; // Negative value for out of range
|
||||
} msp_rangefinder_sensor_t;
|
||||
|
||||
// inav/src/main/io/opflow_msp.c
|
||||
typedef struct PACKED {
|
||||
uint8_t quality; // [0;255]
|
||||
int32_t motion_x;
|
||||
int32_t motion_y;
|
||||
} msp_opflow_sensor_t;
|
||||
|
||||
// inav/src/main/io/gps_msp.c
|
||||
typedef struct PACKED {
|
||||
uint8_t instance; // sensor instance number to support multi-sensor setups
|
||||
uint16_t gps_week; // GPS week, 0xFFFF if not available
|
||||
uint32_t ms_tow;
|
||||
uint8_t fix_type;
|
||||
uint8_t satellites_in_view;
|
||||
uint16_t horizontal_pos_accuracy; // [cm]
|
||||
uint16_t vertical_pos_accuracy; // [cm]
|
||||
uint16_t horizontal_vel_accuracy; // [cm/s]
|
||||
uint16_t hdop;
|
||||
int32_t longitude;
|
||||
int32_t latitude;
|
||||
int32_t msl_altitude; // cm
|
||||
int32_t ned_vel_north; // cm/s
|
||||
int32_t ned_vel_east;
|
||||
int32_t ned_vel_down;
|
||||
uint16_t ground_course; // deg * 100, 0..36000
|
||||
uint16_t true_yaw; // deg * 100, values of 0..36000 are valid. 65535 = no data available
|
||||
uint16_t year;
|
||||
uint8_t month;
|
||||
uint8_t day;
|
||||
uint8_t hour;
|
||||
uint8_t min;
|
||||
uint8_t sec;
|
||||
} msp_gps_data_message_t;
|
||||
|
||||
typedef struct PACKED {
|
||||
uint8_t instance;
|
||||
uint32_t time_ms;
|
||||
float pressure_pa;
|
||||
int16_t temp; // centi-degrees C
|
||||
} msp_baro_data_message_t;
|
||||
|
||||
typedef struct PACKED {
|
||||
uint8_t instance;
|
||||
uint32_t time_ms;
|
||||
int16_t magX; // mGauss, front
|
||||
int16_t magY; // mGauss, right
|
||||
int16_t magZ; // mGauss, down
|
||||
} msp_compass_data_message_t;
|
||||
}
|
Loading…
Reference in New Issue