Ultrasonic replacement to the zranger

Firmware/software/electronics/mechanics
Evan
Beginner
Posts: 10
Joined: Thu Oct 26, 2017 3:04 pm
Location: United States

Ultrasonic replacement to the zranger

Post by Evan »

Hello, all!

Currently, continuing on my project to make an autonomous crazyflie 2.0, I am working on using a MaxBotics ultrasonic sensor to find range in the z direction as opposed to using the actual zranger, since I intend to use the zranger to find horizontal distance for collision mitigation.

Looking at the driver for the MaxBotics ultrasonic sensor that is integrated into the firmware made me somewhat confused, and so I instead thought it would be a good idea to instead take information from the files maxsonar.c, maxsonar.h, zranger.c, and zranger.h, and use them to build a custom driver for the ultrasonic sensor that I will force in config.mk and hope will cause the ultrasonic sensor (model: MaxSonar-EZ4) act more like the zranger in the firmware.

In doing this I have come upon a small issue, but I cannot seem to figure out how to solve it. The issue is an error in my C program, at the point where I declare the pins used by the deck.

Code: Select all

static const DeckDriver MaxSonarMathai_deck = {
	/*
	 * this function tells the drone what the name of the deck is, what ports it uses
	 * and what the name in the makefile is for it. (I think.)
	 * -MK
	 */

		.vid = 0xBC,
		.pid = 0x09,
		.name = "MaxSonarMathai"
		.usedGpio = DECK_GPIO_TX2,

		.init = MaxSonarMathaiInit,
};
Regardless of what I do, there is an error at .usedGpio = DECK_GPIO_TX2 which states "Field 'usedGpio' could not be resolved, despite the fact that I included the line:

Code: Select all

#define MAXSONAR_DECK_GPIO DECK_GPIO_TX2


In the include file for the program.

I think it may have something to do with the deck API, such as the code here:

Code: Select all

typedef struct deck_driver {
  /* Identification of the deck (written in the board) */
  uint8_t vid;
  uint8_t pid;
  char *name;

  /* Periphreal and Gpio used _dirrectly_ by the driver */
  uint32_t usedPeriph;
  uint32_t usedGpio;

  /* Init and test functions */
  void (*init)(struct deckInfo_s *);
  bool (*test)(void);
} DeckDriver;
Which I am not sure what it does or where to implement it, but if it isn't the problem then I won't mess with it.

If anyone has any suggestions or fixes, any help would be greatly appreciated. Especially since at this point I have absolutely no idea what I've done wrong. :? Also, my apologies if this is a super simple fix. I'm still somewhat new to all of this. :oops:
arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: Ultrasonic replacement to the zranger

Post by arnaud »

First of all: this field is not required to get your driver to work, it is only uses to automatically detect conflict between decks.

In your code there is a coma missing after "MaxSonarMathai", maybe it is the problem?

As a side note about the comments in your code: MaxSonarMathai_deck is not a function, it is a structure instantiation. The structure is going to be stored in flash and the "DECK_DRIVER()" macro adds it to a special section that allows the deck API to detect and use it. The name in the makefile tells the deck API what deck driver to force, it looks in the list of structures in the special section and enable the one that matches. The exact details is not important but I though I could clarify what this code is doing :-).
Evan
Beginner
Posts: 10
Joined: Thu Oct 26, 2017 3:04 pm
Location: United States

Re: Ultrasonic replacement to the zranger

Post by Evan »

Thank you, Arnaud! Your clarification of the structure, and your spotting my missing comma did the trick. Now the program will compile, which is great news! Thank you very much!

Now I have another problem. The driver that compiles and builds just fine doesn't work on the Crazyflie. (v 2.0) for some reason, which I cannot figure out. I think it may have something to do with my creation of a task. I tried to use a similar format to that that is found in other deck drivers (for instance the zranger deck) but even then I feel my understanding of how to create a task is somewhat limited. If you could provide some pointers, I'd really appreciate it!

The code that I think may be troubling the drone is below, if anyone wants to take a look at it. Thanks in advance for any assistance! :)

Code: Select all

void MaxSonarTask(void* arg){

	void maxSonarReadDistance(maxSonarSensor_t MB1040, uint32_t *accuracy){
		MaxSonarDistance = 0;
		MaxSonarDistance = (uint32_t) (IN2MM(analogRead(DECK_GPIO_TX2))/8);

		MaxSonarAccuracy = 0;
		if(accuracy){
			*accuracy = MaxSonarAccuracySense(MaxSonarDistance);
		}

	}

	float convert = 0.001;
	bool MaxSonarReadRange(zDistance_t*zrange, const uint32_t tick){
		bool updated = false;

		if(isInit){
			if(MaxSonarDistance != 0 && MaxSonarDistance < RANGE_OUTER_LIMIT()){
				zrange->distance = (float)MaxSonarDistance*convert;
				zrange->timestamp = tick;
				updated = true; //I think this has to be called at a rate of at least 500hz? -MK
			}
		}
		return updated;
	}

}

void MaxSonarInt(DeckInfo*info){
	if(isInit)
		return;

	xTaskCreate(MaxSonarTask, "MaxSonarMK" , configMINIMAL_STACK_SIZE, NULL, 3, NULL);

	isInit = true;
}
Here is the proto in the include file:

Code: Select all

void MaxSonarTask(void* arg);

bool MaxSonarReadRange(zDistance_t* zrange, const uint32_t tick);
arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: Ultrasonic replacement to the zranger

Post by arnaud »

The task creation, and more generally the init function, looks good.

Though the task function does nothing, it is only declaring two nested functions. Nested functions is a quite obscure GCC extension, it is not a standard C functionality and I would avoid using it.

For a good example, the zranger actually has a very typical deck driver task: https://github.com/bitcraze/crazyflie-f ... c#L90-L117

Basically a typical task will call "systemWaitStart" to wait for the system to be started and in working order (self test passed), initialize everything that needs to be initialized, and will have an infinite loop doing some work and sleeping for a while. Sleeping is important to leave some time for the rest of the system to run.

Note that the zranger driver currently has a small bug in the way it uses vTaskDelayUntil.
Evan
Beginner
Posts: 10
Joined: Thu Oct 26, 2017 3:04 pm
Location: United States

Re: Ultrasonic replacement to the zranger

Post by Evan »

Thank you for your help, Arnaud! I however could still use some clarification. I am struggling with understanding what does what in the zranger files. I have been snooping around with it and trying to understand it a bit more. My main question has to do with these two following code snippets. I am wondering exactly what each one does, since I can see where both of them may have a hand in informing the drone of its height off the ground. If anyone could provide some clarification it would be greatly appreciated!

The two functions in question are first this one, which accesses the sensors_cf2.c file:

Code: Select all

bool zRangerReadRange(zDistance_t* zrange, const uint32_t tick)
{
  bool updated = false;

  if (isInit) {
    if (range_last != 0 && range_last < RANGE_OUTLIER_LIMIT) {
      zrange->distance = (float)range_last * 0.001f; // Scale from [mm] to [m]
      zrange->timestamp = tick;
      updated = true;
    }
  }
  return updated;
}
And then this function that accesses the kalman estimator:

Code: Select all

if (getStateEstimator() == kalmanEstimator &&
        range_last < RANGE_OUTLIER_LIMIT) {
      // Form measurement
      tofMeasurement_t tofData;
      tofData.timestamp = xTaskGetTickCount();
      tofData.distance = (float)range_last * 0.001f; // Scale from [mm] to [m]
      tofData.stdDev = expStdA * (1.0f  + expf( expCoeff * ( tofData.distance - expPointA)));
      estimatorKalmanEnqueueTOF(&tofData);
    }
Any clarification with respect to what either of these do would be greatly appreciated! Thank you in advance! :)
arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: Ultrasonic replacement to the zranger

Post by arnaud »

The first sniplet verifies that the last range measurement is correct and fill-up the zDistance_t structure with the range if it is the case. This function is used by other part of the firmware to get the range to the floor using the zranger. I think it is mostly used by the complementary filter (ie. not the kalman filter), this is not good since this is adding a hard-dependency between the core code and the z-ranger deck. I am creating a ticket in the firmware so that we can look at it (https://github.com/bitcraze/crazyflie-f ... issues/307).

The second sniplet pushes the measurement into the kalman filter if the kalman filter is currently used. This is a better design and this is something you can reproduce with your sensor.

So, for now, you can compile the crazyflie with "ESTIMATOR=kalman" in config.mk and use the second sniplet as an example to push your measurements in the kalman filter. stddev should be set to the standard deviation of your sensor measurement.
Evan
Beginner
Posts: 10
Joined: Thu Oct 26, 2017 3:04 pm
Location: United States

Re: Ultrasonic replacement to the zranger

Post by Evan »

Thank you for your help! After I did the procedure and set the estimator to be the kalman estimator in the config makefile, I did get numbers from the drone! :) Thanks!!

Now, however, I have inevitably run across another issue. :cry: When I start up the drone, the drone does successfully start up, but as soon as I try to connect the drone to the control it, the done throws an error and refuses to send or receive information and turns the two LED's solid red. I think this is due to how I have the timing set up (vTaskDelay) for my task loop, but I don't know what else to put in its place. I also think this may be as a result of my standard deviation calculation, which is similar to the one that is in the zranger file. If someone could take a look at my code below and figure out what is going wrong, it would be more than greatly appreciated! Thanks for any help in advance! :)

Here is the code for the main task:

Code: Select all

void MaxSonarTask(void* arg){
	systemWaitStart();
	
	//Start loop
	while(1){

		float MaxSonarDistance2 = MaxSonarReadDistance();
		
		if (getStateEstimator() == kalmanEstimator && MaxSonarDistance2 < RANGE_OUTER_LIMIT()){
			tofMeasurement_t tofData;

			tofData.distance = MaxSonarDistance2*0.001f;
			tofData.stdDev = expStdA * (1.0f  + expf( expCoeff() * ( tofData.distance - expPointA)));
			estimatorKalmanEnqueueTOF(&tofData);
		}
		
		vTaskDelay(M2T(1));
		
	}//end infinite loop
}//end task
Within the above ^ task, are mentioned these functions:
Code to actually read the output from the sensor:

Code: Select all

float MaxSonarReadDistance(){
	MaxSonarDistance = 0;
	MaxSonarDistance = (uint32_t) (IN2MM(analogRead(DECK_GPIO_TX2))/8);
	return MaxSonarDistance;
}
Code to output expCoeff:

Code: Select all

static float expPointA = 0.508f;
static float expStdA = 0.0254f;      // STD at elevation expPointA [m]
static float expPointB = 6.2484f;
static float expStdB = 0.0254f;      // STD at elevation expPointB [m]

float expCoeff(){
	float expCoeff = logf(expStdB / expStdA) / (expPointB - expPointA);
	return expCoeff;
}
Code to change inches to mm:

Code: Select all

float IN2MM(float x){
	float M = 0;
	M = x*25.4f;
	return M;
}
Code to output max value:

Code: Select all

float RANGE_OUTER_LIMIT(){
	float Limit = IN2MM(254);
	return Limit;
}
arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: Ultrasonic replacement to the zranger

Post by arnaud »

Hi,

What you are seeing is a hard-fault or assert, it can have any number of reason. If you have a debug cable you can connect it to the Crazyflie and inspect the situation with GDB to see where the hard fault happens. You can also compile without debug mode: if the Crazyflie locks it is most likely that you are currently in debug mode with DEBUG=1 in config.mk, otherwise the watchdog would kick-in and reset, after reset you will see in the client on the console where there was an assert if any.

As for the delay, delay are good, they allow to give the CPU back to other tasks. Have you tried to increase the delay to 10 or 100ms? This would give more headroom to the rest of the system.
ycolmenares
Beginner
Posts: 10
Joined: Tue May 15, 2018 7:39 am

Re: Ultrasonic replacement to the zranger

Post by ycolmenares »

Hello Evan,
I'm doing the same project, but I'm using the crazyflie 1.0 and the maxsonar EZ0. I have some problems with the code. Can I find a post with your code or something like that? I would like to take a look at your work.
Thanks in advance.
Evan
Beginner
Posts: 10
Joined: Thu Oct 26, 2017 3:04 pm
Location: United States

Re: Ultrasonic replacement to the zranger

Post by Evan »

Hello, Ycolmenares! I would be happy to share my code with you. Do keep well in mind that this code is still a work in progress and thus is *VERY* messy with poorly commented sections and sections removed entirely via commenting. There are also sections and functions that do not need to be in the code that are included in these files since at first I thought they would be useful. Also remember that in the case of the crazyflie 2 at least, this driver must be forced in the config makefile as shown below:

Code: Select all

CFLAGS += DDECK_FORCE=MaxSonarMK
and must be included in the main makefile as shown below:

Code: Select all

PROJ_OBJ_CF2 += MaxSonarMK.o
Also needed is a line implemented into the file sensors_cf2.c inside the sensorsAquire structure on line 183 that declares the MaxSonar sensor, as shown below:

Code: Select all

MaxSonarReadRange(&sensors ->zrange,tick);
The .h and .c file can be found at this link, which is a link to a google drive folder containing the two files.

https://drive.google.com/drive/folders/ ... sp=sharing

I hope that this helps you! I also apologize for how long it took me to respond, as I didn't have access to my work laptop for a few days. :oops: Best of luck to everyone! Have a wonderful day! :D
Post Reply