From 451279821355b33fb1da8af5e3d3fb63da633fc3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9s=20Chavarr=C3=ADa=20Krauser?= Date: Sat, 22 Mar 2014 12:52:49 +0100 Subject: [PATCH 1/9] OP-1271 Added a length and NULL pointer check in go_starting() to manage zero length reads/writes --- flight/pios/stm32f10x/pios_i2c.c | 15 ++++++++++++--- flight/pios/stm32f4xx/pios_i2c.c | 25 +++++++++++++------------ 2 files changed, 25 insertions(+), 15 deletions(-) diff --git a/flight/pios/stm32f10x/pios_i2c.c b/flight/pios/stm32f10x/pios_i2c.c index 4a95b7ee5..88eccb831 100644 --- a/flight/pios/stm32f10x/pios_i2c.c +++ b/flight/pios/stm32f10x/pios_i2c.c @@ -404,9 +404,18 @@ static void go_starting(struct pios_i2c_adapter *i2c_adapter) PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); - i2c_adapter->active_byte = &(i2c_adapter->active_txn->buf[0]); - i2c_adapter->last_byte = &(i2c_adapter->active_txn->buf[i2c_adapter->active_txn->len - 1]); - + // check for an empty read/write + if (i2c_adapter->active_txn->buf != NULL && i2c_adapter->active_txn->len != 0) { + // Data available + i2c_adapter->active_byte = &(i2c_adapter->active_txn->buf[0]); + i2c_adapter->last_byte = &(i2c_adapter->active_txn->buf[i2c_adapter->active_txn->len - 1]); + } else { + // No Data available => Empty read/write + i2c_adapter->last_byte = NULL; + i2c_adapter->active_byte = i2c_adapter->last_byte + 1; + } + + I2C_GenerateSTART(i2c_adapter->cfg->regs, ENABLE); if (i2c_adapter->active_txn->rw == PIOS_I2C_TXN_READ) { I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, ENABLE); diff --git a/flight/pios/stm32f4xx/pios_i2c.c b/flight/pios/stm32f4xx/pios_i2c.c index 4a98cff33..eff4682fe 100644 --- a/flight/pios/stm32f4xx/pios_i2c.c +++ b/flight/pios/stm32f4xx/pios_i2c.c @@ -94,26 +94,19 @@ static void go_bus_error(struct pios_i2c_adapter *i2c_adapter); static void go_stopping(struct pios_i2c_adapter *i2c_adapter); static void go_stopped(struct pios_i2c_adapter *i2c_adapter); static void go_starting(struct pios_i2c_adapter *i2c_adapter); -static void go_r_any_txn_addr(struct pios_i2c_adapter *i2c_adapter); -static void go_r_more_txn_pre_one(struct pios_i2c_adapter *i2c_adapter); -static void go_r_any_txn_pre_first(struct pios_i2c_adapter *i2c_adapter); -static void go_r_any_txn_pre_middle(struct pios_i2c_adapter *i2c_adapter); -static void go_r_more_txn_pre_last(struct pios_i2c_adapter *i2c_adapter); -static void go_r_any_txn_post_last(struct pios_i2c_adapter *i2c_adapter); static void go_r_any_txn_addr(struct pios_i2c_adapter *i2c_adapter); +static void go_r_more_txn_pre_one(struct pios_i2c_adapter *i2c_adapter); static void go_r_last_txn_pre_one(struct pios_i2c_adapter *i2c_adapter); static void go_r_any_txn_pre_first(struct pios_i2c_adapter *i2c_adapter); static void go_r_any_txn_pre_middle(struct pios_i2c_adapter *i2c_adapter); static void go_r_last_txn_pre_last(struct pios_i2c_adapter *i2c_adapter); +static void go_r_more_txn_pre_last(struct pios_i2c_adapter *i2c_adapter); static void go_r_any_txn_post_last(struct pios_i2c_adapter *i2c_adapter); static void go_w_any_txn_addr(struct pios_i2c_adapter *i2c_adapter); static void go_w_any_txn_middle(struct pios_i2c_adapter *i2c_adapter); static void go_w_more_txn_last(struct pios_i2c_adapter *i2c_adapter); - -static void go_w_any_txn_addr(struct pios_i2c_adapter *i2c_adapter); -static void go_w_any_txn_middle(struct pios_i2c_adapter *i2c_adapter); static void go_w_last_txn_last(struct pios_i2c_adapter *i2c_adapter); static void go_nack(struct pios_i2c_adapter *i2c_adapter); @@ -414,9 +407,17 @@ static void go_starting(struct pios_i2c_adapter *i2c_adapter) PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn); PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn); - i2c_adapter->active_byte = &(i2c_adapter->active_txn->buf[0]); - i2c_adapter->last_byte = &(i2c_adapter->active_txn->buf[i2c_adapter->active_txn->len - 1]); - + // check for an empty read/write + if (i2c_adapter->active_txn->buf != NULL && i2c_adapter->active_txn->len != 0) { + // Data available + i2c_adapter->active_byte = &(i2c_adapter->active_txn->buf[0]); + i2c_adapter->last_byte = &(i2c_adapter->active_txn->buf[i2c_adapter->active_txn->len - 1]); + } else { + // No Data available => Empty read/write + i2c_adapter->last_byte = NULL; + i2c_adapter->active_byte = i2c_adapter->last_byte + 1; + } + I2C_GenerateSTART(i2c_adapter->cfg->regs, ENABLE); if (i2c_adapter->active_txn->rw == PIOS_I2C_TXN_READ) { I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, ENABLE); From f8f53d880155bd8ed5e9f687243816a4fbe44284 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9s=20Chavarr=C3=ADa=20Krauser?= Date: Tue, 25 Mar 2014 09:18:25 +0100 Subject: [PATCH 2/9] OP-1273 Adaptations to include MS4525DO PixHawk speed sensor --- flight/modules/Airspeed/airspeed.c | 7 +++++++ flight/pios/pios.h | 6 ++++++ .../targets/boards/revolution/firmware/inc/pios_config.h | 2 ++ flight/targets/boards/revolution/pios_board.h | 1 + make/apps-defs.mk | 1 + shared/uavobjectdefinition/airspeedsensor.xml | 4 ++++ shared/uavobjectdefinition/airspeedsettings.xml | 2 +- 7 files changed, 22 insertions(+), 1 deletion(-) diff --git a/flight/modules/Airspeed/airspeed.c b/flight/modules/Airspeed/airspeed.c index 395cee43a..078aef03f 100644 --- a/flight/modules/Airspeed/airspeed.c +++ b/flight/modules/Airspeed/airspeed.c @@ -41,6 +41,7 @@ #include "hwsettings.h" #include "airspeedsettings.h" #include "airspeedsensor.h" // object that will be updated by the module +#include "baro_airspeed_ms4525do.h" #include "baro_airspeed_etasv3.h" #include "baro_airspeed_mpxv.h" #include "gps_airspeed.h" @@ -165,6 +166,12 @@ static void airspeedTask(__attribute__((unused)) void *parameters) // Eagletree Airspeed v3 baro_airspeedGetETASV3(&airspeedData, &airspeedSettings); break; +#endif +#if defined(PIOS_INCLUDE_MS4525DO) + case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_PIXHAWKAIRSPEEDMS4525DO: + // PixHawk Airpeed based on MS4525DO + baro_airspeedGetMS4525DO(&airspeedData, &airspeedSettings); + break; #endif case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_GROUNDSPEEDBASEDWINDESTIMATION: gps_airspeedGet(&airspeedData, &airspeedSettings); diff --git a/flight/pios/pios.h b/flight/pios/pios.h index 15cdc294e..2142f8224 100644 --- a/flight/pios/pios.h +++ b/flight/pios/pios.h @@ -229,6 +229,12 @@ #include #endif +#ifdef PIOS_INCLUDE_MS4525DO +/* PixHawk Airspeed Sensor based on MS4525DO */ +#include +#endif + + #ifdef PIOS_INCLUDE_HCSR04 /* HC-SR04 Ultrasonic Sensor */ #include diff --git a/flight/targets/boards/revolution/firmware/inc/pios_config.h b/flight/targets/boards/revolution/firmware/inc/pios_config.h index 6ad54cc7e..c60e9a453 100644 --- a/flight/targets/boards/revolution/firmware/inc/pios_config.h +++ b/flight/targets/boards/revolution/firmware/inc/pios_config.h @@ -87,6 +87,8 @@ #define PIOS_INCLUDE_MS5611 #define PIOS_INCLUDE_MPXV #define PIOS_INCLUDE_ETASV3 +#define PIOS_INCLUDE_MS4525DO + /* #define PIOS_INCLUDE_HCSR04 */ /* PIOS receiver drivers */ diff --git a/flight/targets/boards/revolution/pios_board.h b/flight/targets/boards/revolution/pios_board.h index de47db80a..60d3cff53 100644 --- a/flight/targets/boards/revolution/pios_board.h +++ b/flight/targets/boards/revolution/pios_board.h @@ -125,6 +125,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_MS4525DO_ADAPTER (PIOS_I2C_FLEXI_ADAPTER) // ------------------------- // PIOS_USART diff --git a/make/apps-defs.mk b/make/apps-defs.mk index bb5376223..20853c568 100644 --- a/make/apps-defs.mk +++ b/make/apps-defs.mk @@ -69,6 +69,7 @@ SRC += $(PIOSCOMMON)/pios_i2c_esc.c SRC += $(PIOSCOMMON)/pios_l3gd20.c SRC += $(PIOSCOMMON)/pios_mpu6000.c SRC += $(PIOSCOMMON)/pios_mpxv.c +SRC += $(PIOSCOMMON)/pios_ms4525do.c SRC += $(PIOSCOMMON)/pios_ms5611.c SRC += $(PIOSCOMMON)/pios_oplinkrcvr.c SRC += $(PIOSCOMMON)/pios_video.c diff --git a/shared/uavobjectdefinition/airspeedsensor.xml b/shared/uavobjectdefinition/airspeedsensor.xml index 8f55cbeae..f61cb63fc 100644 --- a/shared/uavobjectdefinition/airspeedsensor.xml +++ b/shared/uavobjectdefinition/airspeedsensor.xml @@ -3,7 +3,11 @@ The raw data from the dynamic pressure sensor with pressure, temperature and airspeed. + + + + diff --git a/shared/uavobjectdefinition/airspeedsettings.xml b/shared/uavobjectdefinition/airspeedsettings.xml index 3ed136b16..ea09bba2d 100644 --- a/shared/uavobjectdefinition/airspeedsettings.xml +++ b/shared/uavobjectdefinition/airspeedsettings.xml @@ -4,7 +4,7 @@ - + From 58168538129da7d05bf700b08e39c004e0744f61 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9s=20Chavarr=C3=ADa=20Krauser?= Date: Tue, 25 Mar 2014 09:24:08 +0100 Subject: [PATCH 3/9] OP-1273 New files for the MS4525DO PixHawk speed sensor: PIOS files and module files --- .../modules/Airspeed/baro_airspeed_ms4525do.c | 174 ++++++++++++++++++ .../Airspeed/inc/baro_airspeed_ms4525do.h | 43 +++++ flight/pios/common/pios_ms4525do.c | 127 +++++++++++++ flight/pios/inc/pios_ms4525do.h | 45 +++++ 4 files changed, 389 insertions(+) create mode 100644 flight/modules/Airspeed/baro_airspeed_ms4525do.c create mode 100644 flight/modules/Airspeed/inc/baro_airspeed_ms4525do.h create mode 100644 flight/pios/common/pios_ms4525do.c create mode 100644 flight/pios/inc/pios_ms4525do.h diff --git a/flight/modules/Airspeed/baro_airspeed_ms4525do.c b/flight/modules/Airspeed/baro_airspeed_ms4525do.c new file mode 100644 index 000000000..ea8480e0b --- /dev/null +++ b/flight/modules/Airspeed/baro_airspeed_ms4525do.c @@ -0,0 +1,174 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup AirspeedModule Airspeed Module + * @brief Communicate with airspeed sensors and return values + * @{ + * + * @file baro_airspeed_ms4525do.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @brief Airspeed module, handles temperature and pressure readings from MS4525DO + * + * @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 "taskinfo.h" + +#if defined(PIOS_INCLUDE_MS4525DO) + +#define CALIBRATION_IDLE_MS 0 // Time to wait before calibrating, in [ms] +#define CALIBRATION_COUNT_MS 4000 // Time to spend calibrating, in [ms] +#define FILTER_SHIFT 5 // Barry Dorr filter parameter k + +#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) + +#define max(x,y) ((x)>=(y) ? (x) : (y)) + +// Private types + +// Private functions +static int8_t baro_airspeedReadMS4525DO(AirspeedSensorData *airspeedSensor, AirspeedSettingsData *airspeedSettings); + + +// Private variables +static uint16_t calibrationCount = 0; +static uint32_t filter_reg; // Barry Dorr filter register + + +void baro_airspeedGetMS4525DO(AirspeedSensorData *airspeedSensor, AirspeedSettingsData *airspeedSettings) +{ + // request measurement first + int8_t retVal = PIOS_MS4525DO_Request(); + + if( retVal !=0 ){ + AlarmsSet(SYSTEMALARMS_ALARM_I2C, SYSTEMALARMS_ALARM_ERROR); + return; + } + + // Datasheet of MS4525DO: conversion needs 0.5 ms + 20% more when status bit used + // delay by one Tick or at least 2 ms + const portTickType xDelay = max(2 / portTICK_RATE_MS, 1); + vTaskDelay( xDelay ); + + // read the sensor + retVal = baro_airspeedReadMS4525DO(airspeedSensor,airspeedSettings); + + switch( retVal ){ + case 0 : AlarmsClear(SYSTEMALARMS_ALARM_I2C); + break; + case -4 : + case -5 : AlarmsSet(SYSTEMALARMS_ALARM_I2C, SYSTEMALARMS_ALARM_WARNING); + break; + case -1 : + case -2 : + case -3 : + case -6 : + default : AlarmsSet(SYSTEMALARMS_ALARM_I2C, SYSTEMALARMS_ALARM_ERROR); + } +} + + +static int8_t baro_airspeedReadMS4525DO(AirspeedSensorData *airspeedSensor, AirspeedSettingsData *airspeedSettings) +{ + // Check to see if airspeed sensor is returning airspeedSensor + uint16_t values[2]; + int8_t retVal=PIOS_MS4525DO_Read(values); + + if( retVal == 0 ){ + airspeedSensor->SensorValue = values[0]; + airspeedSensor->SensorValueTemperature = values[1]; + } else { + airspeedSensor->SensorValue = -1; + airspeedSensor->SensorValueTemperature = -1; + airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; + airspeedSensor->CalibratedAirspeed = 0; + return retVal; + } + + // only calibrate if no stored calibration is available + if ( !airspeedSettings->ZeroPoint ) { + // Calibrate sensor by averaging zero point value + if (calibrationCount <= CALIBRATION_IDLE_MS / airspeedSettings->SamplePeriod) { + calibrationCount++; + filter_reg = (airspeedSensor->SensorValue << FILTER_SHIFT ); + + return retVal; + } else if (calibrationCount <= (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS) / airspeedSettings->SamplePeriod) { + calibrationCount++; + // update filter register + filter_reg = filter_reg - ( filter_reg >> FILTER_SHIFT ) + airspeedSensor->SensorValue; + + if (calibrationCount > (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS) / airspeedSettings->SamplePeriod) { + // Scale output for unity gain. + airspeedSettings->ZeroPoint = (uint16_t)( filter_reg >> FILTER_SHIFT ); + + AirspeedSettingsZeroPointSet(&airspeedSettings->ZeroPoint); + calibrationCount = 0; + } + return retVal; + } + } + + /* Compute airspeed + assume sensor is A Type and has a range of 1 psi, i.e. Pmin=-1.0 psi and Pmax=1.0 psi + Datasheet pressure: output = 0.8 * 16383 / (Pmax-Pmin) * (P - Pmin) + 0.1 * 16383 + Inversion: P = (10*output - 81915)/65532 in psi + 1 psi = 6894,757293168 Pa + P = (10*output - 81915)*0.1052120688 in Pa + Datasheet temperature: output = (T+50)*2047 / 200 + Inversion: T = (200*out - 102350)/2047 in C + T = (200*out - 102350)/2047 + 273.15 in K + */ + const float dP = ( 10*(int32_t)(airspeedSensor->SensorValue-airspeedSettings->ZeroPoint) ) * 0.1052120688f; + const float T = (float)( 200*(int32_t)airspeedSensor->SensorValueTemperature - 102350) / 2047 + 273.15f; + + airspeedSensor->DifferentialPressure = dP; + airspeedSensor->Temperature = T; + // CAS = Csound * sqrt( 5 *( (dP/P0 +1)^(2/7) - 1) ) + // TAS = Csound * sqrt( 5 T/T0 *( (dP/P0 +1)^(2/7) - 1) ) + // where Csound = 340.276 m/s at standard condition T0=288.15 K and P0 = 101315 Pa + airspeedSensor->CalibratedAirspeed = airspeedSettings->Scale * CASFACTOR * sqrtf( powf( fabsf(dP) / P0 + 1.0f , CCEXPONENT ) - 1.0f ); + airspeedSensor->TrueAirspeed = airspeedSensor->CalibratedAirspeed * TASFACTOR * sqrtf( T ); + airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE; + + return retVal; +} + + +#endif /* if defined(PIOS_INCLUDE_MS4525DO) */ + +/** + * @} + * @} + */ diff --git a/flight/modules/Airspeed/inc/baro_airspeed_ms4525do.h b/flight/modules/Airspeed/inc/baro_airspeed_ms4525do.h new file mode 100644 index 000000000..5cbcc9111 --- /dev/null +++ b/flight/modules/Airspeed/inc/baro_airspeed_ms4525do.h @@ -0,0 +1,43 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup AirspeedModule Airspeed Module + * @brief Calculate airspeed from measurements of differential pressure from a MS4525DO (PixHawk airspeed sensor) + * @{ + * + * @file baro_airspeed_mas4525do.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @brief Airspeed module, reads temperature and pressure from MS4525DO + * + * @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_MS4525DO_H +#define BARO_AIRSPEED_MS4525DO_H +#if defined(PIOS_INCLUDE_MS4525DO) + +void baro_airspeedGetMS4525DO(AirspeedSensorData *airspeedSensor, AirspeedSettingsData *airspeedSettings); + +#endif /* PIOS_INCLUDE_MS4525DO */ +#endif // BARO_AIRSPEED_MS4525DO_H + +/** + * @} + * @} + */ diff --git a/flight/pios/common/pios_ms4525do.c b/flight/pios/common/pios_ms4525do.c new file mode 100644 index 000000000..f331453aa --- /dev/null +++ b/flight/pios/common/pios_ms4525do.c @@ -0,0 +1,127 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_MS4525DO MS4525DO Functions + * @brief Hardware functions to deal with the PixHawk Airspeed Sensor based on MS4525DO + * @{ + * + * @file pios_ms4525do.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @brief PixHawk MS4525DO 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_MS4525DO + +/* Local Defs and Variables */ +static int8_t PIOS_MS4525DO_ReadI2C(uint8_t *buffer, uint8_t len); +static int8_t PIOS_MS4525DO_WriteI2C(uint8_t *buffer, uint8_t len); + +static bool pios_ms4525do_requested = false; + + + +static int8_t PIOS_MS4525DO_ReadI2C(uint8_t *buffer, uint8_t len) +{ + const struct pios_i2c_txn txn_list[] = { + { + .info = __func__, + .addr = MS4525DO_I2C_ADDR, + .rw = PIOS_I2C_TXN_READ, + .len = len, + .buf = buffer, + } + }; + + return PIOS_I2C_Transfer(PIOS_I2C_MS4525DO_ADAPTER, txn_list, NELEMENTS(txn_list)); +} + + +static int8_t PIOS_MS4525DO_WriteI2C(uint8_t *buffer, uint8_t len) +{ + const struct pios_i2c_txn txn_list[] = { + { + .info = __func__, + .addr = MS4525DO_I2C_ADDR, + .rw = PIOS_I2C_TXN_WRITE, + .len = len, + .buf = buffer, + } + }; + + return PIOS_I2C_Transfer(PIOS_I2C_MS4525DO_ADAPTER, txn_list, NELEMENTS(txn_list)); +} + + + +int8_t PIOS_MS4525DO_Request(void) +{ + // MS4525DO expects a zero length write. + // Sending one byte is a workaround that works for the moment + uint8_t data=0; + int8_t retVal=PIOS_MS4525DO_WriteI2C(&data,sizeof(data)); + /* requested only when transfer worked */ + pios_ms4525do_requested = ( retVal == 0 ); + + return retVal; +} + + +// values has to ba an arrray with two elements +// values stay untouched on error +int8_t PIOS_MS4525DO_Read(uint16_t *values) +{ + if (!pios_ms4525do_requested) { + /* Do not try to read when not requested */ + /* else probably stale data will be obtained */ + return -4; + } + + uint8_t data[4]; + int8_t retVal=PIOS_MS4525DO_ReadI2C(data, sizeof(data)); + + uint8_t status = data[0] & 0xC0; + if (status == 0x80) { + /* stale data */ + return -5; + } else if (status == 0xC0) { + /* device probably broken */ + return -6; + } + + /* differential pressure */ + values[0] = (data[0] << 8); + values[0] += data[1]; + + /* temperature */ + values[1] = (data[2] << 8); + values[1] += data[3]; + values[1] = (values[1] >> 5); + + /* not requested anymore, only when transfer worked */ + pios_ms4525do_requested = !( retVal == 0 ); + + return retVal; +} + +#endif /* PIOS_INCLUDE_MS4525DO */ diff --git a/flight/pios/inc/pios_ms4525do.h b/flight/pios/inc/pios_ms4525do.h new file mode 100644 index 000000000..1bf8778a5 --- /dev/null +++ b/flight/pios/inc/pios_ms4525do.h @@ -0,0 +1,45 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_MS4525DO MS4525DO Functions + * @brief Hardware functions to deal with the PixHawk Airspeed Sensor based on MS4525DO + * @{ + * + * @file pios_ms4525do.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @brief PixHawk MS4525DO 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_MS4525DO_H +#define PIOS_MS4525DO_H + +// Interface Type I chip +#define MS4525DO_I2C_ADDR 0x28 +// Interface Type J chip +/* #define MS4525DO_I2C_ADDR 0x36 */ +// Interface Type K chip +/* #define MS4525DO_I2C_ADDR 0x46 */ + + +extern int8_t PIOS_MS4525DO_Request(void); +extern int8_t PIOS_MS4525DO_Read(uint16_t *values); + +#endif /* PIOS_MS4525DO_H */ From 4dff86dc9cc6e0da0929200eb3fd193fa2a41ac2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9s=20Chavarr=C3=ADa=20Krauser?= Date: Tue, 25 Mar 2014 09:24:08 +0100 Subject: [PATCH 4/9] OP-1273 New files for the MS4525DO PixHawk speed sensor: PIOS files and module files --- .../modules/Airspeed/baro_airspeed_ms4525do.c | 174 ++++++++++++++++++ .../Airspeed/inc/baro_airspeed_ms4525do.h | 43 +++++ flight/pios/common/pios_ms4525do.c | 127 +++++++++++++ flight/pios/inc/pios_ms4525do.h | 45 +++++ 4 files changed, 389 insertions(+) create mode 100644 flight/modules/Airspeed/baro_airspeed_ms4525do.c create mode 100644 flight/modules/Airspeed/inc/baro_airspeed_ms4525do.h create mode 100644 flight/pios/common/pios_ms4525do.c create mode 100644 flight/pios/inc/pios_ms4525do.h diff --git a/flight/modules/Airspeed/baro_airspeed_ms4525do.c b/flight/modules/Airspeed/baro_airspeed_ms4525do.c new file mode 100644 index 000000000..ea8480e0b --- /dev/null +++ b/flight/modules/Airspeed/baro_airspeed_ms4525do.c @@ -0,0 +1,174 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup AirspeedModule Airspeed Module + * @brief Communicate with airspeed sensors and return values + * @{ + * + * @file baro_airspeed_ms4525do.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @brief Airspeed module, handles temperature and pressure readings from MS4525DO + * + * @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 "taskinfo.h" + +#if defined(PIOS_INCLUDE_MS4525DO) + +#define CALIBRATION_IDLE_MS 0 // Time to wait before calibrating, in [ms] +#define CALIBRATION_COUNT_MS 4000 // Time to spend calibrating, in [ms] +#define FILTER_SHIFT 5 // Barry Dorr filter parameter k + +#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) + +#define max(x,y) ((x)>=(y) ? (x) : (y)) + +// Private types + +// Private functions +static int8_t baro_airspeedReadMS4525DO(AirspeedSensorData *airspeedSensor, AirspeedSettingsData *airspeedSettings); + + +// Private variables +static uint16_t calibrationCount = 0; +static uint32_t filter_reg; // Barry Dorr filter register + + +void baro_airspeedGetMS4525DO(AirspeedSensorData *airspeedSensor, AirspeedSettingsData *airspeedSettings) +{ + // request measurement first + int8_t retVal = PIOS_MS4525DO_Request(); + + if( retVal !=0 ){ + AlarmsSet(SYSTEMALARMS_ALARM_I2C, SYSTEMALARMS_ALARM_ERROR); + return; + } + + // Datasheet of MS4525DO: conversion needs 0.5 ms + 20% more when status bit used + // delay by one Tick or at least 2 ms + const portTickType xDelay = max(2 / portTICK_RATE_MS, 1); + vTaskDelay( xDelay ); + + // read the sensor + retVal = baro_airspeedReadMS4525DO(airspeedSensor,airspeedSettings); + + switch( retVal ){ + case 0 : AlarmsClear(SYSTEMALARMS_ALARM_I2C); + break; + case -4 : + case -5 : AlarmsSet(SYSTEMALARMS_ALARM_I2C, SYSTEMALARMS_ALARM_WARNING); + break; + case -1 : + case -2 : + case -3 : + case -6 : + default : AlarmsSet(SYSTEMALARMS_ALARM_I2C, SYSTEMALARMS_ALARM_ERROR); + } +} + + +static int8_t baro_airspeedReadMS4525DO(AirspeedSensorData *airspeedSensor, AirspeedSettingsData *airspeedSettings) +{ + // Check to see if airspeed sensor is returning airspeedSensor + uint16_t values[2]; + int8_t retVal=PIOS_MS4525DO_Read(values); + + if( retVal == 0 ){ + airspeedSensor->SensorValue = values[0]; + airspeedSensor->SensorValueTemperature = values[1]; + } else { + airspeedSensor->SensorValue = -1; + airspeedSensor->SensorValueTemperature = -1; + airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; + airspeedSensor->CalibratedAirspeed = 0; + return retVal; + } + + // only calibrate if no stored calibration is available + if ( !airspeedSettings->ZeroPoint ) { + // Calibrate sensor by averaging zero point value + if (calibrationCount <= CALIBRATION_IDLE_MS / airspeedSettings->SamplePeriod) { + calibrationCount++; + filter_reg = (airspeedSensor->SensorValue << FILTER_SHIFT ); + + return retVal; + } else if (calibrationCount <= (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS) / airspeedSettings->SamplePeriod) { + calibrationCount++; + // update filter register + filter_reg = filter_reg - ( filter_reg >> FILTER_SHIFT ) + airspeedSensor->SensorValue; + + if (calibrationCount > (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS) / airspeedSettings->SamplePeriod) { + // Scale output for unity gain. + airspeedSettings->ZeroPoint = (uint16_t)( filter_reg >> FILTER_SHIFT ); + + AirspeedSettingsZeroPointSet(&airspeedSettings->ZeroPoint); + calibrationCount = 0; + } + return retVal; + } + } + + /* Compute airspeed + assume sensor is A Type and has a range of 1 psi, i.e. Pmin=-1.0 psi and Pmax=1.0 psi + Datasheet pressure: output = 0.8 * 16383 / (Pmax-Pmin) * (P - Pmin) + 0.1 * 16383 + Inversion: P = (10*output - 81915)/65532 in psi + 1 psi = 6894,757293168 Pa + P = (10*output - 81915)*0.1052120688 in Pa + Datasheet temperature: output = (T+50)*2047 / 200 + Inversion: T = (200*out - 102350)/2047 in C + T = (200*out - 102350)/2047 + 273.15 in K + */ + const float dP = ( 10*(int32_t)(airspeedSensor->SensorValue-airspeedSettings->ZeroPoint) ) * 0.1052120688f; + const float T = (float)( 200*(int32_t)airspeedSensor->SensorValueTemperature - 102350) / 2047 + 273.15f; + + airspeedSensor->DifferentialPressure = dP; + airspeedSensor->Temperature = T; + // CAS = Csound * sqrt( 5 *( (dP/P0 +1)^(2/7) - 1) ) + // TAS = Csound * sqrt( 5 T/T0 *( (dP/P0 +1)^(2/7) - 1) ) + // where Csound = 340.276 m/s at standard condition T0=288.15 K and P0 = 101315 Pa + airspeedSensor->CalibratedAirspeed = airspeedSettings->Scale * CASFACTOR * sqrtf( powf( fabsf(dP) / P0 + 1.0f , CCEXPONENT ) - 1.0f ); + airspeedSensor->TrueAirspeed = airspeedSensor->CalibratedAirspeed * TASFACTOR * sqrtf( T ); + airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE; + + return retVal; +} + + +#endif /* if defined(PIOS_INCLUDE_MS4525DO) */ + +/** + * @} + * @} + */ diff --git a/flight/modules/Airspeed/inc/baro_airspeed_ms4525do.h b/flight/modules/Airspeed/inc/baro_airspeed_ms4525do.h new file mode 100644 index 000000000..5cbcc9111 --- /dev/null +++ b/flight/modules/Airspeed/inc/baro_airspeed_ms4525do.h @@ -0,0 +1,43 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup AirspeedModule Airspeed Module + * @brief Calculate airspeed from measurements of differential pressure from a MS4525DO (PixHawk airspeed sensor) + * @{ + * + * @file baro_airspeed_mas4525do.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @brief Airspeed module, reads temperature and pressure from MS4525DO + * + * @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_MS4525DO_H +#define BARO_AIRSPEED_MS4525DO_H +#if defined(PIOS_INCLUDE_MS4525DO) + +void baro_airspeedGetMS4525DO(AirspeedSensorData *airspeedSensor, AirspeedSettingsData *airspeedSettings); + +#endif /* PIOS_INCLUDE_MS4525DO */ +#endif // BARO_AIRSPEED_MS4525DO_H + +/** + * @} + * @} + */ diff --git a/flight/pios/common/pios_ms4525do.c b/flight/pios/common/pios_ms4525do.c new file mode 100644 index 000000000..f331453aa --- /dev/null +++ b/flight/pios/common/pios_ms4525do.c @@ -0,0 +1,127 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_MS4525DO MS4525DO Functions + * @brief Hardware functions to deal with the PixHawk Airspeed Sensor based on MS4525DO + * @{ + * + * @file pios_ms4525do.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @brief PixHawk MS4525DO 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_MS4525DO + +/* Local Defs and Variables */ +static int8_t PIOS_MS4525DO_ReadI2C(uint8_t *buffer, uint8_t len); +static int8_t PIOS_MS4525DO_WriteI2C(uint8_t *buffer, uint8_t len); + +static bool pios_ms4525do_requested = false; + + + +static int8_t PIOS_MS4525DO_ReadI2C(uint8_t *buffer, uint8_t len) +{ + const struct pios_i2c_txn txn_list[] = { + { + .info = __func__, + .addr = MS4525DO_I2C_ADDR, + .rw = PIOS_I2C_TXN_READ, + .len = len, + .buf = buffer, + } + }; + + return PIOS_I2C_Transfer(PIOS_I2C_MS4525DO_ADAPTER, txn_list, NELEMENTS(txn_list)); +} + + +static int8_t PIOS_MS4525DO_WriteI2C(uint8_t *buffer, uint8_t len) +{ + const struct pios_i2c_txn txn_list[] = { + { + .info = __func__, + .addr = MS4525DO_I2C_ADDR, + .rw = PIOS_I2C_TXN_WRITE, + .len = len, + .buf = buffer, + } + }; + + return PIOS_I2C_Transfer(PIOS_I2C_MS4525DO_ADAPTER, txn_list, NELEMENTS(txn_list)); +} + + + +int8_t PIOS_MS4525DO_Request(void) +{ + // MS4525DO expects a zero length write. + // Sending one byte is a workaround that works for the moment + uint8_t data=0; + int8_t retVal=PIOS_MS4525DO_WriteI2C(&data,sizeof(data)); + /* requested only when transfer worked */ + pios_ms4525do_requested = ( retVal == 0 ); + + return retVal; +} + + +// values has to ba an arrray with two elements +// values stay untouched on error +int8_t PIOS_MS4525DO_Read(uint16_t *values) +{ + if (!pios_ms4525do_requested) { + /* Do not try to read when not requested */ + /* else probably stale data will be obtained */ + return -4; + } + + uint8_t data[4]; + int8_t retVal=PIOS_MS4525DO_ReadI2C(data, sizeof(data)); + + uint8_t status = data[0] & 0xC0; + if (status == 0x80) { + /* stale data */ + return -5; + } else if (status == 0xC0) { + /* device probably broken */ + return -6; + } + + /* differential pressure */ + values[0] = (data[0] << 8); + values[0] += data[1]; + + /* temperature */ + values[1] = (data[2] << 8); + values[1] += data[3]; + values[1] = (values[1] >> 5); + + /* not requested anymore, only when transfer worked */ + pios_ms4525do_requested = !( retVal == 0 ); + + return retVal; +} + +#endif /* PIOS_INCLUDE_MS4525DO */ diff --git a/flight/pios/inc/pios_ms4525do.h b/flight/pios/inc/pios_ms4525do.h new file mode 100644 index 000000000..1bf8778a5 --- /dev/null +++ b/flight/pios/inc/pios_ms4525do.h @@ -0,0 +1,45 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_MS4525DO MS4525DO Functions + * @brief Hardware functions to deal with the PixHawk Airspeed Sensor based on MS4525DO + * @{ + * + * @file pios_ms4525do.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @brief PixHawk MS4525DO 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_MS4525DO_H +#define PIOS_MS4525DO_H + +// Interface Type I chip +#define MS4525DO_I2C_ADDR 0x28 +// Interface Type J chip +/* #define MS4525DO_I2C_ADDR 0x36 */ +// Interface Type K chip +/* #define MS4525DO_I2C_ADDR 0x46 */ + + +extern int8_t PIOS_MS4525DO_Request(void); +extern int8_t PIOS_MS4525DO_Read(uint16_t *values); + +#endif /* PIOS_MS4525DO_H */ From 9d30673d18d38c060c3a9474439631729a44e4f5 Mon Sep 17 00:00:00 2001 From: Andres <> Date: Wed, 26 Mar 2014 09:15:56 +0100 Subject: [PATCH 5/9] OP-1273 attitude estimation uses airspeedSensor.TrueAirspeed when available (e.g. from MS4525DO; available when TrueAirspeed >=0 ) instead of using an altitude correction IAS2TAS(alt). When not available it falls back to IAS2TAS(alt) --- flight/modules/Attitude/revolution/attitude.c | 5 +++-- flight/modules/StateEstimation/filterair.c | 2 +- .../modules/StateEstimation/stateestimation.c | 18 ++++++++++++++++-- 3 files changed, 20 insertions(+), 5 deletions(-) diff --git a/flight/modules/Attitude/revolution/attitude.c b/flight/modules/Attitude/revolution/attitude.c index d502d1c8b..0ca789199 100644 --- a/flight/modules/Attitude/revolution/attitude.c +++ b/flight/modules/Attitude/revolution/attitude.c @@ -650,7 +650,7 @@ static int32_t updateAttitudeComplementary(bool first_run) if (airspeedSensor.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE) { // we have airspeed available airspeed.CalibratedAirspeed = airspeedSensor.CalibratedAirspeed; - airspeed.TrueAirspeed = airspeed.CalibratedAirspeed * IAS2TAS(homeLocation.Altitude - positionState.Down); + airspeed.TrueAirspeed = (airspeedSensor.TrueAirspeed < 0.f) ? airspeed.CalibratedAirspeed * IAS2TAS(homeLocation.Altitude - positionState.Down) : airspeedSensor.TrueAirspeed; AirspeedStateSet(&airspeed); } } @@ -1078,7 +1078,8 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) AirspeedStateGet(&airspeed); airspeed.CalibratedAirspeed = airspeedData.CalibratedAirspeed; - airspeed.TrueAirspeed = airspeed.CalibratedAirspeed * IAS2TAS(homeLocation.Altitude - Nav.Pos[2]); + airspeed.TrueAirspeed = (airspeedSensor.TrueAirspeed < 0.f) ? airspeed.CalibratedAirspeed * IAS2TAS(homeLocation.Altitude - positionState.Down) : airspeedSensor.TrueAirspeed; + AirspeedStateSet(&airspeed); if (!gps_vel_updated && !gps_updated) { diff --git a/flight/modules/StateEstimation/filterair.c b/flight/modules/StateEstimation/filterair.c index 3ed89b99d..3c1308f5a 100644 --- a/flight/modules/StateEstimation/filterair.c +++ b/flight/modules/StateEstimation/filterair.c @@ -78,7 +78,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state) this->altitude = state->baro[0]; } // calculate true airspeed estimation - if (IS_SET(state->updated, SENSORUPDATES_airspeed)) { + if (IS_SET(state->updated, SENSORUPDATES_airspeed) && (state->airspeed[1]<0.f) ) { state->airspeed[1] = state->airspeed[0] * IAS2TAS(this->altitude); } diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c index bff326c87..b8dbe8e61 100644 --- a/flight/modules/StateEstimation/stateestimation.c +++ b/flight/modules/StateEstimation/stateestimation.c @@ -85,6 +85,18 @@ UNSET_MASK(states.updated, SENSORUPDATES_##shortname); \ } \ } +#define FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_2_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(sensorname, shortname, a1, a2, EXTRACHECK) \ + if (IS_SET(states.updated, SENSORUPDATES_##shortname)) { \ + sensorname##Data s; \ + sensorname##Get(&s); \ + if (IS_REAL(s.a1) && IS_REAL(s.a2) && EXTRACHECK) { \ + states.shortname[0] = s.a1; \ + states.shortname[1] = s.a2; \ + } \ + else { \ + UNSET_MASK(states.updated, SENSORUPDATES_##shortname); \ + } \ + } // local macros, ONLY to be used in the middle of StateEstimationCb in section RUNSTATE_SAVE before the check of alarms! #define EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(statename, shortname, a1, a2, a3) \ @@ -379,8 +391,10 @@ static void StateEstimationCb(void) FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(MagSensor, mag, x, y, z); FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(GPSVelocitySensor, vel, North, East, Down); FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_1_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(BaroSensor, baro, Altitude, true); - FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_1_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(AirspeedSensor, airspeed, CalibratedAirspeed, s.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE); - states.airspeed[1] = 0.0f; // sensor does not provide true airspeed, needs to be calculated by filter, set to zero for now + FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_2_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(AirspeedSensor, airspeed, CalibratedAirspeed, TrueAirspeed, s.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE); +// FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_1_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(AirspeedSensor, airspeed, CalibratedAirspeed, s.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE); +// states.airspeed[1] = 0.0f; // sensor does not provide true airspeed, needs to be calculated by filter, set to zero for now + // GPS position data (LLA) is not fetched here since it does not contain floats. The filter must do all checks itself // at this point sensor state is stored in "states" with some rudimentary filtering applied From 8e26338f24b05e07d51847a1a9cfc5d44d2816dd Mon Sep 17 00:00:00 2001 From: Andres <> Date: Thu, 27 Mar 2014 09:32:17 +0100 Subject: [PATCH 6/9] OP-1273 I2C Alarm reset to default when AirspeedSensorType is changed. Small cosmetic corrections --- flight/modules/Airspeed/airspeed.c | 19 ++++++++++++++++++- .../modules/Airspeed/baro_airspeed_ms4525do.c | 12 ++++++------ .../modules/StateEstimation/stateestimation.c | 7 ++++--- 3 files changed, 28 insertions(+), 10 deletions(-) diff --git a/flight/modules/Airspeed/airspeed.c b/flight/modules/Airspeed/airspeed.c index 078aef03f..0da2d149d 100644 --- a/flight/modules/Airspeed/airspeed.c +++ b/flight/modules/Airspeed/airspeed.c @@ -60,7 +60,7 @@ static xTaskHandle taskHandle; static bool airspeedEnabled = false; static AirspeedSettingsData airspeedSettings; - +static AirspeedSettingsAirspeedSensorTypeOptions lastAirspeedSensorType = 0; static int8_t airspeedADCPin = -1; @@ -118,6 +118,8 @@ int32_t AirspeedInitialize() airspeedADCPin = i; } } + + lastAirspeedSensorType = airspeedSettings.AirspeedSensorType; AirspeedSensorInitialize(); AirspeedSettingsInitialize(); @@ -153,6 +155,21 @@ static void airspeedTask(__attribute__((unused)) void *parameters) // Update the airspeed object AirspeedSensorGet(&airspeedData); + // if sensor type changed and the last sensor was + // either Eagletree or PixHawk, reset I2C alarms + if(airspeedSettings.AirspeedSensorType != lastAirspeedSensorType){ + switch (lastAirspeedSensorType) { + // Eagletree or PixHawk => Reset I2C alams + case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_EAGLETREEAIRSPEEDV3: + case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_PIXHAWKAIRSPEEDMS4525DO: + AlarmsDefault(SYSTEMALARMS_ALARM_I2C); + break; + // else do not reset I2C alarms + default: break; + } + lastAirspeedSensorType = airspeedSettings.AirspeedSensorType; + } + switch (airspeedSettings.AirspeedSensorType) { #if defined(PIOS_INCLUDE_MPXV) case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002: diff --git a/flight/modules/Airspeed/baro_airspeed_ms4525do.c b/flight/modules/Airspeed/baro_airspeed_ms4525do.c index ea8480e0b..caeb44e99 100644 --- a/flight/modules/Airspeed/baro_airspeed_ms4525do.c +++ b/flight/modules/Airspeed/baro_airspeed_ms4525do.c @@ -44,9 +44,9 @@ #if defined(PIOS_INCLUDE_MS4525DO) -#define CALIBRATION_IDLE_MS 0 // Time to wait before calibrating, in [ms] -#define CALIBRATION_COUNT_MS 4000 // Time to spend calibrating, in [ms] -#define FILTER_SHIFT 5 // Barry Dorr filter parameter k +#define CALIBRATION_IDLE_MS 0 // Time to wait before calibrating, in [ms] +#define CALIBRATION_COUNT_MS 4000 // Time to spend calibrating, in [ms] +#define FILTER_SHIFT 5 // Barry Dorr filter parameter k #define P0 101325.0f // standard pressure #define CCEXPONENT 0.2857142857f // exponent of compressibility correction 2/7 @@ -57,14 +57,13 @@ // Private types -// Private functions +// Private functions definitions static int8_t baro_airspeedReadMS4525DO(AirspeedSensorData *airspeedSensor, AirspeedSettingsData *airspeedSettings); // Private variables static uint16_t calibrationCount = 0; -static uint32_t filter_reg; // Barry Dorr filter register - +static uint32_t filter_reg = 0; // Barry Dorr filter register void baro_airspeedGetMS4525DO(AirspeedSensorData *airspeedSensor, AirspeedSettingsData *airspeedSettings) { @@ -99,6 +98,7 @@ void baro_airspeedGetMS4525DO(AirspeedSensorData *airspeedSensor, AirspeedSettin } +// Private functions static int8_t baro_airspeedReadMS4525DO(AirspeedSensorData *airspeedSensor, AirspeedSettingsData *airspeedSettings) { // Check to see if airspeed sensor is returning airspeedSensor diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c index b8dbe8e61..c4d375f56 100644 --- a/flight/modules/StateEstimation/stateestimation.c +++ b/flight/modules/StateEstimation/stateestimation.c @@ -74,6 +74,7 @@ UNSET_MASK(states.updated, SENSORUPDATES_##shortname); \ } \ } + #define FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_1_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(sensorname, shortname, a1, EXTRACHECK) \ if (IS_SET(states.updated, SENSORUPDATES_##shortname)) { \ sensorname##Data s; \ @@ -85,6 +86,7 @@ UNSET_MASK(states.updated, SENSORUPDATES_##shortname); \ } \ } + #define FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_2_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(sensorname, shortname, a1, a2, EXTRACHECK) \ if (IS_SET(states.updated, SENSORUPDATES_##shortname)) { \ sensorname##Data s; \ @@ -108,6 +110,7 @@ s.a3 = states.shortname[2]; \ statename##Set(&s); \ } + #define EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_2_DIMENSIONS(statename, shortname, a1, a2) \ if (IS_SET(states.updated, SENSORUPDATES_##shortname)) { \ statename##Data s; \ @@ -117,8 +120,8 @@ statename##Set(&s); \ } -// Private types +// Private types struct filterPipelineStruct; typedef const struct filterPipelineStruct { @@ -392,8 +395,6 @@ static void StateEstimationCb(void) FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(GPSVelocitySensor, vel, North, East, Down); FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_1_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(BaroSensor, baro, Altitude, true); FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_2_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(AirspeedSensor, airspeed, CalibratedAirspeed, TrueAirspeed, s.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE); -// FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_1_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(AirspeedSensor, airspeed, CalibratedAirspeed, s.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE); -// states.airspeed[1] = 0.0f; // sensor does not provide true airspeed, needs to be calculated by filter, set to zero for now // GPS position data (LLA) is not fetched here since it does not contain floats. The filter must do all checks itself From c5dc556a43ccab28f7f584030a3f57afa146e373 Mon Sep 17 00:00:00 2001 From: Andres <> Date: Thu, 27 Mar 2014 16:19:22 +0100 Subject: [PATCH 7/9] OP-1273 Uncrustify --- flight/modules/Airspeed/airspeed.c | 22 ++-- .../modules/Airspeed/baro_airspeed_ms4525do.c | 100 +++++++++--------- flight/modules/Attitude/revolution/attitude.c | 6 +- flight/modules/StateEstimation/filterair.c | 2 +- .../modules/StateEstimation/stateestimation.c | 8 +- flight/pios/common/pios_ms4525do.c | 31 +++--- flight/pios/stm32f10x/pios_i2c.c | 4 +- flight/pios/stm32f4xx/pios_i2c.c | 2 +- flight/targets/boards/revolution/pios_board.h | 8 +- 9 files changed, 91 insertions(+), 92 deletions(-) diff --git a/flight/modules/Airspeed/airspeed.c b/flight/modules/Airspeed/airspeed.c index 0da2d149d..1df2438ed 100644 --- a/flight/modules/Airspeed/airspeed.c +++ b/flight/modules/Airspeed/airspeed.c @@ -118,7 +118,7 @@ int32_t AirspeedInitialize() airspeedADCPin = i; } } - + lastAirspeedSensorType = airspeedSettings.AirspeedSensorType; AirspeedSensorInitialize(); @@ -155,21 +155,21 @@ static void airspeedTask(__attribute__((unused)) void *parameters) // Update the airspeed object AirspeedSensorGet(&airspeedData); - // if sensor type changed and the last sensor was + // if sensor type changed and the last sensor was // either Eagletree or PixHawk, reset I2C alarms - if(airspeedSettings.AirspeedSensorType != lastAirspeedSensorType){ + if (airspeedSettings.AirspeedSensorType != lastAirspeedSensorType) { switch (lastAirspeedSensorType) { - // Eagletree or PixHawk => Reset I2C alams - case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_EAGLETREEAIRSPEEDV3: - case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_PIXHAWKAIRSPEEDMS4525DO: - AlarmsDefault(SYSTEMALARMS_ALARM_I2C); - break; - // else do not reset I2C alarms - default: break; + // Eagletree or PixHawk => Reset I2C alams + case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_EAGLETREEAIRSPEEDV3: + case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_PIXHAWKAIRSPEEDMS4525DO: + AlarmsDefault(SYSTEMALARMS_ALARM_I2C); + break; + // else do not reset I2C alarms + default: break; } lastAirspeedSensorType = airspeedSettings.AirspeedSensorType; } - + switch (airspeedSettings.AirspeedSensorType) { #if defined(PIOS_INCLUDE_MPXV) case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002: diff --git a/flight/modules/Airspeed/baro_airspeed_ms4525do.c b/flight/modules/Airspeed/baro_airspeed_ms4525do.c index caeb44e99..af6a6170c 100644 --- a/flight/modules/Airspeed/baro_airspeed_ms4525do.c +++ b/flight/modules/Airspeed/baro_airspeed_ms4525do.c @@ -45,15 +45,15 @@ #if defined(PIOS_INCLUDE_MS4525DO) #define CALIBRATION_IDLE_MS 0 // Time to wait before calibrating, in [ms] -#define CALIBRATION_COUNT_MS 4000 // Time to spend calibrating, in [ms] -#define FILTER_SHIFT 5 // Barry Dorr filter parameter k +#define CALIBRATION_COUNT_MS 4000 // Time to spend calibrating, in [ms] +#define FILTER_SHIFT 5 // Barry Dorr filter parameter k -#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) +#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) -#define max(x,y) ((x)>=(y) ? (x) : (y)) +#define max(x, y) ((x) >= (y) ? (x) : (y)) // Private types @@ -62,38 +62,38 @@ static int8_t baro_airspeedReadMS4525DO(AirspeedSensorData *airspeedSensor, Airs // Private variables -static uint16_t calibrationCount = 0; -static uint32_t filter_reg = 0; // Barry Dorr filter register +static uint16_t calibrationCount = 0; +static uint32_t filter_reg = 0; // Barry Dorr filter register void baro_airspeedGetMS4525DO(AirspeedSensorData *airspeedSensor, AirspeedSettingsData *airspeedSettings) { // request measurement first int8_t retVal = PIOS_MS4525DO_Request(); - - if( retVal !=0 ){ + + if (retVal != 0) { AlarmsSet(SYSTEMALARMS_ALARM_I2C, SYSTEMALARMS_ALARM_ERROR); return; } - + // Datasheet of MS4525DO: conversion needs 0.5 ms + 20% more when status bit used // delay by one Tick or at least 2 ms const portTickType xDelay = max(2 / portTICK_RATE_MS, 1); - vTaskDelay( xDelay ); - + vTaskDelay(xDelay); + // read the sensor - retVal = baro_airspeedReadMS4525DO(airspeedSensor,airspeedSettings); - - switch( retVal ){ - case 0 : AlarmsClear(SYSTEMALARMS_ALARM_I2C); - break; - case -4 : - case -5 : AlarmsSet(SYSTEMALARMS_ALARM_I2C, SYSTEMALARMS_ALARM_WARNING); - break; - case -1 : - case -2 : - case -3 : - case -6 : - default : AlarmsSet(SYSTEMALARMS_ALARM_I2C, SYSTEMALARMS_ALARM_ERROR); + retVal = baro_airspeedReadMS4525DO(airspeedSensor, airspeedSettings); + + switch (retVal) { + case 0: AlarmsClear(SYSTEMALARMS_ALARM_I2C); + break; + case -4: + case -5: AlarmsSet(SYSTEMALARMS_ALARM_I2C, SYSTEMALARMS_ALARM_WARNING); + break; + case -1: + case -2: + case -3: + case -6: + default: AlarmsSet(SYSTEMALARMS_ALARM_I2C, SYSTEMALARMS_ALARM_ERROR); } } @@ -103,38 +103,38 @@ static int8_t baro_airspeedReadMS4525DO(AirspeedSensorData *airspeedSensor, Airs { // Check to see if airspeed sensor is returning airspeedSensor uint16_t values[2]; - int8_t retVal=PIOS_MS4525DO_Read(values); - - if( retVal == 0 ){ - airspeedSensor->SensorValue = values[0]; + int8_t retVal = PIOS_MS4525DO_Read(values); + + if (retVal == 0) { + airspeedSensor->SensorValue = values[0]; airspeedSensor->SensorValueTemperature = values[1]; } else { airspeedSensor->SensorValue = -1; airspeedSensor->SensorValueTemperature = -1; - airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; - airspeedSensor->CalibratedAirspeed = 0; + airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; + airspeedSensor->CalibratedAirspeed = 0; return retVal; } - + // only calibrate if no stored calibration is available - if ( !airspeedSettings->ZeroPoint ) { + if (!airspeedSettings->ZeroPoint) { // Calibrate sensor by averaging zero point value if (calibrationCount <= CALIBRATION_IDLE_MS / airspeedSettings->SamplePeriod) { calibrationCount++; - filter_reg = (airspeedSensor->SensorValue << FILTER_SHIFT ); - + filter_reg = (airspeedSensor->SensorValue << FILTER_SHIFT); + return retVal; } else if (calibrationCount <= (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS) / airspeedSettings->SamplePeriod) { calibrationCount++; // update filter register - filter_reg = filter_reg - ( filter_reg >> FILTER_SHIFT ) + airspeedSensor->SensorValue; + filter_reg = filter_reg - (filter_reg >> FILTER_SHIFT) + airspeedSensor->SensorValue; if (calibrationCount > (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS) / airspeedSettings->SamplePeriod) { - // Scale output for unity gain. - airspeedSettings->ZeroPoint = (uint16_t)( filter_reg >> FILTER_SHIFT ); - + // Scale output for unity gain. + airspeedSettings->ZeroPoint = (uint16_t)(filter_reg >> FILTER_SHIFT); + AirspeedSettingsZeroPointSet(&airspeedSettings->ZeroPoint); - calibrationCount = 0; + calibrationCount = 0; } return retVal; } @@ -149,19 +149,19 @@ static int8_t baro_airspeedReadMS4525DO(AirspeedSensorData *airspeedSensor, Airs Datasheet temperature: output = (T+50)*2047 / 200 Inversion: T = (200*out - 102350)/2047 in C T = (200*out - 102350)/2047 + 273.15 in K - */ - const float dP = ( 10*(int32_t)(airspeedSensor->SensorValue-airspeedSettings->ZeroPoint) ) * 0.1052120688f; - const float T = (float)( 200*(int32_t)airspeedSensor->SensorValueTemperature - 102350) / 2047 + 273.15f; - + */ + const float dP = (10 * (int32_t)(airspeedSensor->SensorValue - airspeedSettings->ZeroPoint)) * 0.1052120688f; + const float T = (float)(200 * (int32_t)airspeedSensor->SensorValueTemperature - 102350) / 2047 + 273.15f; + airspeedSensor->DifferentialPressure = dP; - airspeedSensor->Temperature = T; + airspeedSensor->Temperature = T; // CAS = Csound * sqrt( 5 *( (dP/P0 +1)^(2/7) - 1) ) // TAS = Csound * sqrt( 5 T/T0 *( (dP/P0 +1)^(2/7) - 1) ) // where Csound = 340.276 m/s at standard condition T0=288.15 K and P0 = 101315 Pa - airspeedSensor->CalibratedAirspeed = airspeedSettings->Scale * CASFACTOR * sqrtf( powf( fabsf(dP) / P0 + 1.0f , CCEXPONENT ) - 1.0f ); - airspeedSensor->TrueAirspeed = airspeedSensor->CalibratedAirspeed * TASFACTOR * sqrtf( T ); + airspeedSensor->CalibratedAirspeed = airspeedSettings->Scale * CASFACTOR * sqrtf(powf(fabsf(dP) / P0 + 1.0f, CCEXPONENT) - 1.0f); + airspeedSensor->TrueAirspeed = airspeedSensor->CalibratedAirspeed * TASFACTOR * sqrtf(T); airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE; - + return retVal; } diff --git a/flight/modules/Attitude/revolution/attitude.c b/flight/modules/Attitude/revolution/attitude.c index 0ca789199..8edbf5cb9 100644 --- a/flight/modules/Attitude/revolution/attitude.c +++ b/flight/modules/Attitude/revolution/attitude.c @@ -650,7 +650,7 @@ static int32_t updateAttitudeComplementary(bool first_run) if (airspeedSensor.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE) { // we have airspeed available airspeed.CalibratedAirspeed = airspeedSensor.CalibratedAirspeed; - airspeed.TrueAirspeed = (airspeedSensor.TrueAirspeed < 0.f) ? airspeed.CalibratedAirspeed * IAS2TAS(homeLocation.Altitude - positionState.Down) : airspeedSensor.TrueAirspeed; + airspeed.TrueAirspeed = (airspeedSensor.TrueAirspeed < 0.f) ? airspeed.CalibratedAirspeed *IAS2TAS(homeLocation.Altitude - positionState.Down) : airspeedSensor.TrueAirspeed; AirspeedStateSet(&airspeed); } } @@ -1078,8 +1078,8 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) AirspeedStateGet(&airspeed); airspeed.CalibratedAirspeed = airspeedData.CalibratedAirspeed; - airspeed.TrueAirspeed = (airspeedSensor.TrueAirspeed < 0.f) ? airspeed.CalibratedAirspeed * IAS2TAS(homeLocation.Altitude - positionState.Down) : airspeedSensor.TrueAirspeed; - + airspeed.TrueAirspeed = (airspeedSensor.TrueAirspeed < 0.f) ? airspeed.CalibratedAirspeed *IAS2TAS(homeLocation.Altitude - positionState.Down) : airspeedSensor.TrueAirspeed; + AirspeedStateSet(&airspeed); if (!gps_vel_updated && !gps_updated) { diff --git a/flight/modules/StateEstimation/filterair.c b/flight/modules/StateEstimation/filterair.c index 3c1308f5a..bf5c83a32 100644 --- a/flight/modules/StateEstimation/filterair.c +++ b/flight/modules/StateEstimation/filterair.c @@ -78,7 +78,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state) this->altitude = state->baro[0]; } // calculate true airspeed estimation - if (IS_SET(state->updated, SENSORUPDATES_airspeed) && (state->airspeed[1]<0.f) ) { + if (IS_SET(state->updated, SENSORUPDATES_airspeed) && (state->airspeed[1] < 0.f)) { state->airspeed[1] = state->airspeed[0] * IAS2TAS(this->altitude); } diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c index c4d375f56..c947be522 100644 --- a/flight/modules/StateEstimation/stateestimation.c +++ b/flight/modules/StateEstimation/stateestimation.c @@ -74,7 +74,7 @@ UNSET_MASK(states.updated, SENSORUPDATES_##shortname); \ } \ } - + #define FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_1_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(sensorname, shortname, a1, EXTRACHECK) \ if (IS_SET(states.updated, SENSORUPDATES_##shortname)) { \ sensorname##Data s; \ @@ -86,7 +86,7 @@ UNSET_MASK(states.updated, SENSORUPDATES_##shortname); \ } \ } - + #define FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_2_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(sensorname, shortname, a1, a2, EXTRACHECK) \ if (IS_SET(states.updated, SENSORUPDATES_##shortname)) { \ sensorname##Data s; \ @@ -110,7 +110,7 @@ s.a3 = states.shortname[2]; \ statename##Set(&s); \ } - + #define EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_2_DIMENSIONS(statename, shortname, a1, a2) \ if (IS_SET(states.updated, SENSORUPDATES_##shortname)) { \ statename##Data s; \ @@ -395,7 +395,7 @@ static void StateEstimationCb(void) FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(GPSVelocitySensor, vel, North, East, Down); FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_1_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(BaroSensor, baro, Altitude, true); FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_2_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(AirspeedSensor, airspeed, CalibratedAirspeed, TrueAirspeed, s.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE); - + // GPS position data (LLA) is not fetched here since it does not contain floats. The filter must do all checks itself // at this point sensor state is stored in "states" with some rudimentary filtering applied diff --git a/flight/pios/common/pios_ms4525do.c b/flight/pios/common/pios_ms4525do.c index f331453aa..8a8fc9aab 100644 --- a/flight/pios/common/pios_ms4525do.c +++ b/flight/pios/common/pios_ms4525do.c @@ -40,7 +40,6 @@ static int8_t PIOS_MS4525DO_WriteI2C(uint8_t *buffer, uint8_t len); static bool pios_ms4525do_requested = false; - static int8_t PIOS_MS4525DO_ReadI2C(uint8_t *buffer, uint8_t len) { const struct pios_i2c_txn txn_list[] = { @@ -73,15 +72,15 @@ static int8_t PIOS_MS4525DO_WriteI2C(uint8_t *buffer, uint8_t len) } - int8_t PIOS_MS4525DO_Request(void) { - // MS4525DO expects a zero length write. + // MS4525DO expects a zero length write. // Sending one byte is a workaround that works for the moment - uint8_t data=0; - int8_t retVal=PIOS_MS4525DO_WriteI2C(&data,sizeof(data)); - /* requested only when transfer worked */ - pios_ms4525do_requested = ( retVal == 0 ); + uint8_t data = 0; + int8_t retVal = PIOS_MS4525DO_WriteI2C(&data, sizeof(data)); + + /* requested only when transfer worked */ + pios_ms4525do_requested = (retVal == 0); return retVal; } @@ -98,28 +97,28 @@ int8_t PIOS_MS4525DO_Read(uint16_t *values) } uint8_t data[4]; - int8_t retVal=PIOS_MS4525DO_ReadI2C(data, sizeof(data)); + int8_t retVal = PIOS_MS4525DO_ReadI2C(data, sizeof(data)); uint8_t status = data[0] & 0xC0; if (status == 0x80) { - /* stale data */ + /* stale data */ return -5; } else if (status == 0xC0) { /* device probably broken */ return -6; } - /* differential pressure */ + /* differential pressure */ values[0] = (data[0] << 8); - values[0] += data[1]; - + values[0] += data[1]; + /* temperature */ values[1] = (data[2] << 8); - values[1] += data[3]; + values[1] += data[3]; values[1] = (values[1] >> 5); - - /* not requested anymore, only when transfer worked */ - pios_ms4525do_requested = !( retVal == 0 ); + + /* not requested anymore, only when transfer worked */ + pios_ms4525do_requested = !(retVal == 0); return retVal; } diff --git a/flight/pios/stm32f10x/pios_i2c.c b/flight/pios/stm32f10x/pios_i2c.c index 88eccb831..8cb98444c 100644 --- a/flight/pios/stm32f10x/pios_i2c.c +++ b/flight/pios/stm32f10x/pios_i2c.c @@ -414,8 +414,8 @@ static void go_starting(struct pios_i2c_adapter *i2c_adapter) i2c_adapter->last_byte = NULL; i2c_adapter->active_byte = i2c_adapter->last_byte + 1; } - - + + I2C_GenerateSTART(i2c_adapter->cfg->regs, ENABLE); if (i2c_adapter->active_txn->rw == PIOS_I2C_TXN_READ) { I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, ENABLE); diff --git a/flight/pios/stm32f4xx/pios_i2c.c b/flight/pios/stm32f4xx/pios_i2c.c index eff4682fe..a5a7b6969 100644 --- a/flight/pios/stm32f4xx/pios_i2c.c +++ b/flight/pios/stm32f4xx/pios_i2c.c @@ -417,7 +417,7 @@ static void go_starting(struct pios_i2c_adapter *i2c_adapter) i2c_adapter->last_byte = NULL; i2c_adapter->active_byte = i2c_adapter->last_byte + 1; } - + I2C_GenerateSTART(i2c_adapter->cfg->regs, ENABLE); if (i2c_adapter->active_txn->rw == PIOS_I2C_TXN_READ) { I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, ENABLE); diff --git a/flight/targets/boards/revolution/pios_board.h b/flight/targets/boards/revolution/pios_board.h index 60d3cff53..975a87194 100644 --- a/flight/targets/boards/revolution/pios_board.h +++ b/flight/targets/boards/revolution/pios_board.h @@ -119,12 +119,12 @@ // PIOS_I2C // See also pios_board.c // ------------------------ -#define PIOS_I2C_MAX_DEVS 3 +#define PIOS_I2C_MAX_DEVS 3 extern uint32_t pios_i2c_mag_pressure_adapter_id; -#define PIOS_I2C_MAIN_ADAPTER (pios_i2c_mag_pressure_adapter_id) +#define PIOS_I2C_MAIN_ADAPTER (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_FLEXI_ADAPTER (pios_i2c_flexiport_adapter_id) +#define PIOS_I2C_ETASV3_ADAPTER (PIOS_I2C_FLEXI_ADAPTER) #define PIOS_I2C_MS4525DO_ADAPTER (PIOS_I2C_FLEXI_ADAPTER) // ------------------------- From 4188b492fce1a289c3d0b37f24849dad810524f4 Mon Sep 17 00:00:00 2001 From: Andres <> Date: Thu, 27 Mar 2014 16:59:48 +0100 Subject: [PATCH 8/9] OP-1273 added I2C warning alarm during calibration of zero point, and uncrustify --- flight/modules/Airspeed/baro_airspeed_ms4525do.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/flight/modules/Airspeed/baro_airspeed_ms4525do.c b/flight/modules/Airspeed/baro_airspeed_ms4525do.c index af6a6170c..eaaee6210 100644 --- a/flight/modules/Airspeed/baro_airspeed_ms4525do.c +++ b/flight/modules/Airspeed/baro_airspeed_ms4525do.c @@ -87,7 +87,8 @@ void baro_airspeedGetMS4525DO(AirspeedSensorData *airspeedSensor, AirspeedSettin case 0: AlarmsClear(SYSTEMALARMS_ALARM_I2C); break; case -4: - case -5: AlarmsSet(SYSTEMALARMS_ALARM_I2C, SYSTEMALARMS_ALARM_WARNING); + case -5: + case -7: AlarmsSet(SYSTEMALARMS_ALARM_I2C, SYSTEMALARMS_ALARM_WARNING); break; case -1: case -2: @@ -122,8 +123,7 @@ static int8_t baro_airspeedReadMS4525DO(AirspeedSensorData *airspeedSensor, Airs if (calibrationCount <= CALIBRATION_IDLE_MS / airspeedSettings->SamplePeriod) { calibrationCount++; filter_reg = (airspeedSensor->SensorValue << FILTER_SHIFT); - - return retVal; + return -7; } else if (calibrationCount <= (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS) / airspeedSettings->SamplePeriod) { calibrationCount++; // update filter register @@ -136,7 +136,7 @@ static int8_t baro_airspeedReadMS4525DO(AirspeedSensorData *airspeedSensor, Airs AirspeedSettingsZeroPointSet(&airspeedSettings->ZeroPoint); calibrationCount = 0; } - return retVal; + return -7; } } From 7ec855af5fdda84402457ba7be085577f101725f Mon Sep 17 00:00:00 2001 From: Andres <> Date: Fri, 28 Mar 2014 14:18:14 +0100 Subject: [PATCH 9/9] OP-1273 added and implemented Airpeed alarm for the PixHawk sensor --- flight/modules/Airspeed/airspeed.c | 9 +- .../modules/Airspeed/baro_airspeed_ms4525do.c | 8 +- .../diagrams/default/system-health.svg | 279 ++++++++++++------ .../uavobjectdefinition/airspeedsettings.xml | 2 +- shared/uavobjectdefinition/systemalarms.xml | 1 + 5 files changed, 205 insertions(+), 94 deletions(-) diff --git a/flight/modules/Airspeed/airspeed.c b/flight/modules/Airspeed/airspeed.c index 1df2438ed..b81d5ad5d 100644 --- a/flight/modules/Airspeed/airspeed.c +++ b/flight/modules/Airspeed/airspeed.c @@ -156,15 +156,15 @@ static void airspeedTask(__attribute__((unused)) void *parameters) AirspeedSensorGet(&airspeedData); // if sensor type changed and the last sensor was - // either Eagletree or PixHawk, reset I2C alarms + // either Eagletree or PixHawk, reset Airspeed alarm if (airspeedSettings.AirspeedSensorType != lastAirspeedSensorType) { switch (lastAirspeedSensorType) { - // Eagletree or PixHawk => Reset I2C alams + // Eagletree or PixHawk => Reset Airspeed alams case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_EAGLETREEAIRSPEEDV3: case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_PIXHAWKAIRSPEEDMS4525DO: - AlarmsDefault(SYSTEMALARMS_ALARM_I2C); + AlarmsDefault(SYSTEMALARMS_ALARM_AIRSPEED); break; - // else do not reset I2C alarms + // else do not reset Airspeed alarm default: break; } lastAirspeedSensorType = airspeedSettings.AirspeedSensorType; @@ -193,6 +193,7 @@ static void airspeedTask(__attribute__((unused)) void *parameters) case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_GROUNDSPEEDBASEDWINDESTIMATION: gps_airspeedGet(&airspeedData, &airspeedSettings); break; + case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_NONE: default: airspeedData.SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; } diff --git a/flight/modules/Airspeed/baro_airspeed_ms4525do.c b/flight/modules/Airspeed/baro_airspeed_ms4525do.c index eaaee6210..5ca0469a8 100644 --- a/flight/modules/Airspeed/baro_airspeed_ms4525do.c +++ b/flight/modules/Airspeed/baro_airspeed_ms4525do.c @@ -71,7 +71,7 @@ void baro_airspeedGetMS4525DO(AirspeedSensorData *airspeedSensor, AirspeedSettin int8_t retVal = PIOS_MS4525DO_Request(); if (retVal != 0) { - AlarmsSet(SYSTEMALARMS_ALARM_I2C, SYSTEMALARMS_ALARM_ERROR); + AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_ERROR); return; } @@ -84,17 +84,17 @@ void baro_airspeedGetMS4525DO(AirspeedSensorData *airspeedSensor, AirspeedSettin retVal = baro_airspeedReadMS4525DO(airspeedSensor, airspeedSettings); switch (retVal) { - case 0: AlarmsClear(SYSTEMALARMS_ALARM_I2C); + case 0: AlarmsClear(SYSTEMALARMS_ALARM_AIRSPEED); break; case -4: case -5: - case -7: AlarmsSet(SYSTEMALARMS_ALARM_I2C, SYSTEMALARMS_ALARM_WARNING); + case -7: AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_WARNING); break; case -1: case -2: case -3: case -6: - default: AlarmsSet(SYSTEMALARMS_ALARM_I2C, SYSTEMALARMS_ALARM_ERROR); + default: AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_ERROR); } } diff --git a/ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg b/ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg index 10c6a9d53..914709810 100644 --- a/ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg +++ b/ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg @@ -14,8 +14,8 @@ height="79.57505" id="svg3604" version="1.1" - inkscape:version="0.48.2 r9819" - sodipodi:docname="system-health.svg" + inkscape:version="0.48.4 r9939" + sodipodi:docname="system-health-mod.svg" inkscape:export-filename="C:\NoBackup\OpenPilot\mainboard-health.png" inkscape:export-xdpi="269.53" inkscape:export-ydpi="269.53" @@ -27,16 +27,16 @@ borderopacity="1.0" inkscape:pageopacity="0.0" inkscape:pageshadow="2" - inkscape:zoom="9.3551549" - inkscape:cx="42.866204" - inkscape:cy="35.940147" - inkscape:current-layer="g4794" + inkscape:zoom="10.554213" + inkscape:cx="47.1292" + inkscape:cy="39.787519" + inkscape:current-layer="svg3604" id="namedview3608" showgrid="false" inkscape:window-width="1920" - inkscape:window-height="1178" - inkscape:window-x="0" - inkscape:window-y="0" + inkscape:window-height="1025" + inkscape:window-x="-2" + inkscape:window-y="-3" inkscape:window-maximized="1" showguides="true" inkscape:guide-bbox="true"> @@ -656,39 +656,48 @@ + d="m 524.3844,367.59399 1.32275,0 1.67432,4.46485 1.68311,-4.46485 1.32275,0 0,6.56104 -0.86572,0 0,-5.76123 -1.6919,4.5 -0.89209,0 -1.69189,-4.5 0,5.76123 -0.86133,0 0,-6.56104" + inkscape:connector-curvature="0" /> + d="m 534.35559,371.68091 c -0.65332,0 -1.10596,0.0747 -1.35791,0.22412 -0.25195,0.14942 -0.37793,0.4043 -0.37793,0.76465 0,0.28711 0.0937,0.51562 0.28125,0.68554 0.19043,0.167 0.44824,0.25049 0.77344,0.25049 0.44824,0 0.80712,-0.1582 1.07666,-0.47461 0.27246,-0.31933 0.40869,-0.74267 0.40869,-1.27002 l 0,-0.18017 -0.8042,0 m 1.61279,-0.33399 0,2.80811 -0.80859,0 0,-0.74707 c -0.18457,0.29883 -0.41455,0.52002 -0.68994,0.66357 -0.27539,0.14063 -0.61231,0.21094 -1.01074,0.21094 -0.50391,0 -0.90528,-0.14062 -1.20411,-0.42187 -0.29589,-0.28418 -0.44384,-0.66358 -0.44384,-1.13819 0,-0.55371 0.18457,-0.97119 0.55371,-1.25244 0.37207,-0.28125 0.92578,-0.42187 1.66113,-0.42187 l 1.13379,0 0,-0.0791 c 0,-0.37206 -0.12305,-0.65917 -0.36914,-0.86132 -0.24317,-0.20508 -0.58594,-0.30762 -1.02832,-0.30762 -0.28125,0 -0.55518,0.0337 -0.82178,0.10107 -0.2666,0.0674 -0.52295,0.16846 -0.76904,0.30323 l 0,-0.74707 c 0.2959,-0.11426 0.58301,-0.19922 0.86133,-0.25489 0.27832,-0.0586 0.54931,-0.0879 0.81299,-0.0879 0.71191,10e-6 1.24364,0.18458 1.59521,0.55371 0.35156,0.36915 0.52734,0.92872 0.52734,1.67871" + inkscape:connector-curvature="0" /> + d="m 537.63831,369.23315 0.80859,0 0,4.92188 -0.80859,0 0,-4.92188 m 0,-1.91601 0.80859,0 0,1.02392 -0.80859,0 0,-1.02392" + inkscape:connector-curvature="0" /> + d="m 544.22571,371.18433 0,2.9707 -0.8086,0 0,-2.94434 c 0,-0.46581 -0.0908,-0.81445 -0.27246,-1.0459 -0.18164,-0.23144 -0.4541,-0.34716 -0.81738,-0.34716 -0.43653,0 -0.78076,0.13916 -1.03271,0.41748 -0.25196,0.27832 -0.37794,0.65772 -0.37793,1.13818 l 0,2.78174 -0.81299,0 0,-4.92188 0.81299,0 0,0.76465 c 0.19335,-0.29589 0.4204,-0.51708 0.68115,-0.66357 0.26367,-0.14648 0.56689,-0.21972 0.90967,-0.21973 0.56542,10e-6 0.99316,0.17579 1.2832,0.52735 0.29003,0.34863 0.43505,0.86279 0.43506,1.54248" + inkscape:connector-curvature="0" /> + d="m 549.38049,371.69849 c 0,-0.59473 -0.12305,-1.06055 -0.36914,-1.39746 -0.24317,-0.33984 -0.57861,-0.50977 -1.00635,-0.50977 -0.42773,0 -0.76465,0.16993 -1.01074,0.50977 -0.24316,0.33691 -0.36475,0.80273 -0.36474,1.39746 -10e-6,0.59472 0.12158,1.06201 0.36474,1.40185 0.24609,0.33692 0.58301,0.50537 1.01074,0.50537 0.42774,0 0.76318,-0.16845 1.00635,-0.50537 0.24609,-0.33984 0.36914,-0.80713 0.36914,-1.40185 m -2.75097,-1.71827 c 0.16992,-0.29296 0.38378,-0.50976 0.6416,-0.65039 0.26074,-0.14355 0.57128,-0.21532 0.93164,-0.21533 0.59765,10e-6 1.08251,0.23731 1.45459,0.71192 0.37499,0.47461 0.56249,1.09863 0.5625,1.87207 -10e-6,0.77344 -0.18751,1.39746 -0.5625,1.87207 -0.37208,0.47461 -0.85694,0.71191 -1.45459,0.71191 -0.36036,0 -0.6709,-0.0703 -0.93164,-0.21094 -0.25782,-0.14355 -0.47168,-0.36181 -0.6416,-0.65478 l 0,0.73828 -0.81299,0 0,-6.83789 0.81299,0 0,2.66308" + inkscape:connector-curvature="0" /> + d="m 553.46741,369.80005 c -0.4336,0 -0.77637,0.16992 -1.02832,0.50976 -0.25196,0.33692 -0.37793,0.79981 -0.37793,1.38868 0,0.58887 0.12451,1.05322 0.37353,1.39306 0.25195,0.33692 0.59619,0.50537 1.03272,0.50537 0.43066,0 0.77197,-0.16992 1.02392,-0.50976 0.25195,-0.33984 0.37793,-0.80273 0.37793,-1.38867 0,-0.58301 -0.12598,-1.04443 -0.37793,-1.38428 -0.25195,-0.34277 -0.59326,-0.51416 -1.02392,-0.51416 m 0,-0.68555 c 0.70312,10e-6 1.25536,0.22852 1.65674,0.68555 0.40136,0.45703 0.60204,1.08985 0.60205,1.89844 -10e-6,0.80566 -0.20069,1.43847 -0.60205,1.89843 -0.40138,0.45704 -0.95362,0.68555 -1.65674,0.68555 -0.70606,0 -1.25977,-0.22851 -1.66114,-0.68555 -0.39843,-0.45996 -0.59765,-1.09277 -0.59765,-1.89843 0,-0.80859 0.19922,-1.44141 0.59765,-1.89844 0.40137,-0.45703 0.95508,-0.68554 1.66114,-0.68555" + inkscape:connector-curvature="0" /> + d="m 559.29895,371.68091 c -0.65332,0 -1.10596,0.0747 -1.35791,0.22412 -0.25195,0.14942 -0.37793,0.4043 -0.37793,0.76465 0,0.28711 0.0937,0.51562 0.28125,0.68554 0.19043,0.167 0.44824,0.25049 0.77344,0.25049 0.44824,0 0.80712,-0.1582 1.07666,-0.47461 0.27246,-0.31933 0.40869,-0.74267 0.40869,-1.27002 l 0,-0.18017 -0.8042,0 m 1.61279,-0.33399 0,2.80811 -0.80859,0 0,-0.74707 c -0.18457,0.29883 -0.41455,0.52002 -0.68994,0.66357 -0.2754,0.14063 -0.61231,0.21094 -1.01074,0.21094 -0.50391,0 -0.90528,-0.14062 -1.20411,-0.42187 -0.29589,-0.28418 -0.44384,-0.66358 -0.44384,-1.13819 0,-0.55371 0.18457,-0.97119 0.55371,-1.25244 0.37207,-0.28125 0.92578,-0.42187 1.66113,-0.42187 l 1.13379,0 0,-0.0791 c 0,-0.37206 -0.12305,-0.65917 -0.36914,-0.86132 -0.24317,-0.20508 -0.58594,-0.30762 -1.02832,-0.30762 -0.28125,0 -0.55518,0.0337 -0.82178,0.10107 -0.2666,0.0674 -0.52295,0.16846 -0.76904,0.30323 l 0,-0.74707 c 0.2959,-0.11426 0.583,-0.19922 0.86133,-0.25489 0.27831,-0.0586 0.54931,-0.0879 0.81298,-0.0879 0.71192,10e-6 1.24365,0.18458 1.59522,0.55371 0.35156,0.36915 0.52734,0.92872 0.52734,1.67871" + inkscape:connector-curvature="0" /> + d="m 565.43372,369.98901 c -0.0908,-0.0527 -0.19044,-0.0908 -0.29883,-0.11425 -0.10547,-0.0264 -0.22266,-0.0395 -0.35156,-0.0395 -0.45704,0 -0.8086,0.14941 -1.05469,0.44824 -0.24317,0.2959 -0.36475,0.72217 -0.36475,1.27881 l 0,2.59277 -0.81299,0 0,-4.92188 0.81299,0 0,0.76465 c 0.16992,-0.29882 0.39111,-0.52001 0.66358,-0.66357 0.27245,-0.14648 0.60351,-0.21972 0.99316,-0.21973 0.0557,10e-6 0.11718,0.004 0.18457,0.0132 0.0674,0.006 0.14209,0.0161 0.22412,0.0308 l 0.004,0.83056" + inkscape:connector-curvature="0" /> + d="m 569.37122,369.98022 0,-2.66308 0.80859,0 0,6.83789 -0.80859,0 0,-0.73828 c -0.16993,0.29297 -0.38526,0.51123 -0.646,0.65478 -0.25782,0.14063 -0.56836,0.21094 -0.93164,0.21094 -0.59473,0 -1.07959,-0.2373 -1.45459,-0.71191 -0.37207,-0.47461 -0.55811,-1.09863 -0.55811,-1.87207 0,-0.77344 0.18604,-1.39746 0.55811,-1.87207 0.375,-0.47461 0.85986,-0.71191 1.45459,-0.71192 0.36328,10e-6 0.67382,0.0718 0.93164,0.21533 0.26074,0.14063 0.47607,0.35743 0.646,0.65039 m -2.75538,1.71827 c 0,0.59472 0.12159,1.06201 0.36475,1.40185 0.24609,0.33692 0.58301,0.50537 1.01074,0.50537 0.42773,0 0.76465,-0.16845 1.01075,-0.50537 0.24608,-0.33984 0.36913,-0.80713 0.36914,-1.40185 -10e-6,-0.59473 -0.12306,-1.06055 -0.36914,-1.39746 -0.2461,-0.33984 -0.58302,-0.50977 -1.01075,-0.50977 -0.42773,0 -0.76465,0.16993 -1.01074,0.50977 -0.24316,0.33691 -0.36475,0.80273 -0.36475,1.39746" + inkscape:connector-curvature="0" /> + + x="572.51758" + y="345.79932" /> + x="519.2749" + y="346.19791" /> + id="path3156" + inkscape:connector-curvature="0" /> + inkscape:label="#path4088-8" + inkscape:connector-curvature="0" /> + style="stroke:#cf0e0e;stroke-width:2;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" + transform="matrix(1.2853162,0,0,0.64013573,-26.151312,-11.845582)"> + d="m 38,33.292974 17.85,8" + style="fill:none;stroke:#cf0e0e;stroke-width:2;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none" + inkscape:connector-curvature="0" /> + d="m 38,41.354548 17.85,-8" + style="fill:none;stroke:#cf0e0e;stroke-width:2;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none" + inkscape:connector-curvature="0" /> + + + + @@ -1089,9 +1175,9 @@ @@ -1102,19 +1188,19 @@ inkscape:groupmode="layer"> @@ -1127,9 +1213,9 @@ @@ -1332,7 +1418,7 @@ @@ -1360,15 +1446,18 @@ + id="Telemetry-Error" + transform="translate(-3.4013125,0)"> + id="path4353" + inkscape:connector-curvature="0" /> + id="path4355" + inkscape:connector-curvature="0" /> + id="Power-Error" + transform="translate(-1.9500095,0)"> @@ -1668,7 +1758,7 @@ style="display:none"> @@ -1698,7 +1788,7 @@ id="Battery-Error" style="stroke:#cf0e0e;stroke-width:2;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none" inkscape:label="#g4026" - transform="translate(0,9.5134715)"> + transform="translate(-1.9500095,9.5134715)"> + transform="translate(-2.3409014,-0.20358251)"> + style="font-size:40px;font-style:normal;font-weight:normal;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Bitstream Vera Sans" + transform="translate(2.8817153,-0.30233889)"> + style="font-size:40px;font-style:normal;font-weight:normal;text-align:center;text-anchor:middle;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Bitstream Vera Sans" + transform="translate(2.9776741,0)"> + style="font-size:40px;font-style:normal;font-weight:normal;fill:#ffffff;fill-opacity:1;stroke:none;font-family:Bitstream Vera Sans" + transform="translate(-3.4013125,0)"> + style="font-size:35.79270172px;font-style:normal;font-weight:normal;fill:#ffffff;fill-opacity:1;stroke:none;font-family:Bitstream Vera Sans" + transform="translate(-3.4013125,0)"> + style="font-size:40px;font-style:normal;font-weight:normal;fill:#ffffff;fill-opacity:1;stroke:none;font-family:Bitstream Vera Sans" + transform="translate(-3.4013125,0)"> + style="font-size:40px;font-style:normal;font-weight:normal;fill:#ffffff;fill-opacity:1;stroke:none;font-family:Bitstream Vera Sans" + transform="translate(-3.4013125,0)"> Battery Sensors + Airspeed - + diff --git a/shared/uavobjectdefinition/systemalarms.xml b/shared/uavobjectdefinition/systemalarms.xml index d74ae5e02..f5b32abad 100644 --- a/shared/uavobjectdefinition/systemalarms.xml +++ b/shared/uavobjectdefinition/systemalarms.xml @@ -15,6 +15,7 @@ Actuator Attitude Sensors + Airspeed Stabilization Guidance PathPlan