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 #ifdef MODULE_AIRSPEED_BUILTIN
  97     airspeedEnabled = true;
  98 #else
  99
 100     HwSettingsInitialize();
 101     HwSettingsOptionalModulesOptions optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
 102     HwSettingsOptionalModulesArrayGet(optionalModules);
 103
 104
 105     if (optionalModules[HWSETTINGS_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     AirspeedSettingsInitialize();
 125
 126     AirspeedSettingsConnectCallback(AirspeedSettingsUpdatedCb);
 127
 128     return 0;
 129 }
 130 MODULE_INITCALL(AirspeedInitialize, AirspeedStart);
 131
 132
 133 /**
 134  * Module thread, should not return.
 135  */
 136 static void airspeedTask(__attribute__((unused)) void *parameters)
 137 {
 138     AirspeedSettingsUpdatedCb(AirspeedSettingsHandle());
 139     bool imuAirspeedInitialized = false;
 140     AirspeedSensorData airspeedData;
 141     AirspeedSensorGet(&airspeedData);
 142
 143     AirspeedSettingsUpdatedCb(NULL);
 144
 145     airspeedData.SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
 146
 147     // Main task loop
 148     portTickType lastSysTime = xTaskGetTickCount();
 149     while (1) {
 150         vTaskDelayUntil(&lastSysTime, airspeedSettings.SamplePeriod / portTICK_RATE_MS);
 151
 152         // Update the airspeed object
 153         AirspeedSensorGet(&airspeedData);
 154
 155         // if sensor type changed reset Airspeed alarm
 156         if (airspeedSettings.AirspeedSensorType != lastAirspeedSensorType) {
 157             AirspeedAlarm(SYSTEMALARMS_ALARM_DEFAULT);
 158             lastAirspeedSensorType = airspeedSettings.AirspeedSensorType;
 159             switch (airspeedSettings.AirspeedSensorType) {
 160             case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_NONE:
 161                 // AirspeedSensor will not be updated until a different sensor is selected
 162                 // set the disconencted satus now
 163                 airspeedData.SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
 164                 AirspeedSensorSet(&airspeedData);
 165                 break;
 166             case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_GROUNDSPEEDBASEDWINDESTIMATION:
 167                 if (!imuAirspeedInitialized) {
 168                     imuAirspeedInitialized = true;
 169                     imu_airspeedInitialize(&airspeedSettings);
 170                 }
 171                 break;
 172             default:
 173                 break;
 174             }
 175         }
 176         switch (airspeedSettings.AirspeedSensorType) {
 177 #if defined(PIOS_INCLUDE_MPXV)
 178         case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002:
 179         case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV5004:
 180             // MPXV5004 and MPXV7002 sensors
 181             baro_airspeedGetMPXV(&airspeedData, &airspeedSettings, airspeedADCPin);
 182             break;
 183 #endif
 184 #if defined(PIOS_INCLUDE_ETASV3)
 185         case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_EAGLETREEAIRSPEEDV3:
 186             // Eagletree Airspeed v3
 187             baro_airspeedGetETASV3(&airspeedData, &airspeedSettings);
 188             break;
 189 #endif
 190 #if defined(PIOS_INCLUDE_MS4525DO)
 191         case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_PIXHAWKAIRSPEEDMS4525DO:
 192             // PixHawk Airpeed based on MS4525DO
 193             baro_airspeedGetMS4525DO(&airspeedData, &airspeedSettings);
 194             break;
 195 #endif
 196         case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_GROUNDSPEEDBASEDWINDESTIMATION:
 197             imu_airspeedGet(&airspeedData, &airspeedSettings);
 198             break;
 199         case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_NONE:
 200             // no need to check so often until a sensor is enabled
 201             AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_DEFAULT);
 202             vTaskDelay(2000 / portTICK_RATE_MS);
 203             continue;
 204         default:
 205             airspeedData.SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
 206             break;
 207         }
 208
 209         // Set the UAVO
 210         AirspeedSensorSet(&airspeedData);
 211     }
 212 }
 213
 214
 215 static void AirspeedSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 216 {
 217     AirspeedSettingsGet(&airspeedSettings);
 218 }
 219
 220
 221 /**
 222  * @}
 223  * @}
 224  */