static esp_err_t event_handler(void *ctx, system_event_t *event)
{
switch (event->event_id) {
case SYSTEM_EVENT_STA_START:
esp_wifi_connect();
ESP_LOGI(TAG, "SYSTEM_EVENT_STA_START");
break;
case SYSTEM_EVENT_STA_CONNECTED:
/* enable ipv6 */
tcpip_adapter_create_ip6_linklocal(TCPIP_ADAPTER_IF_STA);
break;
case SYSTEM_EVENT_STA_GOT_IP:
xEventGroupSetBits(wifi_event_group, IPV4_GOTIP_BIT);
ESP_LOGI(TAG, "SYSTEM_EVENT_STA_GOT_IP");
break;
case SYSTEM_EVENT_STA_DISCONNECTED:
/* This is a workaround as ESP32 WiFi libs don't currently auto-reassociate. */
esp_wifi_connect();
xEventGroupClearBits(wifi_event_group, IPV4_GOTIP_BIT);
xEventGroupClearBits(wifi_event_group, IPV6_GOTIP_BIT);
break;
case SYSTEM_EVENT_AP_STA_GOT_IP6:
xEventGroupSetBits(wifi_event_group, IPV6_GOTIP_BIT);
ESP_LOGI(TAG, "SYSTEM_EVENT_STA_GOT_IP6");
char *ip6 = ip6addr_ntoa(&event->event_info.got_ip6.ip6_info.ip);
ESP_LOGI(TAG, "IPv6: %s", ip6);
default:
break;
}
return ESP_OK;
}
static esp_err_t event_handler(void *ctx, system_event_t *event)
{
switch(event->event_id) {
case SYSTEM_EVENT_STA_START:
esp_wifi_connect();
break;
case SYSTEM_EVENT_STA_DISCONNECTED:
esp_wifi_connect();
xEventGroupClearBits(tcp_event_group, WIFI_CONNECTED_BIT);
break;
case SYSTEM_EVENT_STA_CONNECTED:
break;
case SYSTEM_EVENT_STA_GOT_IP:
ESP_LOGI(TAG, "got ip:%s\n",
ip4addr_ntoa(&event->event_info.got_ip.ip_info.ip));
xEventGroupSetBits(tcp_event_group, WIFI_CONNECTED_BIT);
break;
case SYSTEM_EVENT_AP_STACONNECTED:
ESP_LOGI(TAG, "station:"MACSTR" join,AID=%d\n",
MAC2STR(event->event_info.sta_connected.mac),
event->event_info.sta_connected.aid);
xEventGroupSetBits(tcp_event_group, WIFI_CONNECTED_BIT);
break;
case SYSTEM_EVENT_AP_STADISCONNECTED:
ESP_LOGI(TAG, "station:"MACSTR"leave,AID=%d\n",
MAC2STR(event->event_info.sta_disconnected.mac),
event->event_info.sta_disconnected.aid);
xEventGroupClearBits(tcp_event_group, WIFI_CONNECTED_BIT);
break;
default:
break;
}
return ESP_OK;
}
void ButtonHandler(){
uint32_t mask=GPIOIntStatus(GPIO_PORTF_BASE,false);
uint8_t value=0;
if(mask & GPIO_PIN_4){
//Boton izquierdo
value= GPIOPinRead(GPIO_PORTF_BASE,GPIO_PIN_4);
if(value==0){
//boton pulsado
// Activa el Timer4A (empezara a funcionar)
TimerEnable(TIMER4_BASE, TIMER_A);
pulsacionLarga=true;
}else{
TimerDisable(TIMER4_BASE,TIMER_A);
if(pulsacionLarga){
xEventGroupSetBits(xEventGroup, PilotoAutomaticoBit);
if((xTaskCreate(PilAuto, (signed portCHAR *)"Piloto Auto", LED1TASKSTACKSIZE,NULL,tskIDLE_PRIORITY + 1, &PilautTaskHandle) != pdTRUE))
{
while(1);
}
}
}
}
if(mask & GPIO_PIN_0){
//boton derecho
xEventGroupClearBits( xEventGroup, PilotoAutomaticoBit );
}
GPIOIntClear(GPIO_PORTF_BASE,GPIO_PIN_0|GPIO_PIN_4);
}
static bool wifi_cmd_sta_join(const char* ssid, const char* pass)
{
int bits = xEventGroupWaitBits(wifi_event_group, CONNECTED_BIT, 0, 1, 0);
wifi_config_t wifi_config = { 0 };
strlcpy((char*) wifi_config.sta.ssid, ssid, sizeof(wifi_config.sta.ssid));
if (pass) {
strncpy((char*) wifi_config.sta.password, pass, sizeof(wifi_config.sta.password));
}
if (bits & CONNECTED_BIT) {
reconnect = false;
xEventGroupClearBits(wifi_event_group, CONNECTED_BIT);
ESP_ERROR_CHECK( esp_wifi_disconnect() );
xEventGroupWaitBits(wifi_event_group, DISCONNECTED_BIT, 0, 1, portTICK_RATE_MS);
}
reconnect = true;
ESP_ERROR_CHECK( esp_wifi_set_mode(WIFI_MODE_STA) );
ESP_ERROR_CHECK( esp_wifi_set_config(ESP_IF_WIFI_STA, &wifi_config) );
ESP_ERROR_CHECK( esp_wifi_connect() );
xEventGroupWaitBits(wifi_event_group, CONNECTED_BIT, 0, 1, 5000/portTICK_RATE_MS);
return true;
}
void meca_fish_sweep_right(int wait, int high)
{
meca_fish_state_t st = MECA_FISH_SWEEP_RIGHT;
if(high)
{
st = MECA_FISH_SWEEP_HIGH_RIGHT;
}
if(wait)
{
xEventGroupClearBits(busy, 0xFF);
}
if(current_state == st)
{
wait = 0;
}
next_state = st;
if(wait)
{
xEventGroupWaitBits(busy, 0xFF, pdFALSE, pdFALSE, portMAX_DELAY);
}
}
/*FUNCTION**********************************************************************
*
* Function Name : OSA_EventClear
* Description : Clear one or more event flags of an event object.
* Return :osaStatus_Success if clear successfully, osaStatus_Error if failed.
*
*END**************************************************************************/
osaStatus_t OSA_EventClear(osaEventId_t eventId, osaEventFlags_t flagsToClear)
{
#if osNumberOfEvents
osEventStruct_t* pEventStruct;
if(osObjectIsAllocated(&osEventInfo, eventId) == FALSE)
{
return osaStatus_Error;
}
pEventStruct = (osEventStruct_t*)eventId;
if(pEventStruct->event.eventHandler == NULL)
{
return osaStatus_Error;
}
if (__get_IPSR())
{
xEventGroupClearBitsFromISR(pEventStruct->event.eventHandler, (event_flags_t)flagsToClear);
}
else
{
xEventGroupClearBits(pEventStruct->event.eventHandler, (event_flags_t)flagsToClear);
}
return osaStatus_Success;
#else
(void)eventId;
(void)flagsToClear;
return osaStatus_Error;
#endif
}
static void prvExerciseEventGroupAPI( void )
{
EventGroupHandle_t xEventGroup;
EventBits_t xBits;
const EventBits_t xBitsToWaitFor = ( EventBits_t ) 0xff, xBitToClear = ( EventBits_t ) 0x01;
/* Exercise some event group functions. */
xEventGroup = xEventGroupCreate();
configASSERT( xEventGroup );
/* No bits should be set. */
xBits = xEventGroupWaitBits( xEventGroup, xBitsToWaitFor, pdTRUE, pdFALSE, mainDONT_BLOCK );
configASSERT( xBits == ( EventBits_t ) 0 );
/* Set bits and read back to ensure the bits were set. */
xEventGroupSetBits( xEventGroup, xBitsToWaitFor );
xBits = xEventGroupGetBits( xEventGroup );
configASSERT( xBits == xBitsToWaitFor );
/* Clear a bit and read back again using a different API function. */
xEventGroupClearBits( xEventGroup, xBitToClear );
xBits = xEventGroupSync( xEventGroup, 0x00, xBitsToWaitFor, mainDONT_BLOCK );
configASSERT( xBits == ( xBitsToWaitFor & ~xBitToClear ) );
/* Finished with the event group. */
vEventGroupDelete( xEventGroup );
}
mcual_i2c_status_t mcual_i2c_transmit(mcual_i2c_id_t id, uint8_t addr, uint8_t * txbuf, uint8_t tx_size, uint8_t * rxbuf, uint8_t rx_size)
{
(void)rxbuf;
(void)rx_size;
I2C_TypeDef * reg = mcual_i2c_get_register(id);
int i;
addr <<= 1;
xEventGroupClearBits(transfer_done[id], 0xFF);
xQueueReset(tx_queues[id]);
xQueueSend(tx_queues[id], &addr, portMAX_DELAY);
for(i = 0; i < tx_size; i += 1)
{
xQueueSend(tx_queues[id], txbuf + i, portMAX_DELAY);
}
reg->CR2 |= I2C_CR2_ITEVTEN | I2C_CR2_ITERREN;
reg->CR1 = I2C_CR1_START | I2C_CR1_PE;
EventBits_t result = xEventGroupWaitBits(transfer_done[id], 0xFF, pdFALSE, pdFALSE, 1000 / portTICK_PERIOD_MS);
I2C1->CR1 = I2C_CR1_STOP;
if(result & (1 << MCUAL_I2C_SUCCESS))
{
return MCUAL_I2C_SUCCESS;
}
return MCUAL_I2C_FAIL;
}
EventBits_t MPU_xEventGroupClearBits( EventGroupHandle_t xEventGroup, const EventBits_t uxBitsToClear )
{
EventBits_t xReturn;
BaseType_t xRunningPrivileged = xPortRaisePrivilege();
xReturn = xEventGroupClearBits( xEventGroup, uxBitsToClear );
vPortResetPrivilege( xRunningPrivileged );
return xReturn;
}
static void scan_task(void *pvParameters)
{
while(1) {
xEventGroupWaitBits(wifi_event_group, SCAN_DONE_BIT, false, true, portMAX_DELAY);
ESP_LOGI(TAG, "WIFI scan doen");
xEventGroupClearBits(wifi_event_group, SCAN_DONE_BIT);
uint16_t apCount = 0;
esp_wifi_scan_get_ap_num(&apCount);
printf("Number of access points found: %d\n", apCount);
if (apCount == 0) {
ESP_LOGI(TAG, "Nothing AP found");
return;
}
wifi_ap_record_t *list = (wifi_ap_record_t *)malloc(sizeof(wifi_ap_record_t) * apCount);
ESP_ERROR_CHECK(esp_wifi_scan_get_ap_records(&apCount, list));
int i;
printf("======================================================================\n");
printf(" SSID | RSSI | AUTH \n");
printf("======================================================================\n");
for (i=0; i<apCount; i++) {
char *authmode;
switch(list[i].authmode) {
case WIFI_AUTH_OPEN:
authmode = "WIFI_AUTH_OPEN";
break;
case WIFI_AUTH_WEP:
authmode = "WIFI_AUTH_WEP";
break;
case WIFI_AUTH_WPA_PSK:
authmode = "WIFI_AUTH_WPA_PSK";
break;
case WIFI_AUTH_WPA2_PSK:
authmode = "WIFI_AUTH_WPA2_PSK";
break;
case WIFI_AUTH_WPA_WPA2_PSK:
authmode = "WIFI_AUTH_WPA_WPA2_PSK";
break;
default:
authmode = "Unknown";
break;
}
printf("%26.26s | % 4d | %22.22s\n",list[i].ssid, list[i].rssi, authmode);
}
free(list);
printf("\n\n");
// scan again
vTaskDelay(2000 / portTICK_PERIOD_MS);
//The true parameter cause the function to block until the scan is done.
ESP_ERROR_CHECK(esp_wifi_scan_start(&scanConf, true));
}
}
/**
* \brief About task
*
* This task prints a short text about the demo, with a simple zooming
* animation.
*
* \param params Parameters for the task. (Not used.)
*/
static void about_task(void *params)
{
char c;
gfx_coord_t x, y;
uint8_t i, shift;
EventBits_t event_bits;
const TickType_t ticks_to_wait = 10 / portTICK_PERIOD_MS;
const uint8_t max_shift = 8;
shift = 1;
for (;;) {
/* Wait a maximum of 10ms for either bit 3 to be set within
the event group. Not clear the bits before exiting. */
event_bits = xEventGroupWaitBits(
event_group, /* The event group being tested. */
EVENT_DISPLAY_ABOUT, /* The bits within the event group to wait for. */
pdFALSE, /* Bit should not be cleared before returning. */
pdFALSE, /* Don't wait for both bits, either bit will do. */
ticks_to_wait);/* Wait a maximum of 10ms for either bit to be set. */
if(event_bits & EVENT_DISPLAY_ABOUT) {
oled1_set_led_state(&oled1, OLED1_LED2_ID, true);
xSemaphoreTake(display_mutex, portMAX_DELAY);
// Print the about text in an expanding area
for (i = 0; i < (sizeof(about_text) - 1); i++) {
c = about_text[i];
x = (((i % TERMINAL_COLUMNS) * SYSFONT_WIDTH) * shift
+ (CANVAS_WIDTH / 2) * (max_shift - shift))
/ max_shift;
y = (((i / TERMINAL_COLUMNS) * SYSFONT_HEIGHT) * shift
+ (CANVAS_HEIGHT / 2) * (max_shift - shift))
/ max_shift;
gfx_mono_draw_char(c, x, y, &sysfont);
}
xSemaphoreGive(display_mutex);
oled1_set_led_state(&oled1, OLED1_LED2_ID, false);
// Repeat task until we're displaying the text in full size
if (shift < max_shift) {
shift++;
vTaskDelay(ABOUT_TASK_DELAY);
} else {
shift = 0;
xEventGroupClearBits(event_group, EVENT_DISPLAY_ABOUT);
}
}
}
}
static esp_err_t event_handler(void *ctx, system_event_t *event)
{
switch(event->event_id) {
case SYSTEM_EVENT_STA_GOT_IP:
xEventGroupClearBits(wifi_event_group, DISCONNECTED_BIT);
xEventGroupSetBits(wifi_event_group, CONNECTED_BIT);
break;
case SYSTEM_EVENT_STA_DISCONNECTED:
if (reconnect) {
ESP_LOGI(TAG, "sta disconnect, reconnect...");
esp_wifi_connect();
} else {
ESP_LOGI(TAG, "sta disconnect");
}
xEventGroupClearBits(wifi_event_group, CONNECTED_BIT);
xEventGroupSetBits(wifi_event_group, DISCONNECTED_BIT);
break;
default:
break;
}
return ESP_OK;
}
int meca_fish_is_catch(void)
{
xEventGroupClearBits(busy, 0xFF);
next_state = MECA_FISH_CHECK;
xEventGroupWaitBits(busy, 0xFF, pdFALSE, pdFALSE, portMAX_DELAY);
#ifdef AUSBEE_SIM
return ((rand() % 3) == 0);
#else
return ir_value > 300;
#endif
}
/*FUNCTION**********************************************************************
*
* Function Name : OSA_EventClear
* Description : Clear one or more event flags of an event object.
* Return kStatus_OSA_Success if clear successfully, kStatus_OSA_Error if failed.
*
*END**************************************************************************/
osa_status_t OSA_EventClear(event_t *pEvent, event_flags_t flagsToClear)
{
assert(pEvent);
if (__get_IPSR())
{
xEventGroupClearBitsFromISR(pEvent->eventHandler, flagsToClear);
}
else
{
xEventGroupClearBits(pEvent->eventHandler, flagsToClear);
}
return kStatus_OSA_Success;
}
static esp_err_t event_handler(void *ctx, system_event_t *event)
{
switch(event->event_id) {
case SYSTEM_EVENT_STA_START:
xTaskCreate(smartconfig_example_task, "smartconfig_example_task", 4096, NULL, 3, NULL);
break;
case SYSTEM_EVENT_STA_GOT_IP:
xEventGroupSetBits(wifi_event_group, CONNECTED_BIT);
break;
case SYSTEM_EVENT_STA_DISCONNECTED:
esp_wifi_connect();
xEventGroupClearBits(wifi_event_group, CONNECTED_BIT);
break;
default:
break;
}
return ESP_OK;
}
static esp_err_t wifi_event_handler(void *ctx, system_event_t *event)
{
switch (event->event_id) {
case SYSTEM_EVENT_STA_START:
esp_wifi_connect();
break;
case SYSTEM_EVENT_STA_GOT_IP:
xEventGroupSetBits(wifi_event_group, CONNECTED_BIT);
break;
case SYSTEM_EVENT_STA_DISCONNECTED:
esp_wifi_connect();
xEventGroupClearBits(wifi_event_group, CONNECTED_BIT);
break;
default:
break;
}
return ESP_OK;
}
static void prvSanityCheckCreatedEventGroup( EventGroupHandle_t xEventGroup )
{
EventBits_t xEventBits;
const EventBits_t xFirstTestBits = ( EventBits_t ) 0xaa, xSecondTestBits = ( EventBits_t ) 0x55;
/* The event group should not have any bits set yet. */
xEventBits = xEventGroupGetBits( xEventGroup );
if( xEventBits != ( EventBits_t ) 0 )
{
xErrorOccurred = pdTRUE;
}
/* Some some bits, then read them back to check they are as expected. */
xEventGroupSetBits( xEventGroup, xFirstTestBits );
xEventBits = xEventGroupGetBits( xEventGroup );
if( xEventBits != xFirstTestBits )
{
xErrorOccurred = pdTRUE;
}
xEventGroupSetBits( xEventGroup, xSecondTestBits );
xEventBits = xEventGroupGetBits( xEventGroup );
if( xEventBits != ( xFirstTestBits | xSecondTestBits ) )
{
xErrorOccurred = pdTRUE;
}
/* Finally try clearing some bits too and check that operation proceeds as
expected. */
xEventGroupClearBits( xEventGroup, xFirstTestBits );
xEventBits = xEventGroupGetBits( xEventGroup );
if( xEventBits != xSecondTestBits )
{
xErrorOccurred = pdTRUE;
}
}
void meca_fish_walk(int wait)
{
if(wait)
{
xEventGroupClearBits(busy, 0xFF);
}
if(current_state == MECA_FISH_WALK)
{
wait = 0;
}
next_state = MECA_FISH_WALK;
if(wait)
{
xEventGroupWaitBits(busy, 0xFF, pdFALSE, pdFALSE, portMAX_DELAY);
}
}
void meca_fish_swim_right(int wait)
{
if(wait)
{
xEventGroupClearBits(busy, 0xFF);
}
if(current_state == MECA_FISH_SWIM_RIGHT)
{
wait = 0;
}
next_state = MECA_FISH_SWIM_RIGHT;
if(wait)
{
xEventGroupWaitBits(busy, 0xFF, pdFALSE, pdFALSE, portMAX_DELAY);
}
}
void meca_fish_prepare(int wait)
{
if(wait)
{
xEventGroupClearBits(busy, 0xFF);
}
if(current_state == MECA_FISH_PREPARE)
{
wait = 0;
}
next_state = MECA_FISH_PREPARE;
if(wait)
{
xEventGroupWaitBits(busy, 0xFF, pdFALSE, pdFALSE, portMAX_DELAY);
}
}
static esp_err_t eth_event_handler(void *ctx, system_event_t *event)
{
switch (event->event_id) {
case SYSTEM_EVENT_ETH_START:
started = true;
break;
case SYSTEM_EVENT_ETH_GOT_IP:
memset(&ip, 0, sizeof(tcpip_adapter_ip_info_t));
ESP_ERROR_CHECK(tcpip_adapter_get_ip_info(ESP_IF_ETH, &ip));
xEventGroupSetBits(eth_event_group, GOTIP_BIT);
break;
case SYSTEM_EVENT_ETH_STOP:
xEventGroupClearBits(eth_event_group, GOTIP_BIT);
started = false;
default:
break;
}
return ESP_OK;
}
//*****************************************************************************
//
// Funcion main(), Inicializa los perifericos, crea las tareas, etc... y arranca el bucle del sistema
//
//*****************************************************************************
int main(void)
{
//Establecemos el color del led a apagado
color[0]=0x0;
color[1]=0x0;
color[2]=0x0;
confSys();
confUART();
confGPIO();
confADC();
confQueue();
confSemaphores();
//Creamos los flags
xEventGroup = xEventGroupCreate();
xEventGroupClearBits( xEventGroup, TrazaBit | PilotoAutomaticoBit );
//
// Mensaje de bienvenida inicial.
//
UARTprintf("\n\nBienvenido a la aplicacion Simulador Vuelo (curso 2014/15)!\n");
UARTprintf("\nAutores: Anabel Ramirez y Jose Antonio Yebenes ");
confTasks();
//
// Arranca el scheduler. Pasamos a ejecutar las tareas que se hayan activado.
//
confTimer();
vTaskStartScheduler(); //el RTOS habilita las interrupciones al entrar aqui, asi que no hace falta habilitarlas
//De la funcion vTaskStartScheduler no se sale nunca... a partir de aqui pasan a ejecutarse las tareas.
while(1)
{
//Si llego aqui es que algo raro ha pasado
}
}
static esp_err_t event_handler(void *ctx, system_event_t *event)
{
switch(event->event_id) {
case SYSTEM_EVENT_STA_START:
esp_wifi_connect();
break;
case SYSTEM_EVENT_STA_GOT_IP:
xEventGroupSetBits(wifi_event_group, CONNECTED_BIT);
break;
case SYSTEM_EVENT_STA_DISCONNECTED:
/* This is a workaround as ESP32 WiFi libs don't currently
auto-reassociate. */
esp_wifi_connect();
xEventGroupClearBits(wifi_event_group, CONNECTED_BIT);
break;
default:
break;
}
return ESP_OK;
}
/// Clear the specified Signal Flags of an active thread.
/// \param[in] thread_id thread ID obtained by \ref osThreadCreate or \ref osThreadGetId.
/// \param[in] signals specifies the signal flags of the thread that shall be cleared.
/// \return previous signal flags of the specified thread or 0x80000000 in case of incorrect parameters.
/// \note MUST REMAIN UNCHANGED: \b osSignalClear shall be consistent in every CMSIS-RTOS.
int32_t osSignalClear (osThreadId thread_id, int32_t signals)
{
EventGroupHandle_t event_handle;
//portBASE_TYPE taskWoken = pdFALSE;
EventBits_t uxBits_ret=0x80000000;
#ifdef CHECK_VALUE_OF_EVENT_GROUP
EventBits_t uxBits;
#endif
if (signals & (0xFFFFFFFF << osFeature_Signals)) {
return 0x80000000;
}
event_handle = find_signal_by_thread(thread_id);
if (event_handle) {
if (inHandlerMode()) {
uxBits_ret = xEventGroupGetBitsFromISR(event_handle);
#ifdef CHECK_VALUE_OF_EVENT_GROUP
uxBits =
#endif
xEventGroupClearBitsFromISR(
event_handle, /* The event group being updated. */
signals);/* The bits being cleared. */
}
else {
uxBits_ret = xEventGroupGetBits(event_handle);
#ifdef CHECK_VALUE_OF_EVENT_GROUP
uxBits =
#endif
xEventGroupClearBits(
event_handle, /* The event group being updated. */
signals);/* The bits being cleared. */
}
}
return uxBits_ret;
}
int WiFiGenericClass::clearStatusBits(int bits){
if(!_network_event_group){
return 0;
}
return xEventGroupClearBits(_network_event_group, bits);
}
static portTASK_FUNCTION(ConsumoTask,pvParameters)
{
double consumo=0.0374*exp(0.02*((velocidad*100)/240)); //actualizamos el consumo
TickType_t tiempo_ant =xTaskGetTickCount( ); //obtenemos los tick transcurridos
unsigned char frame[MAX_FRAME_SIZE];
int num_datos;
double combustible;
int16_t ejes[3];
double altitud;
while(1)
{
if(xQueueReceive(velocidadQueue,&velocidad, configTICK_RATE_HZ)){
//Si recibimos un nuevo valor de velocidad cambiamos brillo del led azul
color[BLUE]=0xFFFF;
RGBSet(color,((float)velocidad)/241);
}
if((xTaskGetTickCount( )-tiempo_ant)>=configTICK_RATE_HZ*(60/tiempoSim) && combustible!=0){
//Cada minuto real (1 hora simulada)
combustible=getCombustible();
//Modificamos el combustible segun el consumo
combustible -= 0.5*exp(0.02*(velocidad*100/240)) ;
if(combustible<=20){ //Encendemos el Led Verde si el combustible es menor que 20
color[GREEN]=0xFFFF;
RGBColorSet(color);
}
if(combustible<=0){ //Si el combustible es cero desactivamos el ADC
combustible=0;
velocidad=0;
color[BLUE]=0x0;
xEventGroupClearBits( xEventGroup, PilotoAutomaticoBit );
ADCSequenceDisable(ADC0_BASE,0);
}
//Enviamos el combustible
setCombustible(combustible);
num_datos=create_frame(frame, COMANDO_FUEL, &combustible, sizeof(combustible), MAX_FRAME_SIZE);
if (num_datos>=0){
send_frame(frame, num_datos);
}else{
logError(num_datos);
}
tiempo_ant =xTaskGetTickCount( );
}
getEjes(ejes);
altitud=getAltitud();
if(ejes[PITCH]>-45 && combustible==0 && altitud>0){ //si el combustible es cero ponemos PITCH =45 poco a poco
ejes[PITCH]--;
setEjes(ejes[PITCH],ejes[ROLL],ejes[YAW]);
num_datos=create_frame(frame, COMANDO_EJES, ejes, sizeof(ejes), MAX_FRAME_SIZE);
if (num_datos>=0){
send_frame(frame, num_datos);
}else{
logError(num_datos);
}
}
}
}
// Protected
void IMUTask::task()
{
int8_t result;
// eventGroup for notifications
eventGroup = xEventGroupCreate();
if (eventGroup == NULL) {
debug::log("IMUTask: Failed to create eventGroup");
}
EventBits_t uxBits;
/* Push both gyro and accel data into the FIFO. */
// this starts the interrupts firing
//result = mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
//debug::log("IMUTask: mpu_configure_fifo returned " + String(result));
setup();
// everthing setup start the dmp generating interrupts
enable_dmp();
while(true) {
uxBits = xEventGroupWaitBits(eventGroup,
IMU_INT_BIT,
pdFALSE,
pdFALSE,
portMAX_DELAY);
if( ( uxBits & IMU_INT_BIT ) != 0 ) {
// interrupt has fired
if (dmp_state == IMU_DMP_ON) {
// interrupt as a result of DMP
unsigned long sensor_timestamp;
short gyro[3], accel[3], sensors;
unsigned char more = 0;
long quat[4];
/* This function gets new data from the FIFO when the DMP is in
* use. The FIFO can contain any combination of gyro, accel,
* quaternion, and gesture data. The sensors parameter tells the
* caller which data fields were actually populated with new data.
* For example, if sensors == (INV_XYZ_GYRO | INV_WXYZ_QUAT), then
* the FIFO isn't being filled with accel data.
* The driver parses the gesture data to determine if a gesture
* event has occurred; on an event, the application will be notified
* via a callback (assuming that a callback function was properly
* registered). The more parameter is non-zero if there are
* leftover packets in the FIFO.
*/
int success = dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,
&more);
if (!more) {
//hal.new_gyro = 0;
// TODO: more data to pull need a way to fetch it
}
// lets try printing the
} else {
// TODO: interrupt not from DMP
}
xEventGroupClearBits(eventGroup,
IMU_INT_BIT);
} else {
// wait timed out
// SerialUSB.println("IMUTask: Wait time out");
}
}
}
/* For internal use only - execute a 'clear bits' command that was pended from
an interrupt. */
void vEventGroupClearBitsCallback( void *pvEventGroup, const uint32_t ulBitsToClear )
{
( void ) xEventGroupClearBits( pvEventGroup, ( EventBits_t ) ulBitsToClear );
}
static BaseType_t prvPerformTaskSyncTests( BaseType_t xError, TaskHandle_t xTestSlaveTaskHandle )
{
EventBits_t uxBits;
/* The three tasks that take part in the synchronisation (rendezvous) are
expected to be in the suspended state at the start of the test. */
if( eTaskGetState( xTestSlaveTaskHandle ) != eSuspended )
{
xError = pdTRUE;
}
if( eTaskGetState( xSyncTask1 ) != eSuspended )
{
xError = pdTRUE;
}
if( eTaskGetState( xSyncTask2 ) != eSuspended )
{
xError = pdTRUE;
}
/* Try a synch with no other tasks involved. First set all the bits other
than this task's bit. */
xEventGroupSetBits( xEventGroup, ( ebALL_SYNC_BITS & ~ebSET_BIT_TASK_SYNC_BIT ) );
/* Then wait on just one bit - the bit that is being set. */
uxBits = xEventGroupSync( xEventGroup, /* The event group used for the synchronisation. */
ebSET_BIT_TASK_SYNC_BIT,/* The bit set by this task when it reaches the sync point. */
ebSET_BIT_TASK_SYNC_BIT,/* The bits to wait for - in this case it is just waiting for itself. */
portMAX_DELAY ); /* The maximum time to wait for the sync condition to be met. */
/* A sync with a max delay should only exit when all the synchronise
bits are set...check that is the case. In this case there is only one
sync bit anyway. */
if( ( uxBits & ebSET_BIT_TASK_SYNC_BIT ) != ebSET_BIT_TASK_SYNC_BIT )
{
xError = pdTRUE;
}
/* ...but now the sync bits should be clear again, leaving all the other
bits set (as only one bit was being waited for). */
if( xEventGroupGetBits( xEventGroup ) != ( ebALL_SYNC_BITS & ~ebSET_BIT_TASK_SYNC_BIT ) )
{
xError = pdTRUE;
}
/* Clear all the bits to zero again. */
xEventGroupClearBits( xEventGroup, ( ebALL_SYNC_BITS & ~ebSET_BIT_TASK_SYNC_BIT ) );
if( xEventGroupGetBits( xEventGroup ) != 0 )
{
xError = pdTRUE;
}
/* Unsuspend the other tasks then check they have executed up to the
synchronisation point. */
vTaskResume( xTestSlaveTaskHandle );
vTaskResume( xSyncTask1 );
vTaskResume( xSyncTask2 );
if( eTaskGetState( xTestSlaveTaskHandle ) != eBlocked )
{
xError = pdTRUE;
}
if( eTaskGetState( xSyncTask1 ) != eBlocked )
{
xError = pdTRUE;
}
if( eTaskGetState( xSyncTask2 ) != eBlocked )
{
xError = pdTRUE;
}
/* Set this task's sync bit. */
uxBits = xEventGroupSync( xEventGroup, /* The event group used for the synchronisation. */
ebSET_BIT_TASK_SYNC_BIT,/* The bit set by this task when it reaches the sync point. */
ebALL_SYNC_BITS, /* The bits to wait for - these bits are set by the other tasks that take part in the sync. */
portMAX_DELAY ); /* The maximum time to wait for the sync condition to be met. */
/* A sync with a max delay should only exit when all the synchronise
bits are set...check that is the case. */
if( ( uxBits & ebALL_SYNC_BITS ) != ebALL_SYNC_BITS )
{
xError = pdTRUE;
}
/* ...but now the sync bits should be clear again. */
if( xEventGroupGetBits( xEventGroup ) != 0 )
{
xError = pdTRUE;
}
/* The other tasks should now all be suspended again, ready for the next
synchronisation. */
if( eTaskGetState( xTestSlaveTaskHandle ) != eSuspended )
{
xError = pdTRUE;
}
if( eTaskGetState( xSyncTask1 ) != eSuspended )
{
xError = pdTRUE;
}
if( eTaskGetState( xSyncTask2 ) != eSuspended )
{
xError = pdTRUE;
}
/* Sync again - but this time set the last necessary bit as the
highest priority task, rather than the lowest priority task. Unsuspend
the other tasks then check they have executed up to the synchronisation
point. */
vTaskResume( xTestSlaveTaskHandle );
vTaskResume( xSyncTask1 );
vTaskResume( xSyncTask2 );
if( eTaskGetState( xTestSlaveTaskHandle ) != eBlocked )
{
xError = pdTRUE;
}
if( eTaskGetState( xSyncTask1 ) != eBlocked )
{
xError = pdTRUE;
}
if( eTaskGetState( xSyncTask2 ) != eBlocked )
{
xError = pdTRUE;
}
/* Raise the priority of this task above that of the other tasks. */
vTaskPrioritySet( NULL, ebWAIT_BIT_TASK_PRIORITY + 1 );
/* Set this task's sync bit. */
uxBits = xEventGroupSync( xEventGroup, ebSET_BIT_TASK_SYNC_BIT, ebALL_SYNC_BITS, portMAX_DELAY );
/* A sync with a max delay should only exit when all the synchronisation
bits are set... */
if( ( uxBits & ebALL_SYNC_BITS ) != ebALL_SYNC_BITS )
{
xError = pdTRUE;
}
/* ...but now the sync bits should be clear again. */
if( xEventGroupGetBits( xEventGroup ) != 0 )
{
xError = pdTRUE;
}
/* The other tasks should now all be in the ready state again, but not
executed yet as this task still has a higher relative priority. */
if( eTaskGetState( xTestSlaveTaskHandle ) != eReady )
{
xError = pdTRUE;
}
if( eTaskGetState( xSyncTask1 ) != eReady )
{
xError = pdTRUE;
}
if( eTaskGetState( xSyncTask2 ) != eReady )
{
xError = pdTRUE;
}
/* Reset the priority of this task back to its original value. */
vTaskPrioritySet( NULL, ebSET_BIT_TASK_PRIORITY );
/* Now all the other tasks should have reblocked on the event bits
to test the behaviour when the event bits are deleted. */
if( eTaskGetState( xTestSlaveTaskHandle ) != eBlocked )
{
xError = pdTRUE;
}
if( eTaskGetState( xSyncTask1 ) != eBlocked )
{
xError = pdTRUE;
}
if( eTaskGetState( xSyncTask2 ) != eBlocked )
{
xError = pdTRUE;
}
return xError;
}
static BaseType_t prvBitCombinationTestMasterFunction( BaseType_t xError, TaskHandle_t xTestSlaveTaskHandle )
{
EventBits_t uxBits;
/* Resume the other task. It will block, pending a single bit from
within ebCOMBINED_BITS. */
vTaskResume( xTestSlaveTaskHandle );
/* Ensure the other task is blocked on the task. */
if( eTaskGetState( xTestSlaveTaskHandle ) != eBlocked )
{
xError = pdTRUE;
}
/* Set all the bits in ebCOMBINED_BITS - the 'test slave' task is only
blocked waiting for one of them. */
xEventGroupSetBits( xEventGroup, ebCOMBINED_BITS );
/* The 'test slave' task should now have executed, clearing ebBIT_1 (the
bit it was blocked on), then re-entered the Blocked state to wait for
all the other bits in ebCOMBINED_BITS to be set again. First check
ebBIT_1 is clear. */
uxBits = xEventGroupWaitBits( xEventGroup, ebALL_BITS, pdFALSE, pdFALSE, ebDONT_BLOCK );
if( uxBits != ( ebCOMBINED_BITS & ~ebBIT_1 ) )
{
xError = pdTRUE;
}
/* Ensure the other task is still in the blocked state. */
if( eTaskGetState( xTestSlaveTaskHandle ) != eBlocked )
{
xError = pdTRUE;
}
/* Set all the bits other than ebBIT_1 - which is the bit that must be
set before the other task unblocks. */
xEventGroupSetBits( xEventGroup, ebALL_BITS & ~ebBIT_1 );
/* Ensure all the expected bits are still set. */
uxBits = xEventGroupWaitBits( xEventGroup, ebALL_BITS, pdFALSE, pdFALSE, ebDONT_BLOCK );
if( uxBits != ( ebALL_BITS & ~ebBIT_1 ) )
{
xError = pdTRUE;
}
/* Ensure the other task is still in the blocked state. */
if( eTaskGetState( xTestSlaveTaskHandle ) != eBlocked )
{
xError = pdTRUE;
}
/* Now also set ebBIT_1, which should unblock the other task, which will
then suspend itself. */
xEventGroupSetBits( xEventGroup, ebBIT_1 );
/* Ensure the other task is suspended. */
if( eTaskGetState( xTestSlaveTaskHandle ) != eSuspended )
{
xError = pdTRUE;
}
/* The other task should not have cleared the bits - so all the bits
should still be set. */
if( xEventGroupSetBits( xEventGroup, 0x00 ) != ebALL_BITS )
{
xError = pdTRUE;
}
/* Clear ebBIT_1 again. */
if( xEventGroupClearBits( xEventGroup, ebBIT_1 ) != ebALL_BITS )
{
xError = pdTRUE;
}
/* Resume the other task - which will wait on all the ebCOMBINED_BITS
again - this time clearing the bits when it is unblocked. */
vTaskResume( xTestSlaveTaskHandle );
/* Ensure the other task is blocked once again. */
if( eTaskGetState( xTestSlaveTaskHandle ) != eBlocked )
{
xError = pdTRUE;
}
/* Set the bit the other task is waiting for. */
xEventGroupSetBits( xEventGroup, ebBIT_1 );
/* Ensure the other task is suspended once again. */
if( eTaskGetState( xTestSlaveTaskHandle ) != eSuspended )
{
xError = pdTRUE;
}
/* The other task should have cleared the bits in ebCOMBINED_BITS.
Clear the remaining bits. */
uxBits = xEventGroupWaitBits( xEventGroup, ebALL_BITS, pdFALSE, pdFALSE, ebDONT_BLOCK );
if( uxBits != ( ebALL_BITS & ~ebCOMBINED_BITS ) )
{
xError = pdTRUE;
}
/* Clear all bits ready for the sync with the other three tasks. The
value returned is the value prior to the bits being cleared. */
if( xEventGroupClearBits( xEventGroup, ebALL_BITS ) != ( ebALL_BITS & ~ebCOMBINED_BITS ) )
{
xError = pdTRUE;
}
/* The bits should be clear now. */
if( xEventGroupGetBits( xEventGroup ) != 0x00 )
{
xError = pdTRUE;
}
return xError;
}
