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

@@ -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);

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

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

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

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 */
+
+/** 
+  * @}
+  * @}
+  */

flight/PiOS/pios.h

@@ -89,6 +89,9 @@
 #if defined(PIOS_INCLUDE_BMP085)
 #include <pios_bmp085.h>
 #endif
+#if defined(PIOS_INCLUDE_HCSR04)
+#include <pios_hcsr04.h>
+#endif
 #if defined(PIOS_INCLUDE_HMC5843)
 #include <pios_hmc5843.h>
 #endif

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
 

shared/uavobjectdefinition/sonaraltitude.xml

@@ -0,0 +1,10 @@
+<xml>
+    <object name="SonarAltitude" singleinstance="true" settings="false">
+        <description>The raw data from the ultrasound sonar sensor with altitude estimate.</description>
+        <field name="Altitude" units="m" type="float" elements="1"/>
+        <access gcs="readwrite" flight="readwrite"/>
+        <telemetrygcs acked="false" updatemode="manual" period="0"/>
+        <telemetryflight acked="false" updatemode="periodic" period="1000"/>
+        <logging updatemode="never" period="0"/>
+    </object>
+</xml>