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