flight/modules/Airspeed/airspeed.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup OpenPilotModules OpenPilot Modules
   4  * @{
   5  * @addtogroup AirspeedModule Airspeed Module
   6  * @brief Calculate airspeed from diverse sources and update @ref Airspeed "Airspeed UAV Object"
   7  * @{
   8  *
   9  * @file       airspeed.c
  10  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
  11  * @brief      Airspeed module
  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: AirspeedSensor
  34  *
  35  * This module will periodically update the value of the AirspeedSensor object.
  36  *
  37  */
  38
  39 #include <openpilot.h>
  40
  41 #include "hwsettings.h"
  42 #include "airspeedsettings.h"
  43 #include "airspeedsensor.h" // object that will be updated by the module
  44 #include "baro_airspeed_ms4525do.h"
  45 #include "baro_airspeed_etasv3.h"
  46 #include "baro_airspeed_mpxv.h"
  47 #include "imu_airspeed.h"
  48 #include "airspeedalarm.h"
  49 #include "taskinfo.h"
  50
  51 // Private constants
  52
  53 #define STACK_SIZE_BYTES 650
  54
  55
  56 #define TASK_PRIORITY    (tskIDLE_PRIORITY + 1)
  57
  58 // Private types
  59
  60 // Private variables
  61 static xTaskHandle taskHandle;
  62 static bool airspeedEnabled  = false;
  63 static AirspeedSettingsData airspeedSettings;
  64 static AirspeedSettingsAirspeedSensorTypeOptions lastAirspeedSensorType = -1;
  65 static int8_t airspeedADCPin = -1;
  66
  67
  68 // Private functions
  69 static void airspeedTask(void *parameters);
  70 static void AirspeedSettingsUpdatedCb(UAVObjEvent *ev);
  71
  72
  73 /**
  74  * Initialise the module, called on startup
  75  * \returns 0 on success or -1 if initialisation failed
  76  */
  77 int32_t AirspeedStart()
  78 {
  79     // Check if module is enabled or not
  80     if (airspeedEnabled == false) {
  81         return -1;
  82     }
  83
  84     // Start main task
  85     xTaskCreate(airspeedTask, "Airspeed", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
  86     PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_AIRSPEED, taskHandle);
  87     return 0;
  88 }
  89
  90 /**
  91  * Initialise the module, called on startup
  92  * \returns 0 on success or -1 if initialisation failed
  93  */
  94 int32_t AirspeedInitialize()
  95 {
  96     HwSettingsOptionalModulesData optionalModules;
  97
  98     HwSettingsOptionalModulesGet(&optionalModules);
  99
 100 #ifdef MODULE_AIRSPEED_BUILTIN
 101     airspeedEnabled = true;
 102     optionalModules.Airspeed = HWSETTINGS_OPTIONALMODULES_ENABLED;
 103     HwSettingsOptionalModulesSet(&optionalModules);
 104 #else
 105     if (optionalModules.Airspeed == HWSETTINGS_OPTIONALMODULES_ENABLED) {
 106         airspeedEnabled = true;
 107     } else {
 108         airspeedEnabled = false;
 109         return -1;
 110     }
 111 #endif
 112
 113     HwSettingsADCRoutingOptions adcRouting[HWSETTINGS_ADCROUTING_NUMELEM];
 114     HwSettingsADCRoutingArrayGet(adcRouting);
 115
 116     // Determine if the barometric airspeed sensor is routed to an ADC pin
 117     for (int i = 0; i < HWSETTINGS_ADCROUTING_NUMELEM; i++) {
 118         if (adcRouting[i] == HWSETTINGS_ADCROUTING_ANALOGAIRSPEED) {
 119             airspeedADCPin = i;
 120         }
 121     }
 122
 123     AirspeedSensorInitialize();
 124
 125     AirspeedSettingsConnectCallback(AirspeedSettingsUpdatedCb);
 126
 127     return 0;
 128 }
 129 MODULE_INITCALL(AirspeedInitialize, AirspeedStart);
 130
 131
 132 /**
 133  * Module thread, should not return.
 134  */
 135 static void airspeedTask(__attribute__((unused)) void *parameters)
 136 {
 137     AirspeedSettingsUpdatedCb(AirspeedSettingsHandle());
 138     bool imuAirspeedInitialized = false;
 139     AirspeedSensorData airspeedData;
 140     AirspeedSensorGet(&airspeedData);
 141
 142     AirspeedSettingsUpdatedCb(NULL);
 143
 144     airspeedData.SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
 145
 146     // Main task loop
 147     portTickType lastSysTime = xTaskGetTickCount();
 148     while (1) {
 149         vTaskDelayUntil(&lastSysTime, airspeedSettings.SamplePeriod / portTICK_RATE_MS);
 150
 151         // Update the airspeed object
 152         AirspeedSensorGet(&airspeedData);
 153
 154         // if sensor type changed reset Airspeed alarm
 155         if (airspeedSettings.AirspeedSensorType != lastAirspeedSensorType) {
 156             AirspeedAlarm(SYSTEMALARMS_ALARM_DEFAULT);
 157             lastAirspeedSensorType = airspeedSettings.AirspeedSensorType;
 158             switch (airspeedSettings.AirspeedSensorType) {
 159             case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_NONE:
 160                 // AirspeedSensor will not be updated until a different sensor is selected
 161                 // set the disconencted satus now
 162                 airspeedData.SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
 163                 AirspeedSensorSet(&airspeedData);
 164                 break;
 165             case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_GROUNDSPEEDBASEDWINDESTIMATION:
 166                 if (!imuAirspeedInitialized) {
 167                     imuAirspeedInitialized = true;
 168                     imu_airspeedInitialize(&airspeedSettings);
 169                 }
 170                 break;
 171             default:
 172                 break;
 173             }
 174         }
 175         switch (airspeedSettings.AirspeedSensorType) {
 176 #if defined(PIOS_INCLUDE_MPXV)
 177         case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002:
 178         case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV5004:
 179             // MPXV5004 and MPXV7002 sensors
 180             baro_airspeedGetMPXV(&airspeedData, &airspeedSettings, airspeedADCPin);
 181             break;
 182 #endif
 183 #if defined(PIOS_INCLUDE_ETASV3)
 184         case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_EAGLETREEAIRSPEEDV3:
 185             // Eagletree Airspeed v3
 186             baro_airspeedGetETASV3(&airspeedData, &airspeedSettings);
 187             break;
 188 #endif
 189 #if defined(PIOS_INCLUDE_MS4525DO)
 190         case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_PIXHAWKAIRSPEEDMS4525DO:
 191             // PixHawk Airpeed based on MS4525DO
 192             baro_airspeedGetMS4525DO(&airspeedData, &airspeedSettings);
 193             break;
 194 #endif
 195         case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_GROUNDSPEEDBASEDWINDESTIMATION:
 196             imu_airspeedGet(&airspeedData, &airspeedSettings);
 197             break;
 198         case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_NONE:
 199             // no need to check so often until a sensor is enabled
 200             AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_DEFAULT);
 201             vTaskDelay(2000 / portTICK_RATE_MS);
 202             continue;
 203         default:
 204             airspeedData.SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
 205             break;
 206         }
 207
 208         // Set the UAVO
 209         AirspeedSensorSet(&airspeedData);
 210     }
 211 }
 212
 213
 214 static void AirspeedSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 215 {
 216     AirspeedSettingsGet(&airspeedSettings);
 217 }
 218
 219
 220 /**
 221  * @}
 222  * @}
 223  */