[Help Needed] Custom Commander Packet for Individual Motor Control

[sml]

Hi, so I'm trying to send setpoint commands using the crazyflie python library such that I can control the motors independent of each other.

I defined a new function called send_motor_setpoint within commander.py of the crazyflie python library (https://github.com/bitcraze/crazyflie-lib-python)

Code: Select all

    def send_motor_setpoint(self, m1, m2, m3, m4):
        """
        Control mode where the individual motor values are set 
        """
        pk = CRTPPacket()
        pk.port = CRTPPort.COMMANDER_GENERIC
        # <BHHHH as m1 to m4 are uint16_t, TYPE_MOTOR is set to 8
        pk.data = struct.pack('<BHHHH', TYPE_MOTOR, m1, m2, m3, m4)
        self._cf.send_packet(pk)

I also defined the additional parameters in stabilizer_types.h of the crazyflie-firmware (https://github.com/bitcraze/crazyflie-firmware)

Code: Select all

typedef struct motor_s {
  uint16_t m1;
  uint16_t m2;
  uint16_t m3;
  uint16_t m4;
} motor_t

typedef struct setpoint_s {
  uint32_t timestamp;

  attitude_t attitude;      // deg
  attitude_t attitudeRate;  // deg/s
  quaternion_t attitudeQuaternion;
  float thrust;
  point_t position;         // m
  velocity_t velocity;      // m/s
  acc_t acceleration;       // m/s^2
  bool velocity_body;       // true if velocity is given in body frame; false if velocity is given in world frame
  motor_t motor;

  struct {
    stab_mode_t x;
    stab_mode_t y;
    stab_mode_t z;
    stab_mode_t roll;
    stab_mode_t pitch;
    stab_mode_t yaw;
    stab_mode_t quat;
  } mode;
} setpoint_t;

and added a decoder in crtp_commander_generic.c of the firmware

Code: Select all

struct motorPacket_s {
  uint16_t m1;
  uint16_t m2;
  uint16_t m3;
  uint16_t m4;
} __attribute__((packed));
static void motorDecoder(setpoint_t *setpoint, uint8_t type, const void *data, size_t datalen)
{
  const struct motorPacket_s *values = data;

  setpoint->motor.m1 = values->m1;
  setpoint->motor.m2 = values->m2;
  setpoint->motor.m3 = values->m3;
  setpoint->motor.m4 = values->m4;
}

Now, this is the part where I get stuck. I've edited stabilizer.c and power_distribution_stock.c to use the m1 to m4 values to set the motor power ratios, but running the code give me these problems:

• The script cannot even locate send_motor_setpoint (Edit: Solved this problem by reinstalling library from source and making it editable)
• Editing an existing function (example send_position_setpoint) to basically be send_motor_setpoint then allows the script to run but nothing happens

I'm a bit lost as to what to do. Am I missing another script I need to change? Please be gentle with me, I'm new to the whole programming scene haha. Thank you in advance for your help!

I know that there is a set of functions that will let me set individual motor parameters but the issue with that approach is that the motors can only be set one at a time (with latency inbetween) instead of all at once

[jonasdn]

Hi sml!

It is hard to know what the issue here, it could be many things. I would say that a good next step would be to make sure that:

• The packet you are sending is reaching the firmware

• Your modified firmware code is doing what you expect it to do

You can add DEBUG_PRINT statements in the code and check the console output, either in the client or by adding code in your python script to get the console characters (you can check how the the cfclient does this in ConsoleTab.py.

Good luck!

[sml]

Thank you for the advice, didn't know DEBUG_PRINT existed! I have miraculously gotten it working now that I started from scratch. I probably just had a typo somewhere.