void generate(unsigned char length)
{
combination = malloc(sizeof(*combination) * length);
unsigned char i;
for(i = 0; i < length; i++)
{
unsigned char ran = getRandom(3);
if(ran >= 4) ran -= 4;
switch(ran)
{
case 0:
ledSet(0b00000001, 1);
break;
case 1:
ledSet(0b00000010, 1);
break;
case 2:
ledSet(0b00000100, 1);
break;
case 3:
ledSet(0b00001000, 1);
break;
}
_delay_ms(800);
ledSet(0xFF, 0);
_delay_ms(400);
*(combination + i) = ran;
}
}
//Initialize the green led pin as output
void ledInit()
{
if(isInit==TRUE)
return;
GPIO_InitTypeDef GPIO_InitStructure;
// Enable GPIO
RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO | LED_GPIO_PERIF, ENABLE);
// Remap PB4
GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable , ENABLE);
//Initialize the LED pins as an output
GPIO_InitStructure.GPIO_Pin = LED_GPIO_GREEN | LED_GPIO_RED;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_10MHz;
GPIO_Init(LED_GPIO_PORT, &GPIO_InitStructure);
//Turn off the LED:s
ledSet(LED_GREEN, 0);
ledSet(LED_RED, 0);
isInit = TRUE;
}
int main()
{
//Initialize the platform.
int err = platformInit();
if (err != 0) {
// The firmware is running on the wrong hardware. Halt
while(1);
}
//Launch the system task that will initialize and start everything
systemLaunch();
//Start the FreeRTOS scheduler
vTaskStartScheduler();
//TODO: Move to platform launch failed
ledInit();
ledSet(0, 1);
ledSet(1, 1);
//Should never reach this point!
while(1);
return 0;
}
void startDemo(unsigned char times, unsigned char mode)
{
unsigned char i;
if(mode & 0b00000001)
{
for(i = 0; i < times; i++)
{
ledSet(0b00000001, 1);
_delay_ms(100);
ledSet(0b00000001, 0);
ledSet(0b00000010, 1);
_delay_ms(100);
ledSet(0b00000010, 0);
ledSet(0b00000100, 1);
_delay_ms(100);
ledSet(0b00000100, 0);
ledSet(0b00001000, 1);
_delay_ms(100);
ledSet(0b00001000, 0);
}
}
if(mode & 0b00000010)
{
_delay_ms(200);
ledSet(0b00001111, 1);
_delay_ms(1000);
ledSet(0b00001111, 0);
}
}
void assertFail(char *exp, char *file, int line)
{
storeAssertSnapshotData(file, line);
ledClearAll();
ledSet(ERR_LED1, 1);
ledSet(ERR_LED2, 1);
while (1);
}
static void duoledSet(const struct sDuoled * ptr)
{
assert(NULL != ptr);
if(ptr->occupied)
{
switch(ptr->mode)
{
case ledOn: ledSet(ptr->pRed, ledOn); break;
case ledOff: ledSet(ptr->pRed, ledOn); break;
case ledAllOff: ledSet(ptr->pRed, ledAllOff); break;
case ledBlinking: ledSet(ptr->pRed, ledBlinking); break;
case ledBlankable: ledSet(ptr->pRed, ledOn); break;
default:
fprintf(stderr, "falscher Default %d\n", ptr->mode);
assert(false);
};
ledSet(ptr->pYellow, ledOff);
}
else
{
ledSet(ptr->pYellow, ptr->mode);
ledSet(ptr->pRed, ledOff);
}
}
//Initialize the green led pin as output
void ledInit()
{
int i;
if(isInit)
return;
GPIO_InitTypeDef GPIO_InitStructure;
// Enable GPIO
RCC_AHB1PeriphClockCmd(LED_GPIO_PERIF, ENABLE);
for (i = 0; i < LED_NUM; i++)
{
//Initialize the LED pins as an output
GPIO_InitStructure.GPIO_Pin = led_pin[i];
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_25MHz;
GPIO_Init(led_port[i], &GPIO_InitStructure);
//Turn off the LED:s
ledSet(i, 0);
}
isInit = true;
}
void assertFail(char *exp, char *file, int line)
{
portDISABLE_INTERRUPTS();
storeAssertSnapshotData(file, line);
DEBUG_PRINT("Assert failed %s:%d\n", file, line);
motorsSetRatio(MOTOR_M1, 0);
motorsSetRatio(MOTOR_M2, 0);
motorsSetRatio(MOTOR_M3, 0);
motorsSetRatio(MOTOR_M4, 0);
ledClearAll();
ledSet(ERR_LED1, 1);
ledSet(ERR_LED2, 1);
while (1);
}
void ledClearAll(void)
{
int i;
for (i = 0; i < LED_NUM; i++)
{
//Turn off the LED:s
ledSet(i, 0);
}
}
void ledSetAll(void)
{
int i;
for (i = 0; i < LED_NUM; i++)
{
//Turn on the LED:s
ledSet(i, 1);
}
}
void gameOver(unsigned char score)
{
startDemo(2, 0b00000001);
//print score
ledSet(score, 1);
while(!getButton());
startDemo(0, 0b00000010);
_delay_ms(500);
}
unsigned char check(unsigned char length)
{
unsigned char i;
for(i = 0; i < length; i++)
{
unsigned char buttons;
while(1)
{
buttons = getButton();
if(buttons != 0) break;
}
unsigned char buttonCode = 0;
ledSet(buttons, 1);
if(buttons & 0b00000001)
{
buttonCode = 0;
}
else if(buttons & 0b00000010)
{
buttonCode = 1;
}
else if(buttons & 0b00000100)
{
buttonCode = 2;
}
else if(buttons & 0b00001000)
{
buttonCode = 3;
}
if(!(buttonCode == *(combination + i)))
{
return 1;
}
_delay_ms(150);
while(getButton());
ledSet(0xFF, 0);
}
free(combination);
return 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);
}
bool ledTest(void)
{
ledSet(LED_GREEN_L, 1);
ledSet(LED_GREEN_R, 1);
ledSet(LED_RED_L, 0);
ledSet(LED_RED_R, 0);
vTaskDelay(M2T(250));
ledSet(LED_GREEN_L, 0);
ledSet(LED_GREEN_R, 0);
ledSet(LED_RED_L, 1);
ledSet(LED_RED_R, 1);
vTaskDelay(M2T(250));
// LED test end
ledClearAll();
ledSet(LED_BLUE_L, 1);
return isInit;
}
static void updateActive(led_t led)
{
int prio;
activeSeq[led]=LEDSEQ_STOP;
ledSet(led, false);
for(prio=0;prio<SEQ_NUM;prio++)
{
if (state[led][prio] != LEDSEQ_STOP)
{
activeSeq[led]=prio;
break;
}
}
}
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);
}
/* Center of the led sequence machine. This function is executed by the FreeRTOS
* timer and runs the sequences
*/
static void runLedseq( xTimerHandle xTimer )
{
led_t led = (led_t)pvTimerGetTimerID(xTimer);
const ledseq_t *step;
bool leave=false;
if (!ledseqEnabled)
return;
while(!leave) {
int prio = activeSeq[led];
if (prio == LEDSEQ_STOP)
return;
step = &sequences[prio][state[led][prio]];
state[led][prio]++;
xSemaphoreTake(ledseqSem, portMAX_DELAY);
switch(step->action)
{
case LEDSEQ_LOOP:
state[led][prio] = 0;
break;
case LEDSEQ_STOP:
state[led][prio] = LEDSEQ_STOP;
updateActive(led);
break;
default: //The step is a LED action and a time
ledSet(led, step->value);
if (step->action == 0)
break;
xTimerChangePeriod(xTimer, M2T(step->action), 0);
xTimerStart(xTimer, 0);
leave=true;
break;
}
xSemaphoreGive(ledseqSem);
}
}
int platformInit(void)
{
//Low level init: Clock and Interrupt controller
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4);
// Disable the jtag gpio
GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE);
// adcInit();
ledInit();
ledSet(0, 0);
ledSet(1, 0);
ledSet(2, 0);
delay_ms(500);
ledSet(0, 1);
delay_ms(500);
ledSet(1, 1);
delay_ms(500);
ledSet(2, 1);
delay_ms(500);
// while(1)
// {
// uint8_t i;
// for(i = 0; i < 3; i++)
// {
// ledSet(i, 1);
// delay_ms(1000);
// ledSet(0, 0);
// ledSet(1, 0);
// ledSet(2, 0);
// }
// }
uartInit();
timInit();
return 0;
}
/* PRX (Primary receiver mode). The radio will listen to incoming packets and send those to
* the PC. Packets from the PC will be put in the acknowledgment queue.
*/
void prxRun()
{
char tlen; //Transmit length
if (!radioIsRxEmpty())
{
ledTimeout = 2;
ledSet(LED_GREEN, true);
IN1BC = radioRxPacket(IN1BUF);
}
//Send a packet if something is received on the USB
if (!(OUT1CS&EPBSY) && !contCarrier)
{
//Deactivate the USB IN
IN1CS = 0x02;
//Fetch the USB data size. Limit it to 32
tlen = OUT1BC;
if (tlen>32) tlen=32;
radioAckPacket(0, OUT1BUF, tlen);
//reactivate OUT1
OUT1BC=BCDUMMY;
}
}
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);
}
// TODO: Implement!
int platformInit ( void )
{
uint8_t i = 0;
int checksum = 0;
//Low level init: Clock and Interrupt controller
NVIC_PriorityGroupConfig ( NVIC_PriorityGroup_4 );
// Disable the jtag gpio
GPIO_PinRemapConfig ( GPIO_Remap_SWJ_JTAGDisable, ENABLE );
ledInit();
ledSet ( 0, 0 );
ledSet ( 1, 0 );
ledSet ( 2, 0 );
delay_ms ( 500 );
ledSet ( 0, 1 );
delay_ms ( 500 );
ledSet ( 1, 1 );
delay_ms ( 500 );
ledSet ( 2, 1 );
delay_ms ( 500 );
uartInit();
DEBUG_PRINT ( "Too young too simple, sometimes naive.\n" );
DEBUG_PRINT ( "I'm a journalist from Hongkong.\n" );
DEBUG_PRINT ( "I could run very fast.\n" );
DEBUG_PRINT ( "------------------------------\n" );
DEBUG_PRINT ( "uart init successfully.\n" );
ledSet ( 0, 0 );
// adcInit();
DEBUG_PRINT ( "test motor.\n" );
timInit();
DEBUG_PRINT ( "waking up driver.\n" );
wakeupDriver();
timSetPulse ( TIM2, 3, 0 );
timSetPulse ( TIM3, 3, 0 );
timSetPulse ( TIM4, 3, 0 );
timSetPulse ( TIM2, 4, 0 );
timSetPulse ( TIM3, 4, 0 );
timSetPulse ( TIM4, 4, 0 );
delay_ms ( 500 );
timSetPulse ( TIM2, 4, 500 );
timSetPulse ( TIM3, 4, 500 );
timSetPulse ( TIM4, 4, 500 );
delay_ms ( 500 );
timSetPulse ( TIM2, 3, 0 );
timSetPulse ( TIM3, 3, 0 );
timSetPulse ( TIM4, 3, 0 );
timSetPulse ( TIM2, 4, 0 );
timSetPulse ( TIM3, 4, 0 );
timSetPulse ( TIM4, 4, 0 );
delay_ms ( 500 );
timSetPulse ( TIM2, 3, 500 );
timSetPulse ( TIM3, 3, 500 );
timSetPulse ( TIM4, 3, 500 );
delay_ms ( 500 );
timSetPulse ( TIM2, 3, 0 );
timSetPulse ( TIM3, 3, 0 );
timSetPulse ( TIM4, 3, 0 );
timSetPulse ( TIM2, 4, 0 );
timSetPulse ( TIM3, 4, 0 );
timSetPulse ( TIM4, 4, 0 );
ledSet ( 1, 0 );
delay_ms ( 200 );
nrf24l01Init();
i = nrf24l01ConnectCheck();
nrf24l01SetAddress();
// i = nrf24l01ConnectCheck();
if ( i == 1 )
{
checksum --;
ledSet ( 2, 0 );
}
delay_ms ( 1000 );
// mpu9150Init();
// i = mpu9150Status();
// if(!i)
// {
// checksum --;
// ledSet(1, 0);
// }
if ( checksum > 0 )
{
delay_ms ( 1000 );
timSetPulse ( TIM2, 3, 0 );
timSetPulse ( TIM3, 3, 0 );
timSetPulse ( TIM4, 3, 0 );
timSetPulse ( TIM2, 4, 0 );
timSetPulse ( TIM3, 4, 0 );
timSetPulse ( TIM4, 4, 0 );
// timSetPulse(TIM2, 4, 999);
// timSetPulse(TIM3, 4, 999);
// timSetPulse(TIM4, 4, 999);
while ( 1 )
{
ledSet ( 0, 1 );
ledSet ( 1, 1 );
ledSet ( 2, 1 );
delay_ms ( 100 );
ledSet ( 0, 0 );
ledSet ( 1, 0 );
ledSet ( 2, 0 );
delay_ms ( 100 );
}
}
ledSet ( 0, 0 );
ledSet ( 1, 0 );
ledSet ( 2, 0 );
delay_ms ( 500 );
ledSet ( 0, 1 );
ledSet ( 1, 1 );
ledSet ( 2, 1 );
delay_ms ( 500 );
ledSet ( 0, 0 );
ledSet ( 1, 0 );
ledSet ( 2, 0 );
return 0;
}
int main()
{
systemInit();
platformInit();
DEBUG_PRINT ( "init successfully\n" );
// {
// move_specfic temp_params;
// temp_params.angle = 3.1415926f;
// temp_params.velocity = 300;
// temp_params.spin = 0;
// temp_params.duration = 0;
// timImportQueue ( temp_params );
// }
while ( 1 )
{
uint8_t i;
nrf24l01RxMode();
// waiting package
i = 0x00;
while ( i != RX_DR )
{
i = nrf24l01RxData ( nRF_ReceiveBuffer );
}
ledSet(0, 1);
ledSet(1, 1);
ledSet(2, 1);
sbn1HandleReceived();
DEBUG_PRINT ( "Received packet\r\n" );
ledSet(0, 0);
ledSet(1, 0);
ledSet(2, 0);
}
while(1){
uint16_t angle;
for ( angle = 0; angle < 360; angle++ )
{
move_specfic temp_params;
// DEBUG_PRINT("tp1\n");
temp_params.angle = 2.0f * 3.1415926f * ( float ) angle / 360.0f;
temp_params.velocity = 1023;
temp_params.spin = 0;
temp_params.duration = 0;
// DEBUG_PRINT("tp1\n");
timImportQueue ( temp_params );
delay_ms ( 20 );
}
}
while ( 1 )
{
uint8_t i;
nrf24l01RxMode();
// waiting package
i = 0x00;
while ( i != RX_DR )
{
i = nrf24l01RxData ( nRF_ReceiveBuffer );
}
sbn1HandleReceived();
DEBUG_PRINT ( "Received packet\r\n" );
}
return 0;
}
void imu6Init(void)
{
if(isInit)
return;
isHmc5883lPresent = FALSE;
isMs5611Present = FALSE;
// Wait for sensors to startup
while (xTaskGetTickCount() < M2T(IMU_STARTUP_TIME_MS));
i2cdevInit(I2C2);
mpu6050Init(I2C2);
#if 1
if (mpu6050TestConnection() == TRUE)
{
//ledSet(LED_RED, 1);
//ledseqRun(LED_RED, seq_alive);
//vTaskDelay(M2T(10000));
//vTaskDelay(M2T(2000));
DEBUG_PRINT("MPU6050 I2C connection [OK].\n");
}
else
{
//ledSet(LED_GREEN, 1);
DEBUG_PRINT("MPU6050 I2C connection [FAIL].\n");
}
#endif
mpu6050Reset();
vTaskDelay(M2T(50));
// Activate MPU6050
mpu6050SetSleepEnabled(FALSE);
// Enable temp sensor
mpu6050SetTempSensorEnabled(TRUE);
// Disable interrupts
mpu6050SetIntEnabled(FALSE);
// Connect the HMC5883L to the main I2C bus
mpu6050SetI2CBypassEnabled(FALSE);
// Set x-axis gyro as clock source
mpu6050SetClockSource(MPU6050_CLOCK_PLL_XGYRO);
// Set gyro full scale range
mpu6050SetFullScaleGyroRange(IMU_GYRO_FS_CFG);
// Set accelerometer full scale range
mpu6050SetFullScaleAccelRange(IMU_ACCEL_FS_CFG);
#ifdef IMU_MPU6050_DLPF_256HZ
// 256Hz digital low-pass filter only works with little vibrations
// Set output rate (15): 8000 / (1 + 15) = 500Hz
mpu6050SetRate(15);
// Set digital low-pass bandwidth
mpu6050SetDLPFMode(MPU6050_DLPF_BW_256);
#else
// To low DLPF bandwidth might cause instability and decrease agility
// but it works well for handling vibrations and unbalanced propellers
// Set output rate (1): 1000 / (1 + 1) = 500Hz
mpu6050SetRate(1);
// Set digital low-pass bandwidth
mpu6050SetDLPFMode(MPU6050_DLPF_BW_188);
#endif
#if 0
#ifdef IMU_ENABLE_MAG_HMC5883
hmc5883lInit(I2C2);
if (hmc5883lTestConnection() == TRUE)
{
isHmc5883lPresent = TRUE;
DEBUG_PRINT("HMC5883 I2C connection [OK].\n");
}
else
{
DEBUG_PRINT("HMC5883L I2C connection [FAIL].\n");
}
#endif
#ifdef IMU_ENABLE_PRESSURE_MS5611
if (ms5611Init(I2C2) == TRUE)
{
isMs5611Present = TRUE;
DEBUG_PRINT("MS5611 I2C connection [OK].\n");
}
else
{
DEBUG_PRINT("MS5611 I2C connection [FAIL].\n");
}
#endif
#endif
imuBiasInit(&gyroBias);
imuBiasInit(&accelBias);
varianceSampleTime = -GYRO_MIN_BIAS_TIMEOUT_MS + 1;
imuAccLpfAttFactor = IMU_ACC_IIR_LPF_ATT_FACTOR;
cosPitch = cos(configblockGetCalibPitch() * M_PI/180);
sinPitch = sin(configblockGetCalibPitch() * M_PI/180);
cosRoll = cos(configblockGetCalibRoll() * M_PI/180);
sinRoll = sin(configblockGetCalibRoll() * M_PI/180);
#if 0
if (mpu6050TestConnection() == TRUE)
{
ledSet(LED_RED, 1);
//vTaskDelay(M2T(2000));
DEBUG_PRINT("MPU6050 I2C connection [OK].\n");
}
else
{
DEBUG_PRINT("MPU6050 I2C connection [FAIL].\n");
}
#endif
isInit = TRUE;
}
void main()
{
mode = MODE_LEGACY;
//Init the chip ID
initId();
//Init the led and set the leds until the usb is not ready
#ifndef CRPA
ledInit(CR_LED_RED, CR_LED_GREEN);
#else
ledInit(CRPA_LED_RED, CRPA_LED_GREEN);
#endif
ledSet(LED_GREEN | LED_RED, true);
// Initialise the radio
#ifdef CRPA
// Enable LNA (PA RX)
P0DIR &= ~(1<<CRPA_PA_RXEN);
P0 |= (1<<CRPA_PA_RXEN);
#endif
radioInit(RADIO_MODE_PTX);
#ifdef PPM_JOYSTICK
// Initialise the PPM acquisition
ppmInit();
#endif //PPM_JOYSTICK
// Initialise and connect the USB
usbInit();
//Globally activate the interruptions
IEN0 |= 0x80;
//Wait for the USB to be addressed
while (usbGetState() != ADDRESS);
//Reset the LEDs
ledSet(LED_GREEN | LED_RED, false);
//Wait for the USB to be ready
while (usbGetState() != CONFIGURED);
//Activate OUT1
OUT1BC=0xFF;
while(1)
{
if (mode == MODE_LEGACY)
{
// Run legacy mode
legacyRun();
}
else if (mode == MODE_CMD)
{
// Run cmd mode
cmdRun();
}
else if (mode == MODE_PRX)
{
// Run PRX mode
prxRun();
}
//USB vendor setup handling
if(usbIsVendorSetup())
handleUsbVendorSetup();
}
}
void cmdRun()
{
char status;
char tlen; //Transmit length
char cmdPtr;
char resPtr;
unsigned char id;
unsigned char plen;
char rlen; //Received packet length
uint8_t ack;
unsigned char cmd;
if (!(OUT1CS&EPBSY) && !contCarrier) {
//Fetch the USB data size, should not be higher than 64
tlen = OUT1BC;
if (tlen>64) tlen=64;
//Get the command string in a ram buffer
memcpy(tbuffer, OUT1BUF, tlen);
//And re-activate the out EP
OUT1BC=BCDUMMY;
// Run commands!
cmdPtr = 0;
resPtr = 0;
while (cmdPtr < tlen) {
cmd = tbuffer[cmdPtr++];
switch (cmd) {
case CMD_PACKET:
if ((tlen-cmdPtr)<3) {
sendError(ERROR_MALFORMED_CMD, cmd, cmdPtr);
cmdPtr = tlen;
break;
}
id = tbuffer[cmdPtr++];
plen = tbuffer[cmdPtr++];
if ((tlen-(cmdPtr+plen))<0) {
sendError(ERROR_MALFORMED_CMD, cmd, cmdPtr);
cmdPtr = tlen;
break;
}
status = radioSendPacket(&tbuffer[cmdPtr], plen,
rpbuffer, &rlen);
cmdPtr += plen;
ledTimeout = 2;
ledSet(LED_GREEN | LED_RED, false);
if(status)
ledSet(LED_GREEN, true);
else
ledSet(LED_RED, true);
//Check if there is enough place in rbuffer, flush it if not
if ((resPtr+rlen+4)>64) {
//Wait for IN1 to become free
while(IN1CS&EPBSY);
memcpy(IN1BUF, rbuffer, resPtr);
IN1BC = resPtr;
resPtr = 0;
}
//Prepare the USB answer, state and ack data
ack=status?1:0;
if (ack)
{
if (radioGetRpd()) ack |= 0x02;
ack |= radioGetTxRetry()<<4;
}
if(!(status&BIT_TX_DS)) rlen=0;
rbuffer[resPtr] = 0;
rbuffer[resPtr+1] = id;
rbuffer[resPtr+2] =ack;
rbuffer[resPtr+3] = rlen;
memcpy(&rbuffer[resPtr+4], rpbuffer, rlen);
resPtr += rlen+4;
break;
case SET_RADIO_CHANNEL:
if (((tlen-cmdPtr)<1) || (tbuffer[cmdPtr]>125)) {
sendError(ERROR_MALFORMED_CMD, cmd, cmdPtr);
cmdPtr = tlen;
break;
}
radioSetChannel(tbuffer[cmdPtr++]);
break;
case SET_DATA_RATE:
if (((tlen-cmdPtr)<1) || (tbuffer[cmdPtr]>3)) {
sendError(ERROR_MALFORMED_CMD, cmd, cmdPtr);
cmdPtr = tlen;
break;
}
radioSetDataRate(tbuffer[cmdPtr++]);
break;
default:
sendError(ERROR_UNKNOWN_CMD, cmd, cmdPtr);
break;
}
}
// Send data that are still in the TX buffer
if (resPtr != 0) {
//Wait for IN1 to become free
while(IN1CS&EPBSY);
memcpy(IN1BUF, rbuffer, resPtr);
IN1BC = resPtr;
resPtr = 0;
}
}
}
/* 'Legacy' pre-1.0 protocol, handles only radio packets and require the
* computer to ping-pong sending and receiving
*/
void legacyRun()
{
char status;
char tlen; //Transmit length
char rlen; //Received packet length
uint8_t ack;
//Send a packet if something is received on the USB
if (!(OUT1CS&EPBSY) && !contCarrier)
{
//Deactivate the USB IN
IN1CS = 0x02;
//Fetch the USB data size. Limit it to 32
tlen = OUT1BC;
if (tlen>32) tlen=32;
//Send the packet
memcpy(tbuffer, OUT1BUF, tlen);
if (needAck)
{
status = radioSendPacket(tbuffer, tlen, rbuffer, &rlen);
//Set the Green LED on success and the Red one on failure
//The SOF interrupt decrement ledTimeout and will reset the LEDs when it
//reaches 0
ledTimeout = 2;
ledSet(LED_GREEN | LED_RED, false);
if(status)
ledSet(LED_GREEN, true);
else
ledSet(LED_RED, true);
//reactivate OUT1
OUT1BC=BCDUMMY;
//Prepare the USB answer, state and ack data
ack=status?1:0;
if (ack)
{
if (radioGetRpd()) ack |= 0x02;
ack |= radioGetTxRetry()<<4;
}
IN1BUF[0]=ack;
if(!(status&BIT_TX_DS)) rlen=0;
memcpy(IN1BUF+1, rbuffer, rlen);
//Activate the IN EP with length+status
IN1BC = rlen+1;
}
else
{
radioSendPacketNoAck(tbuffer, tlen);
ledTimeout = 2;
ledSet(LED_GREEN | LED_RED, false);
ledSet(LED_GREEN, true);
//reactivate OUT1
OUT1BC=BCDUMMY;
}
}
}
//Handles vendor control messages and ack them
void handleUsbVendorSetup()
{
__xdata struct controllStruct *setup = usbGetSetupPacket();
//The vendor control messages are valide only when the device is configured
if (usbGetState() >= CONFIGURED)
{
if(setup->request == LAUNCH_BOOTLOADER)
{
//Ack the launch request
usbAckSetup();
launchBootloader();
//Will never come back ...
return;
}
else if(setup->request == SET_RADIO_CHANNEL)
{
radioSetChannel(setup->value);
usbAckSetup();
return;
}
else if(setup->request == SET_DATA_RATE)
{
radioSetDataRate(setup->value);
usbAckSetup();
return;
}
else if(setup->request == SET_RADIO_ADDRESS)
{
if(setup->length != 5)
{
usbDismissSetup();
return;
}
//Arm and wait for the out transaction
OUT0BC = BCDUMMY;
while (EP0CS & OUTBSY);
//Set address of the pipe given by setup's index
radioSetAddress(OUT0BUF);
//Ack the setup phase
usbAckSetup();
return;
}
else if(setup->request == SET_RADIO_POWER)
{
radioSetPower(setup->value);
usbAckSetup();
return;
}
else if(setup->request == SET_RADIO_ARD)
{
radioSetArd(setup->value);
usbAckSetup();
return;
}
else if(setup->request == SET_RADIO_ARC)
{
radioSetArc(setup->value);
usbAckSetup();
return;
}
else if(setup->request == SET_CONT_CARRIER)
{
radioSetContCarrier((setup->value)?true:false);
contCarrier = (setup->value)?true:false;
ledTimeout = -1;
ledSet(LED_RED, (setup->value)?true:false);
usbAckSetup();
return;
}
else if(setup->request == ACK_ENABLE)
{
needAck = (setup->value)?true:false;
usbAckSetup();
return;
}
else if(setup->request == CHANNEL_SCANN && setup->requestType == 0x40)
{
int i;
char rlen;
char status;
char inc = 1;
unsigned char start, stop;
scannLength = 0;
if(setup->length < 1)
{
usbDismissSetup();
return;
}
//Start and stop channels
start = setup->value;
stop = (setup->index>125)?125:setup->index;
if (radioGetDataRate() == DATA_RATE_2M)
inc = 2; //2M channel are 2MHz wide
//Arm and wait for the out transaction
OUT0BC = BCDUMMY;
while (EP0CS & OUTBSY);
memcpy(tbuffer, OUT0BUF, setup->length);
for (i=start; i<stop+1 && scannLength<MAX_SCANN_LENGTH; i+=inc)
{
radioSetChannel(i);
status = radioSendPacket(tbuffer, setup->length, rbuffer, &rlen);
if (status)
IN0BUF[scannLength++] = i;
ledTimeout = 2;
ledSet(LED_GREEN | LED_RED, false);
if(status)
ledSet(LED_GREEN, true);
else
ledSet(LED_RED, true);
}
//Ack the setup phase
usbAckSetup();
return;
}
else if(setup->request == CHANNEL_SCANN && setup->requestType == 0xC0)
{
//IN0BUF already contains the right data
//(if a scann has been launched before ...)
IN0BC = (setup->length>scannLength)?scannLength:setup->length;
while (EP0CS & INBSY);
//Ack the setup phase
usbAckSetup();
return;
}
else if(setup->request == SET_MODE && setup->requestType == 0x40)
{
mode = setup->value;
if (mode == MODE_PRX)
{
radioSetMode(RADIO_MODE_PRX);
}
else
{
radioSetMode(RADIO_MODE_PTX);
}
usbAckSetup();
return;
}
}
//Stall in error if nothing executed!
usbDismissSetup();
}
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);
}
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);
}
