flight/modules/Altitude/altitude.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup OpenPilotModules OpenPilot Modules
   4  * @{
   5  * @addtogroup AltitudeModule Altitude Module
   6  * @brief Communicate with BMP085 and update @ref BaroSensor "BaroSensor UAV Object"
   7  * @{
   8  *
   9  * @file       altitude.c
  10  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
  11  * @brief      Altitude module, handles temperature and pressure readings from BMP085
  12  *
  13  * @see        The GNU Public License (GPL) Version 3
  14  *
  15  *****************************************************************************/
  16 /*
  17  * This program is free software; you can redistribute it and/or modify
  18  * it under the terms of the GNU General Public License as published by
  19  * the Free Software Foundation; either version 3 of the License, or
  20  * (at your option) any later version.
  21  *
  22  * This program is distributed in the hope that it will be useful, but
  23  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  24  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  25  * for more details.
  26  *
  27  * You should have received a copy of the GNU General Public License along
  28  * with this program; if not, write to the Free Software Foundation, Inc.,
  29  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  30  */
  31
  32 /**
  33  * Output object: BaroSensor
  34  *
  35  * This module will periodically update the value of the BaroSensor object.
  36  *
  37  */
  38
  39 #include <openpilot.h>
  40
  41 #include "hwsettings.h"
  42 #include "altitude.h"
  43 #if defined(PIOS_INCLUDE_BMP085)
  44 #include "barosensor.h" // object that will be updated by the module
  45 #endif
  46 #if defined(PIOS_INCLUDE_HCSR04)
  47 #include "sonaraltitude.h" // object that will be updated by the module
  48 #endif
  49 #include "taskinfo.h"
  50
  51 // Private constants
  52 #define STACK_SIZE_BYTES 500
  53 #define TASK_PRIORITY    (tskIDLE_PRIORITY + 1)
  54 #define UPDATE_PERIOD    50
  55
  56 // Private types
  57
  58 // Private variables
  59 static xTaskHandle taskHandle;
  60
  61 // down sampling variables
  62 #if defined(PIOS_INCLUDE_BMP085)
  63 #define alt_ds_size 4
  64 static int32_t alt_ds_temp = 0;
  65 static int32_t alt_ds_pres = 0;
  66 static int alt_ds_count    = 0;
  67 #endif
  68
  69 static bool altitudeEnabled;
  70 static uint8_t hwsettings_rcvrport;;
  71
  72 // Private functions
  73 static void altitudeTask(void *parameters);
  74
  75 /**
  76  * Initialise the module, called on startup
  77  * \returns 0 on success or -1 if initialisation failed
  78  */
  79 int32_t AltitudeStart()
  80 {
  81     if (altitudeEnabled) {
  82 #if defined(PIOS_INCLUDE_BMP085)
  83         BaroSensorInitialize();
  84 #endif
  85 #if defined(PIOS_INCLUDE_HCSR04)
  86         SonarAltitudeInitialize();
  87 #endif
  88
  89         // Start main task
  90         xTaskCreate(altitudeTask, (signed char *)"Altitude", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
  91         PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ALTITUDE, taskHandle);
  92         return 0;
  93     }
  94     return -1;
  95 }
  96
  97 /**
  98  * Initialise the module, called on startup
  99  * \returns 0 on success or -1 if initialisation failed
 100  */
 101 int32_t AltitudeInitialize()
 102 {
 103 #ifdef MODULE_ALTITUDE_BUILTIN
 104     altitudeEnabled = 1;
 105 #else
 106     HwSettingsInitialize();
 107     uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
 108     HwSettingsOptionalModulesGet(optionalModules);
 109     if (optionalModules[HWSETTINGS_OPTIONALMODULES_ALTITUDE] == HWSETTINGS_OPTIONALMODULES_ENABLED) {
 110         altitudeEnabled = 1;
 111     } else {
 112         altitudeEnabled = 0;
 113     }
 114 #endif
 115
 116 #if defined(PIOS_INCLUDE_BMP085)
 117     // init down-sampling data
 118     alt_ds_temp  = 0;
 119     alt_ds_pres  = 0;
 120     alt_ds_count = 0;
 121 #endif
 122     HwSettingsCC_RcvrPortGet(&hwsettings_rcvrport);
 123     return 0;
 124 }
 125 MODULE_INITCALL(AltitudeInitialize, AltitudeStart);
 126 /**
 127  * Module thread, should not return.
 128  */
 129 static void altitudeTask(__attribute__((unused)) void *parameters)
 130 {
 131     portTickType lastSysTime;
 132
 133 #if defined(PIOS_INCLUDE_HCSR04)
 134     SonarAltitudeData sonardata;
 135     int32_t value = 0, timeout = 5;
 136     float coeff   = 0.25, height_out = 0, height_in = 0;
 137     if (hwsettings_rcvrport == HWSETTINGS_CC_RCVRPORT_DISABLED) {
 138         PIOS_HCSR04_Trigger();
 139     }
 140 #endif
 141 #if defined(PIOS_INCLUDE_BMP085)
 142     BaroSensorData data;
 143     PIOS_BMP085_Init();
 144 #endif
 145
 146     // Main task loop
 147     lastSysTime = xTaskGetTickCount();
 148     while (1) {
 149 #if defined(PIOS_INCLUDE_HCSR04)
 150         // Compute the current altitude
 151         if (hwsettings_rcvrport == HWSETTINGS_CC_RCVRPORT_DISABLED) {
 152             if (PIOS_HCSR04_Completed()) {
 153                 value = PIOS_HCSR04_Get();
 154                 // from 3.4cm to 5.1m
 155                 if ((value > 100) && (value < 15000)) {
 156                     height_in  = value * 0.00034f / 2.0f;
 157                     height_out = (height_out * (1 - coeff)) + (height_in * coeff);
 158                     sonardata.Altitude = height_out; // m/us
 159                 }
 160
 161                 // Update the AltitudeActual UAVObject
 162                 SonarAltitudeSet(&sonardata);
 163                 timeout = 5;
 164                 PIOS_HCSR04_Trigger();
 165             }
 166             if (!(timeout--)) {
 167                 // retrigger
 168                 timeout = 5;
 169                 PIOS_HCSR04_Trigger();
 170             }
 171         }
 172 #endif /* if defined(PIOS_INCLUDE_HCSR04) */
 173 #if defined(PIOS_INCLUDE_BMP085)
 174         // Update the temperature data
 175         PIOS_BMP085_StartADC(TemperatureConv);
 176 #ifdef PIOS_BMP085_HAS_GPIOS
 177         xSemaphoreTake(PIOS_BMP085_EOC, portMAX_DELAY);
 178 #else
 179         vTaskDelay(5 / portTICK_RATE_MS);
 180 #endif
 181         PIOS_BMP085_ReadADC();
 182         alt_ds_temp += PIOS_BMP085_GetTemperature();
 183
 184         // Update the pressure data
 185         PIOS_BMP085_StartADC(PressureConv);
 186 #ifdef PIOS_BMP085_HAS_GPIOS
 187         xSemaphoreTake(PIOS_BMP085_EOC, portMAX_DELAY);
 188 #else
 189         vTaskDelay(26 / portTICK_RATE_MS);
 190 #endif
 191         PIOS_BMP085_ReadADC();
 192         alt_ds_pres += PIOS_BMP085_GetPressure();
 193
 194         if (++alt_ds_count >= alt_ds_size) {
 195             alt_ds_count     = 0;
 196
 197             // Convert from 1/10ths of degC to degC
 198             data.Temperature = alt_ds_temp / (10.0 * alt_ds_size);
 199             alt_ds_temp   = 0;
 200
 201             // Convert from Pa to kPa
 202             data.Pressure = alt_ds_pres / (1000.0f * alt_ds_size);
 203             alt_ds_pres   = 0;
 204
 205             // Compute the current altitude (all pressures in kPa)
 206             data.Altitude = 44330.0 * (1.0 - powf((data.Pressure / (BMP085_P0 / 1000.0)), (1.0 / 5.255)));
 207
 208             // Update the AltitudeActual UAVObject
 209             BaroSensorSet(&data);
 210         }
 211 #endif /* if defined(PIOS_INCLUDE_BMP085) */
 212
 213         // Delay until it is time to read the next sample
 214         vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD / portTICK_RATE_MS);
 215     }
 216 }
 217
 218 /**
 219  * @}
 220  * @}
 221  */