/**
* 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;
}
