From 853600a2eff3fcf013ec7b9dc10302d0694f56a5 Mon Sep 17 00:00:00 2001 From: Stacey Sheldon Date: Tue, 17 Apr 2012 09:38:27 -0400 Subject: [PATCH 01/11] i2c: enable flexiport i2c on revolution --- flight/PiOS/Boards/STM32F4xx_Revolution.h | 3 +++ flight/Revolution/System/pios_board.c | 5 +---- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/flight/PiOS/Boards/STM32F4xx_Revolution.h b/flight/PiOS/Boards/STM32F4xx_Revolution.h index a69952ec6..1bec9e414 100644 --- a/flight/PiOS/Boards/STM32F4xx_Revolution.h +++ b/flight/PiOS/Boards/STM32F4xx_Revolution.h @@ -103,6 +103,9 @@ TIM8 | | | | #define PIOS_I2C_MAX_DEVS 3 extern uint32_t pios_i2c_mag_adapter_id; #define PIOS_I2C_MAIN_ADAPTER (pios_i2c_mag_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) //------------------------- // PIOS_USART diff --git a/flight/Revolution/System/pios_board.c b/flight/Revolution/System/pios_board.c index 0d57da254..b6c275166 100644 --- a/flight/Revolution/System/pios_board.c +++ b/flight/Revolution/System/pios_board.c @@ -669,15 +669,12 @@ void PIOS_Board_Init(void) { case HWSETTINGS_RV_FLEXIPORT_DISABLED: break; case HWSETTINGS_RV_FLEXIPORT_I2C: -//TODO: Enable I2C #if defined(PIOS_INCLUDE_I2C) -/* { - if (PIOS_I2C_Init(&pios_i2c_flexi_adapter_id, &pios_i2c_flexi_adapter_cfg)) { + if (PIOS_I2C_Init(&pios_i2c_flexiport_adapter_id, &pios_i2c_flexiport_adapter_cfg)) { PIOS_Assert(0); } } -*/ #endif /* PIOS_INCLUDE_I2C */ break; From 6a37e433b69ade0b3dddf265cd377badce84f72b Mon Sep 17 00:00:00 2001 From: Stacey Sheldon Date: Tue, 17 Apr 2012 09:39:00 -0400 Subject: [PATCH 02/11] etasv3: Add driver for Eagle Tree Airspeed Sensor v3 --- flight/PiOS/Common/pios_etasv3.c | 77 ++++++++++++++++++++++++++++++++ flight/PiOS/inc/pios_etasv3.h | 32 +++++++++++++ 2 files changed, 109 insertions(+) create mode 100644 flight/PiOS/Common/pios_etasv3.c create mode 100644 flight/PiOS/inc/pios_etasv3.h diff --git a/flight/PiOS/Common/pios_etasv3.c b/flight/PiOS/Common/pios_etasv3.c new file mode 100644 index 000000000..6c7cac483 --- /dev/null +++ b/flight/PiOS/Common/pios_etasv3.c @@ -0,0 +1,77 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_ETASV3 ETASV3 Functions + * @brief Hardware functions to deal with the Eagle Tree Airspeed MicroSensor V3 + * @{ + * + * @file pios_etasv3.c + * @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_ETASV3) + +#include "pios_etasv3.h" + +static bool PIOS_ETASV3_Read(uint8_t command, uint8_t * buffer, uint8_t len) +{ + uint8_t cmd_buffer[] = { + command, + }; + + const struct pios_i2c_txn txn_list[] = { + { + .info = __func__, + .addr = ETASV3_I2C_ADDR, + .rw = PIOS_I2C_TXN_WRITE, + .len = sizeof(cmd_buffer), + .buf = cmd_buffer, + } + , + { + .info = __func__, + .addr = ETASV3_I2C_ADDR, + .rw = PIOS_I2C_TXN_READ, + .len = len, + .buf = buffer, + } + }; + + return PIOS_I2C_Transfer(PIOS_I2C_ETASV3_ADAPTER, txn_list, NELEMENTS(txn_list)); +} + +int16_t PIOS_ETASV3_ReadAirspeed (void) +{ + uint8_t airspeed_raw[2]; + + if (!PIOS_ETASV3_Read(ETASV3_I2C_READ_CMD, airspeed_raw, sizeof(airspeed_raw))) { + /* Failed to read airspeed */ + return -1; + } + + return (airspeed_raw[0] << 8 || airspeed_raw[1]); +} + +#endif /* PIOS_INCLUDE_ETASV3 */ diff --git a/flight/PiOS/inc/pios_etasv3.h b/flight/PiOS/inc/pios_etasv3.h new file mode 100644 index 000000000..82949a2ef --- /dev/null +++ b/flight/PiOS/inc/pios_etasv3.h @@ -0,0 +1,32 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_ETASV3 ETASV3 Functions + * @brief Hardware functions to deal with the Eagle Tree Airspeed MicroSensor V3 + * @{ + * + * @file pios_etasv3.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 + */ + +#define ETASV3_I2C_ADDR 0x75 +#define ETASV3_I2C_READ_CMD 0x07 From ad75e354eb4308ed5bbb435998240afc76c19cc0 Mon Sep 17 00:00:00 2001 From: Stacey Sheldon Date: Tue, 17 Apr 2012 09:39:47 -0400 Subject: [PATCH 03/11] etasv3: Add etasv3 driver to revolution --- flight/Revolution/Makefile | 1 + flight/Revolution/System/inc/pios_config.h | 1 + 2 files changed, 2 insertions(+) diff --git a/flight/Revolution/Makefile b/flight/Revolution/Makefile index ce8a45b6d..3e82fb113 100644 --- a/flight/Revolution/Makefile +++ b/flight/Revolution/Makefile @@ -144,6 +144,7 @@ include $(PIOS)/STM32F4xx/library.mk ## PIOS Hardware (Common) SRC += $(PIOSCOMMON)/pios_mpu6000.c SRC += $(PIOSCOMMON)/pios_bma180.c +SRC += $(PIOSCOMMON)/pios_etasv3.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 076c06d1e..a6bf3551a 100644 --- a/flight/Revolution/System/inc/pios_config.h +++ b/flight/Revolution/System/inc/pios_config.h @@ -63,6 +63,7 @@ #define PIOS_MPU6000_ACCEL #define PIOS_INCLUDE_L3GD20 #define PIOS_INCLUDE_MS5611 +#define PIOS_INCLUDE_ETASV3 //#define PIOS_INCLUDE_HCSR04 #define PIOS_FLASH_ON_ACCEL /* true for second revo */ #define FLASH_FREERTOS From 5159d2029b91e01296fcbedb1112413481498459 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Tue, 17 Apr 2012 15:54:05 +0200 Subject: [PATCH 04/11] new airspeed module - still needs matching PIOS driver from stac --- flight/Modules/Airspeed/revolution/airspeed.c | 145 ++++++++++++++++++ .../Airspeed/revolution/inc/airspeed.h | 41 +++++ shared/uavobjectdefinition/baroairspeed.xml | 12 ++ shared/uavobjectdefinition/hwsettings.xml | 2 +- 4 files changed, 199 insertions(+), 1 deletion(-) create mode 100644 flight/Modules/Airspeed/revolution/airspeed.c create mode 100644 flight/Modules/Airspeed/revolution/inc/airspeed.h create mode 100644 shared/uavobjectdefinition/baroairspeed.xml diff --git a/flight/Modules/Airspeed/revolution/airspeed.c b/flight/Modules/Airspeed/revolution/airspeed.c new file mode 100644 index 000000000..14831ea08 --- /dev/null +++ b/flight/Modules/Airspeed/revolution/airspeed.c @@ -0,0 +1,145 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup AirspeedModule Airspeed Module + * @brief Communicate with BMP085 and update @ref BaroAirspeed "BaroAirspeed UAV Object" + * @{ + * + * @file 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 "airspeed.h" +#include "baroairspeed.h" // object that will be updated by the module +#if defined(PIOS_INCLUDE_HCSR04) +#include "sonarairspeed.h" // object that will be updated by the module +#endif + +// Private constants +#define STACK_SIZE_BYTES 500 +#define TASK_PRIORITY (tskIDLE_PRIORITY+1) + +// Private types + +// Private variables +static xTaskHandle taskHandle; + +// Private functions +static void airspeedTask(void *parameters); + + +static bool airspeedEnabled = false; + +/** + * Initialise the module, called on startup + * \returns 0 on success or -1 if initialisation failed + */ +int32_t AirspeedStart() +{ + + if (airspeedEnabled == false) { + return -1; + } + // Start main task + xTaskCreate(airspeedTask, (signed char *)"Airspeed", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle); + TaskMonitorAdd(TASKINFO_RUNNING_ALTITUDE, taskHandle); + + return 0; +} + +/** + * Initialise the module, called on startup + * \returns 0 on success or -1 if initialisation failed + */ +int32_t AirspeedInitialize() +{ + #ifdef MODULE_AIRSPEED_BUILTIN + airspeedEnabled = true; + #else + + HwSettingsInitialize(); + uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; + HwSettingsOptionalModulesGet(optionalModules); + + if (optionalModules[HWSETTINGS_OPTIONALMODULES_AIRSPEED] == HWSETTINGS_OPTIONALMODULES_ENABLED) { + airspeedEnabled = true; + } else { + airspeedEnabled = false; + return -1; + } + #endif + + BaroAirspeedInitialize(); + + return 0; +} +MODULE_INITCALL(AirspeedInitialize, AirspeedStart) +/** + * Module thread, should not return. + */ +static void airspeedTask(void *parameters) +{ + BaroAirspeedData data; + + // TODO: Check the pressure sensor and set a warning if it fails test + + // Main task loop + while (1) + { + float temp, press; + + // Update the temperature data + PIOS_MS5611_StartADC(TemperatureConv); + vTaskDelay(PIOS_MS5611_GetDelay()); + PIOS_MS5611_ReadADC(); + + // Update the pressure data + PIOS_MS5611_StartADC(PressureConv); + vTaskDelay(PIOS_MS5611_GetDelay()); + PIOS_MS5611_ReadADC(); + + + temp = PIOS_MS5611_GetTemperature(); + press = PIOS_MS5611_GetPressure(); + + data.Temperature = temp; + data.Pressure = press; + data.Airspeed = 44330.0f * (1.0f - powf(data.Pressure / MS5611_P0, (1.0f / 5.255f))); + + // Update the AirspeedActual UAVObject + BaroAirspeedSet(&data); + } +} + +/** + * @} + * @} + */ diff --git a/flight/Modules/Airspeed/revolution/inc/airspeed.h b/flight/Modules/Airspeed/revolution/inc/airspeed.h new file mode 100644 index 000000000..c1bd9c8a7 --- /dev/null +++ b/flight/Modules/Airspeed/revolution/inc/airspeed.h @@ -0,0 +1,41 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup AirspeedModule Airspeed Module + * @brief Communicate with EagleTree Airspeed Sensor and update @ref BaroAirspeed "BaroAirspeed UAV Object" + * @{ + * + * @file airspeed.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Airspeed module, reads temperature and pressure 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 + */ +#ifndef AIRSPEED_H +#define AIRSPEED_H + +int32_t AirspeedInitialize(); + +#endif // AIRSPEED_H + +/** + * @} + * @} + */ diff --git a/shared/uavobjectdefinition/baroairspeed.xml b/shared/uavobjectdefinition/baroairspeed.xml new file mode 100644 index 000000000..c83095a23 --- /dev/null +++ b/shared/uavobjectdefinition/baroairspeed.xml @@ -0,0 +1,12 @@ + + + The raw data from the dynamic pressure sensor with pressure, temperature and airspeed. + + + + + + + + + diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index 490699196..45b6cac71 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -19,7 +19,7 @@ - + From c5a6ed74867603618a62d6f5b93db80817ee63ee Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Tue, 17 Apr 2012 16:25:39 +0200 Subject: [PATCH 05/11] add missing headers and makefile entries --- flight/PiOS/inc/pios_etasv3.h | 3 +++ flight/Revolution/UAVObjects.inc | 1 + ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro | 2 ++ 3 files changed, 6 insertions(+) diff --git a/flight/PiOS/inc/pios_etasv3.h b/flight/PiOS/inc/pios_etasv3.h index 82949a2ef..f816dc247 100644 --- a/flight/PiOS/inc/pios_etasv3.h +++ b/flight/PiOS/inc/pios_etasv3.h @@ -30,3 +30,6 @@ #define ETASV3_I2C_ADDR 0x75 #define ETASV3_I2C_READ_CMD 0x07 + + +int16_t PIOS_ETASV3_ReadAirspeed (void); diff --git a/flight/Revolution/UAVObjects.inc b/flight/Revolution/UAVObjects.inc index d91eb9c17..f337e86e1 100644 --- a/flight/Revolution/UAVObjects.inc +++ b/flight/Revolution/UAVObjects.inc @@ -36,6 +36,7 @@ UAVOBJSRCFILENAMES += gyrosbias UAVOBJSRCFILENAMES += accels UAVOBJSRCFILENAMES += magnetometer UAVOBJSRCFILENAMES += baroaltitude +UAVOBJSRCFILENAMES += baroairspeed UAVOBJSRCFILENAMES += flightbatterysettings UAVOBJSRCFILENAMES += firmwareiapobj UAVOBJSRCFILENAMES += flightbatterystate diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index 456d25047..2ddf50b4d 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -25,6 +25,7 @@ OTHER_FILES += UAVObjects.pluginspec # Add in all of the synthetic/generated uavobject files HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/baroaltitude.h \ + $$UAVOBJECT_SYNTHETICS/baroairspeed.h \ $$UAVOBJECT_SYNTHETICS/attitudeactual.h \ $$UAVOBJECT_SYNTHETICS/altholdsmoothed.h \ $$UAVOBJECT_SYNTHETICS/altitudeholddesired.h \ @@ -83,6 +84,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ $$UAVOBJECT_SYNTHETICS/baroaltitude.cpp \ + $$UAVOBJECT_SYNTHETICS/baroairspeed.cpp \ $$UAVOBJECT_SYNTHETICS/attitudeactual.cpp \ $$UAVOBJECT_SYNTHETICS/altholdsmoothed.cpp \ $$UAVOBJECT_SYNTHETICS/altitudeholddesired.cpp \ From edf26d15e623497f91b30ba1076e820b78b5e56d Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Tue, 17 Apr 2012 16:38:53 +0200 Subject: [PATCH 06/11] Airspeed: some more fixes --- flight/Modules/Airspeed/revolution/airspeed.c | 24 ++++++------------- flight/Revolution/Makefile | 1 + shared/uavobjectdefinition/baroairspeed.xml | 2 -- 3 files changed, 8 insertions(+), 19 deletions(-) diff --git a/flight/Modules/Airspeed/revolution/airspeed.c b/flight/Modules/Airspeed/revolution/airspeed.c index 14831ea08..4720c1871 100644 --- a/flight/Modules/Airspeed/revolution/airspeed.c +++ b/flight/Modules/Airspeed/revolution/airspeed.c @@ -37,6 +37,7 @@ */ #include "openpilot.h" +#include "hwsettings.h" #include "airspeed.h" #include "baroairspeed.h" // object that will be updated by the module #if defined(PIOS_INCLUDE_HCSR04) @@ -46,6 +47,7 @@ // Private constants #define STACK_SIZE_BYTES 500 #define TASK_PRIORITY (tskIDLE_PRIORITY+1) +#define SENSITIVE_DELAY_MS 500 // Private types @@ -114,25 +116,13 @@ static void airspeedTask(void *parameters) // Main task loop while (1) { - float temp, press; + float airspeed; - // Update the temperature data - PIOS_MS5611_StartADC(TemperatureConv); - vTaskDelay(PIOS_MS5611_GetDelay()); - PIOS_MS5611_ReadADC(); + // Update the airspeed + vTaskDelay(SENSITIVE_DELAY_MS); + airspeed = PIOS_ETASV3_ReadAirspeed(); - // Update the pressure data - PIOS_MS5611_StartADC(PressureConv); - vTaskDelay(PIOS_MS5611_GetDelay()); - PIOS_MS5611_ReadADC(); - - - temp = PIOS_MS5611_GetTemperature(); - press = PIOS_MS5611_GetPressure(); - - data.Temperature = temp; - data.Pressure = press; - data.Airspeed = 44330.0f * (1.0f - powf(data.Pressure / MS5611_P0, (1.0f / 5.255f))); + data.Airspeed = airspeed; // Update the AirspeedActual UAVObject BaroAirspeedSet(&data); diff --git a/flight/Revolution/Makefile b/flight/Revolution/Makefile index 3e82fb113..f4fd2684f 100644 --- a/flight/Revolution/Makefile +++ b/flight/Revolution/Makefile @@ -51,6 +51,7 @@ FLASH_TOOL = OPENOCD # List of modules to include MODULES = Sensors Attitude/revolution ManualControl Stabilization Altitude/revolution Actuator GPS FirmwareIAP MODULES += AltitudeHold +MODULES += Airspeed/revolution MODULES += CameraStab MODULES += Telemetry #MODULES += OveroSync diff --git a/shared/uavobjectdefinition/baroairspeed.xml b/shared/uavobjectdefinition/baroairspeed.xml index c83095a23..7b0f3a6c2 100644 --- a/shared/uavobjectdefinition/baroairspeed.xml +++ b/shared/uavobjectdefinition/baroairspeed.xml @@ -2,8 +2,6 @@ The raw data from the dynamic pressure sensor with pressure, temperature and airspeed. - - From ea4ef46873820ab718739633cfa97f5cd80d4e63 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Tue, 17 Apr 2012 18:59:27 +0200 Subject: [PATCH 07/11] added airspeed module to taskinfo --- shared/uavobjectdefinition/taskinfo.xml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/shared/uavobjectdefinition/taskinfo.xml b/shared/uavobjectdefinition/taskinfo.xml index 40f0d4bdf..5a6e5cb0c 100644 --- a/shared/uavobjectdefinition/taskinfo.xml +++ b/shared/uavobjectdefinition/taskinfo.xml @@ -1,9 +1,9 @@ Task information - - - + + + From 317cff10ddd71c10b90d1c4e515d2e605c16a743 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Tue, 17 Apr 2012 18:59:46 +0200 Subject: [PATCH 08/11] debug code added - revert this when no longer needed --- flight/Modules/Airspeed/revolution/airspeed.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/flight/Modules/Airspeed/revolution/airspeed.c b/flight/Modules/Airspeed/revolution/airspeed.c index 4720c1871..eb6fe093a 100644 --- a/flight/Modules/Airspeed/revolution/airspeed.c +++ b/flight/Modules/Airspeed/revolution/airspeed.c @@ -72,7 +72,7 @@ int32_t AirspeedStart() } // Start main task xTaskCreate(airspeedTask, (signed char *)"Airspeed", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle); - TaskMonitorAdd(TASKINFO_RUNNING_ALTITUDE, taskHandle); + TaskMonitorAdd(TASKINFO_RUNNING_AIRSPEED, taskHandle); return 0; } @@ -113,6 +113,7 @@ static void airspeedTask(void *parameters) // TODO: Check the pressure sensor and set a warning if it fails test + float test=0; // Main task loop while (1) { @@ -122,7 +123,7 @@ static void airspeedTask(void *parameters) vTaskDelay(SENSITIVE_DELAY_MS); airspeed = PIOS_ETASV3_ReadAirspeed(); - data.Airspeed = airspeed; + data.Airspeed = (test++)*1000.0f + airspeed; // Update the AirspeedActual UAVObject BaroAirspeedSet(&data); From 3da88601a1f7766c3befadc08e1b9a0852d08aba Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Tue, 17 Apr 2012 20:32:56 +0200 Subject: [PATCH 09/11] Airspeed module works, needs conversion --- flight/Modules/Airspeed/revolution/airspeed.c | 7 +++---- flight/PiOS/Common/pios_etasv3.c | 8 ++++---- 2 files changed, 7 insertions(+), 8 deletions(-) diff --git a/flight/Modules/Airspeed/revolution/airspeed.c b/flight/Modules/Airspeed/revolution/airspeed.c index eb6fe093a..eec3f015b 100644 --- a/flight/Modules/Airspeed/revolution/airspeed.c +++ b/flight/Modules/Airspeed/revolution/airspeed.c @@ -47,7 +47,7 @@ // Private constants #define STACK_SIZE_BYTES 500 #define TASK_PRIORITY (tskIDLE_PRIORITY+1) -#define SENSITIVE_DELAY_MS 500 +#define SAMPLING_DELAY_MS 50 // Private types @@ -113,17 +113,16 @@ static void airspeedTask(void *parameters) // TODO: Check the pressure sensor and set a warning if it fails test - float test=0; // Main task loop while (1) { float airspeed; // Update the airspeed - vTaskDelay(SENSITIVE_DELAY_MS); + vTaskDelay(SAMPLING_DELAY_MS); airspeed = PIOS_ETASV3_ReadAirspeed(); - data.Airspeed = (test++)*1000.0f + airspeed; + data.Airspeed = airspeed; // Update the AirspeedActual UAVObject BaroAirspeedSet(&data); diff --git a/flight/PiOS/Common/pios_etasv3.c b/flight/PiOS/Common/pios_etasv3.c index 6c7cac483..87e400c81 100644 --- a/flight/PiOS/Common/pios_etasv3.c +++ b/flight/PiOS/Common/pios_etasv3.c @@ -42,14 +42,14 @@ static bool PIOS_ETASV3_Read(uint8_t command, uint8_t * buffer, uint8_t len) }; const struct pios_i2c_txn txn_list[] = { - { + /*{ .info = __func__, .addr = ETASV3_I2C_ADDR, .rw = PIOS_I2C_TXN_WRITE, .len = sizeof(cmd_buffer), .buf = cmd_buffer, } - , + ,*/ { .info = __func__, .addr = ETASV3_I2C_ADDR, @@ -66,12 +66,12 @@ int16_t PIOS_ETASV3_ReadAirspeed (void) { uint8_t airspeed_raw[2]; - if (!PIOS_ETASV3_Read(ETASV3_I2C_READ_CMD, airspeed_raw, sizeof(airspeed_raw))) { + if (PIOS_ETASV3_Read(ETASV3_I2C_READ_CMD, airspeed_raw, sizeof(airspeed_raw)) != 0) { /* Failed to read airspeed */ return -1; } - return (airspeed_raw[0] << 8 || airspeed_raw[1]); + return (airspeed_raw[0] | (airspeed_raw[1]<<8)); } #endif /* PIOS_INCLUDE_ETASV3 */ From d125abf3352e8cde1b4c7288611447facd051d85 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Wed, 18 Apr 2012 10:48:31 +0200 Subject: [PATCH 10/11] Baro calibration --- flight/Modules/Airspeed/revolution/airspeed.c | 51 ++++++++++++++++--- flight/PiOS/Common/pios_etasv3.c | 16 +----- flight/PiOS/inc/pios_etasv3.h | 1 - flight/PiOS/pios.h | 1 + shared/uavobjectdefinition/baroairspeed.xml | 4 ++ 5 files changed, 52 insertions(+), 21 deletions(-) diff --git a/flight/Modules/Airspeed/revolution/airspeed.c b/flight/Modules/Airspeed/revolution/airspeed.c index eec3f015b..d500e449e 100644 --- a/flight/Modules/Airspeed/revolution/airspeed.c +++ b/flight/Modules/Airspeed/revolution/airspeed.c @@ -48,6 +48,9 @@ #define STACK_SIZE_BYTES 500 #define TASK_PRIORITY (tskIDLE_PRIORITY+1) #define SAMPLING_DELAY_MS 50 +#define CALIBRATION_IDLE 40 +#define CALIBRATION_COUNT 40 +#define ETS_AIRSPEED_SCALE 1.8f // Private types @@ -111,18 +114,54 @@ static void airspeedTask(void *parameters) { BaroAirspeedData data; - // TODO: Check the pressure sensor and set a warning if it fails test + uint8_t calibrationCount = 0; + uint32_t calibrationSum = 0; + uint16_t calibrationMin = 0; + uint16_t calibrationMax = 0; // Main task loop while (1) { - float airspeed; - // Update the airspeed vTaskDelay(SAMPLING_DELAY_MS); - airspeed = PIOS_ETASV3_ReadAirspeed(); - - data.Airspeed = airspeed; + + BaroAirspeedGet(&data); + data.SensorValue = PIOS_ETASV3_ReadAirspeed(); + if (data.SensorValue==-1) { + data.Connected = BAROAIRSPEED_CONNECTED_FALSE; + data.Airspeed = 0; + BaroAirspeedSet(&data); + continue; + } + + if (calibrationCountcalibrationMax) + calibrationMax = data.SensorValue; + calibrationCount++; + calibrationSum += data.SensorValue; + if (calibrationCount==CALIBRATION_IDLE+CALIBRATION_COUNT) { + data.ZeroPoint = calibrationSum / CALIBRATION_COUNT; + data.ZeroZone = (calibrationMax - calibrationMin) / 2; + } else { + continue; + } + } + + data.Connected = BAROAIRSPEED_CONNECTED_TRUE; + + int16_t tmp = abs(data.SensorValue - data.ZeroPoint) - data.ZeroZone; + + if (tmp>0) { + data.Airspeed = ETS_AIRSPEED_SCALE * sqrtf((float)tmp); + } else { + data.Airspeed = 0; + } // Update the AirspeedActual UAVObject BaroAirspeedSet(&data); diff --git a/flight/PiOS/Common/pios_etasv3.c b/flight/PiOS/Common/pios_etasv3.c index 87e400c81..2a0ca9b52 100644 --- a/flight/PiOS/Common/pios_etasv3.c +++ b/flight/PiOS/Common/pios_etasv3.c @@ -35,21 +35,9 @@ #include "pios_etasv3.h" -static bool PIOS_ETASV3_Read(uint8_t command, uint8_t * buffer, uint8_t len) +static bool PIOS_ETASV3_Read(uint8_t * buffer, uint8_t len) { - uint8_t cmd_buffer[] = { - command, - }; - const struct pios_i2c_txn txn_list[] = { - /*{ - .info = __func__, - .addr = ETASV3_I2C_ADDR, - .rw = PIOS_I2C_TXN_WRITE, - .len = sizeof(cmd_buffer), - .buf = cmd_buffer, - } - ,*/ { .info = __func__, .addr = ETASV3_I2C_ADDR, @@ -66,7 +54,7 @@ int16_t PIOS_ETASV3_ReadAirspeed (void) { uint8_t airspeed_raw[2]; - if (PIOS_ETASV3_Read(ETASV3_I2C_READ_CMD, airspeed_raw, sizeof(airspeed_raw)) != 0) { + if (PIOS_ETASV3_Read(airspeed_raw, sizeof(airspeed_raw)) != 0) { /* Failed to read airspeed */ return -1; } diff --git a/flight/PiOS/inc/pios_etasv3.h b/flight/PiOS/inc/pios_etasv3.h index f816dc247..afa038d3d 100644 --- a/flight/PiOS/inc/pios_etasv3.h +++ b/flight/PiOS/inc/pios_etasv3.h @@ -29,7 +29,6 @@ */ #define ETASV3_I2C_ADDR 0x75 -#define ETASV3_I2C_READ_CMD 0x07 int16_t PIOS_ETASV3_ReadAirspeed (void); diff --git a/flight/PiOS/pios.h b/flight/PiOS/pios.h index 560eb9b39..6e6e2aa1d 100644 --- a/flight/PiOS/pios.h +++ b/flight/PiOS/pios.h @@ -100,6 +100,7 @@ /* PIOS Hardware Includes (Common) */ #include #include +#include #if defined(PIOS_INCLUDE_BMP085) #include #endif diff --git a/shared/uavobjectdefinition/baroairspeed.xml b/shared/uavobjectdefinition/baroairspeed.xml index 7b0f3a6c2..ea6ecfda8 100644 --- a/shared/uavobjectdefinition/baroairspeed.xml +++ b/shared/uavobjectdefinition/baroairspeed.xml @@ -1,6 +1,10 @@ The raw data from the dynamic pressure sensor with pressure, temperature and airspeed. + + + + From 1c3dc9c9e15720a9b71b39e961f4f428045f43a1 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Wed, 18 Apr 2012 14:41:41 +0200 Subject: [PATCH 11/11] Airspeed: fixed calculation math - no more zero zone --- flight/Modules/Airspeed/revolution/airspeed.c | 19 ++----------------- shared/uavobjectdefinition/baroairspeed.xml | 1 - 2 files changed, 2 insertions(+), 18 deletions(-) diff --git a/flight/Modules/Airspeed/revolution/airspeed.c b/flight/Modules/Airspeed/revolution/airspeed.c index d500e449e..9273fc871 100644 --- a/flight/Modules/Airspeed/revolution/airspeed.c +++ b/flight/Modules/Airspeed/revolution/airspeed.c @@ -50,7 +50,7 @@ #define SAMPLING_DELAY_MS 50 #define CALIBRATION_IDLE 40 #define CALIBRATION_COUNT 40 -#define ETS_AIRSPEED_SCALE 1.8f +#define ETS_AIRSPEED_SCALE 1.0f // Private types @@ -116,8 +116,6 @@ static void airspeedTask(void *parameters) uint8_t calibrationCount = 0; uint32_t calibrationSum = 0; - uint16_t calibrationMin = 0; - uint16_t calibrationMax = 0; // Main task loop while (1) @@ -135,19 +133,12 @@ static void airspeedTask(void *parameters) } if (calibrationCountcalibrationMax) - calibrationMax = data.SensorValue; calibrationCount++; calibrationSum += data.SensorValue; if (calibrationCount==CALIBRATION_IDLE+CALIBRATION_COUNT) { data.ZeroPoint = calibrationSum / CALIBRATION_COUNT; - data.ZeroZone = (calibrationMax - calibrationMin) / 2; } else { continue; } @@ -155,13 +146,7 @@ static void airspeedTask(void *parameters) data.Connected = BAROAIRSPEED_CONNECTED_TRUE; - int16_t tmp = abs(data.SensorValue - data.ZeroPoint) - data.ZeroZone; - - if (tmp>0) { - data.Airspeed = ETS_AIRSPEED_SCALE * sqrtf((float)tmp); - } else { - data.Airspeed = 0; - } + data.Airspeed = ETS_AIRSPEED_SCALE * sqrtf((float)abs(data.SensorValue - data.ZeroPoint)); // Update the AirspeedActual UAVObject BaroAirspeedSet(&data); diff --git a/shared/uavobjectdefinition/baroairspeed.xml b/shared/uavobjectdefinition/baroairspeed.xml index ea6ecfda8..1566b89ce 100644 --- a/shared/uavobjectdefinition/baroairspeed.xml +++ b/shared/uavobjectdefinition/baroairspeed.xml @@ -4,7 +4,6 @@ -