Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Periph: support output of MSP sensor data

This commit is contained in:
Andrew Tridgell 2020-09-03 21:47:34 +10:00
parent 5647aadbcf
commit 9c24415f86
6 changed files with 248 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
Tools/AP_Periph/AP_Periph.cpp
View File

@ -140,6 +140,10 @@ void AP_Periph_FW::init()
#ifdef HAL_PERIPH_ENABLE_HWESC
hwesc_telem.init(hal.uartB);
#endif
#ifdef HAL_PERIPH_ENABLE_MSP
msp_init(hal.uartD);
#endif
start_ms = AP_HAL::millis();
}

18
Tools/AP_Periph/AP_Periph.h
View File

@ -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();

3
Tools/AP_Periph/can.cpp
View File

@ -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();

163
Tools/AP_Periph/msp.cpp Normal file
View File

@ -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

1
Tools/AP_Periph/wscript
View File

@ -34,6 +34,7 @@ def build(bld):
'AP_Airspeed',
'AP_RangeFinder',
'AP_ROMFS',
'AP_MSP',
],
exclude_src=[
'libraries/AP_HAL_ChibiOS/Storage.cpp'

59
libraries/AP_MSP/msp_sensors.h Normal file
View File

@ -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;
}