int16_t
inqueue_get(queue_t *q, int timeout) {
StatusType rc;
uint64_t deadline = 0;
uint8_t value;
if (timeout > TIMEOUT_NOBLOCK) {
deadline = timeout + CoGetOSTime();
}
DISABLE_IRQ();
while (q->count == 0) {
ENABLE_IRQ();
if (timeout == TIMEOUT_NOBLOCK) {
return EERR_TIMEOUT;
}
rc = CoWaitForSingleFlag(q->flag,
timeout == TIMEOUT_FOREVER ? 0 : (CoGetOSTime() - deadline));
if (rc != E_OK) {
return EERR_TIMEOUT;
}
DISABLE_IRQ();
}
q->count--;
value = *q->p_read++;
if (q->p_read == q->p_top) {
q->p_read = q->p_bot;
}
ENABLE_IRQ();
return value;
}
uint8_t waitForKey(uint8_t key, uint32_t timeout){
CoWaitForSingleFlag(keyFlag[key-1],timeout);
return key;
}
void runPID(void *Parameters)
{
float currError = 0, prevError = 0, errorSum = 0, errorDiff = 0,
outD = 0, outP = 0, outI = 0, output = 0;
int delayTicks;
struct PIDParameters * params = Parameters;
CoWaitForSingleFlag(params->InitCompleteFlagID, 0);
while (1)
{
if (RegisterMap[REGISTER_CONTROL_MODE].Bits & CONTROL_MODE_AUTO_MODE && RegisterMap[REGISTER_CONTROL_MODE].Bits & params->ControlBitMask)
{
CoPendSem(params->ThisPIDSemID, 0);
if(!params->IsTopLevelUnit)
CoPendSem(params->PrevPIDSemID, 0);
CoPendSem(params->DataSemID, 0);
currError = *(params->SetPoint) - *(params->ProcessVariable);
errorSum += currError;
errorDiff = currError - prevError;
prevError = currError;
outD = *(params->KD) * errorDiff;
outI = *(params->KI) * errorSum;
outP = *(params->KP) * currError;
output = outD + outP + outI;
*(params->OutputVariable) = output;
CoPostSem(params->ThisPIDSemID);
if(!params->IsTopLevelUnit)
CoPostSem(params->PrevPIDSemID);
CoPostSem(params->DataSemID);
}
else
{
// Reset if the PID controller is turned off
currError = 0;
errorSum = 0;
errorDiff = 0;
prevError = 0;
}
delayTicks = delaymsToTicks(*(params->UpdateTime_ms));
CoTickDelay(delayTicks);
}
}
int16_t
outqueue_put(queue_t *q, const uint8_t *value, uint16_t size, int timeout) {
StatusType rc;
uint64_t deadline = 0;
if (timeout > TIMEOUT_NOBLOCK) {
deadline = timeout + CoGetOSTime();
}
DISABLE_IRQ();
while (size) {
while (q->count == q->size) {
/* Wake this thread up when the buffer has room for all remaining
* bytes, or is empty. */
if (size > q->size) {
q->wakeup = 0;
} else {
q->wakeup = q->size - size;
}
ENABLE_IRQ();
if (q->cb_func) {
q->cb_func(q->cb_arg);
}
if (timeout == TIMEOUT_NOBLOCK) {
return EERR_TIMEOUT;
}
rc = CoWaitForSingleFlag(q->flag,
timeout == TIMEOUT_FOREVER ? 0 : (CoGetOSTime() - deadline));
if (rc != E_OK) {
return EERR_TIMEOUT;
}
DISABLE_IRQ();
}
size--;
q->count++;
*q->p_write++ = *value++;
if (q->p_write == q->p_top) {
q->p_write = q->p_bot;
}
}
ENABLE_IRQ();
if (q->cb_func) {
q->cb_func(q->cb_arg);
}
return EERR_OK;
}
static void dIMUTaskCode(void *unused) {
uint32_t loops = 0;
while (1) {
// wait for work
CoWaitForSingleFlag(dImuData.flag, 0);
if (dImuData.calibReadWriteFlag)
dIMUReadWriteCalib();
// double rate gyo loop
#ifdef DIMU_HAVE_MPU6000
mpu6000DrateDecode();
#endif
#ifdef DIMU_HAVE_MAX21100
max21100DrateDecode();
#endif
imuDImuDRateReady();
// full sensor loop
if (!(loops % (DIMU_OUTER_PERIOD/DIMU_INNER_PERIOD))) {
#ifdef DIMU_HAVE_MPU6000
mpu6000Decode();
#endif
#ifdef DIMU_HAVE_MAX21100
max21100Decode();
#endif
#ifdef DIMU_HAVE_HMC5983
hmc5983Decode();
#endif
#ifdef DIMU_HAVE_MS5611
ms5611Decode();
#endif
dImuData.lastUpdate = timerMicros();
imuDImuSensorReady();
dIMUCalcTempDiff();
}
loops++;
}
}
/**
*******************************************************************************
* @brief time_display task
* @param[in] pdata A pointer to parameter passed to task.
* @param[out] None
* @retval None
*
* @details This task use to display time in LCD.
*******************************************************************************
*/
void time_display(void *pdata)
{
static unsigned char tim[3] = {0,0,0};
static unsigned char *ptr;
pdata = pdata;
for (;;)
{
CoWaitForSingleFlag(time_display_flg,0);
ptr = (unsigned char *)CoPendMail(mbox0,0,&errinfo[30]);
CoEnterMutexSection(mut_lcd);
if (tim[0] != *(ptr+0))
{
tim[0] = *(ptr+0);
chart[6] = tim[0]/10 + '0';
chart[7] = tim[0]%10 + '0';
}
if (tim[1] != *(ptr+1))
{
tim[1] = *(ptr+1);
chart[3] = tim[1]/10 + '0';
chart[4] = tim[1]%10 + '0';
}
if (tim[2] != *(ptr+2))
{
tim[2] = *(ptr+2);
chart[0] = tim[2]/10 + '0';
chart[1] = tim[2]%10 + '0';
}
set_cursor(7, 0);
lcd_print (chart);
CoLeaveMutexSection(mut_lcd);
}
}
void runTaskCode(void *unused) {
uint32_t axis = 0;
uint32_t loops = 0;
AQ_NOTICE("Run task started\n");
while (1) {
// wait for data
CoWaitForSingleFlag(imuData.sensorFlag, 0);
// soft start GPS accuracy
runData.accMask *= 0.999f;
navUkfInertialUpdate();
// record history for acc & mag & pressure readings for smoothing purposes
// acc
runData.sumAcc[0] -= runData.accHist[0][runData.sensorHistIndex];
runData.sumAcc[1] -= runData.accHist[1][runData.sensorHistIndex];
runData.sumAcc[2] -= runData.accHist[2][runData.sensorHistIndex];
runData.accHist[0][runData.sensorHistIndex] = IMU_ACCX;
runData.accHist[1][runData.sensorHistIndex] = IMU_ACCY;
runData.accHist[2][runData.sensorHistIndex] = IMU_ACCZ;
runData.sumAcc[0] += runData.accHist[0][runData.sensorHistIndex];
runData.sumAcc[1] += runData.accHist[1][runData.sensorHistIndex];
runData.sumAcc[2] += runData.accHist[2][runData.sensorHistIndex];
// mag
runData.sumMag[0] -= runData.magHist[0][runData.sensorHistIndex];
runData.sumMag[1] -= runData.magHist[1][runData.sensorHistIndex];
runData.sumMag[2] -= runData.magHist[2][runData.sensorHistIndex];
runData.magHist[0][runData.sensorHistIndex] = IMU_MAGX;
runData.magHist[1][runData.sensorHistIndex] = IMU_MAGY;
runData.magHist[2][runData.sensorHistIndex] = IMU_MAGZ;
runData.sumMag[0] += runData.magHist[0][runData.sensorHistIndex];
runData.sumMag[1] += runData.magHist[1][runData.sensorHistIndex];
runData.sumMag[2] += runData.magHist[2][runData.sensorHistIndex];
// pressure
runData.sumPres -= runData.presHist[runData.sensorHistIndex];
runData.presHist[runData.sensorHistIndex] = AQ_PRESSURE;
runData.sumPres += runData.presHist[runData.sensorHistIndex];
runData.sensorHistIndex = (runData.sensorHistIndex + 1) % RUN_SENSOR_HIST;
if (!((loops+1) % 20)) {
simDoAccUpdate(runData.sumAcc[0]*(1.0f / (float)RUN_SENSOR_HIST), runData.sumAcc[1]*(1.0f / (float)RUN_SENSOR_HIST), runData.sumAcc[2]*(1.0f / (float)RUN_SENSOR_HIST));
}
else if (!((loops+7) % 20)) {
simDoPresUpdate(runData.sumPres*(1.0f / (float)RUN_SENSOR_HIST));
}
#ifndef USE_DIGITAL_IMU
else if (!((loops+13) % 20) && AQ_MAG_ENABLED) {
simDoMagUpdate(runData.sumMag[0]*(1.0f / (float)RUN_SENSOR_HIST), runData.sumMag[1]*(1.0f / (float)RUN_SENSOR_HIST), runData.sumMag[2]*(1.0f / (float)RUN_SENSOR_HIST));
}
#endif
// optical flow update
else if (navUkfData.flowCount >= 10 && !navUkfData.flowLock) {
navUkfFlowUpdate();
}
// only accept GPS updates if there is no optical flow
else if (CoAcceptSingleFlag(gpsData.gpsPosFlag) == E_OK && navUkfData.flowQuality == 0.0f && gpsData.hAcc < NAV_MIN_GPS_ACC && gpsData.tDOP != 0.0f) {
navUkfGpsPosUpdate(gpsData.lastPosUpdate, gpsData.lat, gpsData.lon, gpsData.height, gpsData.hAcc + runData.accMask, gpsData.vAcc + runData.accMask);
CoClearFlag(gpsData.gpsPosFlag);
// refine static sea level pressure based on better GPS altitude fixes
if (gpsData.hAcc < runData.bestHacc && gpsData.hAcc < NAV_MIN_GPS_ACC) {
navPressureAdjust(gpsData.height);
runData.bestHacc = gpsData.hAcc;
}
}
else if (CoAcceptSingleFlag(gpsData.gpsVelFlag) == E_OK && navUkfData.flowQuality == 0.0f && gpsData.sAcc < NAV_MIN_GPS_ACC/2 && gpsData.tDOP != 0.0f) {
navUkfGpsVelUpdate(gpsData.lastVelUpdate, gpsData.velN, gpsData.velE, gpsData.velD, gpsData.sAcc + runData.accMask);
CoClearFlag(gpsData.gpsVelFlag);
}
// observe zero position
else if (!((loops+4) % 20) && (gpsData.hAcc >= NAV_MIN_GPS_ACC || gpsData.tDOP == 0.0f) && navUkfData.flowQuality == 0.0f) {
navUkfZeroPos();
}
// observer zero velocity
else if (!((loops+10) % 20) && (gpsData.sAcc >= NAV_MIN_GPS_ACC/2 || gpsData.tDOP == 0.0f) && navUkfData.flowQuality == 0.0f) {
navUkfZeroVel();
}
// observe that the rates are exactly 0 if not flying or moving
else if (!(supervisorData.state & STATE_FLYING)) {
float stdX, stdY, stdZ;
arm_std_f32(runData.accHist[0], RUN_SENSOR_HIST, &stdX);
arm_std_f32(runData.accHist[1], RUN_SENSOR_HIST, &stdY);
arm_std_f32(runData.accHist[2], RUN_SENSOR_HIST, &stdZ);
if ((stdX + stdY + stdZ) < (IMU_STATIC_STD*2)) {
if (!((axis + 0) % 3))
navUkfZeroRate(IMU_RATEX, 0);
else if (!((axis + 1) % 3))
navUkfZeroRate(IMU_RATEY, 1);
else
navUkfZeroRate(IMU_RATEZ, 2);
axis++;
}
}
navUkfFinish();
altUkfProcess(AQ_PRESSURE);
// determine which altitude estimate to use
if (gpsData.hAcc > p[NAV_ALT_GPS_ACC]) {
runData.altPos = &ALT_POS;
runData.altVel = &ALT_VEL;
}
else {
runData.altPos = &UKF_ALTITUDE;
runData.altVel = &UKF_VELD;
}
CoSetFlag(runData.runFlag); // new state data
navNavigate();
#ifndef HAS_AIMU
analogDecode();
#endif
if (!(loops % (int)(1.0f / AQ_OUTER_TIMESTEP)))
loggerDoHeader();
loggerDo();
gimbalUpdate();
#ifdef CAN_CALIB
canTxIMUData(loops);
#endif
calibrate();
loops++;
}
}
void btTask(void* pdata)
{
uint8_t byte;
btFlag = CoCreateFlag(true, false);
btTx.size = 0;
// Look for BT module baudrate, try 115200, and 9600
// Already initialised to g_eeGeneral.bt_baudrate
// 0 : 115200, 1 : 9600, 2 : 19200
uint32_t x = g_eeGeneral.btBaudrate;
btStatus = btPollDevice() ; // Do we get a response?
for (int y=0; y<2; y++) {
if (btStatus == 0) {
x += 1 ;
if (x > 2) {
x = 0 ;
}
btSetBaudrate(x) ;
CoTickDelay(1) ; // 2mS
btStatus = btPollDevice() ; // Do we get a response?
}
}
if (btStatus) {
btStatus = x + 1 ;
if ( x != g_eeGeneral.btBaudrate ) {
x = g_eeGeneral.btBaudrate ;
// Need to change Bt Baudrate
btChangeBaudrate( x ) ;
btStatus += (x+1) * 10 ;
btSetBaudrate( x ) ;
}
}
else {
btInit();
}
CoTickDelay(1) ;
btPollDevice(); // Do we get a response?
while (1) {
uint32_t x = CoWaitForSingleFlag(btFlag, 10); // Wait for data in Fifo
if (x == E_OK) {
// We have some data in the Fifo
while (btTxFifo.pop(byte)) {
btTxBuffer[btTx.size++] = byte;
if (btTx.size > 31) {
btSendBuffer();
}
}
}
else if (btTx.size) {
btSendBuffer();
}
}
}
void refreshDisplay()
{
Set_Address( 0, 0 ) ;
LCD_NCS_LOW() ;
GPIOC->BSRRL = PIN_LCD_A0 ; // A0 high
convertDisplay() ;
// setupSPIdma() ;
DmaDebugDone = 0 ;
startSpiDma() ;
// DMA1_Stream7->CR |= DMA_SxCR_EN ; // Enable DMA
// SPI3->CR2 |= SPI_CR2_TXDMAEN ;
// uint32_t x ;
if ( Main_running )
{
CoWaitForSingleFlag( LcdFlag, 10 ) ;
DmaDebugDone = 0x80 ;
}
// if ( x == E_OK )
// {
// }
// while ( ( DMA1->HISR & DMA_HISR_TCIF7 ) == 0 )
// {
// // wait
// if ( DmaDone )
// {
// DmaDebugNDTR = DMA1_Stream7->NDTR ;
// DmaDebugDone = 1 ;
// }
// }
// DmaDebugDone |= 0x20 ;
while ( DmaDone == 0 )
{
// wait
// if ( DMA1->HISR & DMA_HISR_TCIF7 )
// {
// SPI3->CR2 &= ~SPI_CR2_TXDMAEN ;
// DMA1_Stream7->CR &= ~DMA_SxCR_EN ; // Disable DMA
// break ;
// }
}
DmaDebugDone |= 1 ;
if ( DMA1_Stream7->CR & DMA_SxCR_EN )
{
DmaDebugDone |= 0x10 ;
}
// if ( DMA1_Stream7->NDTR )
// {
// while ( ( DMA1->HISR & DMA_HISR_TCIF7 ) == 0 )
// {
// // wait
// }
// DmaDebugDone |= 0x20 ;
// }
// SPI3->CR2 &= ~SPI_CR2_TXDMAEN ;
// DMA1_Stream7->CR &= ~DMA_SxCR_EN ; // Disable DMA
while ( ( SPI3->SR & SPI_SR_TXE ) == 0 )
{
} // Last byte being sent
while ( SPI3->SR & SPI_SR_BSY )
{
// wait
}
GPIOA->BSRRL = PIN_LCD_NCS ; // CS high
}
void adcTaskCode(void *unused) {
float dRateVoltageX, dRateVoltageY, dRateVoltageZ;
double sumMagX, sumMagY, sumMagZ;
unsigned char magSign;
int countMag, firstAfterFlip;
float x, y, z;
float a, b, c;
float dTemp, dTemp2, dTemp3;
unsigned long loops;
register int i;
AQ_NOTICE("ADC task started\n");
magSign = ADC_MAG_SIGN;
sumMagX = sumMagY = sumMagZ = 0.0f;
dTemp = dTemp2 = dTemp3 = 0.0f;
countMag = 0;
firstAfterFlip = 0;
while (1) {
// wait for work
CoWaitForSingleFlag(adcData.adcFlag, 0);
loops = adcData.loops;
// sum adc values
for (i = 0; i < ADC_SENSORS; i++) {
adcData.channelSums[i] += adcData.adcSums[i];
}
// IDG500 & ISZ500 gyro double rate
dRateVoltageX = adcData.adcSums[ADC_VOLTS_RATEX] * ADC_DIVISOR * (1.0 / 4.0);
dRateVoltageY = adcData.adcSums[ADC_VOLTS_RATEY] * ADC_DIVISOR * (1.0 / 4.0);
dRateVoltageZ = adcData.adcSums[ADC_VOLTS_RATEZ] * ADC_DIVISOR * (1.0 / 4.0);
// rates
x = +(dRateVoltageX + adcData.rateBiasX + p[IMU_GYO_BIAS1_X]*dTemp + p[IMU_GYO_BIAS2_X]*dTemp2 +
p[IMU_GYO_BIAS3_X]*dTemp3);// / p[IMU_GYO_SCAL_X];
y = -(dRateVoltageY + adcData.rateBiasY + p[IMU_GYO_BIAS1_Y]*dTemp + p[IMU_GYO_BIAS2_Y]*dTemp2 +
p[IMU_GYO_BIAS3_Y]*dTemp3);// / p[IMU_GYO_SCAL_Y];
z = -(dRateVoltageZ + adcData.rateBiasZ + p[IMU_GYO_BIAS1_Z]*dTemp + p[IMU_GYO_BIAS2_Z]*dTemp2 +
p[IMU_GYO_BIAS3_Z]*dTemp3);// / p[IMU_GYO_SCAL_Z];
a = x + y*p[IMU_GYO_ALGN_XY] + z*p[IMU_GYO_ALGN_XZ];
b = x*p[IMU_GYO_ALGN_YX] + y + z*p[IMU_GYO_ALGN_YZ];
c = x*p[IMU_GYO_ALGN_ZX] + y*p[IMU_GYO_ALGN_ZY] + z;
a /= p[IMU_GYO_SCAL_X];
b /= p[IMU_GYO_SCAL_Y];
c /= p[IMU_GYO_SCAL_Z];
adcData.dRateX = (adcData.dRateX + a * imuData.cosRot - b * imuData.sinRot) * 0.5f;
adcData.dRateY = (adcData.dRateY + b * imuData.cosRot + a * imuData.sinRot) * 0.5f;
adcData.dRateZ = (adcData.dRateZ + c) * 0.5f;
// adcData.dRateX = a * imuData.cosRot - b * imuData.sinRot;
// adcData.dRateY = b * imuData.cosRot + a * imuData.sinRot;
// adcData.dRateZ = c;
// notify IMU of double rate GYO readings are ready
imuAdcDRateReady();
// mags
// we need to discard frames after mag bias flips
if (firstAfterFlip) {
adcData.channelSums[ADC_VOLTS_MAGX] -= adcData.adcSums[ADC_VOLTS_MAGX];
adcData.channelSums[ADC_VOLTS_MAGY] -= adcData.adcSums[ADC_VOLTS_MAGY];
adcData.channelSums[ADC_VOLTS_MAGZ] -= adcData.adcSums[ADC_VOLTS_MAGZ];
firstAfterFlip = 0;
}
if (!(loops & 0x01)) {
// calculate voltages
adcData.voltages[ADC_VOLTS_RATEX] = adcData.channelSums[ADC_VOLTS_RATEX] * ADC_DIVISOR * (1.0f / 8.0f); // ratex
adcData.voltages[ADC_VOLTS_RATEY] = adcData.channelSums[ADC_VOLTS_RATEY] * ADC_DIVISOR * (1.0f / 8.0f); // ratey
adcData.voltages[ADC_VOLTS_RATEZ] = adcData.channelSums[ADC_VOLTS_RATEZ] * ADC_DIVISOR * (1.0f / 8.0f); // ratez
adcData.voltages[ADC_VOLTS_MAGX] = adcData.channelSums[ADC_VOLTS_MAGX] * ADC_DIVISOR * (1.0f / 4.0f); // magx
adcData.voltages[ADC_VOLTS_MAGY] = adcData.channelSums[ADC_VOLTS_MAGY] * ADC_DIVISOR * (1.0f / 4.0f); // magy
adcData.voltages[ADC_VOLTS_MAGZ] = adcData.channelSums[ADC_VOLTS_MAGZ] * ADC_DIVISOR * (1.0f / 4.0f); // magz
adcData.voltages[ADC_VOLTS_TEMP1] = adcData.channelSums[ADC_VOLTS_TEMP1] * ADC_DIVISOR * (1.0f / 2.0f); // temp
adcData.voltages[ADC_VOLTS_VIN] = adcData.channelSums[ADC_VOLTS_VIN] * ADC_DIVISOR * (1.0f / 2.0f); // Vin
adcData.voltages[ADC_VOLTS_ACCX] = adcData.channelSums[ADC_VOLTS_ACCX] * ADC_DIVISOR * (1.0f / 8.0f); // accx
adcData.voltages[ADC_VOLTS_ACCY] = adcData.channelSums[ADC_VOLTS_ACCY] * ADC_DIVISOR * (1.0f / 8.0f); // accy
adcData.voltages[ADC_VOLTS_ACCZ] = adcData.channelSums[ADC_VOLTS_ACCZ] * ADC_DIVISOR * (1.0f / 8.0f); // accz
adcData.voltages[ADC_VOLTS_PRES1] = adcData.channelSums[ADC_VOLTS_PRES1] * ADC_DIVISOR * (1.0f / 8.0f); // press1
adcData.voltages[ADC_VOLTS_PRES2] = adcData.channelSums[ADC_VOLTS_PRES2] * ADC_DIVISOR * (1.0f / 8.0f); // press2
adcData.voltages[ADC_VOLTS_TEMP2] = adcData.channelSums[ADC_VOLTS_TEMP2] * ADC_DIVISOR * (1.0f / 2.0f); // temp2
adcData.voltages[ADC_VOLTS_TEMP3] = adcData.channelSums[ADC_VOLTS_TEMP3] * ADC_DIVISOR * (1.0f / 2.0f); // temp3
for (i = 0; i < ADC_SENSORS; i++) {
adcData.channelSums[i] = 0;
}
// temperature from IDG500
adcData.temp1 = adcData.temp1 * (1.0f - ADC_TEMP_SMOOTH) + (adcIDGVoltsToTemp(adcData.voltages[ADC_VOLTS_TEMP1]) + ADC_TEMP1_OFFSET) *
ADC_TEMP_SMOOTH;
// temperature from ISZ500
adcData.temp2 = adcData.temp2 * (1.0f - ADC_TEMP_SMOOTH) + (adcIDGVoltsToTemp(adcData.voltages[ADC_VOLTS_TEMP2]) + ADC_TEMP2_OFFSET) *
ADC_TEMP_SMOOTH;
// temperature from T1
adcData.temp3 = adcData.temp3 * (1.0f - ADC_TEMP_SMOOTH) + (adcT1VoltsToTemp(adcData.voltages[ADC_VOLTS_TEMP3]) + ADC_TEMP3_OFFSET) *
ADC_TEMP_SMOOTH;
adcData.temperature = adcData.temp3;
// temperature difference
dTemp = adcData.temperature - IMU_ROOM_TEMP;
dTemp2 = dTemp*dTemp;
dTemp3 = dTemp2*dTemp;
// rates
x = +(adcData.voltages[ADC_VOLTS_RATEX] + adcData.rateBiasX + p[IMU_GYO_BIAS1_X]*dTemp + p[IMU_GYO_BIAS2_X]*dTemp2 +
p[IMU_GYO_BIAS3_X]*dTemp3);// / p[IMU_GYO_SCAL_X];
y = -(adcData.voltages[ADC_VOLTS_RATEY] + adcData.rateBiasY + p[IMU_GYO_BIAS1_Y]*dTemp + p[IMU_GYO_BIAS2_Y]*dTemp2 +
p[IMU_GYO_BIAS3_Y]*dTemp3);// / p[IMU_GYO_SCAL_Y];
z = -(adcData.voltages[ADC_VOLTS_RATEZ] + adcData.rateBiasZ + p[IMU_GYO_BIAS1_Z]*dTemp + p[IMU_GYO_BIAS2_Z]*dTemp2 +
p[IMU_GYO_BIAS3_Z]*dTemp3);// / p[IMU_GYO_SCAL_Z];
a = x + y*p[IMU_GYO_ALGN_XY] + z*p[IMU_GYO_ALGN_XZ];
b = x*p[IMU_GYO_ALGN_YX] + y + z*p[IMU_GYO_ALGN_YZ];
c = x*p[IMU_GYO_ALGN_ZX] + y*p[IMU_GYO_ALGN_ZY] + z;
a /= p[IMU_GYO_SCAL_X];
b /= p[IMU_GYO_SCAL_Y];
c /= p[IMU_GYO_SCAL_Z];
adcData.rateX = a * imuData.cosRot - b * imuData.sinRot;
adcData.rateY = b * imuData.cosRot + a * imuData.sinRot;
adcData.rateZ = c;
// Vin
analogData.vIn = analogData.vIn * (1.0f - ADC_TEMP_SMOOTH) + adcVsenseToVin(adcData.voltages[ADC_VOLTS_VIN]) * ADC_TEMP_SMOOTH;
// ADXL335: bias
x = -(adcData.voltages[ADC_VOLTS_ACCX] + p[IMU_ACC_BIAS_X] + p[IMU_ACC_BIAS1_X]*dTemp + p[IMU_ACC_BIAS2_X]*dTemp2 +
p[IMU_ACC_BIAS3_X]*dTemp3);
y = +(adcData.voltages[ADC_VOLTS_ACCY] + p[IMU_ACC_BIAS_Y] + p[IMU_ACC_BIAS1_Y]*dTemp + p[IMU_ACC_BIAS2_Y]*dTemp2 +
p[IMU_ACC_BIAS3_X]*dTemp3);
z = -(adcData.voltages[ADC_VOLTS_ACCZ] + p[IMU_ACC_BIAS_Z] + p[IMU_ACC_BIAS1_Z]*dTemp + p[IMU_ACC_BIAS2_Z]*dTemp2 +
p[IMU_ACC_BIAS3_X]*dTemp3);
// misalignment
a = x + y*p[IMU_ACC_ALGN_XY] + z*p[IMU_ACC_ALGN_XZ];
b = x*p[IMU_ACC_ALGN_YX] + y + z*p[IMU_ACC_ALGN_YZ];
c = x*p[IMU_ACC_ALGN_ZX] + y*p[IMU_ACC_ALGN_ZY] + z;
// scale
a /= (p[IMU_ACC_SCAL_X] + p[IMU_ACC_SCAL1_X]*dTemp + p[IMU_ACC_SCAL2_X]*dTemp2 + p[IMU_ACC_SCAL3_X]*dTemp3);
b /= (p[IMU_ACC_SCAL_Y] + p[IMU_ACC_SCAL1_Y]*dTemp + p[IMU_ACC_SCAL2_Y]*dTemp2 + p[IMU_ACC_SCAL3_Y]*dTemp3);
c /= (p[IMU_ACC_SCAL_Z] + p[IMU_ACC_SCAL1_Z]*dTemp + p[IMU_ACC_SCAL2_Z]*dTemp2 + p[IMU_ACC_SCAL3_Z]*dTemp3);
adcData.accX = a * imuData.cosRot - b * imuData.sinRot;
adcData.accY = b * imuData.cosRot + a * imuData.sinRot;
adcData.accZ = c;
#ifdef ADC_PRESSURE_3V3
// MP3H61115A
adcData.pressure1 = (adcData.pressure1 + ((adcData.voltages[ADC_VOLTS_PRES1] + (0.095f * ADC_REF_VOLTAGE)) * (1000.0f /
(0.009f * ADC_REF_VOLTAGE)))) * 0.5f;
adcData.pressure2 = (adcData.pressure2 + ((adcData.voltages[ADC_VOLTS_PRES2] + (0.095f * ADC_REF_VOLTAGE)) * (1000.0f /
(0.009f * ADC_REF_VOLTAGE)))) * 0.5f;
#endif
#ifdef ADC_PRESSURE_5V
// MPXH6101A
adcData.pressure1 = (adcData.pressure1 + (((ADC_REF_VOLTAGE - adcData.voltages[ADC_VOLTS_PRES1]) *
(5.0f / ADC_REF_VOLTAGE)) + 0.54705f) / 0.05295f * 1000.0f) * 0.5f;
#endif
if (p[IMU_PRESS_SENSE] == 0.0f) {
adcData.pressure = adcData.pressure1;
} else if (p[IMU_PRESS_SENSE] == 1.0f) {
adcData.pressure = adcData.pressure2;
} else if (p[IMU_PRESS_SENSE] == 2.0f) {
adcData.pressure = (adcData.pressure1 + adcData.pressure2) * 0.5f;
}
// MAGS: bias
x = +((adcData.voltages[ADC_VOLTS_MAGX] - adcData.magBridgeBiasX) * (magSign ? -1 : 1) + p[IMU_MAG_BIAS_X] + p[IMU_MAG_BIAS1_X]*dTemp +
p[IMU_MAG_BIAS2_X]*dTemp2 + p[IMU_MAG_BIAS3_X]*dTemp3);
y = +((adcData.voltages[ADC_VOLTS_MAGY] - adcData.magBridgeBiasY) * (magSign ? -1 : 1) + p[IMU_MAG_BIAS_Y] + p[IMU_MAG_BIAS1_Y]*dTemp +
p[IMU_MAG_BIAS2_Y]*dTemp2 + p[IMU_MAG_BIAS3_Y]*dTemp3);
z = -((adcData.voltages[ADC_VOLTS_MAGZ] - adcData.magBridgeBiasZ) * (magSign ? -1 : 1) + p[IMU_MAG_BIAS_Z] + p[IMU_MAG_BIAS1_Z]*dTemp +
p[IMU_MAG_BIAS2_Z]*dTemp2 + p[IMU_MAG_BIAS3_Z]*dTemp3);
// store the mag sign used for this iteration
adcData.magSign = (magSign ? -1 : 1);
// misalignment
a = x + y*p[IMU_MAG_ALGN_XY] + z*p[IMU_MAG_ALGN_XZ];
b = x*p[IMU_MAG_ALGN_YX] + y + z*p[IMU_MAG_ALGN_YZ];
c = x*p[IMU_MAG_ALGN_ZX] + y*p[IMU_MAG_ALGN_ZY] + z;
// scale
a /= (p[IMU_MAG_SCAL_X] + p[IMU_MAG_SCAL1_X]*dTemp + p[IMU_MAG_SCAL2_X]*dTemp2 + p[IMU_MAG_SCAL3_X]*dTemp3);
b /= (p[IMU_MAG_SCAL_Y] + p[IMU_MAG_SCAL1_Y]*dTemp + p[IMU_MAG_SCAL2_Y]*dTemp2 + p[IMU_MAG_SCAL3_Y]*dTemp3);
c /= (p[IMU_MAG_SCAL_Z] + p[IMU_MAG_SCAL1_Z]*dTemp + p[IMU_MAG_SCAL2_Z]*dTemp2 + p[IMU_MAG_SCAL3_Z]*dTemp3);
adcData.magX = a * imuData.cosRot - b * imuData.sinRot;
adcData.magY = b * imuData.cosRot + a * imuData.sinRot;
adcData.magZ = c;
sumMagX += (double)adcData.voltages[ADC_VOLTS_MAGX];
sumMagY += (double)adcData.voltages[ADC_VOLTS_MAGY];
sumMagZ += (double)adcData.voltages[ADC_VOLTS_MAGZ];
countMag++;
adcData.dt = (float)(adcData.sampleTime * 2) * (1.0f / (float)AQ_US_PER_SEC);
adcData.lastUpdate = adcData.lastSample;
imuAdcSensorReady();
}
// flip our mag sign and try to refine bridge bias estimate
if (magSign != ADC_MAG_SIGN) {
magSign = ADC_MAG_SIGN;
if (magSign) {
adcData.magBridgeBiasX = sumMagX / countMag;
adcData.magBridgeBiasY = sumMagY / countMag;
adcData.magBridgeBiasZ = sumMagZ / countMag;
if (countMag > 100) {
sumMagX -= (double)(adcData.magBridgeBiasX * 2.0f);
sumMagY -= (double)(adcData.magBridgeBiasY * 2.0f);
sumMagZ -= (double)(adcData.magBridgeBiasZ * 2.0f);
countMag -= 2;
}
}
firstAfterFlip = 1;
}
}
}
void controlTaskCode(void *unused) {
float yaw;
float throttle;
float ratesDesired[3];
uint16_t overrides[3];
#ifdef USE_QUATOS
float quatDesired[4];
float ratesActual[3];
#else
float pitch, roll;
float pitchCommand, rollCommand, ruddCommand;
#endif // USE_QUATOS
AQ_NOTICE("Control task started\n");
// disable all axes' rate overrides
overrides[0] = 0;
overrides[1] = 0;
overrides[2] = 0;
while (1) {
// wait for work
CoWaitForSingleFlag(imuData.dRateFlag, 0);
// this needs to be done ASAP with the freshest of data
if (supervisorData.state & STATE_ARMED) {
if (RADIO_THROT > p[CTRL_MIN_THROT] || navData.mode > NAV_STATUS_MANUAL) {
supervisorThrottleUp(1);
// are we in altitude hold mode?
if (navData.mode > NAV_STATUS_MANUAL) {
// override throttle with nav's request
throttle = pidUpdate(navData.altSpeedPID, navData.holdSpeedAlt, -VELOCITYD) * MOTORS_SCALE / RADIO_MID_THROTTLE;
// don't allow negative throttle to be built up
if (navData.altSpeedPID->iState < 0.0f) {
navData.altSpeedPID->iState = 0.0f;
}
} else {
throttle = ((uint32_t)RADIO_THROT - p[CTRL_MIN_THROT]) * MOTORS_SCALE / RADIO_MID_THROTTLE * p[CTRL_FACT_THRO];
}
// limit
throttle = constrainInt(throttle, 1, MOTORS_SCALE);
// if motors are not yet running, use this heading as hold heading
if (motorsData.throttle == 0) {
navData.holdHeading = AQ_YAW;
controlData.yaw = navData.holdHeading;
// Reset all PIDs
pidZeroIntegral(controlData.pitchRatePID, 0.0f, 0.0f);
pidZeroIntegral(controlData.rollRatePID, 0.0f, 0.0f);
pidZeroIntegral(controlData.yawRatePID, 0.0f, 0.0f);
pidZeroIntegral(controlData.pitchAnglePID, 0.0f, 0.0f);
pidZeroIntegral(controlData.rollAnglePID, 0.0f, 0.0f);
pidZeroIntegral(controlData.yawAnglePID, 0.0f, 0.0f);
// also set this position as hold position
if (navData.mode == NAV_STATUS_POSHOLD) {
navUkfSetHereAsPositionTarget();
}
}
// constrict nav (only) yaw rates
yaw = compassDifference(controlData.yaw, navData.holdHeading);
yaw = constrainFloat(yaw, -p[CTRL_NAV_YAW_RT]/400.0f, +p[CTRL_NAV_YAW_RT]/400.0f);
controlData.yaw = compassNormalize(controlData.yaw + yaw);
// DVH overrides direct user pitch / roll requests
if (navData.mode != NAV_STATUS_DVH) {
controlData.userPitchTarget = RADIO_PITCH * p[CTRL_FACT_PITC];
controlData.userRollTarget = RADIO_ROLL * p[CTRL_FACT_ROLL];
} else {
controlData.userPitchTarget = 0.0f;
controlData.userRollTarget = 0.0f;
}
// navigation requests
if (navData.mode > NAV_STATUS_ALTHOLD) {
controlData.navPitchTarget = navData.holdTiltN;
controlData.navRollTarget = navData.holdTiltE;
} else {
controlData.navPitchTarget = 0.0f;
controlData.navRollTarget = 0.0f;
}
// manual rate cut through for yaw
if (RADIO_RUDD > p[CTRL_DEAD_BAND] || RADIO_RUDD < -p[CTRL_DEAD_BAND]) {
// fisrt remove dead band
if (RADIO_RUDD > p[CTRL_DEAD_BAND]) {
ratesDesired[2] = (RADIO_RUDD - p[CTRL_DEAD_BAND]);
} else {
ratesDesired[2] = (RADIO_RUDD + p[CTRL_DEAD_BAND]);
}
// calculate desired rate based on full stick scale
ratesDesired[2] = ratesDesired[2] * p[CTRL_MAN_YAW_RT] * DEG_TO_RAD * (1.0f / 700.0f);
// keep up with actual craft heading
controlData.yaw = AQ_YAW;
navData.holdHeading = AQ_YAW;
// request override
overrides[2] = CONTROL_MIN_YAW_OVERRIDE;
} else {
// currently overriding?
if (overrides[2] > 0) {
// request zero rate
ratesDesired[2] = 0.0f;
// follow actual craft heading
controlData.yaw = AQ_YAW;
navData.holdHeading = AQ_YAW;
// decrease override timer
overrides[2]--;
}
}
#ifdef USE_QUATOS
// determine which frame of reference to control from
if (navData.mode <= NAV_STATUS_ALTHOLD)
// craft frame - manual
{
eulerToQuatYPR(quatDesired, controlData.yaw, controlData.userPitchTarget, controlData.userRollTarget);
} else
// world frame - autonomous
{
eulerToQuatRPY(quatDesired, controlData.navRollTarget, controlData.navPitchTarget, controlData.yaw);
}
// reset controller on startup
if (motorsData.throttle == 0) {
quatDesired[0] = UKF_Q1;
quatDesired[1] = UKF_Q2;
quatDesired[2] = UKF_Q3;
quatDesired[3] = UKF_Q4;
quatosReset(quatDesired);
}
ratesActual[0] = IMU_DRATEX + UKF_GYO_BIAS_X;
ratesActual[1] = IMU_DRATEY + UKF_GYO_BIAS_Y;
ratesActual[2] = IMU_DRATEZ + UKF_GYO_BIAS_Z;
quatos(&UKF_Q1, quatDesired, ratesActual, ratesDesired, overrides);
quatosPowerDistribution(throttle);
motorsSendThrust();
motorsData.throttle = throttle;
#else
// smooth
controlData.userPitchTarget = utilFilter3(controlData.userPitchFilter, controlData.userPitchTarget);
controlData.userRollTarget = utilFilter3(controlData.userRollFilter, controlData.userRollTarget);
// smooth
controlData.navPitchTarget = utilFilter3(controlData.navPitchFilter, controlData.navPitchTarget);
controlData.navRollTarget = utilFilter3(controlData.navRollFilter, controlData.navRollTarget);
// rotate nav's NE frame of reference to our craft's local frame of reference
pitch = controlData.navPitchTarget * navUkfData.yawCos - controlData.navRollTarget * navUkfData.yawSin;
roll = controlData.navRollTarget * navUkfData.yawCos + controlData.navPitchTarget * navUkfData.yawSin;
// combine nav & user requests (both are already smoothed)
controlData.pitch = pitch + controlData.userPitchTarget;
controlData.roll = roll + controlData.userRollTarget;
if (p[CTRL_PID_TYPE] == 0) {
// pitch angle
pitchCommand = pidUpdate(controlData.pitchAnglePID, controlData.pitch, AQ_PITCH);
// rate
pitchCommand += pidUpdate(controlData.pitchRatePID, 0.0f, IMU_DRATEY);
// roll angle
rollCommand = pidUpdate(controlData.rollAnglePID, controlData.roll, AQ_ROLL);
// rate
rollCommand += pidUpdate(controlData.rollRatePID, 0.0f, IMU_DRATEX);
} else if (p[CTRL_PID_TYPE] == 1) {
// pitch rate from angle
pitchCommand = pidUpdate(controlData.pitchRatePID, pidUpdate(controlData.pitchAnglePID, controlData.pitch, AQ_PITCH), IMU_DRATEY);
// roll rate from angle
rollCommand = pidUpdate(controlData.rollRatePID, pidUpdate(controlData.rollAnglePID, controlData.roll, AQ_ROLL), IMU_DRATEX);
}
else if (p[CTRL_PID_TYPE] == 2) {
// pitch angle
pitchCommand = pidUpdate(controlData.pitchAnglePID, controlData.pitch, AQ_PITCH);
// rate
pitchCommand += pidUpdate(controlData.pitchRatePID, 0.0f, IMU_DRATEY);
int axis = 0; // ROLL
float PID_P = 3.7;
float PID_I = 0.031;
float PID_D = 23.0;
float error, errorAngle, AngleRateTmp, RateError, delta, deltaSum;
float PTerm, ITerm, PTermACC = 0, ITermACC = 0, PTermGYRO = 0, ITermGYRO = 0, DTerm;
static int16_t lastGyro[3] = { 0, 0, 0 };
static float delta1[3], delta2[3];
static float errorGyroI[3] = { 0, 0, 0 }, errorAngleI[2] = { 0, 0 };
static float lastError[3] = { 0, 0, 0 }, lastDTerm[3] = { 0, 0, 0 }; // pt1 element http://www.multiwii.com/forum/viewtopic.php?f=23&t=2624;
static int16_t axisPID[3];
static float rollPitchRate = 0.0;
static float newpidimax = 0.0;
float dT;
uint8_t ANGLE_MODE = 0;
uint8_t HORIZON_MODE = 0;
uint16_t cycleTime = IMU_LASTUPD -
controlData.lastUpdate; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop
if ((ANGLE_MODE || HORIZON_MODE)) { // MODE relying on ACC
errorAngle = constrainFloat(2.0f * (float)controlData.roll, -500.0f, +500.0f) - AQ_ROLL;
}
if (!ANGLE_MODE) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID
AngleRateTmp = (float)(rollPitchRate + 27) * (float)controlData.roll *
0.0625f; // AngleRateTmp = ((int32_t) (cfg.rollPitchRate + 27) * rcCommand[axis]) >> 4;
if (HORIZON_MODE) {
AngleRateTmp += PID_I * errorAngle *
0.0390625f; //increased by x10 //0.00390625f AngleRateTmp += (errorAngle * (float)cfg.I8[PIDLEVEL]) >> 8;
}
} else { // it's the ANGLE mode - control is angle based, so control loop is needed
AngleRateTmp = PID_P * errorAngle * 0.0223214286f; // AngleRateTmp = (errorAngle * (float)cfg.P8[PIDLEVEL]) >> 4; * LevelPprescale;
}
RateError = AngleRateTmp - IMU_DRATEX;
PTerm = PID_P * RateError * 0.0078125f;
errorGyroI[axis] += PID_I * RateError * (float)cycleTime / 2048.0f;
errorGyroI[axis] = constrainFloat(errorGyroI[axis], -newpidimax, newpidimax);
ITerm = errorGyroI[axis] / 8192.0f;
delta = RateError - lastError[axis];
lastError[axis] = RateError;
delta = delta * 16383.75f / (float)cycleTime;
deltaSum = delta1[axis] + delta2[axis] + delta;
delta2[axis] = delta1[axis];
delta1[axis] = delta;
DTerm = PID_D * deltaSum * 0.00390625f;
axisPID[axis] = PTerm + ITerm + DTerm;
rollCommand = AngleRateTmp;
}
else {
pitchCommand = 0.0f;
rollCommand = 0.0f;
ruddCommand = 0.0f;
}
// yaw rate override?
if (overrides[2] > 0)
// manual yaw rate
{
ruddCommand = pidUpdate(controlData.yawRatePID, ratesDesired[2], IMU_DRATEZ);
} else
// seek a 0 deg difference between hold heading and actual yaw
{
ruddCommand = pidUpdate(controlData.yawRatePID, pidUpdate(controlData.yawAnglePID, 0.0f, compassDifference(controlData.yaw, AQ_YAW)),
IMU_DRATEZ);
}
rollCommand = constrainFloat(rollCommand, -p[CTRL_MAX], p[CTRL_MAX]);
pitchCommand = constrainFloat(pitchCommand, -p[CTRL_MAX], p[CTRL_MAX]);
ruddCommand = constrainFloat(ruddCommand, -p[CTRL_MAX], p[CTRL_MAX]);
motorsCommands(throttle, pitchCommand, rollCommand, ruddCommand);
#endif
}
// no throttle input
else {
supervisorThrottleUp(0);
motorsOff();
}
}
// not armed
else {
motorsOff();
}
controlData.lastUpdate = IMU_LASTUPD;
controlData.loops++;
}
}
/**
*******************************************************************************
* @brief lcd_blink task
* @param[in] pdata A pointer to parameter passed to task.
* @param[out] None
* @retval None
*
* @details This task use to blink lcd when setting time.
*******************************************************************************
*/
void lcd_blink (void *pdata)
{
char flag = 0;
pdata = pdata;
for (;;)
{
CoWaitForSingleFlag(lcd_blink_flg,0);
CoEnterMutexSection(mut_lcd);
set_cursor(7, 0);
lcd_print (chart);
switch (timeflag)
{
case 0:
break;
case 1:
set_cursor (13, 0);
if (flag == 0)
{
lcd_putchar (0);
lcd_putchar (0);
flag = 1;
}
else
{
lcd_putchar (chart[6]);
lcd_putchar (chart[7]);
flag = 0;
}
break;
case 2:
set_cursor (10, 0);
if (flag == 0)
{
lcd_putchar (0);
lcd_putchar (0);
flag = 1;
}
else
{
lcd_putchar (chart[3]);
lcd_putchar (chart[4]);
flag = 0;
}
break;
case 3:
set_cursor (7, 0);
if (flag == 0)
{
lcd_putchar (0);
lcd_putchar (0);
flag = 1;
}
else
{
lcd_putchar (chart[0]);
lcd_putchar (chart[1]);
flag = 0;
}
break;
default: break;
};
CoLeaveMutexSection(mut_lcd);
CoTickDelay (55);
}
}
