Ejemplo n.º 1
0
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!
  }
}
Ejemplo n.º 2
0
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!
  }
}