LibrePilot/flight/modules/Altitude/revolution/altitude.c

/**
 ******************************************************************************
 * @addtogroup OpenPilotModules OpenPilot Modules
 * @{
 * @addtogroup AltitudeModule Altitude Module
 * @brief Communicate with BMP085 and update @ref BaroSensor "BaroSensor UAV Object"
 * @{
 *
 * @file       altitude.c
 * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
 * @brief      Altitude module, handles temperature and pressure readings from BMP085
 *
 * @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
 */

/**
 * Output object: BaroSensor
 *
 * This module will periodically update the value of the BaroSensor object.
 *
 */

#include <openpilot.h>

#include "altitude.h"
#include "barosensor.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
#include "taskinfo.h"

// Private constants
#define STACK_SIZE_BYTES 500
#define TASK_PRIORITY    (tskIDLE_PRIORITY + 1)

// Private types

// Private variables
static xTaskHandle taskHandle;

// Private functions
static void altitudeTask(void *parameters);

/**
 * Initialise the module, called on startup
 * \returns 0 on success or -1 if initialisation failed
 */
int32_t AltitudeStart()
{
    // Start main task
    xTaskCreate(altitudeTask, (signed char *)"Altitude", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
    PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ALTITUDE, taskHandle);

    return 0;
}

/**
 * Initialise the module, called on startup
 * \returns 0 on success or -1 if initialisation failed
 */
int32_t AltitudeInitialize()
{
    BaroSensorInitialize();
#if defined(PIOS_INCLUDE_HCSR04)
    SonarAltitudeInitialize();
#endif
    return 0;
}
MODULE_INITCALL(AltitudeInitialize, AltitudeStart)
/**
 * Module thread, should not return.
 */
static void altitudeTask(__attribute__((unused)) void *parameters)
{
    BaroSensorData data;

#if defined(PIOS_INCLUDE_HCSR04)
    SonarAltitudeData sonardata;
    int32_t value = 0, timeout = 10, sample_rate = 0;
    float coeff   = 0.25, height_out = 0, height_in = 0;
    PIOS_HCSR04_Trigger();
#endif

    // TODO: Check the pressure sensor and set a warning if it fails test

// Option to change the interleave between Temp and Pressure conversions
// Undef for normal operation
// #define PIOS_MS5611_SLOW_TEMP_RATE 20

#ifdef PIOS_MS5611_SLOW_TEMP_RATE
    uint8_t temp_press_interleave_count = 1;
#endif
    // Main task loop
    while (1) {
#if defined(PIOS_INCLUDE_HCSR04)
        // Compute the current altitude
        // depends on baro samplerate
        if (!(sample_rate--)) {
            if (PIOS_HCSR04_Completed()) {
                value = PIOS_HCSR04_Get();
                // from 3.4cm to 5.1m
                if ((value > 100) && (value < 15000)) {
                    height_in  = value * 0.00034f / 2.0f;
                    height_out = (height_out * (1 - coeff)) + (height_in * coeff);
                    sonardata.Altitude = height_out; // m/us
                }

                // Update the SonarAltitude UAVObject
                SonarAltitudeSet(&sonardata);
                timeout = 10;
                PIOS_HCSR04_Trigger();
            }
            if (!(timeout--)) {
                // retrigger
                timeout = 10;
                PIOS_HCSR04_Trigger();
            }
            sample_rate = 25;
        }
#endif /* if defined(PIOS_INCLUDE_HCSR04) */
        float temp, press;
#ifdef PIOS_MS5611_SLOW_TEMP_RATE
        temp_press_interleave_count--;
        if (temp_press_interleave_count == 0) {
#endif
        // Update the temperature data
        PIOS_MS5611_StartADC(TemperatureConv);
        vTaskDelay(PIOS_MS5611_GetDelay());
        PIOS_MS5611_ReadADC();

#ifdef PIOS_MS5611_SLOW_TEMP_RATE
        temp_press_interleave_count = PIOS_MS5611_SLOW_TEMP_RATE;
    }
#endif

        // Update the pressure data
        PIOS_MS5611_StartADC(PressureConv);
        vTaskDelay(PIOS_MS5611_GetDelay());
        PIOS_MS5611_ReadADC();


        temp  = PIOS_MS5611_GetTemperature();
        press = PIOS_MS5611_GetPressure();

        data.Temperature = temp;
        data.Pressure    = press;
        data.Altitude    = 44330.0f * (1.0f - powf(data.Pressure / MS5611_P0, (1.0f / 5.255f)));

        // Update the AltitudeActual UAVObject
        BaroSensorSet(&data);
    }
}

/**
 * @}
 * @}
 */