rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-28 06:24:10 +01:00
Code Issues Releases Activity

LP-623 Added suport SDP3x I2C Differential Pressure/Airspeed sensor on Flexi Port - tested

Browse Source
This commit is contained in:
Eric Price 2021-10-08 17:51:10 +02:00
parent 7c9f04d87c
commit 00b9d8ae97
17 changed files with 349 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
flight/make/apps-defs.mk
View File

@ -74,6 +74,7 @@ SRC += $(PIOSCOMMON)/pios_adxl345.c
SRC += $(PIOSCOMMON)/pios_bma180.c
SRC += $(PIOSCOMMON)/pios_bmp085.c
SRC += $(PIOSCOMMON)/pios_etasv3.c
SRC += $(PIOSCOMMON)/pios_sdp3x.c
SRC += $(PIOSCOMMON)/pios_gcsrcvr.c
SRC += $(PIOSCOMMON)/pios_hcsr04.c
SRC += $(PIOSCOMMON)/pios_hmc5843.c

7
flight/modules/Airspeed/airspeed.c
View File

@ -43,6 +43,7 @@
#include "airspeedsensor.h" // object that will be updated by the module
#include "baro_airspeed_ms4525do.h"
#include "baro_airspeed_etasv3.h"
#include "baro_airspeed_sdp3x.h"
#include "baro_airspeed_mpxv.h"
#include "imu_airspeed.h"
#include "airspeedalarm.h"
@ -186,6 +187,12 @@ static void airspeedTask(__attribute__((unused)) void *parameters)
baro_airspeedGetETASV3(&airspeedData, &airspeedSettings);
break;
#endif
#if defined(PIOS_INCLUDE_SDP3X)
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_SDP3X:
// SDP3X
baro_airspeedGetSDP3X(&airspeedData, &airspeedSettings);
break;
#endif
#if defined(PIOS_INCLUDE_MS4525DO)
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_PIXHAWKAIRSPEEDMS4525DO:
// PixHawk Airpeed based on MS4525DO

95
flight/modules/Airspeed/baro_airspeed_sdp3x.c Normal file
View File

@ -0,0 +1,95 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup AirspeedModule Airspeed Module
* @brief Communicate with airspeed sensors and return values
* @{
*
* @file baro_airspeed.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Airspeed module, handles temperature and pressure readings from BMP085
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
/**
* Output object: BaroAirspeed
*
* This module will periodically update the value of the BaroAirspeed object.
*
*/
#include "openpilot.h"
#include "hwsettings.h"
#include "airspeedsettings.h"
#include "airspeedsensor.h" // object that will be updated by the module
#include "airspeedalarm.h"
#if defined(PIOS_INCLUDE_SDP3X)
#define CALIBRATION_IDLE_MS 2000 // Time to wait before calibrating, in [ms]
#define CALIBRATION_COUNT_MS 2000 // Time to spend calibrating, in [ms]
#define P0 101325.0f // standard pressure
#define CCEXPONENT 0.2857142857f // exponent of compressibility correction 2/7
#define CASFACTOR 760.8802669f // sqrt(5) * speed of sound at standard
#define TASFACTOR 0.05891022589f // 1/sqrt(T0)
// Private types
// Private variables
// Private functions
static volatile struct PIOS_SDP3X_STATE state = { .status = SDP3X_STATUS_NOTFOUND, .pressure = 0, .temperature = 0 };
void baro_airspeedGetSDP3X(AirspeedSensorData *airspeedSensor, AirspeedSettingsData *airspeedSettings)
{
// Check to see if airspeed sensor is returning airspeedSensor
PIOS_SDP3X_ReadAirspeed(&state);
airspeedSensor->SensorValue = state.pressure;
airspeedSensor->SensorValueTemperature = state.temperature;
if (state.status != SDP3X_STATUS_READY) {
airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
airspeedSensor->CalibratedAirspeed = 0;
AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR);
return;
}
// No calibration, sensor comes factory calibrated!
// Compute airspeed
airspeedSensor->Temperature = ((float)state.temperature / 200.0f) + 273.15f; // convert to kelvin
airspeedSensor->DifferentialPressure = (1.0f / (((float)state.scale) + 1e-9f)) * ((float)(state.pressure) - (float)((int16_t)(airspeedSettings->ZeroPoint)));
airspeedSensor->CalibratedAirspeed = airspeedSettings->Scale * CASFACTOR * sqrtf(powf(fabsf(airspeedSensor->DifferentialPressure) / P0 + 1.0f, CCEXPONENT) - 1.0f);
airspeedSensor->TrueAirspeed = airspeedSensor->CalibratedAirspeed * TASFACTOR * sqrtf(airspeedSensor->Temperature);
airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE;
AirspeedAlarm(SYSTEMALARMS_ALARM_OK);
}
#endif /* if defined(PIOS_INCLUDE_SDP3X) */
/**
* @}
* @}
*/

43
flight/modules/Airspeed/inc/baro_airspeed_sdp3x.h Normal file
View File

@ -0,0 +1,43 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup AirspeedModule Airspeed Module
* @brief Calculate airspeed as a function of the difference between sequential GPS velocity and attitude measurements
* @{
*
* @file baro_airspeed_etasv3.h
* @author The LibrePilot Team, http://www.librepilot.org Copyright (C) 2021.
* @brief Airspeed module, reads temperature and pressure from SDP3X
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef BARO_AIRSPEED_SDP3X_H
#define BARO_AIRSPEED_SDP3X_H
#if defined(PIOS_INCLUDE_SDP3X)
void baro_airspeedGetSDP3X(AirspeedSensorData *airspeedSensor, AirspeedSettingsData *airspeedSettings);
#endif
#endif // BARO_AIRSPEED_SDP3X_H
/**
* @}
* @}
*/

126
flight/pios/common/pios_sdp3x.c Normal file
View File

@ -0,0 +1,126 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_SDP3x SDP3x Differential pressure sensors
* @brief Hardware functions to deal with the Eagle Tree Airspeed MicroSensor V3
* @{
*
* @file pios_stp3x.c
* @author The LibrePilot Team, http://www.librepilot.org Copyright (C) 2021.
* @brief SDP3x Airspeed Sensor Driver
* @see The GNU Public License (GPL) Version 3
*
******************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "pios.h"
#ifdef PIOS_INCLUDE_SDP3X
static bool PIOS_SDP3X_WriteCommand(uint16_t command)
{
uint8_t commandbuffer[2];
commandbuffer[0] = (uint8_t)((uint16_t)(command >> 8));
commandbuffer[1] = (uint8_t)((uint16_t)(command & 0xff));
const struct pios_i2c_txn txn_list[] = {
{
.info = __func__,
.addr = SDP3X_I2C_ADDR1,
.rw = PIOS_I2C_TXN_WRITE,
.len = 2,
.buf = commandbuffer,
}
};
return PIOS_I2C_Transfer(PIOS_I2C_SDP3X_ADAPTER, txn_list, NELEMENTS(txn_list));
}
static bool PIOS_SDP3X_Read(uint8_t *buffer, uint8_t len)
{
const struct pios_i2c_txn txn_list[] = {
{
.info = __func__,
.addr = SDP3X_I2C_ADDR1,
.rw = PIOS_I2C_TXN_READ,
.len = len,
.buf = buffer,
}
};
for (uint8_t retry = PIOS_SDP3X_RETRY_LIMIT; retry > 0; --retry) {
if (PIOS_I2C_Transfer(PIOS_I2C_SDP3X_ADAPTER, txn_list, NELEMENTS(txn_list)) == 0) {
return 0;
}
}
return -1;
}
uint8_t PIOS_SDP3X_crc(const uint8_t buffer[], unsigned len, uint8_t crc)
{
uint8_t c = SDP3X_CRC_INIT;
for (uint8_t i = 0; i < len; i++) {
c ^= buffer[i];
for (uint8_t b = 8; b > 0; --b) {
if (c & 0x80) {
c = (c << 1) ^ SDP3X_CRC_POLY;
} else {
c = c << 1;
}
}
}
return c == crc;
}
void PIOS_SDP3X_ReadAirspeed(volatile struct PIOS_SDP3X_STATE *state)
{
PIOS_Assert(state);
if (state->status == SDP3X_STATUS_NOTFOUND) {
if (PIOS_SDP3X_WriteCommand(SDP3X_CONT_MODE_STOP) == 0) {
PIOS_DELAY_WaituS(500);
if (PIOS_SDP3X_WriteCommand(SDP3X_CONT_MEAS_AVG_MODE) == 0) {
state->status = SDP3X_STATUS_CONFIGURED;
}
}
return;
}
if (state->status == SDP3X_STATUS_CONFIGURED || state->status == SDP3X_STATUS_READY) {
state->status = SDP3X_STATUS_READY;
uint8_t airspeed_raw[9];
if (PIOS_SDP3X_Read(airspeed_raw, sizeof(airspeed_raw)) == 0) {
// check checksum
if (!PIOS_SDP3X_crc(&airspeed_raw[0], 2, airspeed_raw[2]) ||
!PIOS_SDP3X_crc(&airspeed_raw[0], 2, airspeed_raw[2]) ||
!PIOS_SDP3X_crc(&airspeed_raw[0], 2, airspeed_raw[2])) {
// CRC error
state->status = SDP3X_STATUS_CONFIGURED;
return;
}
state->pressure = (int16_t)((((uint16_t)airspeed_raw[0]) << 8) + (uint16_t)airspeed_raw[1]);
state->temperature = (int16_t)((((uint16_t)airspeed_raw[3]) << 8) + (uint16_t)airspeed_raw[4]);
state->scale = (int16_t)((((uint16_t)airspeed_raw[6]) << 8) + (uint16_t)airspeed_raw[7]);
} else {
state->status = SDP3X_STATUS_NOTFOUND;
}
}
}
#endif /* PIOS_INCLUDE_SDP3X */

62
flight/pios/inc/pios_sdp3x.h Normal file
View File

@ -0,0 +1,62 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_SDP3x SDP3x Differential pressure sensors
* @brief Hardware functions to deal with the Eagle Tree Airspeed MicroSensor V3
* @{
*
* @file pios_sdp3x.h
* @author The LibrePilot Team, http://www.librepilot.org Copyright (C) 2021.
* @brief SDP3x Airspeed Sensor Driver
* @see The GNU Public License (GPL) Version 3
*
******************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef PIOS_SDP3X_H
#define PIOS_SDP3X_H
#define SDP3X_I2C_ADDR1 0x21
#define SDP3X_I2C_ADDR2 0x22
#define SDP3X_I2C_ADDR3 0x23
#define SDP3X_SCALE_TEMPERATURE 200.0f
#define SDP3X_RESET_ADDR 0x00
#define SDP3X_RESET_CMD 0x06
#define SDP3X_CONT_MEAS_AVG_MODE 0x3615
#define SDP3X_CONT_MODE_STOP 0x3FF9
#define SDP3X_SCALE_PRESSURE_SDP31 60
#define SDP3X_SCALE_PRESSURE_SDP32 240
#define SDP3X_SCALE_PRESSURE_SDP33 20
#define SDP3X_CRC_INIT 0xff
#define SDP3X_CRC_POLY 0x31
#define SDP3X_CONVERSION_INTERVAL (1000000 / SDP3X_MEAS_RATE) /* microseconds */
#define PIOS_SDP3X_RETRY_LIMIT 3
struct PIOS_SDP3X_STATE {
enum { SDP3X_STATUS_NOTFOUND, SDP3X_STATUS_CONFIGURED, SDP3X_STATUS_READY } status;
int16_t pressure;
int16_t temperature;
int16_t scale;
};
void PIOS_SDP3X_ReadAirspeed(volatile struct PIOS_SDP3X_STATE *state);
#endif /* PIOS_SDP3X_H */

5
flight/pios/pios.h
View File

@ -199,6 +199,11 @@ extern "C" {
#include <pios_etasv3.h>
#endif
#ifdef PIOS_INCLUDE_SDP3X
/* SDP3X Airspeed MicroSensor*/
#include <pios_sdp3x.h>
#endif
#ifdef PIOS_INCLUDE_MS4525DO
/* PixHawk Airspeed Sensor based on MS4525DO */
#include <pios_ms4525do.h>

1
flight/targets/boards/discoveryf4bare/pios_board.h
View File

@ -112,6 +112,7 @@ extern uint32_t pios_i2c_mag_pressure_adapter_id;
extern uint32_t pios_i2c_flexiport_adapter_id;
#define PIOS_I2C_FLEXI_ADAPTER (pios_i2c_flexiport_adapter_id)
#define PIOS_I2C_ETASV3_ADAPTER (PIOS_I2C_FLEXI_ADAPTER)
#define PIOS_I2C_SDP3X_ADAPTER (PIOS_I2C_FLEXI_ADAPTER)
#define PIOS_I2C_EXTERNAL_ADAPTER (PIOS_I2C_FLEXI_ADAPTER)
// -------------------------

1
flight/targets/boards/revolution/firmware/inc/pios_config.h
View File

@ -92,6 +92,7 @@
#define PIOS_INCLUDE_MS56XX
#define PIOS_INCLUDE_MPXV
#define PIOS_INCLUDE_ETASV3
#define PIOS_INCLUDE_SDP3X
#define PIOS_INCLUDE_MS4525DO
/* #define PIOS_INCLUDE_HCSR04 */

1
flight/targets/boards/revolution/pios_board.h
View File

@ -131,6 +131,7 @@ extern uint32_t pios_i2c_mag_pressure_adapter_id;
extern uint32_t pios_i2c_flexiport_adapter_id;
#define PIOS_I2C_FLEXI_ADAPTER (pios_i2c_flexiport_adapter_id)
#define PIOS_I2C_ETASV3_ADAPTER (PIOS_I2C_FLEXI_ADAPTER)
#define PIOS_I2C_SDP3X_ADAPTER (PIOS_I2C_FLEXI_ADAPTER)
#define PIOS_I2C_MS4525DO_ADAPTER (PIOS_I2C_FLEXI_ADAPTER)
#define PIOS_I2C_EXTERNAL_ADAPTER (PIOS_I2C_FLEXI_ADAPTER)

1
flight/targets/boards/revonano/firmware/inc/pios_config.h
View File

@ -91,6 +91,7 @@
#define PIOS_INCLUDE_MS56XX
#define PIOS_INCLUDE_MPXV
#define PIOS_INCLUDE_ETASV3
#define PIOS_INCLUDE_SDP3X
#define PIOS_INCLUDE_MS4525DO
#define PIOS_INCLUDE_MPU9250
#define PIOS_MPU9250_ACCEL

1
flight/targets/boards/revonano/pios_board.h
View File

@ -127,6 +127,7 @@ extern uint32_t pios_i2c_eeprom_pressure_adapter_id;
extern uint32_t pios_i2c_flexiport_adapter_id;
#define PIOS_I2C_FLEXI_ADAPTER (pios_i2c_flexiport_adapter_id)
#define PIOS_I2C_ETASV3_ADAPTER (PIOS_I2C_FLEXI_ADAPTER)
#define PIOS_I2C_SDP3X_ADAPTER (PIOS_I2C_FLEXI_ADAPTER)
#define PIOS_I2C_MS4525DO_ADAPTER (PIOS_I2C_FLEXI_ADAPTER)
#define PIOS_I2C_EXTERNAL_ADAPTER (PIOS_I2C_FLEXI_ADAPTER)

1
flight/targets/boards/revoproto/firmware/inc/pios_config.h
View File

@ -89,6 +89,7 @@
#define PIOS_INCLUDE_MS56XX
#define PIOS_INCLUDE_MPXV
#define PIOS_INCLUDE_ETASV3
#define PIOS_INCLUDE_SDP3X
/* #define PIOS_INCLUDE_HCSR04 */
#define PIOS_SENSOR_RATE 500.0f

1
flight/targets/boards/revoproto/pios_board.h
View File

@ -114,6 +114,7 @@ extern uint32_t pios_i2c_pressure_adapter_id;
extern uint32_t pios_i2c_flexiport_adapter_id;
#define PIOS_I2C_FLEXI_ADAPTER (pios_i2c_flexiport_adapter_id)
#define PIOS_I2C_ETASV3_ADAPTER (PIOS_I2C_FLEXI_ADAPTER)
#define PIOS_I2C_SDP3X_ADAPTER (PIOS_I2C_FLEXI_ADAPTER)
#define PIOS_I2C_EXTERNAL_ADAPTER (pios_i2c_flexiport_adapter_id)

1
flight/targets/boards/sparky2/firmware/inc/pios_config.h
View File

@ -91,6 +91,7 @@
#define PIOS_INCLUDE_MS56XX
#define PIOS_INCLUDE_MPXV
#define PIOS_INCLUDE_ETASV3
#define PIOS_INCLUDE_SDP3X
#define PIOS_INCLUDE_MS4525DO
#define PIOS_INCLUDE_MPU9250
#define PIOS_MPU9250_ACCEL

1
flight/targets/boards/sparky2/pios_board.h
View File

@ -133,6 +133,7 @@ extern uint32_t pios_i2c_pressure_adapter_id;
extern uint32_t pios_i2c_flexiport_adapter_id;
#define PIOS_I2C_FLEXI_ADAPTER (pios_i2c_flexiport_adapter_id)
#define PIOS_I2C_ETASV3_ADAPTER (PIOS_I2C_FLEXI_ADAPTER)
#define PIOS_I2C_SDP3X_ADAPTER (PIOS_I2C_FLEXI_ADAPTER)
#define PIOS_I2C_MS4525DO_ADAPTER (PIOS_I2C_FLEXI_ADAPTER)
// -------------------------

2
shared/uavobjectdefinition/airspeedsettings.xml
View File

@ -4,7 +4,7 @@
<field name="SamplePeriod" units="ms" type="uint8" elements="1" defaultvalue="100"/>
<field name="ZeroPoint" units="raw" type="uint16" elements="1" defaultvalue="0"/>
<field name="Scale" units="raw" type="float" elements="1" defaultvalue="1.0"/>
<field name="AirspeedSensorType" units="" type="enum" elements="1" options="PixHawkAirspeedMS4525DO,EagleTreeAirspeedV3,DIYDronesMPXV5004,DIYDronesMPXV7002,GroundSpeedBasedWindEstimation,None" defaultvalue="None"/>
<field name="AirspeedSensorType" units="" type="enum" elements="1" options="PixHawkAirspeedMS4525DO,EagleTreeAirspeedV3,SDP3x,DIYDronesMPXV5004,DIYDronesMPXV7002,GroundSpeedBasedWindEstimation,None" defaultvalue="None"/>
<field name="IMUBasedEstimationLowPassPeriod1" units="s" type="float" elements="1" defaultvalue="0.5" />
<field name="IMUBasedEstimationLowPassPeriod2" units="s" type="float" elements="1" defaultvalue="10" />
<access gcs="readwrite" flight="readwrite"/>