/*
* Main Task
*/
void MainTask(void* pdata) {
// int frameDone = 0; // RC required
printf("Starting Main task...\n");
int16_t avgSensorData[9] = { 0 }; //Order: ACC- GYR - COMP
uint32_t averagedDataDeltaT = 0;
float filteredSensorData[9] = { 0 }; //Order: ACC- GYR - COMP
int16_t rcValues[8];
INT8U os_err = OS_ERR_NONE;
uint8_t data_err = NO_ERR;
struct logData* loggData = malloc(sizeof(struct logData));
while (1) {
//Semaphore for periodic task
OSSemPend(mainTaskSem, 0, &os_err);
//Sensor Test
if (SDM_NEW_DATA_AVAILABLE == 1) {
os_err = getSensorData(avgSensorData, &averagedDataDeltaT);
data_err = filterSensorData(avgSensorData, filteredSensorData, averagedDataDeltaT); //only filter new data
}
if (RC_RECEIVER_NEW_DATA_AVAILABLE == 1 ){
getRCvalues(rcValues); //new rc commands will be copied in local array
printf("THROTTLE: %d\tROLL: %d\tYAW: %d\tPITCH: %d\n", rcValues[RC_THROTTLE_INDEX], rcValues[RC_ROLL_INDEX], rcValues[RC_YAW_INDEX], rcValues[RC_PITCH_INDEX]);
}
//assuming X axis from Filter is PITCH
float pidPITCHMidVal = PIDPitchCalculation(rcValues[RC_PITCH_INDEX], (float) filteredSensorData[EULER_PITCH_INDEX]);
//assuming X axis from Filter is ROLL
float pidROLLMidVal = PIDRollCalculation(rcValues[RC_ROLL_INDEX], (float) filteredSensorData[EULER_ROLL_INDEX]);
//assuming X axis from Filter is YAW
float pidYAWMidVal = PIDYawCalculation(rcValues[RC_YAW_INDEX], (float) filteredSensorData[EULER_YAW_INDEX]);
// printf("PITCH: %f\tROLL: %f\tYAW: %f\n", pidPITCHMidVal, pidROLLMidVal, pidYAWMidVal); //debug print
mapToMotors(rcValues[RC_THROTTLE_INDEX], pidROLLMidVal, pidPITCHMidVal, pidYAWMidVal);
//TODO Copy new Data to Struct for logging
// int i;
// for (i = 0; i < 9; ++i) { //copy logging data to txStruct
// loggData->raw[i] = avgSensorData[i];
// loggData->filter[i] = filteredSensorData[i];
// }
//
// loggData->pid[0] = pidPITCHMidVal;
// loggData->pid[1] = pidROLLMidVal;
// loggData->pid[2] = pidYAWMidVal;
//
// int j;
// for (j = 0; j < 8; ++j) {
// loggData->rawRadio[i] = rcValues[i];
// }
// OSQPost(loggerQsem, (void*) loggData);
}
}
void SensorDht22::getSensorData(void) {
getRawSensorData();
filterSensorData();
}
