/** * Module task */ static void stabilizationTask(void* parameters) { portTickType lastSysTime; portTickType thisSysTime; UAVObjEvent ev; QuadMotorsDesiredData actuatorDesired; FlightStatusData flightStatus; //SettingsUpdatedCb((UAVObjEvent *) NULL); // Main task loop lastSysTime = xTaskGetTickCount(); while(1) { PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION); // Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe if ( xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE ) { AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION,SYSTEMALARMS_ALARM_WARNING); continue; } // Check how long since last update thisSysTime = xTaskGetTickCount(); if(thisSysTime > lastSysTime) // reuse dt in case of wraparound dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f; lastSysTime = thisSysTime; FlightStatusGet(&flightStatus); QuadMotorsDesiredGet(&actuatorDesired); //if(flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED) //{ // ZERO MOTORS //} //else //{ //} //ActuatorSettingsData settings; //ActuatorSettingsGet(&settings); //PIOS_SetMKSpeed(settings.ChannelAddr[mixer_channel],value); PIOS_SetMKSpeed(0,actuatorDesired.motorFront_NW); PIOS_SetMKSpeed(1,actuatorDesired.motorRight_NE); PIOS_SetMKSpeed(2,actuatorDesired.motorBack_SE); PIOS_SetMKSpeed(3,actuatorDesired.motorLeft_SW); // Clear alarms AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION); } }
bool PIOS_I2C_ESC_Config() { base_address = MK_I2C_ADDR; valid_motors = 0; for(uint8_t i = 0; i < 4; i ++) if(PIOS_SetMKSpeed(i, 0)) valid_motors |= (1 << i); return true; }