[crazyflie 2.0] [solved] constant crashing and motors failing to activate.

Post here to get support
Post Reply
Noamco
Beginner
Posts: 3
Joined: Sun Jan 03, 2021 10:04 pm

[crazyflie 2.0] [solved] constant crashing and motors failing to activate.

Post by Noamco »

We are trying to make multiple crazyflies broadcast a message constantly and to pass along any message they receive. We made them signal when they received a message and passed it along by activating a single motor.
But we encountered two problems we cannot identify.
The first is that on random accordance one of the crazyflies will stop working. When in this stage only one blue led remain lit and nothing else. It needs to be restarted to work.
The second is that sometimes the crazyfiles keep passing the message along without activating the motors. They usually need to be restarted to work, but sometimes they just work again without doing anything.

If anyone has any suggestions for the source of the two problems please answer.

Here is the code we use, its a modified version of the peer_to_peer example:

Code: Select all

#include <string.h>
#include <stdint.h>
#include <stdbool.h>
#include <stdio.h>

#include "app.h"

#include "FreeRTOS.h"
#include "task.h"
#include "deck.h"
#include "radiolink.h"
#include "configblock.h"

#define DEBUG_MODULE "P2P"

#include "debug.h"

#include "log.h"
#include "param.h"

#include "commander.h"

#include "deck_digital.h"
#include "deck_analog.h"

#define MESSAGE "warning!!!"
#define MESSAGE_LENGHT 11


void p2pcallbackHandler(P2PPacket *p)
{
  // Parse the data from the other crazyflie and print it
  uint8_t other_id = p->data[0];
  static char msg[MESSAGE_LENGHT + 1];
  memcpy(&msg, &p->data[1], sizeof(char)*MESSAGE_LENGHT);
  msg[MESSAGE_LENGHT] = 0;
  uint8_t rssi = p->rssi;

  DEBUG_PRINT("[RSSI: -%d dBm] Message from CF nr. %d, %s\n", rssi, other_id, msg);

  static P2PPacket p_reply;
  p_reply.port=0x00;
    
  // Get the current address of the crazyflie and obtain
  //   the last two digits and send it as the first byte
  //   of the payload
  uint64_t address = configblockGetRadioAddress();
  uint8_t my_id =(uint8_t)((address) & 0x00000000ff);
  p_reply.data[0]=my_id;

  //Put a string in the payload
  char *str="warning!!!";
  memcpy(&p_reply.data[1], str, sizeof(char)*MESSAGE_LENGHT);

  // Set the size, which is the amount of bytes the payload with ID and the string 
  p_reply.size=sizeof(char)*MESSAGE_LENGHT+1;
  vTaskDelay(M2T(200));
  radiolinkSendP2PPacketBroadcast(&p_reply);

  //------------------------------------------------------------------------------------------
 // pinMode(DECK_GPIO_IO1, OUTPUT);     // Set my Led pin to output
 // digitalWrite(DECK_GPIO_IO1, HIGH);  // Light it up
  //------------------------------------------------------------------------------------------

  //------------------------------------------------------------------------------------------
  paramVarId_t idEstimator = paramGetVarId("motorPowerSet", "m1");
  uint16_t new_value = 10000;
  paramSetInt(idEstimator, new_value);

  vTaskDelay(M2T(500));

  new_value = 0;
  paramSetInt(idEstimator, new_value);
  //------------------------------------------------------------------------------------------
}

void appMain()
{
  DEBUG_PRINT("Waiting for activation ...\n");

    uint16_t idUp = logGetVarId("range", "up");
    uint16_t up = logGetUint(idUp);

    paramVarId_t idEstimator = paramGetVarId("motorPowerSet", "enable");
    uint8_t new_value = 1;
    paramSetInt(idEstimator, new_value);
    // In                     itialize the p2p packet 
    static P2PPacket p_reply;
    p_reply.port=0x00;
    
    // Get the current address of the crazyflie and obtain
    //   the last two digits and send it as the first byte
    //   of the payload
    uint64_t address = configblockGetRadioAddress();
    uint8_t my_id =(uint8_t)((address) & 0x00000000ff);
    p_reply.data[0]=my_id;

    //Put a string in the payload
    char *str="warning!!!";
    memcpy(&p_reply.data[1], str, sizeof(char)*MESSAGE_LENGHT);

    // Set the size, which is the amount of bytes the payload with ID and the string 
    p_reply.size=sizeof(char)*MESSAGE_LENGHT+1;

    // Register the callback function so that the CF can receive packets as well.
    p2pRegisterCB(p2pcallbackHandler);

    while(1) 
    {
      // Send a message every 0.5 seconds
      //   Note: if they are sending at the exact same time, there will be message collisions, 
      //    however since they are sending every 0.5 seconds, and they are not started up at the same
      //    time and their internal clocks are different, there is not really something to worry about
        up = logGetUint(idUp);
        if(up < 300.f && up > 0.001f)
        {
            vTaskDelay(M2T(500));
            radiolinkSendP2PPacketBroadcast(&p_reply);
        }
    
    }
}
Last edited by Noamco on Mon Jan 11, 2021 9:22 am, edited 1 time in total.
arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: [crazyflie 2.0] constant crashing and motors failing to activate.

Post by arnaud »

For the first problem, one potential problem is that you are doing too much in the callback function: the P2P callback is called in the context of the radio link and so it should not block. You should send the received packet over a queue or unlock a semaphore in the callback and process the data in another task. I would expect the watchdog to kick-in when freezing the Crazyflie: are you running in debug mode?

As for the motors, there is a mechanism that cuts the motors if the Crazyflie is flipped, maybe you are hitting this?
Noamco
Beginner
Posts: 3
Joined: Sun Jan 03, 2021 10:04 pm

Re: [crazyflie 2.0] constant crashing and motors failing to activate.

Post by Noamco »

Arnaud, thanks for the reply, we tried your suggestion and and made the callback only move the message to a separate queue in a non blocking way, but sadly the problem persisted.

we tried making a version the only receives without sending messages, and it didn't crash even once, we also made one that sent messages without receiving and it crashed regularly, so we are certain the problem has something to do with sending messages.

the version that only sends:

Code: Select all

#include <string.h>
#include <stdint.h>
#include <stdbool.h>
#include <stdio.h>
#include <stdlib.h>
#include <pthread.h>
//#include <semphr.h>
//#include <unistd.h>

#include "app.h"

#include "FreeRTOS.h"
#include "task.h"
#include "deck.h"
#include "radiolink.h"
#include "configblock.h"

#define DEBUG_MODULE "P2P"

#include "debug.h"

#include "log.h"
#include "param.h"

#include "commander.h"

#include "deck_digital.h"
#include "deck_analog.h"

#define MESSAGE "warning!!!"
#define MESSAGE_LENGHT 11


void appMain()
{
  DEBUG_PRINT("Waiting for activation ...\n");

  static P2PPacket p_reply;
  p_reply.port = 0x00;


  uint64_t address = configblockGetRadioAddress();
  uint8_t my_id = (uint8_t)((address)&0x00000000ff);
  p_reply.data[0] = my_id;

  //Put a string in the payload
  char *str = "warning!!!";
  memcpy(&p_reply.data[1], str, sizeof(char) * MESSAGE_LENGHT);

  // Set the size, which is the amount of bytes the payload with ID and the string
  p_reply.size = sizeof(char) * MESSAGE_LENGHT + 1;

  while (1)
  {
    radiolinkSendP2PPacketBroadcast(&p_reply);
    vTaskDelay(M2T(100));
  }
}
the version that only receives:

Code: Select all

#include <string.h>
#include <stdint.h>
#include <stdbool.h>
#include <stdio.h>
#include <stdlib.h>
#include <pthread.h>
//#include <semphr.h>
//#include <unistd.h>

#include "app.h"

#include "FreeRTOS.h"
#include "task.h"
#include "deck.h"
#include "radiolink.h"
#include "configblock.h"

#define DEBUG_MODULE "P2P"

#include "debug.h"

#include "log.h"
#include "param.h"

#include "commander.h"

#include "deck_digital.h"
#include "deck_analog.h"

#define MESSAGE "warning!!!"
#define MESSAGE_LENGHT 11

// A linked list (LL) node to store a queue entry
struct QNode
{
  P2PPacket key;
  struct QNode *next;
};

// The queue, front stores the front node of LL and rear stores the
// last node of LL
struct Queue
{
  struct QNode *front, *rear;
};

struct QNode *newNode(P2PPacket k);
struct Queue *createQueue();
void enQueue(struct Queue *q, P2PPacket k);
void deQueue(struct Queue *q);

uint8_t Mutex;
uint8_t len;
struct Queue *P2PPacket_que;

void p2pcallbackHandler(P2PPacket *p)
{
  /*
  P2PPacket tmp = *p;

  while (Mutex == 1)
  {
  }
  Mutex = 1;

  if(len < 20)
  {
    enQueue(P2PPacket_que, tmp);
    len++;
  }


  Mutex = 0;
  */
  len++;
  DEBUG_PRINT("%d num, from %d\n", len, p->data[0]);
}

void appMain()
{
  DEBUG_PRINT("Waiting for activation ...\n");

  P2PPacket_que = createQueue();
  Mutex = 0;
  len = 0;

  uint16_t idUp = logGetVarId("range", "up");
  uint16_t up = logGetUint(idUp);

  paramVarId_t idEstimator = paramGetVarId("motorPowerSet", "enable");
  uint8_t new_value = 1;
  paramSetInt(idEstimator, new_value);
  // In                     itialize the p2p packet
  static P2PPacket p_reply;
  p_reply.port = 0x00;

  // Get the current address of the crazyflie and obtain
  //   the last two digits and send it as the first byte
  //   of the payload
  uint64_t address = configblockGetRadioAddress();
  uint8_t my_id = (uint8_t)((address)&0x00000000ff);
  p_reply.data[0] = my_id;

  //Put a string in the payload
  char *str = "warning!!!";
  memcpy(&p_reply.data[1], str, sizeof(char) * MESSAGE_LENGHT);

  // Set the size, which is the amount of bytes the payload with ID and the string
  p_reply.size = sizeof(char) * MESSAGE_LENGHT + 1;

  // Register the callback function so that the CF can receive packets as well.
  p2pRegisterCB(p2pcallbackHandler);

  while (1)
  {
    vTaskDelay(M2T(500));
    up = logGetUint(idUp);
    if (up < 300.f && up > 0.001f)
    {
      radiolinkSendP2PPacketBroadcast(&p_reply);
    }


    /*
    while (P2PPacket_que->front != NULL)
    {
      DEBUG_PRINT("recived\n");
      radiolinkSendP2PPacketBroadcast(&p_reply);
      paramVarId_t idEstimator = paramGetVarId("motorPowerSet", "m1");
      uint16_t new_value = 10000;
      paramSetInt(idEstimator, new_value);

      vTaskDelay(M2T(300));

      new_value = 0;
      paramSetInt(idEstimator, new_value);

      while (Mutex == 1)
      {
      }
      Mutex = 1;
      deQueue(P2PPacket_que);
      len--;
      Mutex = 0;
    }
    */
  }
}

// A utility function to create a new linked list node.
struct QNode *newNode(P2PPacket k)
{
  struct QNode *temp = (struct QNode *)malloc(sizeof(struct QNode));
  temp->key = k;
  temp->next = NULL;
  return temp;
}

// A utility function to create an empty queue
struct Queue *createQueue()
{
  struct Queue *q = (struct Queue *)malloc(sizeof(struct Queue));
  q->front = q->rear = NULL;
  return q;
}

// The function to add a key k to q
void enQueue(struct Queue *q, P2PPacket k)
{
  // Create a new LL node
  struct QNode *temp = newNode(k);

  // If queue is empty, then new node is front and rear both
  if (q->rear == NULL)
  {
    q->front = q->rear = temp;
    return;
  }

  // Add the new node at the end of queue and change rear
  q->rear->next = temp;
  q->rear = temp;
}

// Function to remove a key from given queue q
void deQueue(struct Queue *q)
{
  // If queue is empty, return NULL.
  if (q->front == NULL)
    return;

  // Store previous front and move front one node ahead
  struct QNode *temp = q->front;

  q->front = q->front->next;

  // If front becomes NULL, then change rear also as NULL
  if (q->front == NULL)
    q->rear = NULL;

  free(temp);
}

the simplified callback version:

Code: Select all

#include <string.h>
#include <stdint.h>
#include <stdbool.h>
#include <stdio.h>
#include <stdlib.h>
#include <pthread.h>
//#include <semphr.h>
//#include <unistd.h>

#include "app.h"

#include "FreeRTOS.h"
#include "task.h"
#include "deck.h"
#include "radiolink.h"
#include "configblock.h"

#define DEBUG_MODULE "P2P"

#include "debug.h"

#include "log.h"
#include "param.h"

#include "commander.h"

#include "deck_digital.h"
#include "deck_analog.h"

#define MESSAGE "warning!!!"
#define MESSAGE_LENGHT 11

// A linked list (LL) node to store a queue entry
struct QNode
{
  P2PPacket key;
  struct QNode *next;
};

// The queue, front stores the front node of LL and rear stores the
// last node of LL
struct Queue
{
  struct QNode *front, *rear;
};

struct QNode *newNode(P2PPacket k);
struct Queue *createQueue();
void enQueue(struct Queue *q, P2PPacket k);
void deQueue(struct Queue *q);

uint8_t Mutex;
uint8_t len;
struct Queue *P2PPacket_que;

void p2pcallbackHandler(P2PPacket *p)
{
  P2PPacket tmp = *p;

  if (Mutex == 1)
  {
  }
  Mutex = 1;

  if(len < 20)
  {
    enQueue(P2PPacket_que, tmp);
    len++;
  }


  Mutex = 0;
}

void appMain()
{
  DEBUG_PRINT("Waiting for activation ...\n");

  P2PPacket_que = createQueue();
  Mutex = 0;
  len = 0;

  uint16_t idUp = logGetVarId("range", "up");
  uint16_t up = logGetUint(idUp);

  paramVarId_t idEstimator = paramGetVarId("motorPowerSet", "enable");
  uint8_t new_value = 1;
  paramSetInt(idEstimator, new_value);
  // In                     itialize the p2p packet
  static P2PPacket p_reply;
  p_reply.port = 0x00;

  // Get the current address of the crazyflie and obtain
  //   the last two digits and send it as the first byte
  //   of the payload
  uint64_t address = configblockGetRadioAddress();
  uint8_t my_id = (uint8_t)((address)&0x00000000ff);
  p_reply.data[0] = my_id;

  //Put a string in the payload
  char *str = "warning!!!";
  memcpy(&p_reply.data[1], str, sizeof(char) * MESSAGE_LENGHT);

  // Set the size, which is the amount of bytes the payload with ID and the string
  p_reply.size = sizeof(char) * MESSAGE_LENGHT + 1;

  // Register the callback function so that the CF can receive packets as well.
  p2pRegisterCB(p2pcallbackHandler);

  while (1)
  {
    vTaskDelay(M2T(500));
    up = logGetUint(idUp);
    if (up < 300.f && up > 0.001f)
    {
      radiolinkSendP2PPacketBroadcast(&p_reply);
    }

    while (P2PPacket_que->front != NULL)
    {
      DEBUG_PRINT("recived\n");
      radiolinkSendP2PPacketBroadcast(&p_reply);
      paramVarId_t idEstimator = paramGetVarId("motorPowerSet", "m1");
      uint16_t new_value = 10000;
      paramSetInt(idEstimator, new_value);

      vTaskDelay(M2T(300));

      new_value = 0;
      paramSetInt(idEstimator, new_value);

      while (Mutex == 1)
      {
      }
      Mutex = 1;
      deQueue(P2PPacket_que);
      len--;
      Mutex = 0;
    }
  }
}

// A utility function to create a new linked list node.
struct QNode *newNode(P2PPacket k)
{
  struct QNode *temp = (struct QNode *)malloc(sizeof(struct QNode));
  temp->key = k;
  temp->next = NULL;
  return temp;
}

// A utility function to create an empty queue
struct Queue *createQueue()
{
  struct Queue *q = (struct Queue *)malloc(sizeof(struct Queue));
  q->front = q->rear = NULL;
  return q;
}

// The function to add a key k to q
void enQueue(struct Queue *q, P2PPacket k)
{
  // Create a new LL node
  struct QNode *temp = newNode(k);

  // If queue is empty, then new node is front and rear both
  if (q->rear == NULL)
  {
    q->front = q->rear = temp;
    return;
  }

  // Add the new node at the end of queue and change rear
  q->rear->next = temp;
  q->rear = temp;
}

// Function to remove a key from given queue q
void deQueue(struct Queue *q)
{
  // If queue is empty, return NULL.
  if (q->front == NULL)
    return;

  // Store previous front and move front one node ahead
  struct QNode *temp = q->front;

  q->front = q->front->next;

  // If front becomes NULL, then change rear also as NULL
  if (q->front == NULL)
    q->rear = NULL;

  free(temp);
}
as for the motors locking up, the problem indeed was to not flip them.

please answer if you have any more suggestion or know of any way for us to better find the source of the crash.
arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: [crazyflie 2.0] constant crashing and motors failing to activate.

Post by arnaud »

Hi,

I can see the same problem and I cannot explain it. My current assumption is that somehow the nRF51 which is the radio as well as the power management chip is receiving a shutdown command. Maybe there is some problem with the serial communication between the CPUs. I created a ticket to track the issue, this is reproducable using the p2p app in the Crazyflie: https://github.com/bitcraze/crazyflie-f ... issues/670.

I can look at it early next week. If my assumption is right, disabling the syslink packet that shut-down the STM32 in the nRF51 code should solve the problem.
Noamco
Beginner
Posts: 3
Joined: Sun Jan 03, 2021 10:04 pm

Re: [crazyflie 2.0] constant crashing and motors failing to activate.

Post by Noamco »

Hello arnaud,

You were right the problem was exactly what you described and here is what we did to fix it:
  • From crazyflie2-nrf-firmware/src/main.c we Removed line #401

    Code: Select all

     pmSetState(pmSysOff); 
  • ran make
  • Uploaded the cf2_nrf.bin using cfclient's bootloader into both crazyflies
  • Turned them both on and it worked! (both of them were sending and receiving messages from one another)
(Warning: An unintended consequence of doing this would be that it might now become impossible to shut off the stm32 via the nrf51.)

edit:
It seems updating the git also fixes it.
Post Reply