implementing collision avoidance in FW

Firmware/software/electronics/mechanics
wydmynd
Beginner
Posts: 26
Joined: Thu Dec 06, 2018 9:25 am

implementing collision avoidance in FW

Post by wydmynd » Tue Jun 11, 2019 2:51 pm

Hello
are you aware of anyone who implemented collision avoidance in firmware?
I am using the mellinger controller so I would prefer to implement on it.
I tried running collision avoidance routine on the PC but sometimes it has lag or communication glitches and doesn't work well.

even pointers on any of these sub-tasks -
1. sample a log variable (ranges from range deck)
2. create a "task" that checks every X ms , or maybe implement this on the ranger deck driver itself?
3. create a message for the mellinger controller with the new setpoint if collision is detected (I would like to send a simpler command, but from my experience, if a low-level command is sent while high-level controller is running, the localization of the flow deck will stop working for the duration of the command)

any samples, references to articles or posts where people post new functions in CF firmware will be very welcome.



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

Re: implementing collision avoidance in FW

Post by arnaud » Wed Jun 12, 2019 7:25 am

Hi,
We have a version of the multiranger 'push' demo that works in the firmware, this is a simple form of obstacle avoidance. We also have the ICRA2019 demo that used the high-level-commander from the firmware.

So for 1) and 2) you can look at the firmware-implemented push demo: https://github.com/bitcraze/crazyflie-f ... src/push.c. This is implemented as a deck driver, as it is one of the easiest way to add a task running in the Crazyflie (ie. it is the way that requires the minimum amount of changes). There is an API to read log variables from within the firmware.

For 3) you can look at our ICRA2019 demo, in this demo we send commands to the high level commander from the Crazyflie firmware: https://github.com/bitcraze/crazyflie-f ... app.c#L325. This branch was more of an experimentation since it is not using the deck driver methods, an app.c file has been added and a couple of function have been added to control the high-level commander from within the firmware. If you want to use the same approach you can start from the ICRA2019 branch. We are planning to look at making an easy-to-use app layer with documented public C API a bit later in the summer and the ICRA demo was a way to play with the idea.

wydmynd
Beginner
Posts: 26
Joined: Thu Dec 06, 2018 9:25 am

Re: implementing collision avoidance in FW

Post by wydmynd » Thu Jun 13, 2019 1:49 pm

thank you very much! I have searched for few hours and found no similar code on the web, academic articles or in the forums.
I will try to implement using a "virtual deck" method first, as I assume it has lower chance of creating bugs.
report here when I have progress

wydmynd
Beginner
Posts: 26
Joined: Thu Dec 06, 2018 9:25 am

Re: implementing collision avoidance in FW

Post by wydmynd » Tue Jun 18, 2019 9:57 am

Hi again!

I tried to follow the instructions. for the first method (push.c as a "virtual deck")
first I tried cloning the proper branch of crazyflie-experimental and building it, but there were many errors.
So I started from a fresh VM ,
added only the push.c file in the deck/driver/src folder
added the compile flag to the makefile -

Code: Select all

PROJ_OBJ += push.o

made a copy of the config.mk.example and named it config.mk
added the lines to config.mk -

Code: Select all

CFLAGS += -DDECK_FORCE=bcPush
DEBUG=1
and then -

Code: Select all

make clean
make all
make cload
the message "CC push.o" appears in the make history ..

when connecting I get

Code: Select all

SYS: Crazyflie 2.0 is up and running!
SYS: Build 0:a203d748e139 (2018.12) MODIFIED
SYS: I am 0x30343631303747100031002D and I have 1024KB of flash!
CFGBLK: v1, verification [OK]
DECK_DRIVERS: Found 17 drivers
DECK_DRIVERS: VID:PID 00:00 (bcRZR)
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 00:00 (bcLH8)
DECK_DRIVERS: VID:PID 00:00 (bcPush)
DECK_DRIVERS: VID:PID BC:FF (bcExpTest)
DECK_DRIVERS: VID:PID BC:FE (bcExpTestRR)
DECK_INFO: Found 2 deck memories.
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: Enumerating deck 1
DECK_INFO: Deck BC:0C bcMultiranger (Rev. E)
DECK_INFO: Used pin: 0000000C
DECK_INFO: Driver implements: [ init test ]
DECK_INFO: compile-time forced driver bcPush added
DECK_CORE: 3 deck enumerated
DECK_CORE: Calling INIT from driver bcFlow2 for deck 0
ZR2: Z-down sensor [OK]
PMW: Motion chip id: 0x49:0xB6
DECK_CORE: Calling INIT from driver bcMultiranger for deck 1
DECK_CORE: Calling INIT from driver bcPush for deck 2
MPU9250 I2C connection [OK].
AK8963 I2C connection [OK].
LPS25H I2C connection [OK].
ESTIMATOR: Using estimator 2
CONTROLLER: Using controller 1
EEPROM: I2C connection [OK].
AK8963: Self test [OK].
DECK_CORE: Deck 0 test [OK].
MR: Init front sensor [OK]
MR: Init back sensor [OK]
MR: Init up sensor [OK]
MR: Init left sensor [OK]
MR: Init right sensor [OK]
DECK_CORE: Deck 1 test [OK].
DECK_CORE: Deck 2 test [OK].
SYS: Free heap: 7360 bytes
Waiting for activation ...
I try to takeoff using high level controller , the drone does not take off. after stop command is sent, two red LEDs turn on.
can you suggest the reason?
I understand the push.c code is sending hover commands, so maybe there is a conflict with the high level controller?

thanks

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

Re: implementing collision avoidance in FW

Post by arnaud » Wed Jun 19, 2019 9:18 am

Hi,

From what you are describing you successfuly managed to run the push demo in your firmware, now if you have both a flow and a multiranger deck installed you will be able to have the Crazyflie taking off by putting your hand close to the top of the multiranger deck. To achieve this functionality the push.c file controls the Crazyflie and you should not expect to be able to control it any other way.

If you want to be able to both control the Crazyflie from outside and having it avoiding obstacle using the high level controller, you have to modify push.c to send high-level commands and not low level commands as it is the case right now.

The low level set point system implemented in the crazyflie is designed to handle multiple sources with priority however I do not think the high-level commander is designed that way. This means that if you want to send high-level commands both from the ground and from withing the firmware you have to be a bit careful about how things are implemented and if it would work at all (I think that as long as you are only using GOTO commands, you should be safe for instance).

The best might be to pass all commands through your own code: send all commands to your code and have your code deciding what commands needs to be sent to the high-level commander, this way you do not risk conflicts between the firmware and the ground commands, and you can implement higher level functionalities like going around obstacles to reach the wanted setpoint.

wydmynd
Beginner
Posts: 26
Joined: Thu Dec 06, 2018 9:25 am

Re: implementing collision avoidance in FW

Post by wydmynd » Wed Jun 19, 2019 1:23 pm

Thanks! it works! I was focused on the compiling and running I missed this functionality in the code.

regarding your comment on high level control, from my experience you can send low level set points to a drone running high level controller, but after the command finishes executing the drone will return to the starting point where the command was issued. I guess something with the state estimate is not being updated.
This is actually too bad , because now if I am running a high level controller and have a collision (for example front sensor) I need to calculate the angle of the front sensor in world coordinates (the drone may rotated) and send a position setpoint in that direction.
I plan to do this by sampling the stateestimate log variable, and then doing the transformation.

Regarding your comment on conflicts between the firmware and the ground commands, I need to find a function that can "sample" the high level setpoints being sent to the drone. currently the method in your code can sample log variables (logGetUint)

I will be very thankful if you can point me to code in the experimental repo that I can analyze

regards

wydmynd
Beginner
Posts: 26
Joined: Thu Dec 06, 2018 9:25 am

Re: implementing collision avoidance in FW

Post by wydmynd » Wed Jun 19, 2019 2:36 pm

I found crtpCommanderHighLevelGetSetpoint

https://github.com/bitcraze/crazyflie-f ... nder.c#L87

I assume it allows polling the current go_to setpoint. but if I want to pass all commands through my code I will have to modify the highlevelcommander
-- insert some code between where it retrieves the setpoint from the crtp queue and the section where it is sent to the planner

If you have any thoughts/comments I will be glad to hear..

wydmynd
Beginner
Posts: 26
Joined: Thu Dec 06, 2018 9:25 am

Re: implementing collision avoidance in FW

Post by wydmynd » Wed Jun 26, 2019 1:48 pm

OK , I have done alot of testing and have some success!
I succeeded in making the CF stop whenever it encounters obstacles.
But it is doing it with a high-level position command , so it works when I use a high-level controller.

This required changing the go_to function in crtp_commander_high_level.c and crtp_commander_high_level.h to add the crtpCommanderHighLevelGoTo function from the ICRA2019 link.

I am still having problems as this is not very stable. one out of 3-4 attempts the drone will light two red leds and go crazy .. :D
I cannot isolate the cause or the situation when this happens, seems random. can't imagine how it can be collision of commands as I am sending very little commands (see code below)

debugging is very hard!
- when sending commands using python I cannot see the console output while flying. only way I know is to use crazyflie_ROS (not tested yet)
- I do not understand what the "debug driver" does.
- if I turn the DEBUG=1 flag to 0 in config.mk it will stop on all warnings (there are like a 100 warnings..) I need to learn how to turn it off..

attached is the code -- all code and binary for instant flashing are in https://github.com/wydmynd/collision_avoidance

1. push.c example was stripped to a bare minimum -- it just checks if the raw ranges are smaller than X , sends a stop command (relative) and waits for 1.5 second to allow the pilot/autopilot to move away from the obstacle. the function is enabled only over specific height.

Code: Select all

#include "deck.h"
#include "system.h"
#include "commander.h"
#include "crtp_commander_high_level.h"
#include <float.h>
#include "FreeRTOS.h"
#include "task.h"

#include "debug.h"

#include "log.h"

#define DEBUG_MODULE "PUSH"

typedef enum {
	idle, lowUnlock, unlocked, stopping
} State;

static State state = idle;

static const uint16_t unlockThHigh = 200;
static const uint16_t collisionTh = 170;

static uint16_t idUp = 0;
static uint16_t idDown = 0;
static uint16_t idLeft = 0;
static uint16_t idRight = 0;
static uint16_t idFront = 0;
static uint16_t idBack = 0;

static uint16_t up = 0;
static uint16_t down = 0;
static uint16_t front = 0;
static uint16_t back = 0;
static uint16_t left = 0;
static uint16_t right = 0;


#define MAX(a,b) ((a>b)?a:b)
#define MIN(a,b) ((a<b)?a:b)

static void sequenceTask() {
	static setpoint_t setpoint;

	systemWaitStart();

	vTaskDelay(M2T(3000));

	idUp = logGetVarId("range", "up");
	idDown = logGetVarId("range", "zrange");
	idLeft = logGetVarId("range", "left");
	idRight = logGetVarId("range", "right");
	idFront = logGetVarId("range", "front");
	idBack = logGetVarId("range", "back");

	DEBUG_PRINT("Waiting for activation ...\n");

	while (1) {
		vTaskDelay(M2T(100));
		//DEBUG_PRINT(".");
		down = logGetUint(idDown); //check height for unlocking coll avoid

		if (state == unlocked) {
			up = logGetUint(idUp);
			left = logGetUint(idLeft);
			right = logGetUint(idRight);
			front = logGetUint(idFront);
			back = logGetUint(idBack);

			if (front<collisionTh && front>0 || left<collisionTh && left>0 || right<collisionTh && right>0 || back<collisionTh && back>0) {
				//DEBUG_PRINT("coll. detected\n");
				float timeToStop = 0.65;
				crtpCommanderHighLevelGoTo(0, 0, 0, 0.0, timeToStop, true, 0);
				vTaskDelay(M2T(1500)); //chance to move in opposite direction
			}


		} else {


			if (down > unlockThHigh && state == idle) {
				DEBUG_PRINT("coll. avoid unlocked!\n");
				state = unlocked;
			}


		}
	}
}

static void sequenceInit() {
	xTaskCreate(sequenceTask, "sequence", 2*configMINIMAL_STACK_SIZE, NULL,
	/*priority*/3, NULL);
}

static bool sequenceTest() {
	return true;
}

const DeckDriver sequence_deck = { .vid = 0, .pid = 0, .name = "bcPush",

.usedGpio = 0,  // FIXME: set the used pins

		.init = sequenceInit, .test = sequenceTest, };

DECK_DRIVER(sequence_deck);
2. this is the code I am using to fly the drone. simply takeoff, move 1 meter forward, land. tested with and without mellinger controller.

Code: Select all

Simple example that connects to one crazyflie (check the address at the top
and update it to your crazyflie address) and uses the high level commander
to send setpoints and trajectory to fly a figure 8.

This example is intended to work with any positioning system (including LPS).
It aims at documenting how to set the Crazyflie in position control mode
and how to send setpoints using the high level commander.
"""
import time

import cflib.crtp
from cflib.crazyflie import Crazyflie
from cflib.crazyflie.log import LogConfig
from cflib.crazyflie.mem import MemoryElement
from cflib.crazyflie.mem import Poly4D
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
from cflib.crazyflie.syncLogger import SyncLogger

# URI to the Crazyflie to connect to
#uri = 'radio://0/120/2M/E7E7E7E707'
uri = 'radio://0/120/2M/E7E7E7E701'

# The trajectory to fly
# See https://github.com/whoenig/uav_trajectories for a tool to generate
# trajectories



def reset_estimator(cf):
    cf.param.set_value('kalman.resetEstimation', '1')
    time.sleep(0.1)
    cf.param.set_value('kalman.resetEstimation', '0')
    time.sleep(2)

def activate_high_level_commander(cf):
    cf.param.set_value('commander.enHighLevel', '1')


def activate_mellinger_controller(cf):
    cf.param.set_value('stabilizer.controller', '2')
    time.sleep(0.1)
    cf.param.set_value('ctrlMel.kp_z', '0.6')

def run_sequence(cf, trajectory_id, duration):
    commander = cf.high_level_commander
    print('taking off')
    commander.takeoff(0.4, 2.0)
    time.sleep(3.0)
    commander.go_to(1.0,0,0,0,duration,relative=True)
    time.sleep(duration)
    print('landing')
    commander.land(0.1, 2.0)
    time.sleep(1.2)
    commander.stop()


if __name__ == '__main__':
    cflib.crtp.init_drivers(enable_debug_driver=False)

    with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf:
        cf = scf.cf
        trajectory_id = 1

        activate_high_level_commander(cf)
        time.sleep(0.5)
        activate_mellinger_controller(cf)
        time.sleep(0.5)
        duration = 6.5
        # print('The sequence is {:.1f} seconds long'.format(duration))
        reset_estimator(cf)
        run_sequence(cf, trajectory_id, duration)

kimberly
Expert
Posts: 107
Joined: Fri Jul 06, 2018 11:13 am

Re: implementing collision avoidance in FW

Post by kimberly » Mon Jul 01, 2019 11:40 am

Hi!

Two red LED's means there is a hard fault, and since you disabled watchdog (which is the mode of DEBUG=1), which this can happen during runtime.

If you put DEBUG=0 (and fix all the warnings first ofcourse), and your problem will occur again, watchdog will reboot the Crazyflie and save the location of the hard fault / assert in the code. So if you start the Crazyflie again and look at the read-out in the cfclient console, it should show where it went wrong in your code.

If you do manage to do that, could you copy paste the error message in this thread?

wydmynd
Beginner
Posts: 26
Joined: Thu Dec 06, 2018 9:25 am

Re: implementing collision avoidance in FW

Post by wydmynd » Mon Jul 01, 2019 1:18 pm

it gave me about 100 warnings (mostly about variable casting, and none of them from code I actually wrote..)
but I disabled "treat warnings as errors" and compiled with DEBUG=0

next time I have this problem I will report back

Post Reply