Setting AltHold mode directly in the firmware

[twh]

Hi all,

I am trying to make the Crazyflie 2.0 be autonomous such that it can fly up vertically and stay at certain altitude. The following is what I have written to replace the commanderGetSetpoint() in stabiliser task. However, it doesn't work but behaves differently.

The code I wrote is exactly based on the commnderGetSetpoint(). I have assigned all the same variables as what have assigned in the commanderGetSetpoint().

However,
I was trying to let it fly to certain height vertically by setting thrust to 34000 for the first 1600 ticks.
Then, after 1600 ticks, it is expected to stay at that height by setting thrust to 0 and velocity to ((float)32767-32767.f)/32767.f, however, in this case, it falls directly and seems like it doesn't go into alt hold mode. . While it is falling, the four propellers are still spinning.

Could anyone help me solve it? I am not sure if I miss anything important e.g. sensors, wrong value setting? Thank you very much.

Code: Select all

void exampleGetSetpoint(setpoint_t *setpoint, const state_t * state)
{
	static int counter = 0;
	int ticks;
	ticks = xTaskGetTickCount();
	static int initial;
	if (counter == 0) {
		initial = ticks;
	}

	setpoint->mode.x = modeDisable;
	setpoint->mode.y = modeDisable;

	setpoint->mode.roll = modeAbs;
	setpoint->attitudeRate.roll = 0;
	setpoint->attitude.roll = 0;

	setpoint->mode.pitch = modeAbs;
	setpoint->attitudeRate.pitch = 0;
	setpoint->attitude.pitch = 0;

	setpoint->velocity.x = 0;
	setpoint->velocity.y = 0;

	setpoint->attitudeRate.yaw = 0;
	setpoint->mode.yaw = modeVelocity;

	if (counter == 0) {
		setpoint->thrust = 0;
		setpoint->mode.z = modeDisable;
	}
	else if (ticks < (1600+initial)) {
		setpoint->thrust = 34000;
		setpoint->mode.z = modeDisable;
		setpoint->attitude.roll = 1;
		setpoint->attitude.pitch = 1;

	}
	else if ((ticks >= (1600+initial)) && (ticks<(6000+initial))) {
		setpoint->thrust = 0;
		setpoint->mode.z = modeVelocity;
		setpoint->velocity.z = ((float)32767-32767.f)/32767.f;
	}
	else {
		setpoint->thrust = 0;
		setpoint->mode.z = modeDisable;
	}

	counter ++;
}

[arnaud]

Hi,

Can you test what you are trying with a gamepad? This way you will see if it is a problem of the alt-hold algorithm or if there is a problem with your sequencing.

Your code looks OK, my only comment is that the structure you are filling up is mostly in SI units so if you want 0m/s of Z velocity you should just write:

Code: Select all

      setpoint->thrust = 0;
      setpoint->mode.z = modeVelocity;
      setpoint->velocity.z = 0;

This is a bit more readable .

If you do not have a positioning system of any way to measure accurately the altitude, it will be very hard to obtain autonomous flight that way though.

[twh]

Hi,

Can you test what you are trying with a gamepad? This way you will see if it is a problem of the alt-hold algorithm or if there is a problem with your sequencing.

Your code looks OK, my only comment is that the structure you are filling up is mostly in SI units so if you want 0m/s of Z velocity you should just write:

Code: Select all

      setpoint->thrust = 0;
      setpoint->mode.z = modeVelocity;
      setpoint->velocity.z = 0;

This is a bit more readable .

If you do not have a positioning system of any way to measure accurately the altitude, it will be very hard to obtain autonomous flight that way though.

Hi, thanks for your reply.

Due to lack of Crazyradio PA (ordered but arrives two weeks later), I am not able to fly it by using gamepad for now, which requires the Crazyradio PA (If I am not wrong).

Without Crazyradio for now, I started to do other observations to inspect which part goes wrong. One of what I did was dealing with sensors.I plot the value of barometer from the client. (I directly connect Crazyflie to USB port). I placed the Crazyflie on the flat surface (table) in a indoor aircon room. The result is as follows,


Is it normal? I m not sure how the barometer behave. Is there any wiki page or references talking about the collaboration/interactions between altitude hold mode and barometer or other important elements? Or, is there any other way to figure out where the problem is without using Crazyradio PA, e.g. code inspection? inter-task communication?

Thank you.

[twh]

The following link is the video which Crazyflie seems failing to get into altitude hold mode based on the code I wrote

http://sendvid.com/2wgdb9rw

[arnaud]

The log you took looks OK, the altitude taken by the pressure sensor is not super stable and it depends a lot of the air pressure stability in the room.

You can access log variable from within the crazyflie (the LED ring driver is doing that for some effects), but since you have no way to communicate them to the ground it will not help you much. Having a Crazyradio is a must for this kind of development.

One random idea: you are taking off very quickly after starting the copter. You can try to wait a bit, like at least 10 seconds, before taking off. It is possible that the pressure reading is not stable so early and take some time to stabilize.

[twh]

Hi,

Thanks for your reply. I finally got the Crazyradio PA and now am able to set the altitude hold mode.

One thing I realised is that when it is in altitude hold mode, the Crazyflie 2.0 drifts to one side (M4). I couldn't find any clues from this forum. May I know whether it is related to the PID or assembly issue? Sometimes, the Crazyflie 2.0 couldn't flying up straight and would drift to the M4 sides too. Will the rate/attitude PID help in this situation?

Can I know what factors (e.g. sensor calibration, PID, position control etc) actually affects the flight/ flight path. According to some posts from the forums, it seems like there is no any direct argument talking about this. (Sorry, I am not a hardware guy).

Thank you.

[arnaud]

You will not achieve X/Y stability for very long without extra sensors.

The only thing the Crazyflie can do is to keep its orientation, so you can ask for roll and pitch to be 0 degrees which in theory should keep it stable with 0 X/Y acceleration. However there is a lot of other factor that will make the Crazyflie drift away: difference between the motors, the propeller, alignement of the motors, wind, .... These can only be compensated using extra sensors, for example a camera looking at the Crazyflie to estimate its X/Y position.

[kenjichanhkg]

I'm planning to add more sensors to do that, something like Z-ranger.

Do you sell any Z-ranger? But it is only enough to avoid collision, since the VL53L0x ToF sensor can only measure distance up to 2 meters, which too short for SLAM.

[arnaud]

The Z-ranger has a VL53 facing down so we mainly intend it as an help for altitude-hold.

It is currently reaching end of production and should be available soon.