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 ..
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)