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));
	}
}
Example #2
0
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);
}
Example #3
0
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);
	
}
Example #4
0
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

}
Example #5
0
static int usblinkReceiveCRTPPacket(CRTPPacket *p)
{
  if (xQueueReceive(crtpPacketDelivery, p, M2T(100)) == pdTRUE)
  {
    ledseqRun(LINK_LED, seq_linkup);
    return 0;
  }

  return -1;
}
Example #6
0
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;
}
Example #7
0
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);		
	}
}
Example #8
0
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;
}
Example #10
0
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();
      }
    }
  }
}
Example #13
0
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);
}
Example #14
0
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);
}
Example #15
0
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);
}