Question related to the state setpoint

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

Question related to the state setpoint

Post by nicuvlad94 »

Hello!

I would like to ask if it is possible that, given a position setpoint, to be also able to adjust the flying velocity to that setpoint.

I use the following configuration to command a setpoint, but the drone flies there too aggressively and sometimes it crashes.

Code: Select all

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;

  setpoint->mode.roll = modeDisable;
  setpoint->mode.pitch = modeDisable;
  setpoint->mode.quat = modeDisable;
}
And I would have a second question: with the above configuration, is it possible to command the drone to spin around the z axis (yaw) while flying somewhere? One option would be to gradually increase the yaw value in modeAbs, but I am curious if it is possible to set a rate, so the drone turns by itself. I tried to put the yaw in mode Velocity while keeping the other settings, but nothing happens (i.e. drone does not spin at all).

Thank you,
Vlad
kimberly
Bitcraze
Posts: 1050
Joined: Fri Jul 06, 2018 11:13 am

Re: Question related to the state setpoint

Post by kimberly »

Hi!

Which controller do you have configured for this? I would imagine that mellinger would be too aggressive but i'd suprised if the PID controller was not able to handle this. At least in the PID controller, you can set the maximum velocity you want the controller to act on, so try to set that to a lower level.

Have you looked at the go_to function of the high level commander? that one creates a more smooth trajectory from one point to another which will prevent crashing as well.

About the second question, could you also share the code snippet for the setpoint of yaw. Are you sure that you are giving it a value in the setpoint?
nicuvlad94
Beginner
Posts: 28
Joined: Sun Sep 08, 2019 12:44 pm
Contact:

Re: Question related to the state setpoint

Post by nicuvlad94 »

Thank you for your answer! I use the PID, but I have the Vicon markers on the drone, the flow deck, and my custom UWB shield (which weights 6g :o ) and this is probably why sometimes it tends to go unstable. But I will have a look at the go_to function and I will tell you how that worked.

Regarding the second problem, here is the code which should set a spinning rate for the yaw.

Code: Select all

static void positionSet_yaw_velocity(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 = modeVelocity;

  setpoint->attitude.yaw = yaw;

  setpoint->mode.roll = modeDisable;
  setpoint->mode.pitch = modeDisable;
  setpoint->mode.quat = modeDisable;
}
and this is the higher level function which calls the above one:

Code: Select all

void headToSetpoint_yaw_velocity(float x, float y, float z, float yaw)
{
    positionSet_yaw_velocity(&setpoint_vlad, x, y, z, yaw);
    commanderSetSetpoint(&setpoint_vlad, 3);
}
I tried to give it several values for the "yaw" parameter, in the range of 0.1 - 100. But the yaw did not move at all.
kimberly
Bitcraze
Posts: 1050
Joined: Fri Jul 06, 2018 11:13 am

Re: Question related to the state setpoint

Post by kimberly »

Also, you are using 3 different positioning systems on the crazyflie, Mocap flowdeck and UWB. I'm even surprised that the kalman filter is able to handle all of those at the same time! Probably try to see if you are able to get one of those position system to work first...

With the yaw rate problem, you should probably fill in this thing instead

Code: Select all

setpoint->attitudeRate.yaw
But just to take a step back, is there a reason why you want to control this behavior onboard? Isn't it enough to use the crazyflie python libary for this instead?
nicuvlad94
Beginner
Posts: 28
Joined: Sun Sep 08, 2019 12:44 pm
Contact:

Re: Question related to the state setpoint

Post by nicuvlad94 »

I don't think that the issue is related to having too many measurements to incorporate. Firstly, because the mocap and flow deck are never used at the same time. I either use the Vicon, which sends positions at every 20ms, or the FlowDeck. The UWB does not work with the EKF. I have my own sensor node which does ranging with the UWB on the Crazyflie (so there is a different task in the CF which just acquires the range). The goal of my project is to perform node localization with CF.

Also, something offtopic: I tried (just out of curiosity) to use my node as a fixed, known-position anchor to localize the drone. So I had the Flow Deck and that one anchor, whose range measurement was fed into the EKF. But it seems that the CF was flying in the wall (which is quite unexpected, in theory, the drone should be stable even without the flow deck, because with one UWB anchor and IMU the system is observable).

Coming to the topic, the second problem: thank you for the hint! I will try to adjust that code line accordingly. I need to have all the logic working in the drone, because this is the purpose of my research. To have the drone working autonomously.
kimberly
Bitcraze
Posts: 1050
Joined: Fri Jul 06, 2018 11:13 am

Re: Question related to the state setpoint

Post by kimberly »

Ah thank you for clarifying :) Thought you used all 3 positioning systems at the same time.

Reason I was asking for if you needed to do it all onboard is that it is not completely obvious to everyone how the general ecosystem works and that there is actually a python library to be used and mingle with things they do not really understand. But it seems so that you know what you are doing so please carry on! :D

Hope the change of code will help you! give us an update.
nicuvlad94
Beginner
Posts: 28
Joined: Sun Sep 08, 2019 12:44 pm
Contact:

Re: Question related to the state setpoint

Post by nicuvlad94 »

Hello!

It seems that, by adjusting the yaw rate as you suggested works really well!

Yes, I know about the python library, I use it to log data for postprocessing (I have a two-thread script, which simultaneously logs data from the CF and Vicon), but the final demonstrator is supposed to work independently.

One more issue:D. I want to use your go_to function from "crtp_commander_high_level".
So I took the essential part from there and I put it in my functions like this:

Code: Select all

void init_planner()
{
    plan_init(&planner);
}

int go_to(float x_setpoint, float y_setpoint, float z_setpoint, float yaw)
{
  int result = 0;
  struct vec hover_pos = mkvec(x_setpoint, y_setpoint, z_setpoint);
  float t = usecTimestamp() / 1e6;
  result = plan_go_to(&planner, 0, hover_pos, yaw, 3.0f, t);
  
  return result;
}
So I first call the init_planner function and then I try to use go_to to send the drone to specific waypoints, but it seems that the drone does not move at all. I mention that for takeoff I use my own function, implemented as above (first post, with a setpoint structure).

Do you have any guess regarding this issue?

Thank you!
kristoffer
Bitcraze
Posts: 630
Joined: Tue Jun 30, 2015 7:47 am

Re: Question related to the state setpoint

Post by kristoffer »

You have to activate the high level commander first by setting

Code: Select all

cf.param.set_value('commander.enHighLevel', '1')
Post Reply