Flying Crazyflie to a specific position setpoint

Discussions about all things Bitcraze
nicuvlad94
Beginner
Posts: 15
Joined: Sun Sep 08, 2019 12:44 pm
Contact:

Flying Crazyflie to a specific position setpoint

Post by nicuvlad94 » Mon Jan 20, 2020 10:08 am

Hello!

I have a question regarding driving crazyflie to different locations. I basically want to do this completely in firmware, where a specific algorithm (coded by me) generates the (x,y,z) coordinates where the Crazyflie has to fly. The issue I have is how to send commands from my algorithm to the position controller of Crazyflie.
As far as I read through the code, the stabilizer deals with state estimation and drone control, so I will probably have to inform the stabilizer (more exactly commander) about the setpoint the drone has to reach, right? Would simply calling commanderSetSetpoint("targer setpoint") solve my problem?

Another question I have regards the setpoint structure. When I have to set the mode for x,y,z, it is quite clear that I choose modeAbs. What would you recommend to set for the mode of roll, pitch and yaw?

And a third question: I noticed that the stabilizer task starts when the system boots. What is the thing that changes in the stabilizer flow when the drone takes off? What prevents the motor to spin after the drone boots and what has to be set to trigger a take-off?

Thank you!



arnaud
Bitcraze
Posts: 2275
Joined: Tue Feb 06, 2007 12:36 pm

Re: Flying Crazyflie to a specific position setpoint

Post by arnaud » Mon Jan 20, 2020 12:57 pm

Hi,

For your first question, yes you should call commanderSetSetpoint() at regular interval. You can see an example of that in the multiranger push demo: https://github.com/ataffanel/crazyflie- ... #L105-L106.

You need to fill up the setpoint structure to set X,Y,Z as absolute indded, you can set YAW to whatever you want, absolute or relative (absolute is likely what you need). Roll and pitch should be left untouched/disabled since the position controller will need to control roll and pitch to control the Crazyflie acceleration and hence its position. You can find an example of how to fill-up the structure for position control in the commander generic packet decoder: https://github.com/bitcraze/crazyflie-f ... #L352-L368. Note that the full structure is zeroed before the decoding function is called, you should make sure it is the case in your code, this is what allows us to only set what we need and not care about roll/pitch.

If you zero the full structure and send it to the commander, it is going to stop the motors. Zeroing the structure will set it in manual thrust mode and set the thrust to 0. The take-off happens when a non-0 setpoint structure is set. There is a watchdog in the commander that will cut the motors if setpoints are not sent often enough, you should always sent at least one setpoint per seconds. In the hover demo we cut the motors there: https://github.com/ataffanel/crazyflie- ... #L133-L134

nicuvlad94
Beginner
Posts: 15
Joined: Sun Sep 08, 2019 12:44 pm
Contact:

Re: Flying Crazyflie to a specific position setpoint

Post by nicuvlad94 » Mon Jan 20, 2020 3:19 pm

Thank you for the answers! These solve my issues!
One thing looks a bit odd to me. I am trying to print the state information in the client, when the drone is on, but no motor spinning. The data I am getting for the position is 0 for x and y, and around 297 for the z. While z varies when I move the drone, x and y stay to zero. Do you know why this happens? Also, why is the initialization of z close to 297m?
Thank you!

Note: I am calling the print function right after these lines in stabilizer.c
stateEstimator(&state, &sensorData, &control, tick);
compressState();

nicuvlad94
Beginner
Posts: 15
Joined: Sun Sep 08, 2019 12:44 pm
Contact:

Re: Flying Crazyflie to a specific position setpoint

Post by nicuvlad94 » Tue Jan 21, 2020 1:32 pm

I thought it could be that fact that it lacks the Flow Deck! So I attached the flow deck, I wrote the following code:

This is for the function:

Code: Select all

static setpoint_t setpoint;

static void positionSet(setpoint_t *setpoint, float x, float y, float z, float yaw)
{
  memset(setpoint, 0, sizeof(setpoint_t));

  setpoint->mode.x = modeAbs;
  setpoint->mode.y = modeAbs;
  setpoint->mode.z = modeAbs;

  setpoint->position.x = x;
  setpoint->position.y = y;
  setpoint->position.z = z;


  setpoint->mode.yaw = modeAbs;

  setpoint->attitude.yaw = yaw;
}

And then I create a task which calls this function:

Code: Select all

static void flyTask(void* parameters) {

  systemWaitStart();
  vTaskDelay(5000);

  while(1) 
  {
    vTaskDelay(10);

    positionSet(&setpoint, 0, 0, 1, 0);
    commanderSetSetpoint(&setpoint, 3);
  }
}
The drone tries to take off, but it just flips itself and then stops. I would expect it to go to one meter height and hover there. Do you have any idea why this happens? I mention that, if I try to lift the drone from the phone app, it works well!

Thank you!
Vlad

kimberly
Bitcraze
Posts: 250
Joined: Fri Jul 06, 2018 11:13 am

Re: Flying Crazyflie to a specific position setpoint

Post by kimberly » Thu Jan 23, 2020 9:28 am

Hi!

you might need to check if the positioning estimate is correct. Also with the flow deck, the crazyflie' state estimate does not start exactly at 0,0,0. So maybe it would be good to include that in the script as well.

Since you haven't tried the flowdeck before, have you tried the examples/motion_commander_demo.py from the https://github.com/bitcraze/crazyflie-lib-python. ? That is a good test to see if everything is going correctly and you can also use it as an example as well.

nicuvlad94
Beginner
Posts: 15
Joined: Sun Sep 08, 2019 12:44 pm
Contact:

Re: Flying Crazyflie to a specific position setpoint

Post by nicuvlad94 » Thu Jan 23, 2020 10:47 am

I've just tried to run the drone with the client. I call just this function: pc.go_to(0.0, 0.0, 1.0);
Drone goes up slowly towards 1m height, but in 2 seconds it goes unstable and crashes.
Same with the Z Deck v2. Drones takes off but it immediately turns around, spinning the motors against the ground.
Again, if I try to use the phone client, it takes of smoothly. So it is not a mechanical issue.
When I print the state information in the client, before take off, it looks fine.
Do you have any ideas why would that happen?

kimberly
Bitcraze
Posts: 250
Joined: Fri Jul 06, 2018 11:13 am

Re: Flying Crazyflie to a specific position setpoint

Post by kimberly » Fri Jan 24, 2020 7:04 am

If manual flight with the mobile client shows that is flying properly, it is indeed not a problem with the hardware of the crazyflie. It seems that the state estimation of the crazyflie is off. Actually, the go_to function should not be possible if the flowdeck is not attached (or any other external positioning system), so for the zranger I understand this behaviour, but I'm suprised you are seeing this behavior with the flowdeck as well.

Could you paste the console output of the cfclient so we can determine that the flowdeck is correctly detected?

nicuvlad94
Beginner
Posts: 15
Joined: Sun Sep 08, 2019 12:44 pm
Contact:

Re: Flying Crazyflie to a specific position setpoint

Post by nicuvlad94 » Fri Jan 24, 2020 9:39 am

This is the output:

Code: Select all

SYS: ----------------------------
SYS: Crazyflie 2.1 is up and running!
SYS: Build 137:26b58b428a10 (2019.09 +137) MODIFIED
SYS: I am 0x203937434848500D004B0023 and I have 1024KB of flash!
CFGBLK: v1, verification [OK]
DECK_DRIVERS: Found 17 drivers
DECK_DRIVERS: VID:PID BC:01 (bcLedRing)
DECK_DRIVERS: VID:PID BC:04 (bcBuzzer)
DECK_DRIVERS: VID:PID BC:07 (bcGTGPS)
DECK_DRIVERS: VID:PID 00:00 (bcCPPM)
DECK_DRIVERS: VID:PID BC:08 (bcUSD)
DECK_DRIVERS: VID:PID BC:09 (bcZRanger)
DECK_DRIVERS: VID:PID BC:0E (bcZRanger2)
DECK_DRIVERS: VID:PID BC:06 (bcDWM1000)
DECK_DRIVERS: VID:PID BC:0A (bcFlow)
DECK_DRIVERS: VID:PID BC:0F (bcFlow2)
DECK_DRIVERS: VID:PID BC:0B (bcOA)
DECK_DRIVERS: VID:PID BC:0C (bcMultiranger)
DECK_DRIVERS: VID:PID BC:11 (bcActiveM)
DECK_DRIVERS: VID:PID 00:00 (vlad)
DECK_DRIVERS: VID:PID BC:FF (bcExpTest)
DECK_DRIVERS: VID:PID BC:FE (bcExpTestRR)
DECK_DRIVERS: VID:PID BC:FD (bcBoltTest)
DECK_INFO: Found 1 deck memory.
DECK_INFO: Enumerating deck 0
DECK_INFO: Deck BC:0F bcFlow2 (Rev. A)
DECK_INFO: Used pin: 0000000C
DECK_INFO: Driver implements: [ init test ]
DECK_INFO: DECK_FORCE=vlad found
DECK_INFO: compile-time forced driver vlad added
DECK_CORE: 2 deck(s) found
DECK_CORE: Calling INIT on driver bcFlow2 for deck 0
ZR2: Z-down sensor [OK]
PMW: Motion chip id: 0x49:0xB6
DECK_CORE: Calling INIT on driver vlad for deck 1
vlad: Vlad's task started
vlad: Checking DWM
vlad: DWM1000 init failed
IMU: BMI088 Gyro I2C connection [OK].
IMU: BMI088 Accel I2C connection [OK]
IMU: BMP388 I2C connection [OK]
ESTIMATOR: Estimator type forced
ESTIMATOR: Using Kalman (2) estimator
CONTROLLER: Using PID (1) controller
MTR-DRV: Using brushed motor driver
EEPROM: I2C connection [OK].
DECK_CORE: Deck 0 test [OK].
DECK_CORE: Deck 1 test [OK].
STAB: Wait for sensor calibration...
SYS: Free heap: 4608 bytes
STAB: Ready to fly.

I also created my custom deck, but all the tasks which are run in that deck are commented for the moment!

nicuvlad94
Beginner
Posts: 15
Joined: Sun Sep 08, 2019 12:44 pm
Contact:

Re: Flying Crazyflie to a specific position setpoint

Post by nicuvlad94 » Fri Jan 24, 2020 2:27 pm

I think I found the issue! It seems that having debug mode on ("DEBUG=1 in the makefile") crashes the drone while flying.
So basically, if I compile the project with DEBUG=1 and then I command a trajectory with "go_to" from Python, drone flies for 2 or 3 seconds and then the system crashes (because two LEDs stay on and red). Also propellers continue to spin forever, therefore is definitely a task/system crash.

Thank you!
Vlad

nicuvlad94
Beginner
Posts: 15
Joined: Sun Sep 08, 2019 12:44 pm
Contact:

Re: Flying Crazyflie to a specific position setpoint

Post by nicuvlad94 » Fri Jan 24, 2020 4:32 pm

There would be other two questions I need to ask:
1. How would be a nice way to get acknowledge of when a setpoint has been reached. For instance if I command the drone to go somewhere, how do I know that target position was reached? I guess I can look into the setpoint Queue in the commander but that would imply modifying the file. Does it have any built in functions?
2. How do I access the current state, without using a shared variable with the stabilizer?
Usually, I would like my projects to keep the base sources untouched and only add the additional files. So it is easy for someone else to replicate my work!

Thank you!
Vlad

Post Reply