diff --git a/flight/Modules/Battery/battery.c b/flight/Modules/Battery/battery.c index afcfd9300..f25b925b1 100644 --- a/flight/Modules/Battery/battery.c +++ b/flight/Modules/Battery/battery.c @@ -88,7 +88,7 @@ int32_t BatteryInitialize(void) HwSettingsOptionalModulesGet(optionalModules); HwSettingsADCRoutingGet(adcRouting); - //Determine if the + //Determine if the battery sensors are routed to ADC pins for (int i=0; i < HWSETTINGS_ADCROUTING_NUMELEM; i++) { if (adcRouting[i] == HWSETTINGS_ADCROUTING_VOLTAGE) { voltageADCPin = i; diff --git a/flight/PiOS/Common/pios_mpxv5004.c b/flight/PiOS/Common/pios_mpxv5004.c new file mode 100644 index 000000000..fbab5001b --- /dev/null +++ b/flight/PiOS/Common/pios_mpxv5004.c @@ -0,0 +1,105 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_MPXV5004 MPXV5004 Functions + * @brief Hardware functions to deal with the DIYDrones airspeed kit, using MPXV5004. + * This is a differential sensor, so the value returned is first converted into + * calibrated airspeed, using http://en.wikipedia.org/wiki/Calibrated_airspeed + * @{ + * + * @file pios_mpxv5004.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief ETASV3 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 + */ + +/* Project Includes */ +#include "pios.h" + +#if defined(PIOS_INCLUDE_MPXV5004) + +#include "pios_mpxv5004.h" + +static uint32_t calibrationSum = 0; //static? +static int16_t calibrationOffset; //static? + + +/* + * Reads ADC. + */ +uint16_t PIOS_MPXV5004_Measure(uint8_t airspeedADCPin) +{ + return PIOS_ADC_PinGet(airspeedADCPin); +} + +/* + *Returns zeroPoint so that the user can inspect the calibration vs. the sensor value + */ +uint16_t PIOS_MPXV5004_Calibrate(uint8_t airspeedADCPin, uint16_t calibrationCount){ + calibrationSum += PIOS_MPXV5004_Measure(airspeedADCPin); + uint16_t zeroPoint = (uint16_t)(((float)calibrationSum) / calibrationCount + 0.5f); + + calibrationOffset = zeroPoint - (int16_t)(1.0f/3.3f*4096.0f+0.5f); //The offset should set the zero point to 1.0V + + return zeroPoint; +} + + +/* + * Updates the calibration when zero point is manually set by user. + */ +void PIOS_MPXV5004_UpdateCalibration(uint16_t zeroPoint){ + calibrationOffset = zeroPoint - (int16_t)(1.0f/3.3f*4096.0f+0.5f); //The offset should set the zero point to 1.0V +} + + +/* + * Reads the airspeed and returns CAS (calibrated airspeed) in the case of success. + * In the case of a failed read, returns -1. + */ +float PIOS_MPXV5004_ReadAirspeed(uint8_t airspeedADCPin) +{ + + float sensorVal = PIOS_MPXV5004_Measure(airspeedADCPin); + if (sensorVal == 0) //As of June 20th, 2012, the ADC read module was written in such a way that it reset the value to 0 immediately after a read. In case another module inadvertently reads the ADC, we prefer not to use a reading of 0 + return -1; + + //Calculate dynamic pressure, as per docs + float Qc = 5.0f*(((sensorVal - calibrationOffset)/4096.0f*3.3f)/VCC - 0.2f); + + //Saturate Qc on the lower bound, in order to make sure we don't have negative airspeeds. No need + // to saturate on the upper bound, we'll handle that later with calibratedAirspeed. + if (Qc < 0) { + Qc=0; + } + + //Compute calibraterd airspeed, as per http://en.wikipedia.org/wiki/Calibrated_airspeed + float calibratedAirspeed = A0*sqrt(5.0f*(pow(Qc/P0+1.0f,POWER)-1.0f)); + + //Upper bound airspeed. No need to lower bound it, that comes from Qc + if (calibratedAirspeed > 80) { //in [m/s] + calibratedAirspeed=80; + } + + + return calibratedAirspeed; +} + +#endif /* PIOS_INCLUDE_MPXV5004 */ diff --git a/flight/PiOS/Common/pios_mpxv7002.c b/flight/PiOS/Common/pios_mpxv7002.c new file mode 100644 index 000000000..68384ccc8 --- /dev/null +++ b/flight/PiOS/Common/pios_mpxv7002.c @@ -0,0 +1,105 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_MPXV7002 MPXV7002 Functions + * @brief Hardware functions to deal with the DIYDrones airspeed kit, using MPXV7002. + * This is a differential sensor, so the value returned is first converted into + * calibrated airspeed, using http://en.wikipedia.org/wiki/Calibrated_airspeed + * @{ + * + * @file pios_mpxv7002.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief ETASV3 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 + */ + +/* Project Includes */ +#include "pios.h" + +#if defined(PIOS_INCLUDE_MPXV7002) + +#include "pios_mpxv7002.h" + +static uint32_t calibrationSum = 0; //static? +static int16_t calibrationOffset; //static? + + +/* + * Reads ADC. + */ +uint16_t PIOS_MPXV7002_Measure(uint8_t airspeedADCPin) +{ + return PIOS_ADC_PinGet(airspeedADCPin); +} + +/* + *Returns zeroPoint so that the user can inspect the calibration vs. the sensor value + */ +uint16_t PIOS_MPXV7002_Calibrate(uint8_t airspeedADCPin, uint16_t calibrationCount){ + calibrationSum += PIOS_MPXV7002_Measure(airspeedADCPin); + uint16_t zeroPoint = (uint16_t)(((float)calibrationSum) / calibrationCount + 0.5f); + + calibrationOffset = zeroPoint - (int16_t)(2.5f/3.3f*4096.0f+0.5f); //The offset should set the zero point to 2.5V + + return zeroPoint; +} + + +/* + * Updates the calibration when zero point is manually set by user. + */ +void PIOS_MPXV7002_UpdateCalibration(uint16_t zeroPoint){ + calibrationOffset = zeroPoint - (int16_t)(2.5f/3.3f*4096.0f+0.5f); //The offset should set the zero point to 2.5V +} + + +/* + * Reads the airspeed and returns CAS (calibrated airspeed) in the case of success. + * In the case of a failed read, returns -1. + */ +float PIOS_MPXV7002_ReadAirspeed(uint8_t airspeedADCPin) +{ + + float sensorVal = PIOS_MPXV7002_Measure(airspeedADCPin); + if (sensorVal == 0) //As of June 20th, 2012, the ADC read module was written in such a way that it reset the value to 0 immediately after a read. In case someone else reads the ADC inadvertently, we prefer not to use a reading of 0 + return -1; + + //Calculate dynamic pressure, as per docs + float Qc = 5.0f*(((sensorVal - calibrationOffset)/4096.0f*3.3f)/VCC - 0.5f); + + //Saturate Qc on the lower bound, in order to make sure we don't have negative airspeeds. No need + // to saturate on the upper bound, we'll handle that later with calibratedAirspeed. + if (Qc < 0) { + Qc=0; + } + + //Compute calibraterd airspeed, as per http://en.wikipedia.org/wiki/Calibrated_airspeed + float calibratedAirspeed = A0*sqrt(5.0f*(pow(Qc/P0+1.0f,POWER)-1.0f)); + + //Upper bound airspeed. No need to lower bound it, that comes from Qc + if (calibratedAirspeed > 59) { //in [m/s] + calibratedAirspeed=59; + } + + + return calibratedAirspeed; +} + +#endif /* PIOS_INCLUDE_MPXV7002 */ diff --git a/flight/PiOS/inc/pios_mpxv5004.h b/flight/PiOS/inc/pios_mpxv5004.h new file mode 100644 index 000000000..2ebf1dee9 --- /dev/null +++ b/flight/PiOS/inc/pios_mpxv5004.h @@ -0,0 +1,43 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_MPXV5004 MPXV5004 Functions + * @brief Hardware functions to deal with the DIYDrones airspeed kit, using MPXV5004 + * @{ + * + * @file pios_mpxv5004.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief ETASV3 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_MPXV5004_H__ + +#define A0 340.27f //speed of sound at standard sea level in [m/s] +#define P0 101.325f //static air pressure at standard sea level in kPa +#define VCC 5.0f //Supply voltage in V +#define POWER (2.0f/7.0f) + +uint16_t PIOS_MPXV5004_Measure(uint8_t airspeedADCPin); +uint16_t PIOS_MPXV5004_Calibrate(uint8_t airspeedADCPin, uint16_t calibrationCount); +void PIOS_MPXV5004_UpdateCalibration(uint16_t zeroPoint); +float PIOS_MPXV5004_ReadAirspeed (uint8_t airspeedADCPin); + +#endif // PIOS_MPXV5004_H__ \ No newline at end of file diff --git a/flight/PiOS/inc/pios_mpxv7002.h b/flight/PiOS/inc/pios_mpxv7002.h new file mode 100644 index 000000000..5f9a8f3a7 --- /dev/null +++ b/flight/PiOS/inc/pios_mpxv7002.h @@ -0,0 +1,43 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_MPXV7002 MPXV7002 Functions + * @brief Hardware functions to deal with the DIYDrones airspeed kit, using MPXV7002 + * @{ + * + * @file pios_mpxv7002.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief ETASV3 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_MPXV7002_H__ + +#define A0 340.27f //speed of sound at standard sea level in [m/s] +#define P0 101.325f //static air pressure at standard sea level in kPa +#define VCC 5.0f //Supply voltage in V +#define POWER (2.0f/7.0f) + +uint16_t PIOS_MPXV7002_Measure(uint8_t airspeedADCPin); +uint16_t PIOS_MPXV7002_Calibrate(uint8_t airspeedADCPin, uint16_t calibrationCount); +void PIOS_MPXV7002_UpdateCalibration(uint16_t zeroPoint); +float PIOS_MPXV7002_ReadAirspeed (uint8_t airspeedADCPin); + +#endif // PIOS_MPXV7002_H__ \ No newline at end of file diff --git a/flight/PiOS/pios.h b/flight/PiOS/pios.h index 2f84dd8de..015985662 100644 --- a/flight/PiOS/pios.h +++ b/flight/PiOS/pios.h @@ -100,7 +100,15 @@ /* PIOS Hardware Includes (Common) */ #include #include +#if defined(PIOS_INCLUDE_MPXV7002) +#include +#endif +#if defined(PIOS_INCLUDE_MPXV5004) +#include +#endif +#if defined(PIOS_INCLUDE_ETASV3) #include +#endif #if defined(PIOS_INCLUDE_BMP085) #include #endif diff --git a/flight/Revolution/Makefile b/flight/Revolution/Makefile index 96e3fb4de..dd8bfdba4 100644 --- a/flight/Revolution/Makefile +++ b/flight/Revolution/Makefile @@ -149,6 +149,8 @@ include $(PIOS)/STM32F4xx/library.mk SRC += $(PIOSCOMMON)/pios_mpu6000.c SRC += $(PIOSCOMMON)/pios_bma180.c SRC += $(PIOSCOMMON)/pios_etasv3.c +SRC += $(PIOSCOMMON)/pios_mpxv5004.c +SRC += $(PIOSCOMMON)/pios_mpxv7002.c SRC += $(PIOSCOMMON)/pios_l3gd20.c SRC += $(PIOSCOMMON)/pios_hmc5883.c SRC += $(PIOSCOMMON)/pios_ms5611.c diff --git a/flight/Revolution/System/inc/pios_config.h b/flight/Revolution/System/inc/pios_config.h index 3129439e5..d4afe9d36 100644 --- a/flight/Revolution/System/inc/pios_config.h +++ b/flight/Revolution/System/inc/pios_config.h @@ -64,6 +64,8 @@ #define PIOS_INCLUDE_L3GD20 #define PIOS_INCLUDE_MS5611 #define PIOS_INCLUDE_ETASV3 +//#define PIOS_INCLUDE_MPXV5004 +//#define PIOS_INCLUDE_MPXV7002 //#define PIOS_INCLUDE_HCSR04 #define PIOS_FLASH_ON_ACCEL /* true for second revo */ #define FLASH_FREERTOS