void radiolinkSyslinkDispatch(SyslinkPacket *slp)
{
static SyslinkPacket txPacket;
if (slp->type == SYSLINK_RADIO_RAW)
{
slp->length--; // Decrease to get CRTP size.
xQueueSend(crtpPacketDelivery, &slp->length, 0);
ledseqRun(LINK_LED, seq_linkup);
// If a radio packet is received, one can be sent
if (xQueueReceive(txQueue, &txPacket, 0) == pdTRUE)
{
ledseqRun(LINK_DOWN_LED, seq_linkup);
syslinkSendPacket(&txPacket);
}
} else if (slp->type == SYSLINK_RADIO_RAW_BROADCAST)
{
slp->length--; // Decrease to get CRTP size.
xQueueSend(crtpPacketDelivery, &slp->length, 0);
ledseqRun(LINK_LED, seq_linkup);
// no ack for broadcasts
} else if (slp->type == SYSLINK_RADIO_RSSI)
{
//Extract RSSI sample sent from radio
memcpy(&rssi, slp->data, sizeof(uint8_t));
}
}
void systemTask(void *arg) {
bool pass = true;
//Init the high-levels modules
systemInit();
#ifndef USE_UART_CRTP
#ifdef UART_OUTPUT_TRACE_DATA
debugInitTrace();
#endif
#ifdef HAS_UART
uartInit();
#endif
#endif //ndef USE_UART_CRTP
commInit();
DEBUG_PRINT("Crazyflie is up and running!\n");
DEBUG_PRINT("Build %s:%s (%s) %s\n", V_SLOCAL_REVISION, V_SREVISION, V_STAG, (V_MODIFIED) ? "MODIFIED" : "CLEAN");
DEBUG_PRINT("I am 0x%X%X%X and I have %dKB of flash!\n", *((int* )(0x1FFFF7E8 + 8)), *((int* )(0x1FFFF7E8 + 4)), *((int* )(0x1FFFF7E8 + 0)), *((short* )(0x1FFFF7E0)));
commanderInit();
stabilizerInit();
//Test the modules
pass &= systemTest();
pass &= commTest();
pass &= commanderTest();
pass &= stabilizerTest();
//Start the firmware
if (pass) {
systemStart();
ledseqRun(LED_RED, seq_alive);
ledseqRun(LED_GREEN, seq_testPassed);
} else {
if (systemTest()) {
while (1) {
ledseqRun(LED_RED, seq_testPassed); //Red passed == not passed!
vTaskDelay(M2T(2000) );
}
} else {
ledInit();
ledSet(LED_RED, true);
}
}
workerLoop();
//Should never reach this point!
while (1)
vTaskDelay(portMAX_DELAY);
}
void systemTask(void *arg)
{
bool pass = true;
/* Init the high-levels modules */
systemInit();
uartInit();
commInit();
stabilizerInit();
//Test the modules
pass &= systemTest();
pass &= commTest();
// pass &= commanderTest();
pass &= stabilizerTest();
if (pass)
{
systemStart();
ledseqRun(LED_RED, seq_alive);
ledseqRun(LED_GREEN, seq_testPassed);
}
else
{
if (systemTest())
{
while(1)
{
ledseqRun(LED_RED, seq_testPassed); //Red passed == not passed!
vTaskDelay(M2T(2000));
}
}
else
{
ledInit();
ledSet(LED_RED, true);
}
}
pmSetChargeState(charge500mA);
//Should never reach this point!
while(1)
vTaskDelay(portMAX_DELAY);
}
void imu6Read(Axis3f* gyroOut, Axis3f* accOut)
{
mpu6500GetMotion6(&accelMpu.y, &accelMpu.x, &accelMpu.z, &gyroMpu.y, &gyroMpu.x, &gyroMpu.z);
imuAddBiasValue(&gyroBias, &gyroMpu);
#ifdef IMU_TAKE_ACCEL_BIAS
if (!accelBias.isBiasValueFound)
{
imuAddBiasValue(&accelBias, &accelMpu);
}
#endif
if (!gyroBias.isBiasValueFound)
{
imuFindBiasValue(&gyroBias);
if (gyroBias.isBiasValueFound)
{
soundSetEffect(SND_CALIB);
ledseqRun(SYS_LED, seq_calibrated);
}
}
#ifdef IMU_TAKE_ACCEL_BIAS
if (gyroBias.isBiasValueFound &&
!accelBias.isBiasValueFound)
{
Axis3i32 mean;
imuCalculateBiasMean(&accelBias, &mean);
accelBias.bias.x = mean.x;
accelBias.bias.y = mean.y;
accelBias.bias.z = mean.z - IMU_1G_RAW;
accelBias.isBiasValueFound = true;
}
#endif
imuAccIIRLPFilter(&accelMpu, &accelLPF, &accelStoredFilterValues,
(int32_t)imuAccLpfAttFactor);
imuAccAlignToGravity(&accelLPF, &accelLPFAligned);
// Re-map outputs
gyroOut->x = -(gyroMpu.x - gyroBias.bias.x) * IMU_DEG_PER_LSB_CFG;
gyroOut->y = (gyroMpu.y - gyroBias.bias.y) * IMU_DEG_PER_LSB_CFG;
gyroOut->z = (gyroMpu.z - gyroBias.bias.z) * IMU_DEG_PER_LSB_CFG;
#ifdef IMU_TAKE_ACCEL_BIAS
accOut->x = (accelLPFAligned.x - accelBias.bias.x) * IMU_G_PER_LSB_CFG;
accOut->y = (accelLPFAligned.y - accelBias.bias.y) * IMU_G_PER_LSB_CFG;
accOut->z = (accelLPFAligned.z - accelBias.bias.z) * IMU_G_PER_LSB_CFG;
#else
accOut->x = -(accelLPFAligned.x) * IMU_G_PER_LSB_CFG;
accOut->y = (accelLPFAligned.y) * IMU_G_PER_LSB_CFG;
accOut->z = (accelLPFAligned.z) * IMU_G_PER_LSB_CFG;
#endif
}
static int usblinkReceiveCRTPPacket(CRTPPacket *p)
{
if (xQueueReceive(crtpPacketDelivery, p, M2T(100)) == pdTRUE)
{
ledseqRun(LINK_LED, seq_linkup);
return 0;
}
return -1;
}
void imu6Read(Axis3f* gyroOut, Axis3f* accOut)
{
mpu6050GetMotion6(&accelMpu.x, &accelMpu.y, &accelMpu.z, &gyroMpu.x, &gyroMpu.y, &gyroMpu.z);
imuAddBiasValue(&gyroBias, &gyroMpu);
if (!accelBias.isBiasValueFound)
{
imuAddBiasValue(&accelBias, &accelMpu);
}
if (!gyroBias.isBiasValueFound)
{
imuFindBiasValue(&gyroBias);
if (gyroBias.isBiasValueFound)
{
ledseqRun(LED_RED, seq_calibrated);
// uartPrintf("Gyro bias: %i, %i, %i\n",
// gyroBias.bias.x, gyroBias.bias.y, gyroBias.bias.z);
}
}
#ifdef IMU_TAKE_ACCEL_BIAS
if (gyroBias.isBiasValueFound &&
!accelBias.isBiasValueFound)
{
Axis3i32 mean;
imuCalculateBiasMean(&accelBias, &mean);
accelBias.bias.x = mean.x;
accelBias.bias.y = mean.y;
accelBias.bias.z = mean.z - IMU_1G_RAW;
accelBias.isBiasValueFound = TRUE;
//uartPrintf("Accel bias: %i, %i, %i\n",
// accelBias.bias.x, accelBias.bias.y, accelBias.bias.z);
}
#endif
imuAccIIRLPFilter(&accelMpu, &accelLPF, &accelStoredFilterValues,
(int32_t)imuAccLpfAttFactor);
imuAccAlignToGravity(&accelLPF, &accelLPFAligned);
// Re-map outputs
gyroOut->x = (gyroMpu.x - gyroBias.bias.x) * IMU_DEG_PER_LSB_CFG;
gyroOut->y = (gyroMpu.y - gyroBias.bias.y) * IMU_DEG_PER_LSB_CFG;
gyroOut->z = (gyroMpu.z - gyroBias.bias.z) * IMU_DEG_PER_LSB_CFG;
accOut->x = (accelLPFAligned.x - accelBias.bias.x) * IMU_G_PER_LSB_CFG;
accOut->y = (accelLPFAligned.y - accelBias.bias.y) * IMU_G_PER_LSB_CFG;
accOut->z = (accelLPFAligned.z - accelBias.bias.z) * IMU_G_PER_LSB_CFG;
}
static void wifilinkTask(void * arg)
{
// wifiLinkInit();
// unsigned char id;
//
// esp8266EnableMultiId(ENABLE);
//// esp8266StartOrShutServer(1, 8080, 3000);
//// id = esp8233CIPStatus();
//
// while(!(esp8233LinkServer("TCP", "192.168.1.110", 8080, 0) ||
// esp8233LinkServer("TCP", "192.168.1.110", 8080, 1) ||
// esp8233LinkServer("TCP", "192.168.1.110", 8080, 2) ||
// esp8233LinkServer("TCP", "192.168.1.110", 8080, 3) ||
// esp8233LinkServer("TCP", "192.168.1.110", 8080, 4)))
// {
// vTaskDelay(200);
// }
/* multiLink */
esp8266EnableMultiId(ENABLE);
/* creat server */
while(!esp8266StartOrShutServer(ENABLE, 8080, 2000));
ledseqRun(LED_GREEN, seq_linkup);
while(1) {
memset(wifiRecvData, 0, sizeof(wifiRecvData));
esp8266GetData(wifiRecvData);
if(strstr((const char *)wifiRecvData, "CONNECT"))
break;
vTaskDelay(200);
}
memset(wifiRecvData, 0, sizeof(wifiRecvData));
while(1)
{
if(esp8266ReceiveData(0, wifiRecvData, &recvLen)) {
// handle the cmd
if(dataHandler(wifiRecvData, wifiSendData, &sendLen))
//ack
esp8266SendData(0, 0, wifiSendData, sendLen);
}
vTaskDelay(500);
}
}
static int usblinkSendPacket(CRTPPacket *p)
{
int dataSize;
ASSERT(p->size < SYSLINK_MTU);
sendBuffer[0] = p->header;
if (p->size <= CRTP_MAX_DATA_SIZE)
{
memcpy(&sendBuffer[1], p->data, p->size);
}
dataSize = p->size + 1;
ledseqRun(LINK_DOWN_LED, seq_linkup);
return usbSendData(dataSize, sendBuffer);
}
/**
* Calculates the bias first when the gyro variance is below threshold. Requires a buffer
* but calibrates platform first when it is stable.
*/
static bool processGyroBias(int16_t gx, int16_t gy, int16_t gz, Axis3f *gyroBiasOut)
{
sensorsAddBiasValue(&gyroBiasRunning, gx, gy, gz);
if (!gyroBiasRunning.isBiasValueFound)
{
sensorsFindBiasValue(&gyroBiasRunning);
if (gyroBiasRunning.isBiasValueFound)
{
soundSetEffect(SND_CALIB);
ledseqRun(SYS_LED, seq_calibrated);
}
}
gyroBiasOut->x = gyroBiasRunning.bias.x;
gyroBiasOut->y = gyroBiasRunning.bias.y;
gyroBiasOut->z = gyroBiasRunning.bias.z;
return gyroBiasRunning.isBiasValueFound;
}
static void eskylinkTask(void * arg)
{
int channel = 7;
int channel1 = -1; //As long as channel1<0 the copter is in scann mode
int channel2 = 0;
//Waiting for pairing packet
while (!state.paired)
{
xSemaphoreTake(dataRdy, portMAX_DELAY);
ledseqRun(LED_GREEN, seq_linkup);
eskylinkFetchData(packet, 13);
if (packet[4]==0x18 && packet[5]==0x29)
{
address[2]=packet[0];
address[1]=packet[1];
address[0]=packet[2];
state.band = packet[3];
state.paired = true;
}
}
ledseqRun(LED_GREEN, seq_testPassed);
nrfSetEnable(false);
eskylinkInitPaired(channel);
nrfSetEnable(true);
//Paired! handling packets.
while(1)
{
if (xSemaphoreTake(dataRdy, M2T(10))==pdTRUE)
{
ledseqRun(LED_GREEN, seq_linkup);
eskylinkFetchData(packet, 13);
eskylinkDecode(packet);
if (channel1<0) //Channels found!
{
channel1 = channel;
channel2 = channel1+37;
if (channel2>83) channel2 = channel1 - 37;
}
}
else
{
if (channel1<0)
{
channel++;
if(channel>83) channel=7;
nrfSetEnable(false);
nrfSetChannel(channel);
nrfSetEnable(true);
}
else
{
if (channel == channel1)
channel = channel2;
else
channel = channel1;
nrfSetEnable(false);
nrfSetChannel(channel);
nrfSetEnable(true);
}
}
}
}
static void sensorsTask(void *param)
{
systemWaitStart();
uint32_t lastWakeTime = xTaskGetTickCount();
static BiasObj bmi160GyroBias;
static BiasObj bmi055GyroBias;
#ifdef SENSORS_TAKE_ACCEL_BIAS
static BiasObj bmi160AccelBias;
static BiasObj bmi055AccelBias;
#endif
Axis3i16 gyroPrim;
Axis3i16 accelPrim;
Axis3f accelPrimScaled;
Axis3i16 accelPrimLPF;
Axis3i32 accelPrimStoredFilterValues;
#ifdef LOG_SEC_IMU
Axis3i16 gyroSec;
Axis3i16 accelSec;
Axis3f accelSecScaled;
Axis3i16 accelSecLPF;
Axis3i32 accelSecStoredFilterValues;
#endif /* LOG_SEC_IMU */
/* wait an additional second the keep bus free
* this is only required by the z-ranger, since the
* configuration will be done after system start-up */
//vTaskDelayUntil(&lastWakeTime, M2T(1500));
while (1)
{
vTaskDelayUntil(&lastWakeTime, F2T(SENSORS_READ_RATE_HZ));
/* calibrate if necessary */
if (!allSensorsAreCalibrated)
{
if (!bmi160GyroBias.found) {
sensorsGyroCalibrate(&bmi160GyroBias, SENSORS_BMI160);
#ifdef SENSORS_TAKE_ACCEL_BIAS
sensorsAccelCalibrate(&bmi160AccelBias,
&bmi160GyroBias, SENSORS_BMI160);
#endif
}
if (!bmi055GyroBias.found)
{
sensorsGyroCalibrate(&bmi055GyroBias, SENSORS_BMI055);
#ifdef SENSORS_TAKE_ACCEL_BIAS
sensorsAccelCalibrate(&bmi055AccelBias,
&bmi055GyroBias, SENSORS_BMI055);
#endif
}
if ( bmi160GyroBias.found && bmi055GyroBias.found
#ifdef SENSORS_TAKE_ACCEL_BIAS
&& bmi160AccelBias.found && bmi055AccelBias.found
#endif
)
{
// soundSetEffect(SND_CALIB);
DEBUG_PRINT("Sensor calibration [OK].\n");
ledseqRun(SYS_LED, seq_calibrated);
allSensorsAreCalibrated= true;
}
}
else {
/* get data from chosen sensors */
sensorsGyroGet(&gyroPrim, gyroPrimInUse);
sensorsAccelGet(&accelPrim, accelPrimInUse);
#ifdef LOG_SEC_IMU
sensorsGyroGet(&gyroSec, gyroSecInUse);
sensorsAccelGet(&accelSec, accelSecInUse);
#endif
/* FIXME: for sensor deck v1 realignment has to be added her */
switch(gyroPrimInUse) {
case SENSORS_BMI160:
sensorsApplyBiasAndScale(&sensors.gyro, &gyroPrim,
&bmi160GyroBias.value,
SENSORS_BMI160_DEG_PER_LSB_CFG);
break;
case SENSORS_BMI055:
sensorsApplyBiasAndScale(&sensors.gyro, &gyroPrim,
&bmi055GyroBias.value,
SENSORS_BMI055_DEG_PER_LSB_CFG);
break;
}
sensorsAccIIRLPFilter(&accelPrim, &accelPrimLPF,
&accelPrimStoredFilterValues,
(int32_t)sensorsAccLpfAttFactor);
switch(accelPrimInUse) {
case SENSORS_BMI160:
sensorsApplyBiasAndScale(&accelPrimScaled, &accelPrimLPF,
&bmi160AccelBias.value,
SENSORS_BMI160_G_PER_LSB_CFG);
break;
case SENSORS_BMI055:
sensorsApplyBiasAndScale(&accelPrimScaled, &accelPrimLPF,
&bmi055AccelBias.value,
SENSORS_BMI055_G_PER_LSB_CFG);
break;
}
sensorsAccAlignToGravity(&accelPrimScaled, &sensors.acc);
#ifdef LOG_SEC_IMU
switch(gyroSecInUse) {
case SENSORS_BMI160:
sensorsApplyBiasAndScale(&sensors.gyroSec, &gyroSec,
&bmi160GyroBias.value,
SENSORS_BMI160_DEG_PER_LSB_CFG);
break;
case SENSORS_BMI055:
sensorsApplyBiasAndScale(&sensors.gyroSec, &gyroSec,
&bmi055GyroBias.value,
SENSORS_BMI055_DEG_PER_LSB_CFG);
break;
}
sensorsAccIIRLPFilter(&accelSec, &accelSecLPF,
&accelSecStoredFilterValues,
(int32_t)sensorsAccLpfAttFactor);
switch(accelSecInUse) {
case SENSORS_BMI160:
sensorsApplyBiasAndScale(&accelSecScaled, &accelSecLPF,
&bmi160AccelBias.value,
SENSORS_BMI160_G_PER_LSB_CFG);
break;
case SENSORS_BMI055:
sensorsApplyBiasAndScale(&accelSecScaled, &accelSecLPF,
&bmi055AccelBias.value,
SENSORS_BMI055_G_PER_LSB_CFG);
break;
}
sensorsAccAlignToGravity(&accelSecScaled, &sensors.accSec);
#endif
}
if (isMagnetometerPresent)
{
static uint8_t magMeasDelay = SENSORS_DELAY_MAG;
if (--magMeasDelay == 0)
{
bmm150_read_mag_data(&bmm150Dev);
sensors.mag.x = bmm150Dev.data.x;
sensors.mag.y = bmm150Dev.data.y;
sensors.mag.z = bmm150Dev.data.z;
magMeasDelay = SENSORS_DELAY_MAG;
}
}
if (isBarometerPresent)
{
static uint8_t baroMeasDelay = SENSORS_DELAY_BARO;
static int32_t v_temp_s32;
static uint32_t v_pres_u32;
static baro_t* baro280 = &sensors.baro;
if (--baroMeasDelay == 0)
{
bmp280_read_pressure_temperature(&v_pres_u32, &v_temp_s32);
sensorsScaleBaro(baro280, (float)v_pres_u32, (float)v_temp_s32/100.0f);
baroMeasDelay = baroMeasDelayMin;
}
}
/* ensure all queues are populated at the same time */
vTaskSuspendAll();
xQueueOverwrite(accelPrimDataQueue, &sensors.acc);
xQueueOverwrite(gyroPrimDataQueue, &sensors.gyro);
#ifdef LOG_SEC_IMU
xQueueOverwrite(gyroSecDataQueue, &sensors.gyroSec);
xQueueOverwrite(accelSecDataQueue, &sensors.accSec);
#endif
if (isBarometerPresent)
{
xQueueOverwrite(baroPrimDataQueue, &sensors.baro);
}
if (isMagnetometerPresent)
{
xQueueOverwrite(magPrimDataQueue, &sensors.mag);
}
xTaskResumeAll();
}
}
static void stabilizerTask(void* param)
{
uint32_t attitudeCounter = 0;
uint32_t altHoldCounter = 0;
uint32_t lastWakeTime;
vTaskSetApplicationTaskTag(0, (void*)TASK_STABILIZER_ID_NBR);
//Wait for the system to be fully started to start stabilization loop
systemWaitStart();
ledseqRun(SYS_LED, seq_testPassed);
lastWakeTime = xTaskGetTickCount ();
while(1)
{
vTaskDelayUntil(&lastWakeTime, F2T(IMU_UPDATE_FREQ)); // 500Hz
// Magnetometer not yet used more then for logging.
imu9Read(&gyro, &acc, &mag);
if (imu6IsCalibrated())
{
commanderGetRPY(&eulerRollDesired, &eulerPitchDesired, &eulerYawDesired);
commanderGetRPYType(&rollType, &pitchType, &yawType);
// 250HZ
if (++attitudeCounter >= ATTITUDE_UPDATE_RATE_DIVIDER)
{
sensfusion6UpdateQ(gyro.x, gyro.y, gyro.z, acc.x, acc.y, acc.z, FUSION_UPDATE_DT);
sensfusion6GetEulerRPY(&eulerRollActual, &eulerPitchActual, &eulerYawActual);
accWZ = sensfusion6GetAccZWithoutGravity(acc.x, acc.y, acc.z);
accMAG = (acc.x*acc.x) + (acc.y*acc.y) + (acc.z*acc.z);
// Estimate speed from acc (drifts)
vSpeed += deadband(accWZ, vAccDeadband) * FUSION_UPDATE_DT;
controllerCorrectAttitudePID(eulerRollActual, eulerPitchActual, eulerYawActual,
eulerRollDesired, eulerPitchDesired, -eulerYawDesired,
&rollRateDesired, &pitchRateDesired, &yawRateDesired);
attitudeCounter = 0;
}
// 100HZ
if (imuHasBarometer() && (++altHoldCounter >= ALTHOLD_UPDATE_RATE_DIVIDER))
{
stabilizerAltHoldUpdate();
altHoldCounter = 0;
}
if (rollType == RATE)
{
rollRateDesired = eulerRollDesired;
}
if (pitchType == RATE)
{
pitchRateDesired = eulerPitchDesired;
}
if (yawType == RATE)
{
yawRateDesired = -eulerYawDesired;
}
// TODO: Investigate possibility to subtract gyro drift.
controllerCorrectRatePID(gyro.x, -gyro.y, gyro.z,
rollRateDesired, pitchRateDesired, yawRateDesired);
controllerGetActuatorOutput(&actuatorRoll, &actuatorPitch, &actuatorYaw);
if (!altHold || !imuHasBarometer())
{
// Use thrust from controller if not in altitude hold mode
commanderGetThrust(&actuatorThrust);
}
else
{
// Added so thrust can be set to 0 while in altitude hold mode after disconnect
commanderWatchdog();
}
if (actuatorThrust > 0)
{
#if defined(TUNE_ROLL)
distributePower(actuatorThrust, actuatorRoll, 0, 0);
#elif defined(TUNE_PITCH)
distributePower(actuatorThrust, 0, actuatorPitch, 0);
#elif defined(TUNE_YAW)
distributePower(actuatorThrust, 0, 0, -actuatorYaw);
#else
distributePower(actuatorThrust, actuatorRoll, actuatorPitch, -actuatorYaw);
#endif
}
else
{
distributePower(0, 0, 0, 0);
controllerResetAllPID();
}
}
}
}
void systemTask(void *arg)
{
bool pass = true;
ledInit();
ledSet(CHG_LED, 1);
uartInit();
//Init the high-levels modules
systemInit();
#ifndef USE_RADIOLINK_CRTP
#ifdef UART_OUTPUT_TRACE_DATA
//debugInitTrace();
#endif
#ifdef ENABLE_UART
// uartInit();
#endif
#endif //ndef USE_RADIOLINK_CRTP
commInit();
DEBUG_PRINT("----------------------------\n");
DEBUG_PRINT("Crazyflie is up and running!\n");
DEBUG_PRINT("Build %s:%s (%s) %s\n", V_SLOCAL_REVISION,
V_SREVISION, V_STAG, (V_MODIFIED)?"MODIFIED":"CLEAN");
DEBUG_PRINT("I am 0x%X%X%X and I have %dKB of flash!\n",
*((int*)(MCU_ID_ADDRESS+8)), *((int*)(MCU_ID_ADDRESS+4)),
*((int*)(MCU_ID_ADDRESS+0)), *((short*)(MCU_FLASH_SIZE_ADDRESS)));
commanderInit();
stabilizerInit();
expbrdInit();
memInit();
//Test the modules
pass &= systemTest();
pass &= configblockTest();
pass &= commTest();
pass &= commanderTest();
pass &= stabilizerTest();
pass &= expbrdTest();
pass &= memTest();
//Start the firmware
if(pass)
{
selftestPassed = 1;
systemStart();
ledseqRun(SYS_LED, seq_alive);
ledseqRun(LINK_LED, seq_testPassed);
}
else
{
selftestPassed = 0;
if (systemTest())
{
while(1)
{
ledseqRun(SYS_LED, seq_testPassed); //Red passed == not passed!
vTaskDelay(M2T(2000));
// System can be forced to start by setting the param to 1 from the cfclient
if (selftestPassed)
{
DEBUG_PRINT("Start forced.\n");
systemStart();
break;
}
}
}
else
{
ledInit();
ledSet(SYS_LED, true);
}
}
DEBUG_PRINT("Free heap: %d bytes\n", xPortGetFreeHeapSize());
workerLoop();
//Should never reach this point!
while(1)
vTaskDelay(portMAX_DELAY);
}
void systemTask(void *arg)
{
bool pass = true;
ledInit();
ledSet(CHG_LED, 1);
#ifdef DEBUG_QUEUE_MONITOR
queueMonitorInit();
#endif
#ifdef ENABLE_UART1
uart1Init();
#endif
#ifdef ENABLE_UART2
uart2Init();
#endif
//Init the high-levels modules
systemInit();
commInit();
commanderInit();
StateEstimatorType estimator = anyEstimator;
deckInit();
estimator = deckGetRequiredEstimator();
stabilizerInit(estimator);
if (deckGetRequiredLowInterferenceRadioMode())
{
platformSetLowInterferenceRadioMode();
}
soundInit();
memInit();
#ifdef PROXIMITY_ENABLED
proximityInit();
#endif
//Test the modules
pass &= systemTest();
pass &= configblockTest();
pass &= commTest();
pass &= commanderTest();
pass &= stabilizerTest();
pass &= deckTest();
pass &= soundTest();
pass &= memTest();
pass &= watchdogNormalStartTest();
//Start the firmware
if(pass)
{
selftestPassed = 1;
systemStart();
soundSetEffect(SND_STARTUP);
ledseqRun(SYS_LED, seq_alive);
ledseqRun(LINK_LED, seq_testPassed);
}
else
{
selftestPassed = 0;
if (systemTest())
{
while(1)
{
ledseqRun(SYS_LED, seq_testPassed); //Red passed == not passed!
vTaskDelay(M2T(2000));
// System can be forced to start by setting the param to 1 from the cfclient
if (selftestPassed)
{
DEBUG_PRINT("Start forced.\n");
systemStart();
break;
}
}
}
else
{
ledInit();
ledSet(SYS_LED, true);
}
}
DEBUG_PRINT("Free heap: %d bytes\n", xPortGetFreeHeapSize());
workerLoop();
//Should never reach this point!
while(1)
vTaskDelay(portMAX_DELAY);
}
void imu6Read(Axis3f* gyroOut, Axis3f* accOut)
{
mpu6050GetMotion6(&accelMpu.x, &accelMpu.y, &accelMpu.z, &gyroMpu.x, &gyroMpu.y, &gyroMpu.z);
imuAddBiasValue(&gyroBias, &gyroMpu);
if (!accelBias.isBiasValueFound)
{
imuAddBiasValue(&accelBias, &accelMpu);
}
if (!gyroBias.isBiasValueFound)
{
imuFindBiasValue(&gyroBias);
if (gyroBias.isBiasValueFound)
{
ledseqRun(LED_RED, seq_calibrated);
// uartPrintf("Gyro bias: %i, %i, %i\n",
// gyroBias.bias.x, gyroBias.bias.y, gyroBias.bias.z);
}
}
#ifdef IMU_TAKE_ACCEL_BIAS
if (gyroBias.isBiasValueFound &&
!accelBias.isBiasValueFound)
{
Axis3i32 mean;
imuCalculateBiasMean(&accelBias, &mean);
accelBias.bias.x = mean.x;
accelBias.bias.y = mean.y;
accelBias.bias.z = mean.z - IMU_1G_RAW;
accelBias.isBiasValueFound = TRUE;
//uartPrintf("Accel bias: %i, %i, %i\n",
// accelBias.bias.x, accelBias.bias.y, accelBias.bias.z);
}
#endif
imuAccIIRLPFilter(&accelMpu, &accelLPF, &accelStoredFilterValues,
(int32_t)imuAccLpfAttFactor);
imuAccAlignToGravity(&accelLPF, &accelLPFAligned);
// Re-map outputs
#if 1
gyroOut->y = -((gyroMpu.x - gyroBias.bias.x) * IMU_DEG_PER_LSB_CFG);
gyroOut->x = (gyroMpu.y - gyroBias.bias.y) * IMU_DEG_PER_LSB_CFG;
gyroOut->z = (gyroMpu.z - gyroBias.bias.z) * IMU_DEG_PER_LSB_CFG;
accOut->y = -((accelLPFAligned.x - accelBias.bias.x) * IMU_G_PER_LSB_CFG);
accOut->x = (accelLPFAligned.y - accelBias.bias.y) * IMU_G_PER_LSB_CFG;
accOut->z = (accelLPFAligned.z - accelBias.bias.z) * IMU_G_PER_LSB_CFG;
#else
gyroOut->x = (gyroMpu.x - gyroBias.bias.x) * IMU_DEG_PER_LSB_CFG;
gyroOut->y = (gyroMpu.y - gyroBias.bias.y) * IMU_DEG_PER_LSB_CFG;
gyroOut->z = (gyroMpu.z - gyroBias.bias.z) * IMU_DEG_PER_LSB_CFG;
accOut->x = (accelLPFAligned.x - accelBias.bias.x) * IMU_G_PER_LSB_CFG;
accOut->y = (accelLPFAligned.y - accelBias.bias.y) * IMU_G_PER_LSB_CFG;
accOut->z = (accelLPFAligned.z - accelBias.bias.z) * IMU_G_PER_LSB_CFG;
#endif
// uartSendData(sizeof(Axis3f), (uint8_t*)gyroOut);
// uartSendData(sizeof(Axis3f), (uint8_t*)accOut);
#if 0
static uint32_t count = 0;
if (++count >= 19)
{
count = 0;
uartPrintf("%d, %d, %d, %d, %d, %d, %d, %d, %d\n",
(int32_t)(gyroOut->x * 10),
(int32_t)(gyroOut->y * 10),
(int32_t)(gyroOut->z * 10),
(int32_t)(accOut->x * 1000),
(int32_t)(accOut->y * 1000),
(int32_t)(accOut->z * 1000),
mag.x,
mag.y,
mag.z);
}
#endif
}
void systemTask(void *arg)
{
bool pass = true;
ledInit();
ledSet(CHG_LED, 1);
#ifdef DEBUG_QUEUE_MONITOR
queueMonitorInit();
#endif
uartInit();
#ifdef ENABLE_UART1
uart1Init();
#endif
#ifdef ENABLE_UART2
uart2Init();
#endif
//Init the high-levels modules
systemInit();
#ifndef USE_RADIOLINK_CRTP
#ifdef UART_OUTPUT_TRACE_DATA
//debugInitTrace();
#endif
#ifdef ENABLE_UART
// uartInit();
#endif
#endif //ndef USE_RADIOLINK_CRTP
commInit();
commanderAdvancedInit();
stabilizerInit();
#ifdef PLATFORM_CF2
deckInit();
#endif
soundInit();
memInit();
#ifdef PROXIMITY_ENABLED
proximityInit();
#endif
//Test the modules
pass &= systemTest();
pass &= configblockTest();
pass &= commTest();
pass &= commanderAdvancedTest();
pass &= stabilizerTest();
#ifdef PLATFORM_CF2
pass &= deckTest();
#endif
pass &= soundTest();
pass &= memTest();
pass &= watchdogNormalStartTest();
//Start the firmware
if(pass)
{
selftestPassed = 1;
systemStart();
soundSetEffect(SND_STARTUP);
ledseqRun(SYS_LED, seq_alive);
ledseqRun(LINK_LED, seq_testPassed);
}
else
{
selftestPassed = 0;
if (systemTest())
{
while(1)
{
ledseqRun(SYS_LED, seq_testPassed); //Red passed == not passed!
vTaskDelay(M2T(2000));
// System can be forced to start by setting the param to 1 from the cfclient
if (selftestPassed)
{
DEBUG_PRINT("Start forced.\n");
systemStart();
break;
}
}
}
else
{
ledInit();
ledSet(SYS_LED, true);
}
}
DEBUG_PRINT("Free heap: %d bytes\n", xPortGetFreeHeapSize());
workerLoop();
//Should never reach this point!
while(1)
vTaskDelay(portMAX_DELAY);
}