static void handleBootloaderCmd(struct esbPacket_s *packet)
{
static bool resetInit = false;
static struct esbPacket_s txpk;
switch (packet->data[2]) {
case BOOTLOADER_CMD_RESET_INIT:
resetInit = true;
txpk.data[0] = 0xff;
txpk.data[1] = 0xfe;
txpk.data[2] = BOOTLOADER_CMD_RESET_INIT;
memcpy(&(txpk.data[3]), (uint32_t*)NRF_FICR->DEVICEADDR, 6);
txpk.size = 9;
#if BLE
bleCrazyfliesSendPacket(&txpk);
#endif
if (esbCanTxPacket()) {
struct esbPacket_s *pk = esbGetTxPacket();
memcpy(pk, &txpk, sizeof(struct esbPacket_s));
esbSendTxPacket(pk);
}
break;
case BOOTLOADER_CMD_RESET:
if (resetInit && (packet->size == 4)) {
msDelay(100);
if (packet->data[3] == 0) {
NRF_POWER->GPREGRET |= 0x40;
} else {
//Set bit 0x20 forces boot to firmware
NRF_POWER->GPREGRET |= 0x20U;
}
#ifdef BLE
sd_nvic_SystemReset();
#else
NVIC_SystemReset();
#endif
}
break;
default:
break;
}
}
void mainLoop(void)
{
bool resetToFw = false;
static CrtpPacket crtpPacket;
static bool stmStarted = false;
while(!resetToFw) {
EsbPacket *packet;
buttonProcess();
if (buttonGetState() != buttonIdle) {
resetToFw = true;
}
if ((stmStarted == false) && (nrf_gpio_pin_read(UART_RX_PIN))) {
nrf_gpio_cfg_input(UART_RTS_PIN, NRF_GPIO_PIN_NOPULL);
uartInit();
stmStarted = true;
}
if (cstate != connect_ble) {
packet = esbGetRxPacket();
if (packet != NULL) {
if ( ((packet->size >= 2) &&
(packet->data[0]==0xff) &&
(packet->data[1]==0xff)) ||
((packet->size >= 2) &&
(packet->data[0]==0xff) &&
(packet->data[1]==0xfe))
) {
// Disable Bluetooth advertizing when receiving a bootloader SB packet
if (cstate == connect_idle) {
//sd_ble_gap_adv_stop();
cstate = connect_sb;
}
}
// If we are connected SB, the packet is read and used
if (cstate == connect_sb) {
memcpy(crtpPacket.raw, packet->data, packet->size);
crtpPacket.datalen = packet->size-1;
}
esbReleaseRxPacket(packet);
}
}
if (cstate != connect_sb) {
if (bleCrazyfliesIsPacketReceived()) {
cstate = connect_ble;
packet = bleCrazyfliesGetRxPacket();
memcpy(crtpPacket.raw, packet->data, packet->size);
crtpPacket.datalen = packet->size-1;
bleCrazyfliesReleaseRxPacket(packet);
}
}
if (crtpPacket.datalen != 0xffu) {
struct syslinkPacket slPacket;
slPacket.type = SYSLINK_RADIO_RAW;
memcpy(slPacket.data, crtpPacket.raw, crtpPacket.datalen+1);
slPacket.length = crtpPacket.datalen+1;
if (bootloaderProcess(&crtpPacket) == false) {
// Send packet to stm32
syslinkSend(&slPacket);
crtpPacket.datalen = 0xFFU;
// If packet received from stm32, send it back
if (syslinkReceive(&slPacket)) {
if (slPacket.type == SYSLINK_RADIO_RAW) {
memcpy(crtpPacket.raw, slPacket.data, slPacket.length);
crtpPacket.datalen = slPacket.length-1;
}
}
}
}
if (crtpPacket.datalen != 0xFFU) {
if (cstate == connect_sb) {
EsbPacket *pk = esbGetTxPacket();
if (pk) {
memcpy(pk->data, crtpPacket.raw, crtpPacket.datalen+1);
pk->size = crtpPacket.datalen+1;
esbSendTxPacket(pk);
}
} else if (cstate == connect_ble) {
static EsbPacket pk;
memcpy(pk.data, crtpPacket.raw, crtpPacket.datalen+1);
pk.size = crtpPacket.datalen+1;
bleCrazyfliesSendPacket(&pk);
}
}
crtpPacket.datalen = 0xFFU;
// Blink the LED
if (NRF_TIMER1->EVENTS_COMPARE[0]) {
NRF_TIMER1->EVENTS_COMPARE[0] = 0;
#ifndef DEBUG_TIMESLOT
NRF_GPIOTE->TASKS_OUT[0] = 1;
#endif
}
}
//Set bit 0x20 forces boot to firmware
NRF_POWER->GPREGRET |= 0x20U;
sd_nvic_SystemReset();
while(1);
}
void mainloop()
{
static struct syslinkPacket slRxPacket;
static struct syslinkPacket slTxPacket;
static EsbPacket esbRxPacket;
bool esbReceived = false;
bool slReceived;
static int vbatSendTime;
while(1)
{
#ifdef BLE
if ((esbReceived == false) && bleCrazyfliesIsPacketReceived()) {
EsbPacket* packet = bleCrazyfliesGetRxPacket();
memcpy(esbRxPacket.data, packet->data, packet->size);
esbRxPacket.size = packet->size;
esbReceived = true;
bleCrazyfliesReleaseRxPacket(packet);
}
#endif
#ifndef CONT_WAVE_TEST
if ((esbReceived == false) && esbIsRxPacket())
{
EsbPacket* packet = esbGetRxPacket();
memcpy(esbRxPacket.data, packet->data, packet->size);
esbRxPacket.size = packet->size;
esbReceived = true;
esbReleaseRxPacket(packet);
}
if (esbReceived)
{
EsbPacket* packet = &esbRxPacket;
esbReceived = false;
if((packet->size == 4) && (packet->data[0]==0xff) && (packet->data[1]==0x03))
{
handleRadioCmd(packet);
}
else if ((packet->size >2) && (packet->data[0]==0xff) && (packet->data[1]==0xfe))
{
handleBootloaderCmd(packet);
}
else
{
memcpy(slTxPacket.data, packet->data, packet->size);
slTxPacket.length = packet->size;
slTxPacket.type = SYSLINK_RADIO_RAW;
syslinkSend(&slTxPacket);
}
}
slReceived = syslinkReceive(&slRxPacket);
if (slReceived)
{
switch (slRxPacket.type)
{
case SYSLINK_RADIO_RAW:
if (esbCanTxPacket() && (slRxPacket.length < SYSLINK_MTU))
{
EsbPacket* packet = esbGetTxPacket();
if (packet) {
memcpy(packet->data, slRxPacket.data, slRxPacket.length);
packet->size = slRxPacket.length;
#if BLE
bleCrazyfliesSendPacket(packet);
#endif
esbSendTxPacket(packet);
}
bzero(slRxPacket.data, SYSLINK_MTU);
} // else the packet is dropped!
break;
case SYSLINK_RADIO_CHANNEL:
if(slRxPacket.length == 1)
{
esbSetChannel(slRxPacket.data[0]);
slTxPacket.type = SYSLINK_RADIO_CHANNEL;
slTxPacket.data[0] = slRxPacket.data[0];
slTxPacket.length = 1;
syslinkSend(&slTxPacket);
}
break;
case SYSLINK_RADIO_DATARATE:
if(slRxPacket.length == 1)
{
esbSetDatarate(slRxPacket.data[0]);
slTxPacket.type = SYSLINK_RADIO_DATARATE;
slTxPacket.data[0] = slRxPacket.data[0];
slTxPacket.length = 1;
syslinkSend(&slTxPacket);
}
break;
case SYSLINK_RADIO_CONTWAVE:
if(slRxPacket.length == 1) {
esbSetContwave(slRxPacket.data[0]);
slTxPacket.type = SYSLINK_RADIO_CONTWAVE;
slTxPacket.data[0] = slRxPacket.data[0];
slTxPacket.length = 1;
syslinkSend(&slTxPacket);
}
break;
case SYSLINK_PM_ONOFF_SWITCHOFF:
pmSetState(pmAllOff);
break;
case SYSLINK_OW_GETINFO:
case SYSLINK_OW_READ:
case SYSLINK_OW_SCAN:
case SYSLINK_OW_WRITE:
if (memorySyslink(&slRxPacket)) {
syslinkSend(&slRxPacket);
}
break;
}
}
// Send the battery voltage and state to the STM every 100ms
if (systickGetTick() >= vbatSendTime+10) {
float fdata;
uint8_t flags = 0;
vbatSendTime = systickGetTick();
slTxPacket.type = SYSLINK_PM_BATTERY_STATE;
slTxPacket.length = 9;
flags |= (pmIsCharging() == true)?0x01:0;
flags |= (pmUSBPower() == true)?0x02:0;
slTxPacket.data[0] = flags;
fdata = pmGetVBAT();
memcpy(slTxPacket.data+1, &fdata, sizeof(float));
fdata = pmGetISET();
memcpy(slTxPacket.data+1+4, &fdata, sizeof(float));
syslinkSend(&slTxPacket);
}
#endif
// Button event handling
ButtonEvent be = buttonGetState();
bool usbConnected = pmUSBPower();
if ((pmGetState() != pmSysOff) && be && !usbConnected)
{
pmSetState(pmAllOff);
/*swdInit();
swdTest();*/
}
else if ((pmGetState() != pmSysOff) && be && usbConnected)
{
//pmSetState(pmSysOff);
pmSetState(pmAllOff);
/*swdInit();
swdTest();*/
}
else if ((pmGetState() == pmSysOff) && (be == buttonShortPress))
{
//Normal boot
pmSysBootloader(false);
pmSetState(pmSysRunning);
}
else if ((pmGetState() == pmSysOff) && boottedFromBootloader)
{
//Normal boot after bootloader
pmSysBootloader(false);
pmSetState(pmSysRunning);
}
else if ((pmGetState() == pmSysOff) && (be == buttonLongPress))
{
//stm bootloader
pmSysBootloader(true);
pmSetState(pmSysRunning);
}
boottedFromBootloader = false;
// processes loop
buttonProcess();
pmProcess();
//owRun(); //TODO!
}
}
void mainloop()
{
static struct syslinkPacket slRxPacket;
static struct syslinkPacket slTxPacket;
static EsbPacket esbRxPacket;
bool esbReceived = false;
bool slReceived;
static int vbatSendTime;
static int radioRSSISendTime;
static uint8_t rssi;
while(1)
{
#ifdef BLE
if ((esbReceived == false) && bleCrazyfliesIsPacketReceived()) {
EsbPacket* packet = bleCrazyfliesGetRxPacket();
memcpy(esbRxPacket.data, packet->data, packet->size);
esbRxPacket.size = packet->size;
esbReceived = true;
bleCrazyfliesReleaseRxPacket(packet);
}
#endif
#ifndef CONT_WAVE_TEST
if ((esbReceived == false) && esbIsRxPacket())
{
EsbPacket* packet = esbGetRxPacket();
//Store RSSI here so that we can send it to STM later
rssi = packet->rssi;
memcpy(esbRxPacket.data, packet->data, packet->size);
esbRxPacket.size = packet->size;
esbReceived = true;
esbReleaseRxPacket(packet);
}
if (esbReceived)
{
EsbPacket* packet = &esbRxPacket;
esbReceived = false;
if((packet->size >= 4) && (packet->data[0]==0xff) && (packet->data[1]==0x03))
{
handleRadioCmd(packet);
}
else if ((packet->size >2) && (packet->data[0]==0xff) && (packet->data[1]==0xfe))
{
handleBootloaderCmd(packet);
}
else
{
memcpy(slTxPacket.data, packet->data, packet->size);
slTxPacket.length = packet->size;
slTxPacket.type = SYSLINK_RADIO_RAW;
syslinkSend(&slTxPacket);
}
}
slReceived = syslinkReceive(&slRxPacket);
if (slReceived)
{
switch (slRxPacket.type)
{
case SYSLINK_RADIO_RAW:
if (esbCanTxPacket() && (slRxPacket.length < SYSLINK_MTU))
{
EsbPacket* packet = esbGetTxPacket();
if (packet) {
memcpy(packet->data, slRxPacket.data, slRxPacket.length);
packet->size = slRxPacket.length;
esbSendTxPacket(packet);
}
bzero(slRxPacket.data, SYSLINK_MTU);
}
#ifdef BLE
if (slRxPacket.length < SYSLINK_MTU) {
static EsbPacket pk;
memcpy(pk.data, slRxPacket.data, slRxPacket.length);
pk.size = slRxPacket.length;
bleCrazyfliesSendPacket(&pk);
}
#endif
break;
case SYSLINK_RADIO_CHANNEL:
if(slRxPacket.length == 1)
{
esbSetChannel(slRxPacket.data[0]);
slTxPacket.type = SYSLINK_RADIO_CHANNEL;
slTxPacket.data[0] = slRxPacket.data[0];
slTxPacket.length = 1;
syslinkSend(&slTxPacket);
}
break;
case SYSLINK_RADIO_DATARATE:
if(slRxPacket.length == 1)
{
esbSetDatarate(slRxPacket.data[0]);
slTxPacket.type = SYSLINK_RADIO_DATARATE;
slTxPacket.data[0] = slRxPacket.data[0];
slTxPacket.length = 1;
syslinkSend(&slTxPacket);
}
break;
case SYSLINK_RADIO_CONTWAVE:
if(slRxPacket.length == 1) {
esbSetContwave(slRxPacket.data[0]);
slTxPacket.type = SYSLINK_RADIO_CONTWAVE;
slTxPacket.data[0] = slRxPacket.data[0];
slTxPacket.length = 1;
syslinkSend(&slTxPacket);
}
break;
case SYSLINK_RADIO_ADDRESS:
if(slRxPacket.length == 5)
{
uint64_t address = 0;
memcpy(&address, &slRxPacket.data[0], 5);
esbSetAddress(address);
slTxPacket.type = SYSLINK_RADIO_ADDRESS;
memcpy(slTxPacket.data, slRxPacket.data, 5);
slTxPacket.length = 5;
syslinkSend(&slTxPacket);
}
break;
case SYSLINK_PM_ONOFF_SWITCHOFF:
pmSetState(pmAllOff);
break;
case SYSLINK_OW_GETINFO:
case SYSLINK_OW_READ:
case SYSLINK_OW_SCAN:
case SYSLINK_OW_WRITE:
if (memorySyslink(&slRxPacket)) {
syslinkSend(&slRxPacket);
}
break;
}
}
// Wait a while to start pushing over the syslink since UART pins are used to launch STM32 i bootloader as well
if (systickGetTick() > SYSLINK_STARTUP_DELAY_TIME_MS) {
// Send the battery voltage and state to the STM every SYSLINK_SEND_PERIOD_MS
if (systickGetTick() >= vbatSendTime + SYSLINK_SEND_PERIOD_MS) {
float fdata;
uint8_t flags = 0;
vbatSendTime = systickGetTick();
slTxPacket.type = SYSLINK_PM_BATTERY_STATE;
slTxPacket.length = 9;
flags |= (pmIsCharging() == true)?0x01:0;
flags |= (pmUSBPower() == true)?0x02:0;
slTxPacket.data[0] = flags;
fdata = pmGetVBAT();
memcpy(slTxPacket.data+1, &fdata, sizeof(float));
fdata = pmGetISET();
memcpy(slTxPacket.data+1+4, &fdata, sizeof(float));
syslinkSend(&slTxPacket);
}
//Send an RSSI sample to the STM every 10ms(100Hz)
if (systickGetTick() >= radioRSSISendTime + 10) {
radioRSSISendTime = systickGetTick();
slTxPacket.type = SYSLINK_RADIO_RSSI;
//This message contains only the RSSI measurement which consist
//of a single uint8_t
slTxPacket.length = sizeof(uint8_t);
memcpy(slTxPacket.data, &rssi, sizeof(uint8_t));
syslinkSend(&slTxPacket);
}
}
#endif
// Button event handling
ButtonEvent be = buttonGetState();
bool usbConnected = pmUSBPower();
if ((pmGetState() != pmSysOff) && (be == buttonShortPress) && !usbConnected)
{
pmSetState(pmAllOff);
/*swdInit();
swdTest();*/
}
else if ((pmGetState() != pmSysOff) && (be == buttonShortPress)
&& usbConnected)
{
//pmSetState(pmSysOff);
pmSetState(pmAllOff);
/*swdInit();
swdTest();*/
}
else if ((pmGetState() == pmSysOff) && (be == buttonShortPress))
{
//Normal boot
pmSysBootloader(false);
pmSetState(pmSysRunning);
}
else if ((pmGetState() == pmSysOff) && boottedFromBootloader)
{
//Normal boot after bootloader
pmSysBootloader(false);
pmSetState(pmSysRunning);
}
else if ((pmGetState() == pmSysOff) && (be == buttonLongPress))
{
//stm bootloader
pmSysBootloader(true);
pmSetState(pmSysRunning);
}
boottedFromBootloader = false;
// processes loop
buttonProcess();
pmProcess();
//owRun(); //TODO!
}
}
