static void TaskTesting(void *pvParameters)
{
portTickType xDelay = 500 / portTICK_RATE_MS;
portTickType xTimeout = 10 / portTICK_RATE_MS;
PIOS_BMP085_Init();
for(;;)
{
PIOS_LED_Toggle(LED2);
int16_t temp;
int32_t pressure;
PIOS_BMP085_StartADC(TemperatureConv);
xSemaphoreTake(PIOS_BMP085_EOC, xTimeout);
PIOS_BMP085_ReadADC();
PIOS_BMP085_StartADC(PressureConv);
xSemaphoreTake(PIOS_BMP085_EOC, xTimeout);
PIOS_BMP085_ReadADC();
temp = PIOS_BMP085_GetTemperature();
pressure = PIOS_BMP085_GetPressure();
PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_TELEM_RF, "%3u,%4u\r", temp, pressure);
vTaskDelay(xDelay);
}
}
/**
* Verify that the barometric pressure sensor can be initialised and that it is returning
* data that looks approximately correct.
*/
static void
testBaro(void)
{
int16_t temperature;
int32_t pressure;
puts("BMP085...");
PIOS_BMP085_Init();
puts(" init OK");
PIOS_BMP085_StartADC(TemperatureConv);
PIOS_BMP085_ReadADC();
temperature = PIOS_BMP085_GetTemperature();
puts(" temperature");
if (temperature < 50) {
print(" FAIL: undertemp(%d)", temperature);
} else if (temperature > 500) {
print(" FAIL: overtemp(%d)", temperature);
} else {
puts(" OK");
}
PIOS_BMP085_StartADC(PressureConv);
PIOS_BMP085_ReadADC();
pressure = PIOS_BMP085_GetPressure();
puts(" pressure");
if (pressure < 80000) {
println(" FAIL: underpressure(%d)", pressure);
} else if (pressure > 12000) {
println(" FAIL: overpressure(%d)", pressure);
} else {
putln(" OK");
}
}
/**
* Module thread, should not return.
*/
static void altitudeTask(void* parameters)
{
BaroAltitudeData data;
portTickType lastSysTime;
PIOS_BMP085_Init();
// Main task loop
lastSysTime = xTaskGetTickCount();
while (1)
{
// Update the temperature data
PIOS_BMP085_StartADC(TemperatureConv);
xSemaphoreTake(PIOS_BMP085_EOC, portMAX_DELAY);
PIOS_BMP085_ReadADC();
// Convert from 1/10ths of degC to degC
data.Temperature = PIOS_BMP085_GetTemperature() / 10.0;
// Update the pressure data
PIOS_BMP085_StartADC(PressureConv);
xSemaphoreTake(PIOS_BMP085_EOC, portMAX_DELAY);
PIOS_BMP085_ReadADC();
// Convert from Pa to kPa
data.Pressure = PIOS_BMP085_GetPressure() / 1000.0;
// Compute the current altitude (all pressures in kPa)
data.Altitude = 44330.0 * (1.0 - powf((data.Pressure/ (BMP085_P0 / 1000.0)), (1.0/5.255)));
// Update the AltitudeActual UAVObject
BaroAltitudeSet(&data);
// Delay until it is time to read the next sample
vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD / portTICK_RATE_MS );
}
}
static void magbaroTask(void *parameters)
{
BaroAltitudeData data;
portTickType lastSysTime;
#if defined(PIOS_INCLUDE_BMP085)
PIOS_BMP085_Init();
#endif
#if defined(PIOS_INCLUDE_HMC5883)
PIOS_HMC5883_Init(PIOS_I2C_MAIN_ADAPTER, &pios_hmc5883_cfg);
#endif
mag_test = 0;
// Main task loop
lastSysTime = xTaskGetTickCount();
while (1)
{
#if defined(PIOS_INCLUDE_BMP085)
// Update the temperature data
PIOS_BMP085_StartADC(TemperatureConv);
#ifdef PIOS_BMP085_HAS_GPIOS
xSemaphoreTake(PIOS_BMP085_EOC, portMAX_DELAY);
#else
vTaskDelay(5 / portTICK_RATE_MS);
#endif
PIOS_BMP085_ReadADC();
alt_ds_temp += PIOS_BMP085_GetTemperature();
// Update the pressure data
PIOS_BMP085_StartADC(PressureConv);
#ifdef PIOS_BMP085_HAS_GPIOS
xSemaphoreTake(PIOS_BMP085_EOC, portMAX_DELAY);
#else
vTaskDelay(26 / portTICK_RATE_MS);
#endif
PIOS_BMP085_ReadADC();
alt_ds_pres += PIOS_BMP085_GetPressure();
if (++alt_ds_count >= alt_ds_size)
{
alt_ds_count = 0;
// Convert from 1/10ths of degC to degC
data.Temperature = alt_ds_temp / (10.0 * alt_ds_size);
alt_ds_temp = 0;
// Convert from Pa to kPa
data.Pressure = alt_ds_pres / (1000.0f * alt_ds_size);
alt_ds_pres = 0;
// Compute the current altitude (all pressures in kPa)
data.Altitude = 44330.0 * (1.0 - powf((data.Pressure / (BMP085_P0 / 1000.0)), (1.0 / 5.255)));
// Update the AltitudeActual UAVObject
BaroAltitudeSet(&data);
}
#endif
#if defined(PIOS_INCLUDE_HMC5883)
struct pios_sensor_mag_data mags;
xQueueHandle queue = PIOS_SENSORS_GetQueue(PIOS_SENSOR_MAG);
if (queue != NULL && xQueueReceive(queue, (void *) &mags, 0) != errQUEUE_EMPTY) {
update_mags(&mags);
}
#endif
// Delay until it is time to read the next sample
vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD / portTICK_RATE_MS);
}
}
/**
* Module thread, should not return.
*/
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);
#ifdef PIOS_BMP085_HAS_GPIOS
xSemaphoreTake(PIOS_BMP085_EOC, portMAX_DELAY);
#else
vTaskDelay(5 / portTICK_RATE_MS);
#endif
PIOS_BMP085_ReadADC();
alt_ds_temp += PIOS_BMP085_GetTemperature();
// Update the pressure data
PIOS_BMP085_StartADC(PressureConv);
#ifdef PIOS_BMP085_HAS_GPIOS
xSemaphoreTake(PIOS_BMP085_EOC, portMAX_DELAY);
#else
vTaskDelay(26 / portTICK_RATE_MS);
#endif
PIOS_BMP085_ReadADC();
alt_ds_pres += PIOS_BMP085_GetPressure();
if (++alt_ds_count >= alt_ds_size)
{
alt_ds_count = 0;
// Convert from 1/10ths of degC to degC
data.Temperature = alt_ds_temp / (10.0 * alt_ds_size);
alt_ds_temp = 0;
// Convert from Pa to kPa
data.Pressure = alt_ds_pres / (1000.0f * alt_ds_size);
alt_ds_pres = 0;
// Compute the current altitude (all pressures in kPa)
data.Altitude = 44330.0 * (1.0 - powf((data.Pressure / (BMP085_P0 / 1000.0)), (1.0 / 5.255)));
// Update the AltitudeActual UAVObject
BaroAltitudeSet(&data);
}
// Delay until it is time to read the next sample
vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD / portTICK_RATE_MS);
}
}
