AP_Periph: support output of MSP airspeed sensor

This commit is contained in:
giacomo892 2020-10-24 10:52:29 +02:00 committed by Andrew Tridgell
parent 19f3fda31b
commit 75333dcd2f
4 changed files with 42 additions and 0 deletions

View File

@ -78,6 +78,7 @@ public:
uint32_t last_gps_ms; uint32_t last_gps_ms;
uint32_t last_baro_ms; uint32_t last_baro_ms;
uint32_t last_mag_ms; uint32_t last_mag_ms;
uint32_t last_airspeed_ms;
} msp; } msp;
void msp_init(AP_HAL::UARTDriver *_uart); void msp_init(AP_HAL::UARTDriver *_uart);
void msp_sensor_update(void); void msp_sensor_update(void);
@ -85,6 +86,7 @@ public:
void send_msp_GPS(void); void send_msp_GPS(void);
void send_msp_compass(void); void send_msp_compass(void);
void send_msp_baro(void); void send_msp_baro(void);
void send_msp_airspeed(void);
#endif #endif
#ifdef HAL_PERIPH_ENABLE_ADSB #ifdef HAL_PERIPH_ENABLE_ADSB

View File

@ -48,6 +48,9 @@ void AP_Periph_FW::msp_sensor_update(void)
#ifdef HAL_PERIPH_ENABLE_MAG #ifdef HAL_PERIPH_ENABLE_MAG
send_msp_compass(); send_msp_compass();
#endif #endif
#ifdef HAL_PERIPH_ENABLE_AIRSPEED
send_msp_airspeed();
#endif
} }
@ -160,4 +163,33 @@ void AP_Periph_FW::send_msp_compass(void)
} }
#endif // HAL_PERIPH_ENABLE_MAG #endif // HAL_PERIPH_ENABLE_MAG
#ifdef HAL_PERIPH_ENABLE_AIRSPEED
/*
send MSP airspeed packet
*/
void AP_Periph_FW::send_msp_airspeed(void)
{
MSP::msp_airspeed_data_message_t p;
const uint32_t last_update_ms = airspeed.last_update_ms();
if (msp.last_airspeed_ms == last_update_ms) {
return;
}
msp.last_airspeed_ms = last_update_ms;
p.instance = 0;
p.time_ms = msp.last_airspeed_ms;
p.pressure = airspeed.get_corrected_pressure();
float temp;
if (!airspeed.get_temperature(temp)) {
p.temp = INT16_MIN; //invalid temperature
} else {
p.temp = temp * 100;
}
send_msp_packet(MSP2_SENSOR_AIRSPEED, &p, sizeof(p));
}
#endif // HAL_PERIPH_ENABLE_AIRSPEED
#endif // HAL_PERIPH_ENABLE_MSP #endif // HAL_PERIPH_ENABLE_MSP

View File

@ -349,3 +349,4 @@
#define MSP2_SENSOR_GPS 0x1F03 #define MSP2_SENSOR_GPS 0x1F03
#define MSP2_SENSOR_COMPASS 0x1F04 #define MSP2_SENSOR_COMPASS 0x1F04
#define MSP2_SENSOR_BAROMETER 0x1F05 #define MSP2_SENSOR_BAROMETER 0x1F05
#define MSP2_SENSOR_AIRSPEED 0x1F06

View File

@ -54,4 +54,11 @@ typedef struct PACKED {
int16_t magY; // mGauss, right int16_t magY; // mGauss, right
int16_t magZ; // mGauss, down int16_t magZ; // mGauss, down
} msp_compass_data_message_t; } msp_compass_data_message_t;
typedef struct PACKED {
uint8_t instance;
uint32_t time_ms;
float pressure;
int16_t temp; // centi-degrees C
} msp_airspeed_data_message_t;
} }