Modifying usbLinkTask causing queue error

Firmware/software/electronics/mechanics
Post Reply
sammy-qq
Beginner
Posts: 4
Joined: Thu Jan 07, 2021 5:57 pm

Modifying usbLinkTask causing queue error

Post by sammy-qq »

Hello,

I'm trying to modify the usbLinkTask and create a custom parsing function for incoming USB packets so that I can control the Crazyflie solely using USB.

Originally I had modified sysLinkTask to read from USB instead of from the radio chip UART, like this:

Code: Select all

/* Syslink task, handles communication between nrf and stm and dispatch messages
 */
static USBPacket usbIn;
static void syslinkTask(void *param) {

  SyslinkPacket slp;

  while (1) {
      /*ORIGINAL
     //get an incoming radio packet (blocking fxn), read it into slp
      uartslkGetPacketBlocking(&slp);
      syslinkRouteIncommingPacket(&slp);
  */

     //MY VERSION
      usbGetDataBlocking(&usbIn);
      if (!usbSlkParseIncomingPacket(&slp) {
             //incoming packet is now in slp

            //route the packet to the appropriate location
            syslinkRouteIncommingPacket(&slp);
       }
      else {
            //DEBUG_PRINT something
      }
   } 
}
So I wrote a new parsing function called usbSlkParseIncomingPacket, modified the syslinkRouteIncommingPacket() function and some other functions a bit, pushed the setpoints to the controller, modified the Crazyflie Android client, created a fake temporary ack that the Crazyflie would send back over USB...then I plugged an Android into the Crazyflie, and it worked: I was able to send basic control packets to the Crazyflie over USB and control the motors that way.

I want to keep the original syslink, though, and just modify the usbLinkTask to accept my control packets. But I'm running into a problem.

This is what I'm trying to do in usbLinkTask:

Code: Select all

static USBPacket usbIn;
//static CRTPPacket p;

static void usblinkTask(void *param) {

	SyslinkPacket slp;

  while (1) {
	/*
	//ORIGINAL
	// Fetch a USB packet off the queue
	usbGetDataBlocking(&usbIn);

	p.size = usbIn.size - 1;
	memcpy(&p.raw, usbIn.data, usbIn.size);
	// This queuing will copy a CRTP packet size from usbIn
	ASSERT(xQueueSend(crtpPacketDelivery, &p, 0) == pdTRUE);
	DEBUG_PRINT("Got some USB data\n");*/



	 //MY VERSION
	 usbGetDataBlocking(&usbIn);


	//DEBUG_PRINT("USB data read in successfully\n");
	//DEBUG_PRINT("Read in data %c, now sending...\n", usbIn.data[0]);

	//data[0] = 0x30;

	//usbSendData(1, data);
	//DEBUG_PRINT("USB send complete\n");


	//if the packet looks valid
	if (!usbslkParseIncomingPacket(&slp)) {
		//incoming packet is now in slp

		//DEBUG_PRINT("Parsed USB successfully, routing...\n");

		//route the packet to the appropriate location
		usblinkRouteIncomingPacket(&slp);
	}
	//otherwise the parsing found invalidity, ignore this pkt and get the next incoming USB packet
	else {
	  DEBUG_PRINT("syslinkTask: parse failed, not routing the packet\n");
	}
  }

}
But for some reason, even when I just add the line "if (!usbslkParseIncomingPacket(&slp))", the Crazyflie crashes when I have the Android plugged in and start sending packets to the Crazyflie. This is what's going on if I just add the line "if (!usbslkParseIncomingPacket(&slp))", and comment out the rest of the code above: https://drive.google.com/file/d/1z00OPF ... sp=sharing

If I then comment out the line "usblinkRouteIncomingPacket(&slp);" as well, I get this: https://drive.google.com/file/d/1Tuazq7 ... sp=sharing, which is showing queue inexistent from within an ISR. So I'm guessing it has something to do with one of the functions in usb.c: either usbd_cf_DataIn() or usbd_cf_SOF()...any ideas?

I do see that usblinkTask is by default set up to create CRTP packets from the incoming USB data, but let's say I wanted to change the protocol of the packets and set up my own parser.

Thanks a lot!
Last edited by arnaud on Fri Jan 08, 2021 10:36 am, edited 1 time in total.
Reason: Adding code blocks
arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: Modifying usbLinkTask causing queue error

Post by arnaud »

Hi,

Side note: I edited your message to add code blocks, it makes it much easier to read.

The FreeRTOS assert you are getting might actually comes from memory problem. We have seen it when a task would have its stack overflow. So the first debug advice I would have would be to increase the stack size of the task you are running in. Stack sizes are in config.h: https://github.com/bitcraze/crazyflie-f ... #L154-L184.

Beside that, I am curious as of why you are doing this? As you noted there is already CRTP over USB implemented and syslink is only used to control very platform-specific things like power management and radio settings. Since you are communicating from outside the Crazyflie why not stick to CRTP.
sammy-qq
Beginner
Posts: 4
Joined: Thu Jan 07, 2021 5:57 pm

Re: Modifying usbLinkTask causing queue error

Post by sammy-qq »

Thanks a lot for the response! I'm actually controlling the Crazyflie over Wifi Direct: one phone has the Android client open (modified) and streams packets over Wifi to another phone which is plugged into the drone via USB. CRTP is a little cumbersome to use (both for firmware and Android client) when there's no radio involved, so I'm testing my own basic packet protocol which is a slight modification of CRTP. Like I said, it worked in syslinkTask, but I'm baffled as to why it won't work in usblinkTask.

I did try changing the stack size as you said...that didn't seem to fix anything. Any other ideas?
arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: Modifying usbLinkTask causing queue error

Post by arnaud »

I have new ideas abut what can go wrong but I have a proposal that could allow you to make your own protocol without modifying the low level packet handling:

You can use the "app channel" to transmit your packets in a CRTP packet. There is an API ready to use in the Crazyflie to send and receive packets through this channel, and on the application side you "just" need to prepend your packet with one byte (corresponding to the CRTP header) and when sending them to the Crazyflie and remove the header byte from the packets you receive. The CRTP header for app channel is 0xD3 so:
  • You send to the USB endpoint "0xD3 <payload>"
  • When you receive from the USB endpoint, you discard anything that does not start by 0xD3 and receive packets that have the form "0xD3 <payload>"
On the API side, there is an example showing how to send and receive packet from the app channel: https://github.com/bitcraze/crazyflie-f ... test.c#L35
sammy-qq
Beginner
Posts: 4
Joined: Thu Jan 07, 2021 5:57 pm

Re: Modifying usbLinkTask causing queue error

Post by sammy-qq »

Hi again,

Thanks a lot for that idea, I'm going to play around with that...it looks like I will after all just use CRTP :).

I'd like to use the original radio communication, but also accept USB control packets during the flight (we have something mounted on the drone). With the usbLink enabled and set up, I try changing the CRTP tasks to:

Code: Select all

void crtpTxTask(void *param) {
  CRTPPacket p;

  while (true) {
    if (link != &nopLink) {
      if (xQueueReceive(txQueue, &p, portMAX_DELAY) == pdTRUE) {
        // Keep testing, if the link changes to USB it will go though
        while (link->sendPacket(&p) == false) {
          // Relaxation time
          vTaskDelay(M2T(10));
        }
        stats.txCount++;
        updateStats();
      }
    }

    else if (linkusb != &nopLink && linkusb != link) {
      if (xQueueReceive(txQueue, &p, portMAX_DELAY) == pdTRUE) {
        // Keep testing, if the link changes to USB it will go though
        while (linkusb->sendPacket(&p) == false) {
          // Relaxation time
          vTaskDelay(M2T(10));
        }
        stats.utxCount++;     //usb txt count
        updateStats();
      }

    } else {
      vTaskDelay(M2T(10));
    }
  }
}

void crtpRxTask(void *param) {
  CRTPPacket p;

  while (true) {

    if (link != &nopLink) {
      if (!link->receivePacket(&p)) {

        if (queues[p.port]) {
          if (xQueueSend(queues[p.port], &p, 0) == errQUEUE_FULL) {
            // We should never drop packet
            ASSERT(0);
          }          
        }

        if (callbacks[p.port]) {
          callbacks[p.port](&p);
        }

        stats.rxCount++;
        updateStats();
      }
    }

    //also always check for incoming USB control packets, hoping to override this setpoint from the USB controller
    if (linkusb != &nopLink && linkusb != link) {
      if (!linkusb->receivePacket(&p)) {
        if (queues[p.port]) {
          if (xQueueSend(queues[p.port], &p, 0) == errQUEUE_FULL) {
            // We should never drop packet
            ASSERT(0);
          }
        }

        if (callbacks[p.port]) {
          callbacks[p.port](&p);
        }

        stats.urxCount++;    //usb rx count
        updateStats();
      }
    }


    else {
      vTaskDelay(M2T(10));
    }
  }
}
so that it will always check for queued USB packets, and then while the drone is in the air, I can stream packets to the onboard device via Wifi to adjust the pitch and roll of the drone, etc. (the onboard device will eventually send USB cmds by itself).

It does indeed work (latency isn't as good as the radio, obviously) -- I can control the drone using the modified cf client I made (which uses Wifi Direct -> USB for control). But something interesting is happening now that I added the "if (linkusb != &nopLink && linkusb != link)" section to the rxTask: the drone's ability to take off seems to be impaired. For example, I'm running a simple hover test using the flow deck and the python lib. Our drone can easily lift a pretty heavy payload if I change the "if (linkusb != &nopLink && linkusb != link)" line to an "else if." But when I change it back to an "if" and it checks for USB packets every time, the drone can't quite take off.

I'm guessing some kind of synchronization issue is happening here...?
arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: Modifying usbLinkTask causing queue error

Post by arnaud »

Hi,

One reason I can think about for this problem is that you are sending control packets from both sides: if the Crazyflie is receiving control packets both from USB and from RADIO, both will drive the motors.

One way to test that would be to disconnect any gamepad on the computer that runs the Crazyflie client, this way the client will not send any control packets.

I also see two problems in your design: the link receive functions are blocking, so you are blocking radio packet if there is no USB packet and vice-versa. The other problem is that CRTP has not been designed to accept 2 clients at the same time.

The first problem could be solved by creating a second RX task for the USB port, this way both link are served at full speed.

For the second one, make sure you are not sending any state-full packets from the USB port, ie. no log/param. If you are only sending control packets, there should be not practical problems.
sammy-qq
Beginner
Posts: 4
Joined: Thu Jan 07, 2021 5:57 pm

Re: Modifying usbLinkTask causing queue error

Post by sammy-qq »

Thanks a lot! Problem solved.
feedtalenine
Beginner
Posts: 1
Joined: Mon Mar 15, 2021 2:36 pm

Re: Modifying usbLinkTask causing queue error

Post by feedtalenine »

Thanks for the clarification.
Post Reply