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!
}
}