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

AP_MSP: added support for MSP sensors

This commit is contained in:
yaapu 2020-09-03 11:15:14 +02:00 committed by Andrew Tridgell
parent 844db20db0
commit 266b747ffc
5 changed files with 47 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
libraries/AP_MSP/AP_MSP_Telem_Backend.cpp
View File

@ -468,13 +468,28 @@ MSPCommandResult AP_MSP_Telem_Backend::msp_process_sensor_command(uint16_t cmd_m
switch (cmd_msp) {
case MSP2_SENSOR_RANGEFINDER: {
const MSP::msp_rangefinder_sensor_t pkt = *(const MSP::msp_rangefinder_sensor_t *)src->ptr;
msp_handle_rangefinder(pkt);
const MSP::msp_rangefinder_sensor_t *pkt = (const MSP::msp_rangefinder_sensor_t *)src->ptr;
msp_handle_rangefinder(*pkt);
}
break;
case MSP2_SENSOR_OPTIC_FLOW: {
const MSP::msp_opflow_sensor_t pkt = *(const MSP::msp_opflow_sensor_t *)src->ptr;
msp_handle_opflow(pkt);
const MSP::msp_opflow_sensor_t *pkt = (const MSP::msp_opflow_sensor_t *)src->ptr;
msp_handle_opflow(*pkt);
}
break;
case MSP2_SENSOR_GPS: {
const MSP::msp_gps_data_message_t *pkt = (const MSP::msp_gps_data_message_t *)src->ptr;
msp_handle_gps(*pkt);
}
break;
case MSP2_SENSOR_COMPASS: {
const MSP::msp_compass_data_message_t *pkt = (const MSP::msp_compass_data_message_t *)src->ptr;
msp_handle_compass(*pkt);
}
break;
case MSP2_SENSOR_BAROMETER: {
const MSP::msp_baro_data_message_t *pkt = (const MSP::msp_baro_data_message_t *)src->ptr;
msp_handle_baro(*pkt);
}
break;
}
@ -504,6 +519,27 @@ void AP_MSP_Telem_Backend::msp_handle_rangefinder(const MSP::msp_rangefinder_sen
#endif
}
void AP_MSP_Telem_Backend::msp_handle_gps(const MSP::msp_gps_data_message_t &pkt)
{
#if HAL_MSP_GPS_ENABLED
AP::gps().handle_msp(pkt);
#endif
}
void AP_MSP_Telem_Backend::msp_handle_compass(const MSP::msp_compass_data_message_t &pkt)
{
#if HAL_MSP_COMPASS_ENABLED
AP::compass().handle_msp(pkt);
#endif
}
void AP_MSP_Telem_Backend::msp_handle_baro(const MSP::msp_baro_data_message_t &pkt)
{
#if HAL_MSP_BARO_ENABLED
AP::baro().handle_msp(pkt);
#endif
}
MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_raw_gps(sbuf_t *dst)
{
#if OSD_ENABLED

3
libraries/AP_MSP/AP_MSP_Telem_Backend.h
View File

@ -161,6 +161,9 @@ protected:
// MSP sensor command processing
void msp_handle_opflow(const MSP::msp_opflow_sensor_t &pkt);
void msp_handle_rangefinder(const MSP::msp_rangefinder_sensor_t &pkt);
void msp_handle_gps(const MSP::msp_gps_data_message_t &pkt);
void msp_handle_compass(const MSP::msp_compass_data_message_t &pkt);
void msp_handle_baro(const MSP::msp_baro_data_message_t &pkt);
// implementation specific helpers
// custom masks are needed for vendor specific settings

16
libraries/AP_MSP/msp.h
View File

@ -11,6 +11,7 @@
#include "msp_protocol.h"
#include "msp_sbuf.h"
#include "msp_version.h"
#include "msp_sensors.h"
#if HAL_MSP_ENABLED
@ -25,10 +26,8 @@
#define MSP_PORT_INBUF_SIZE 192
#define MSP_PORT_OUTBUF_SIZE 512
#define MSP_MAX_HEADER_SIZE 9
// betaflight/src/main/msp/msp_protocol_v2_sensor.h
// inav/src/main/msp/msp_protocol_v2_sensor.h
#define MSP2_IS_SENSOR_MESSAGE(x) ((x) >= 0x1F00U && (x) <= 0x1FFFU)
#define MSP2_SENSOR_RANGEFINDER 0x1F01
#define MSP2_SENSOR_OPTIC_FLOW 0x1F02
class AP_MSP_Telem_Backend;
@ -108,17 +107,6 @@ typedef struct msp_port_s {
uint8_t checksum2;
} msp_port_t;
typedef struct PACKED {
uint8_t quality; // [0;255]
int32_t distance_mm; // Negative value for out of range
} msp_rangefinder_sensor_t;
typedef struct PACKED {
uint8_t quality; // [0;255]
int32_t motion_x;
int32_t motion_y;
} msp_opflow_sensor_t;
// betaflight/src/main/sensors/battery.h
typedef enum : uint8_t {
MSP_BATTERY_OK = 0,

1
libraries/AP_MSP/msp_protocol.h
View File

@ -344,6 +344,7 @@
#define MSP_SET_SIGNATURE 249 //in message Sets the signature of the board and serial number
// V2 commands
#define MSP2_SENSOR_RANGEFINDER 0x1F01
#define MSP2_SENSOR_OPTIC_FLOW 0x1F02
#define MSP2_SENSOR_GPS 0x1F03
#define MSP2_SENSOR_COMPASS 0x1F04