From ba5ebb6b134c1848d6835ad80bda61dabeb2b030 Mon Sep 17 00:00:00 2001 From: sambas Date: Fri, 28 Jan 2011 19:21:22 +0000 Subject: [PATCH] OP-303 PIOS/Sonar: preliminary support for HCSR04 ultrasound altimeter module, tested and working :) git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2610 ebee16cc-31ac-478f-84a7-5cbb03baadba --- flight/Modules/Altitude/altitude.c | 34 +++ flight/OpenPilot/Makefile | 2 + flight/OpenPilot/System/inc/pios_config.h | 1 + flight/PiOS/Common/pios_hcsr04.c | 210 ++++++++++++++++++ flight/PiOS/inc/pios_hcsr04.h | 45 ++++ flight/PiOS/pios.h | 3 + .../src/plugins/uavobjects/uavobjects.pro | 4 +- shared/uavobjectdefinition/sonaraltitude.xml | 10 + 8 files changed, 308 insertions(+), 1 deletion(-) create mode 100644 flight/PiOS/Common/pios_hcsr04.c create mode 100644 flight/PiOS/inc/pios_hcsr04.h create mode 100644 shared/uavobjectdefinition/sonaraltitude.xml diff --git a/flight/Modules/Altitude/altitude.c b/flight/Modules/Altitude/altitude.c index 269003f6c..705eb061d 100644 --- a/flight/Modules/Altitude/altitude.c +++ b/flight/Modules/Altitude/altitude.c @@ -38,6 +38,9 @@ #include "openpilot.h" #include "baroaltitude.h" // object that will be updated by the module +#if defined(PIOS_INCLUDE_HCSR04) +#include "sonaraltitude.h" // object that will be updated by the module +#endif // Private constants #define STACK_SIZE_BYTES 500 @@ -85,12 +88,43 @@ static void altitudeTask(void *parameters) BaroAltitudeData data; portTickType lastSysTime; +#if defined(PIOS_INCLUDE_HCSR04) + SonarAltitudeData sonardata; + int32_t value=0,timeout=5; + float coeff=0.25,height_out=0,height_in=0; + PIOS_HCSR04_Init(); + PIOS_HCSR04_Trigger(); +#endif PIOS_BMP085_Init(); // Main task loop lastSysTime = xTaskGetTickCount(); while (1) { +#if defined(PIOS_INCLUDE_HCSR04) + // Compute the current altitude (all pressures in kPa) + if(PIOS_HCSR04_Completed()) + { + value = PIOS_HCSR04_Get(); + if((value>100) && (value < 15000)) //from 3.4cm to 5.1m + { + height_in = value*0.00034; + height_out = (height_out * (1 - coeff)) + (height_in * coeff); + sonardata.Altitude = height_out; // m/us + } + + // Update the AltitudeActual UAVObject + SonarAltitudeSet(&sonardata); + timeout=5; + PIOS_HCSR04_Trigger(); + } + if(timeout--) + { + //retrigger + timeout=5; + PIOS_HCSR04_Trigger(); + } +#endif // Update the temperature data PIOS_BMP085_StartADC(TemperatureConv); xSemaphoreTake(PIOS_BMP085_EOC, portMAX_DELAY); diff --git a/flight/OpenPilot/Makefile b/flight/OpenPilot/Makefile index 803bcf995..e406a79af 100644 --- a/flight/OpenPilot/Makefile +++ b/flight/OpenPilot/Makefile @@ -211,6 +211,7 @@ SRC += $(OPUAVSYNTHDIR)/flightplansettings.c SRC += $(OPUAVSYNTHDIR)/taskinfo.c SRC += $(OPUAVSYNTHDIR)/watchdogstatus.c SRC += $(OPUAVSYNTHDIR)/nedaccel.c +SRC += $(OPUAVSYNTHDIR)/sonaraltitude.c SRC += $(OPUAVSYNTHDIR)/uavobjectsinit.c endif @@ -245,6 +246,7 @@ SRC += $(PIOSSTM32F10X)/pios_usb_hid_pwr.c SRC += $(PIOSCOMMON)/pios_sdcard.c SRC += $(PIOSCOMMON)/pios_com.c SRC += $(PIOSCOMMON)/pios_bmp085.c +SRC += $(PIOSCOMMON)/pios_hcsr04.c SRC += $(PIOSCOMMON)/pios_i2c_esc.c SRC += $(PIOSCOMMON)/pios_iap.c #SRC += $(PIOSCOMMON)/pios_opahrs.c diff --git a/flight/OpenPilot/System/inc/pios_config.h b/flight/OpenPilot/System/inc/pios_config.h index 33e462ba4..a9e9b7ad7 100644 --- a/flight/OpenPilot/System/inc/pios_config.h +++ b/flight/OpenPilot/System/inc/pios_config.h @@ -50,6 +50,7 @@ #define PIOS_INCLUDE_USART #define PIOS_INCLUDE_USB_HID #define PIOS_INCLUDE_BMP085 +//#define PIOS_INCLUDE_HCSR04 #define PIOS_INCLUDE_OPAHRS #define PIOS_INCLUDE_COM #define PIOS_INCLUDE_SDCARD diff --git a/flight/PiOS/Common/pios_hcsr04.c b/flight/PiOS/Common/pios_hcsr04.c new file mode 100644 index 000000000..aa6dece83 --- /dev/null +++ b/flight/PiOS/Common/pios_hcsr04.c @@ -0,0 +1,210 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_HCSR04 HCSR04 Functions + * @brief Hardware functions to deal with the altitude pressure sensor + * @{ + * + * @file pios_hcsr04.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief HCSR04 sonar Sensor Routines + * @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_HCSR04) +#if !defined(PIOS_INCLUDE_SPEKTRUM) +#error Only supported with spektrum interface! +#endif + +/* Local Variables */ + +static TIM_ICInitTypeDef TIM_ICInitStructure; +static uint8_t CaptureState; +static uint16_t RiseValue; +static uint16_t FallValue; +static uint32_t CaptureValue; +static uint32_t CapCounter; + +#define PIOS_HCSR04_TRIG_GPIO_PORT GPIOD +#define PIOS_HCSR04_TRIG_PIN GPIO_Pin_2 + +/** +* Initialise the HC-SR04 sensor +*/ +void PIOS_HCSR04_Init(void) +{ + /* Init triggerpin */ + GPIO_InitTypeDef GPIO_InitStructure; + GPIO_StructInit(&GPIO_InitStructure); + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD; + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; + GPIO_InitStructure.GPIO_Pin = PIOS_HCSR04_TRIG_PIN; + GPIO_Init(PIOS_HCSR04_TRIG_GPIO_PORT, &GPIO_InitStructure); + PIOS_HCSR04_TRIG_GPIO_PORT->BSRR = PIOS_HCSR04_TRIG_PIN; + + /* Flush counter variables */ + CaptureState = 0; + RiseValue = 0; + FallValue = 0; + CaptureValue = 0; + + /* Setup RCC */ + RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE); + + /* Enable timer interrupts */ + NVIC_InitTypeDef NVIC_InitStructure; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID; + NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; + NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; + NVIC_InitStructure.NVIC_IRQChannel = TIM3_IRQn; + NVIC_Init(&NVIC_InitStructure); + + /* Partial pin remap for TIM3 (PB5) */ + GPIO_PinRemapConfig(GPIO_PartialRemap_TIM3, ENABLE); + + /* Configure input pins */ + GPIO_StructInit(&GPIO_InitStructure); + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPD; + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; + GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5; + GPIO_Init(GPIOB, &GPIO_InitStructure); + + /* Configure timer for input capture */ + TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising; + TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI; + TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1; + TIM_ICInitStructure.TIM_ICFilter = 0x0; + TIM_ICInitStructure.TIM_Channel = TIM_Channel_2; + TIM_ICInit(TIM3, &TIM_ICInitStructure); + + /* Configure timer clocks */ + TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; + TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); + TIM_TimeBaseStructure.TIM_Period = 0xFFFF; + TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_MASTER_CLOCK / 500000) - 1; + TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; + TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; + TIM_InternalClockConfig(TIM3); + TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure); + + /* Enable the Capture Compare Interrupt Request */ + //TIM_ITConfig(PIOS_PWM_CH8_TIM_PORT, PIOS_PWM_CH8_CCR, ENABLE); + TIM_ITConfig(TIM3, TIM_IT_CC2, DISABLE); + + /* Enable timers */ + TIM_Cmd(TIM3, ENABLE); + + /* Setup local variable which stays in this scope */ + /* Doing this here and using a local variable saves doing it in the ISR */ + TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI; + TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1; + TIM_ICInitStructure.TIM_ICFilter = 0x0; +} + +/** +* Get the value of an sonar timer +* \output >0 timer value +*/ +int32_t PIOS_HCSR04_Get(void) +{ + return CaptureValue; +} + +/** +* Get the value of an sonar timer +* \output >0 timer value +*/ +int32_t PIOS_HCSR04_Completed(void) +{ + return CapCounter; +} +/** +* Trigger sonar sensor +*/ +void PIOS_HCSR04_Trigger(void) +{ + CapCounter=0; + PIOS_HCSR04_TRIG_GPIO_PORT->BSRR = PIOS_HCSR04_TRIG_PIN; + PIOS_DELAY_WaituS(15); + PIOS_HCSR04_TRIG_GPIO_PORT->BRR = PIOS_HCSR04_TRIG_PIN; + TIM_ITConfig(TIM3, TIM_IT_CC2, ENABLE); +} + + +/** +* Handle TIM3 global interrupt request +*/ +//void PIOS_PWM_irq_handler(TIM_TypeDef * timer) +void TIM3_IRQHandler(void) +{ + /* Zero value always will be changed but this prevents compiler warning */ + int32_t i = 0; + + /* Do this as it's more efficient */ + if (TIM_GetITStatus(TIM3, TIM_IT_CC2) == SET) { + i = 7; + if (CaptureState == 0) { + RiseValue = TIM_GetCapture2(TIM3); + } else { + FallValue = TIM_GetCapture2(TIM3); + } + } + + /* Clear TIM3 Capture compare interrupt pending bit */ + TIM_ClearITPendingBit(TIM3, TIM_IT_CC2); + + /* Simple rise or fall state machine */ + if (CaptureState == 0) { + /* Switch states */ + CaptureState = 1; + + /* Switch polarity of input capture */ + TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling; + TIM_ICInitStructure.TIM_Channel = TIM_Channel_2; + TIM_ICInit(TIM3, &TIM_ICInitStructure); + + } else { + /* Capture computation */ + if (FallValue > RiseValue) { + CaptureValue = (FallValue - RiseValue); + } else { + CaptureValue = ((0xFFFF - RiseValue) + FallValue); + } + + /* Switch states */ + CaptureState = 0; + + /* Increase supervisor counter */ + CapCounter++; + TIM_ITConfig(TIM3, TIM_IT_CC2, DISABLE); + + /* Switch polarity of input capture */ + TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising; + TIM_ICInitStructure.TIM_Channel = TIM_Channel_2; + TIM_ICInit(TIM3, &TIM_ICInitStructure); + + } +} + + +#endif diff --git a/flight/PiOS/inc/pios_hcsr04.h b/flight/PiOS/inc/pios_hcsr04.h new file mode 100644 index 000000000..d2574a826 --- /dev/null +++ b/flight/PiOS/inc/pios_hcsr04.h @@ -0,0 +1,45 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_HCSR04 HCSR04 Functions + * @brief Hardware functions to deal with the altitude sonar sensor + * @{ + * + * @file pios_hcsr04.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief HCSR04 functions header. + * @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_HCSR04_H +#define PIOS_HCSR04_H + +/* Public Functions */ +extern void PIOS_HCSR04_Init(void); +extern int32_t PIOS_HCSR04_Get(void); +extern int32_t PIOS_HCSR04_Completed(void); +extern void PIOS_HCSR04_Trigger(void); + +#endif /* PIOS_HCSR04_H */ + +/** + * @} + * @} + */ diff --git a/flight/PiOS/pios.h b/flight/PiOS/pios.h index cee4517a0..684084f57 100644 --- a/flight/PiOS/pios.h +++ b/flight/PiOS/pios.h @@ -89,6 +89,9 @@ #if defined(PIOS_INCLUDE_BMP085) #include #endif +#if defined(PIOS_INCLUDE_HCSR04) +#include +#endif #if defined(PIOS_INCLUDE_HMC5843) #include #endif diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index 667d5cbe0..e44ee0f16 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -64,7 +64,8 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/ahrsstatus.h \ $$UAVOBJECT_SYNTHETICS/flightplansettings.h \ $$UAVOBJECT_SYNTHETICS/flightplancontrol.h \ $$UAVOBJECT_SYNTHETICS/watchdogstatus.h \ - $$UAVOBJECT_SYNTHETICS/nedaccel.h + $$UAVOBJECT_SYNTHETICS/nedaccel.h \ + $$UAVOBJECT_SYNTHETICS/sonaraltitude.h SOURCES += $$UAVOBJECT_SYNTHETICS/ahrsstatus.cpp \ $$UAVOBJECT_SYNTHETICS/ahrscalibration.cpp \ @@ -108,5 +109,6 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/ahrsstatus.cpp \ $$UAVOBJECT_SYNTHETICS/flightplancontrol.cpp \ $$UAVOBJECT_SYNTHETICS/watchdogstatus.cpp \ $$UAVOBJECT_SYNTHETICS/nedaccel.cpp \ + $$UAVOBJECT_SYNTHETICS/sonaraltitude.cpp \ $$UAVOBJECT_SYNTHETICS/uavobjectsinit.cpp diff --git a/shared/uavobjectdefinition/sonaraltitude.xml b/shared/uavobjectdefinition/sonaraltitude.xml new file mode 100644 index 000000000..9c52bda24 --- /dev/null +++ b/shared/uavobjectdefinition/sonaraltitude.xml @@ -0,0 +1,10 @@ + + + The raw data from the ultrasound sonar sensor with altitude estimate. + + + + + + +