From af442fbd6e90ff18f775cdae123e06e984106e67 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Wed, 7 Mar 2018 13:08:40 -0800 Subject: [PATCH] Add High-Level Commander, see #293 The initial high-level commander supports takeoff, landing, stop, and goTo. Each CF can be part of one of 8 groups, and commands can be broadcasted for specific groups, only. Tested in a mocap system with kalman estimator and mellinger controller using crazyflie_ros (high-level-setpoints branch; test_high_level.py). --- Makefile | 2 + src/config/config.h | 3 + src/modules/interface/crtp.h | 1 + .../interface/crtp_commander_high_level.h | 62 +++ src/modules/interface/planner.h | 81 ++++ src/modules/interface/pptraj.h | 182 +++++++++ src/modules/src/commander.c | 16 +- src/modules/src/controller_mellinger.c | 4 +- src/modules/src/crtp_commander_high_level.c | 292 +++++++++++++ src/modules/src/planner.c | 145 +++++++ src/modules/src/pptraj.c | 383 ++++++++++++++++++ 11 files changed, 1169 insertions(+), 2 deletions(-) create mode 100644 src/modules/interface/crtp_commander_high_level.h create mode 100644 src/modules/interface/planner.h create mode 100644 src/modules/interface/pptraj.h create mode 100644 src/modules/src/crtp_commander_high_level.c create mode 100644 src/modules/src/planner.c create mode 100644 src/modules/src/pptraj.c diff --git a/Makefile b/Makefile index 366a6d98ec..93c6fc0d58 100644 --- a/Makefile +++ b/Makefile @@ -151,6 +151,8 @@ PROJ_OBJ += controller_$(CONTROLLER).o PROJ_OBJ += power_distribution_$(POWER_DISTRIBUTION).o PROJ_OBJ_CF2 += estimator_kalman.o +# High-Level Commander +PROJ_OBJ += crtp_commander_high_level.o planner.o pptraj.o # Deck Core PROJ_OBJ_CF2 += deck.o deck_info.o deck_drivers.o deck_test.o diff --git a/src/config/config.h b/src/config/config.h index 0c84cea65d..ce01fece71 100644 --- a/src/config/config.h +++ b/src/config/config.h @@ -91,6 +91,7 @@ #define USDLOG_TASK_PRI 1 #define USDWRITE_TASK_PRI 0 #define PCA9685_TASK_PRI 3 +#define CMD_HIGH_LEVEL_TASK_PRI 2 #define SYSLINK_TASK_PRI 5 #define USBLINK_TASK_PRI 3 @@ -125,6 +126,7 @@ #define USDLOG_TASK_NAME "USDLOG" #define USDWRITE_TASK_NAME "USDWRITE" #define PCA9685_TASK_NAME "PCA9685" +#define CMD_HIGH_LEVEL_TASK_NAME "CMDHL" //Task stack sizes #define SYSTEM_TASK_STACKSIZE (2* configMINIMAL_STACK_SIZE) @@ -149,6 +151,7 @@ #define USDLOG_TASK_STACKSIZE (2 * configMINIMAL_STACK_SIZE) #define USDWRITE_TASK_STACKSIZE (2 * configMINIMAL_STACK_SIZE) #define PCA9685_TASK_STACKSIZE (2 * configMINIMAL_STACK_SIZE) +#define CMD_HIGH_LEVEL_TASK_STACKSIZE configMINIMAL_STACK_SIZE //The radio channel. From 0 to 125 #define RADIO_CHANNEL 80 diff --git a/src/modules/interface/crtp.h b/src/modules/interface/crtp.h index 18c63b7dfb..f06d7fd0df 100644 --- a/src/modules/interface/crtp.h +++ b/src/modules/interface/crtp.h @@ -44,6 +44,7 @@ typedef enum { CRTP_PORT_LOG = 0x05, CRTP_PORT_LOCALIZATION = 0x06, CRTP_PORT_SETPOINT_GENERIC = 0x07, + CRTP_PORT_SETPOINT_HL = 0x08, CRTP_PORT_PLATFORM = 0x0D, CRTP_PORT_LINK = 0x0F, } CRTPPort; diff --git a/src/modules/interface/crtp_commander_high_level.h b/src/modules/interface/crtp_commander_high_level.h new file mode 100644 index 0000000000..d26e4dc065 --- /dev/null +++ b/src/modules/interface/crtp_commander_high_level.h @@ -0,0 +1,62 @@ +/* + * ______ + * / ____/________ _____ __ ________ ______ __________ ___ + * / / / ___/ __ `/_ / / / / / ___/ | /| / / __ `/ ___/ __ `__ \ + * / /___/ / / /_/ / / /_/ /_/ (__ )| |/ |/ / /_/ / / / / / / / / + * \____/_/ \__,_/ /___/\__, /____/ |__/|__/\__,_/_/ /_/ /_/ /_/ + * /____/ + * + * Crazyswarm advanced control firmware for Crazyflie + * + +The MIT License (MIT) + +Copyright (c) 2018 Wolfgang Hoenig and James Alan Preiss + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. +*/ + +/* +Header file for high-level commander that computes smooth setpoints based on high-level inputs. +*/ + +#ifndef CRTP_COMMANDER_HIGH_LEVEL_H_ +#define CRTP_COMMANDER_HIGH_LEVEL_H_ + +#include +#include + +#include "math3d.h" + +#include "stabilizer_types.h" + +/* Public functions */ +void crtpCommanderHighLevelInit(void); + +// Retrieves the current setpoint +void crtpCommanderHighLevelGetSetpoint(setpoint_t* setpoint, const state_t *state); + +// Tell the trajectory planner that it should cut power. +// Should be used if an emergency is detected. +void crtpCommanderHighLevelStop(); + +// True if we have landed or emergency-stopped. +bool crtpCommanderHighLevelIsStopped(); + +#endif /* CRTP_COMMANDER_HIGH_LEVEL_H_ */ diff --git a/src/modules/interface/planner.h b/src/modules/interface/planner.h new file mode 100644 index 0000000000..ba336eddc9 --- /dev/null +++ b/src/modules/interface/planner.h @@ -0,0 +1,81 @@ +/* + * ______ + * / ____/________ _____ __ ________ ______ __________ ___ + * / / / ___/ __ `/_ / / / / / ___/ | /| / / __ `/ ___/ __ `__ \ + * / /___/ / / /_/ / / /_/ /_/ (__ )| |/ |/ / /_/ / / / / / / / / + * \____/_/ \__,_/ /___/\__, /____/ |__/|__/\__,_/_/ /_/ /_/ /_/ + * /____/ + * + * Crazyswarm advanced control firmware for Crazyflie + * + +The MIT License (MIT) + +Copyright (c) 2018 Wolfgang Hoenig and James Alan Preiss + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. +*/ + +/* +Header file for planning state machine +*/ + +#pragma once + +#include "math3d.h" +#include "pptraj.h" + +enum trajectory_state +{ + TRAJECTORY_STATE_IDLE = 0, + TRAJECTORY_STATE_FLYING = 1, + TRAJECTORY_STATE_LANDING = 3, +}; + +struct planner +{ + enum trajectory_state state; // current state + bool reversed; // true, if trajectory should be evaluated in reverse + const struct piecewise_traj* trajectory; // pointer to trajectory +}; + +// initialize the planner +void plan_init(struct planner *p); + +// tell the planner to stop. +// subsequently, plan_is_stopped(p) will return true, +// and it is no longer valid to call plan_current_goal(p). +void plan_stop(struct planner *p); + +// query if the planner is stopped. +// currently this is true at startup before we take off, +// and also after an emergency stop. +bool plan_is_stopped(struct planner *p); + +// get the planner's current goal. +struct traj_eval plan_current_goal(struct planner *p, float t); + +// start a takeoff trajectory. +int plan_takeoff(struct planner *p, struct vec pos, float yaw, float height, float duration, float t); + +// start a landing trajectory. +int plan_land(struct planner *p, struct vec pos, float yaw, float height, float duration, float t); + +// move to a given position, then hover there. +int plan_go_to(struct planner *p, bool relative, struct vec hover_pos, float hover_yaw, float duration, float t); diff --git a/src/modules/interface/pptraj.h b/src/modules/interface/pptraj.h new file mode 100644 index 0000000000..b05cf2eaa3 --- /dev/null +++ b/src/modules/interface/pptraj.h @@ -0,0 +1,182 @@ +/* + * ______ + * / ____/________ _____ __ ________ ______ __________ ___ + * / / / ___/ __ `/_ / / / / / ___/ | /| / / __ `/ ___/ __ `__ \ + * / /___/ / / /_/ / / /_/ /_/ (__ )| |/ |/ / /_/ / / / / / / / / + * \____/_/ \__,_/ /___/\__, /____/ |__/|__/\__,_/_/ /_/ /_/ /_/ + * /____/ + * + * Crazyswarm advanced control firmware for Crazyflie + * + +The MIT License (MIT) + +Copyright (c) 2018 Wolfgang Hoenig and James Alan Preiss + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. +*/ + +/* +Header file for piecewise polynomial trajectories +*/ + +#pragma once + +#include "math3d.h" + +#define PP_DEGREE (7) +#define PP_SIZE (PP_DEGREE + 1) +#define PP_MAX_PIECES (30) + + +// +// 1d polynomial functions. +// coefficients are stored in ascending order, i.e. p[0] is the constant term. +// + +// evaluate a polynomial using horner's rule. +float polyval(float const p[PP_SIZE], float t); + +// construct a linear polynomial from p(0) = x0 to p(duration) = x1. +void polylinear(float p[PP_SIZE], float duration, float x0, float x1); + +// plan a degree-5 polynomial with the given duration T, +// and given initial/final position, velocity, and acceleration +void poly5(float poly[PP_SIZE], float T, + float x0, float dx0, float ddx0, + float xf, float dxf, float ddxf); + +// scale a polynomial in place. +void polyscale(float p[PP_SIZE], float s); + +// compute the derivate of a polynomial in place. +void polyder(float p[PP_SIZE]); + +// e.g. if s==2 the new polynomial will be stretched to take 2x longer +void polystretchtime(float p[PP_SIZE], float s); + +// reflect a polynomial about the x-axis, e.g. p_after(-x) == p_before(x) +void polyreflect(float p[PP_SIZE]); + + +// +// 4d single polynomial piece for x-y-z-yaw, includes duration. +// + +struct poly4d +{ + float p[4][PP_SIZE]; + float duration; // TODO use int millis instead? +}; + +// construct a 4d zero polynomial. +struct poly4d poly4d_zero(float duration); + +// construct a 4d linear polynomial. +struct poly4d poly4d_linear(float duration, + struct vec p0, struct vec p1, float yaw0, float yaw1); + +// scale a 4d polynomial in-place. +void poly4d_scale(struct poly4d *p, float x, float y, float z, float yaw); + +// shift a 4d polynomial by the given values in-place. +void poly4d_shift(struct poly4d *p, float x, float y, float z, float yaw); +static inline void poly4d_shift_vec(struct poly4d *p, struct vec pos, float yaw) { + poly4d_shift(p, pos.x, pos.y, pos.z, yaw); +} + +// e.g. if s==2 the new polynomial will be stretched to take 2x longer. +void poly4d_stretchtime(struct poly4d *p, float s); + +// compute the derivative of a 4d polynomial in-place. +void polyder4d(struct poly4d *p); + +// compute loose maximum of acceleration - +// uses L1 norm instead of Euclidean, evaluates polynomial instead of root-finding +float poly4d_max_accel_approx(struct poly4d const *p); + + +// output of differentially flat 4d polynomials. +struct traj_eval +{ + struct vec pos; + struct vec vel; + struct vec acc; + struct vec omega; + float yaw; +}; + +// a special value of traj_eval that indicates an invalid result. +struct traj_eval traj_eval_invalid(void); + +// check if a traj_eval represents an invalid result. +bool is_traj_eval_valid(struct traj_eval const *ev); + +// evaluate a single polynomial piece +struct traj_eval poly4d_eval(struct poly4d const *p, float t); + + + +// ----------------------------------// +// piecewise polynomial trajectories // +// ----------------------------------// + +struct piecewise_traj +{ + struct poly4d pieces[PP_MAX_PIECES]; + float t_begin; + unsigned char n_pieces; +}; + +static inline float piecewise_duration(struct piecewise_traj const *pp) +{ + float total_dur = 0; + for (int i = 0; i < pp->n_pieces; ++i) { + total_dur += pp->pieces[i].duration; + } + return total_dur; +} + +void piecewise_plan_5th_order(struct piecewise_traj *p, float duration, + struct vec p0, float y0, struct vec v0, float dy0, struct vec a0, + struct vec p1, float y1, struct vec v1, float dy1, struct vec a1); + +void piecewise_plan_7th_order_no_jerk(struct piecewise_traj *p, float duration, + struct vec p0, float y0, struct vec v0, float dy0, struct vec a0, + struct vec p1, float y1, struct vec v1, float dy1, struct vec a1); + +struct traj_eval piecewise_eval( + struct piecewise_traj const *traj, float t); + +struct traj_eval piecewise_eval_reversed( + struct piecewise_traj const *traj, float t); + +void piecewise_scale(struct piecewise_traj *pp, float x, float y, float z, float yaw); + +void piecewise_shift(struct piecewise_traj *pp, float x, float y, float z, float yaw); +static inline void piecewise_shift_vec(struct piecewise_traj *pp, struct vec pos, float yaw) { + piecewise_shift(pp, pos.x, pos.y, pos.z, yaw); +} + +void piecewise_stretchtime(struct piecewise_traj *pp, float s); + +static inline bool piecewise_is_finished(struct piecewise_traj const *traj, float t) +{ + return (t - traj->t_begin) >= piecewise_duration(traj); +} diff --git a/src/modules/src/commander.c b/src/modules/src/commander.c index 180d2a36a4..3e293f58ae 100644 --- a/src/modules/src/commander.c +++ b/src/modules/src/commander.c @@ -31,12 +31,16 @@ #include "commander.h" #include "crtp_commander.h" +#include "crtp_commander_high_level.h" + +#include "param.h" static bool isInit; const static setpoint_t nullSetpoint; const static int priorityDisable = COMMANDER_PRIORITY_DISABLE; static uint32_t lastUpdate; +static bool enableHighLevel = false; QueueHandle_t setpointQueue; QueueHandle_t priorityQueue; @@ -53,6 +57,7 @@ void commanderInit(void) xQueueSend(priorityQueue, &priorityDisable, 0); crtpCommanderInit(); + crtpCommanderHighLevelInit(); lastUpdate = xTaskGetTickCount(); isInit = true; @@ -78,7 +83,12 @@ void commanderGetSetpoint(setpoint_t *setpoint, const state_t *state) uint32_t currentTime = xTaskGetTickCount(); if ((currentTime - setpoint->timestamp) > COMMANDER_WDT_TIMEOUT_SHUTDOWN) { - memcpy(setpoint, &nullSetpoint, sizeof(nullSetpoint)); + if (enableHighLevel) { + crtpCommanderHighLevelGetSetpoint(setpoint, state); + } + if (!enableHighLevel || crtpCommanderHighLevelIsStopped()) { + memcpy(setpoint, &nullSetpoint, sizeof(nullSetpoint)); + } } else if ((currentTime - setpoint->timestamp) > COMMANDER_WDT_TIMEOUT_STABILIZE) { xQueueOverwrite(priorityQueue, &priorityDisable); // Leveling ... @@ -110,3 +120,7 @@ int commanderGetActivePriority(void) xQueuePeek(priorityQueue, &priority, 0); return priority; } + +PARAM_GROUP_START(commander) +PARAM_ADD(PARAM_UINT8, enHighLevel, &enableHighLevel) +PARAM_GROUP_STOP(commander) diff --git a/src/modules/src/controller_mellinger.c b/src/modules/src/controller_mellinger.c index 905222205a..9912ba3eb3 100644 --- a/src/modules/src/controller_mellinger.c +++ b/src/modules/src/controller_mellinger.c @@ -179,7 +179,9 @@ void stateController(control_t *control, setpoint_t *setpoint, // Rate-controled YAW is moving YAW angle setpoint if (setpoint->mode.yaw == modeVelocity) { - desiredYaw = state->attitude.yaw - setpoint->attitudeRate.yaw * dt; + desiredYaw = state->attitude.yaw - setpoint->attitudeRate.yaw * dt; + } else if (setpoint->mode.yaw == modeAbs) { + desiredYaw = setpoint->attitude.yaw; } else if (setpoint->mode.quat == modeAbs) { struct quat setpoint_quat = mkquat(setpoint->attitudeQuaternion.x, setpoint->attitudeQuaternion.y, setpoint->attitudeQuaternion.z, setpoint->attitudeQuaternion.w); struct vec rpy = quat2rpy(setpoint_quat); diff --git a/src/modules/src/crtp_commander_high_level.c b/src/modules/src/crtp_commander_high_level.c new file mode 100644 index 0000000000..00031bdcf2 --- /dev/null +++ b/src/modules/src/crtp_commander_high_level.c @@ -0,0 +1,292 @@ +/* + * ______ + * / ____/________ _____ __ ________ ______ __________ ___ + * / / / ___/ __ `/_ / / / / / ___/ | /| / / __ `/ ___/ __ `__ \ + * / /___/ / / /_/ / / /_/ /_/ (__ )| |/ |/ / /_/ / / / / / / / / + * \____/_/ \__,_/ /___/\__, /____/ |__/|__/\__,_/_/ /_/ /_/ /_/ + * /____/ + * + * Crazyswarm advanced control firmware for Crazyflie + * + +The MIT License (MIT) + +Copyright (c) 2018 Wolfgang Hoenig and James Alan Preiss + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. +*/ + + +/* +High-level commander: computes smooth setpoints based on high-level inputs +such as: take-off, landing, polynomial trajectories. +*/ + +#include +#include +#include + +/* FreeRtos includes */ +#include "FreeRTOS.h" +#include "task.h" +#include "semphr.h" + +// Crazyswarm includes +#include "crtp.h" +#include "crtp_commander_high_level.h" +#include "debug.h" +#include "planner.h" +#include "log.h" +#include "param.h" + +// Global variables +static bool isInit = false; +static struct planner planner; +static uint8_t group_mask; +static struct vec pos; // last known state (position [m]) +static float yaw; // last known state (yaw [rad]) + +// makes sure that we don't evaluate the trajectory while it is being changed +static xSemaphoreHandle lockTraj; + +// CRTP Packet definitions + +// trajectory command (first byte of crtp packet) +enum TrajectoryCommand_e { + COMMAND_SET_GROUP_MASK = 0, + COMMAND_TAKEOFF = 1, + COMMAND_LAND = 2, + COMMAND_STOP = 3, + COMMAND_GO_TO = 4, +}; + +struct data_set_group_mask { + uint8_t groupMask; // mask for which groups this CF belongs to +} __attribute__((packed)); + +// vertical takeoff from current x-y position to given height +struct data_takeoff { + uint8_t groupMask; // mask for which CFs this should apply to + float height; // m (absolute) + float duration; // s (time it should take until target height is reached) +} __attribute__((packed)); + +// vertical land from current x-y position to given height +struct data_land { + uint8_t groupMask; // mask for which CFs this should apply to + float height; // m (absolute) + float duration; // s (time it should take until target height is reached) +} __attribute__((packed)); + +// stops the current trajectory (turns off the motors) +struct data_stop { + uint8_t groupMask; // mask for which CFs this should apply to +} __attribute__((packed)); + +// "take this much time to go here, then hover" +struct data_go_to { + uint8_t groupMask; // mask for which CFs this should apply to + uint8_t relative; // set to true, if position/yaw are relative to current setpoint + float x; // m + float y; // m + float z; // m + float yaw; // deg + float duration; // sec +} __attribute__((packed)); + +// Private functions +static void crtpCommanderHighLevelTask(void * prm); + +static int set_group_mask(const struct data_set_group_mask* data); +static int takeoff(const struct data_takeoff* data); +static int land(const struct data_land* data); +static int stop(const struct data_stop* data); +static int go_to(const struct data_go_to* data); + +// Helper functions +static struct vec state2vec(struct vec3_s v) +{ + return mkvec(v.x, v.y, v.z); +} + +bool isInGroup(uint8_t g) { + return g == 0 || (g & group_mask) != 0; +} + +void crtpCommanderHighLevelInit(void) +{ + if (isInit) { + return; + } + + plan_init(&planner); + + //Start the trajectory task + xTaskCreate(crtpCommanderHighLevelTask, CMD_HIGH_LEVEL_TASK_NAME, + CMD_HIGH_LEVEL_TASK_STACKSIZE, NULL, CMD_HIGH_LEVEL_TASK_PRI, NULL); + + lockTraj = xSemaphoreCreateMutex(); + + pos = vzero(); + yaw = 0; + + isInit = true; + DEBUG_PRINT("traj. initialized.\n"); +} + +void crtpCommanderHighLevelStop() +{ + plan_stop(&planner); +} + +bool crtpCommanderHighLevelIsStopped() +{ + return plan_is_stopped(&planner); +} + +void crtpCommanderHighLevelGetSetpoint(setpoint_t* setpoint, const state_t *state) +{ + pos = state2vec(state->position); + yaw = radians(state->attitude.yaw); + + xSemaphoreTake(lockTraj, portMAX_DELAY); + float t = usecTimestamp() / 1e6; + struct traj_eval ev = plan_current_goal(&planner, t); + if (!is_traj_eval_valid(&ev)) { + // programming error + plan_stop(&planner); + } + xSemaphoreGive(lockTraj); + + if (is_traj_eval_valid(&ev)) { + setpoint->position.x = ev.pos.x; + setpoint->position.y = ev.pos.y; + setpoint->position.z = ev.pos.z; + setpoint->velocity.x = ev.vel.x; + setpoint->velocity.y = ev.vel.y; + setpoint->velocity.z = ev.vel.z; + setpoint->attitude.yaw = degrees(ev.yaw); + setpoint->attitudeRate.roll = degrees(ev.omega.x); + setpoint->attitudeRate.pitch = degrees(ev.omega.y); + setpoint->attitudeRate.yaw = degrees(ev.omega.z); + setpoint->mode.x = modeAbs; + setpoint->mode.y = modeAbs; + setpoint->mode.z = modeAbs; + setpoint->mode.roll = modeDisable; + setpoint->mode.pitch = modeDisable; + setpoint->mode.yaw = modeAbs; + setpoint->mode.quat = modeDisable; + setpoint->acceleration.x = ev.acc.x; + setpoint->acceleration.y = ev.acc.y; + setpoint->acceleration.z = ev.acc.z; + } +} + +void crtpCommanderHighLevelTask(void * prm) +{ + int ret; + CRTPPacket p; + crtpInitTaskQueue(CRTP_PORT_SETPOINT_HL); + + while(1) { + crtpReceivePacketBlock(CRTP_PORT_SETPOINT_HL, &p); + + switch(p.data[0]) + { + case COMMAND_SET_GROUP_MASK: + ret = set_group_mask((const struct data_set_group_mask*)&p.data[1]); + break; + case COMMAND_TAKEOFF: + ret = takeoff((const struct data_takeoff*)&p.data[1]); + break; + case COMMAND_LAND: + ret = land((const struct data_land*)&p.data[1]); + break; + case COMMAND_STOP: + ret = stop((const struct data_stop*)&p.data[1]); + break; + case COMMAND_GO_TO: + ret = go_to((const struct data_go_to*)&p.data[1]); + break; + default: + ret = ENOEXEC; + break; + } + + //answer + p.data[3] = ret; + p.size = 4; + crtpSendPacket(&p); + } +} + +int set_group_mask(const struct data_set_group_mask* data) +{ + group_mask = data->groupMask; + + return 0; +} + +int takeoff(const struct data_takeoff* data) +{ + int result = 0; + if (isInGroup(data->groupMask)) { + xSemaphoreTake(lockTraj, portMAX_DELAY); + float t = usecTimestamp() / 1e6; + result = plan_takeoff(&planner, pos, yaw, data->height, data->duration, t); + xSemaphoreGive(lockTraj); + } + return result; +} + +int land(const struct data_land* data) +{ + int result = 0; + if (isInGroup(data->groupMask)) { + xSemaphoreTake(lockTraj, portMAX_DELAY); + float t = usecTimestamp() / 1e6; + result = plan_land(&planner, pos, yaw, data->height, data->duration, t); + xSemaphoreGive(lockTraj); + } + return result; +} + +int stop(const struct data_stop* data) +{ + int result = 0; + if (isInGroup(data->groupMask)) { + xSemaphoreTake(lockTraj, portMAX_DELAY); + plan_stop(&planner); + xSemaphoreGive(lockTraj); + } + return result; +} + +int go_to(const struct data_go_to* data) +{ + int result = 0; + if (isInGroup(data->groupMask)) { + struct vec hover_pos = mkvec(data->x, data->y, data->z); + xSemaphoreTake(lockTraj, portMAX_DELAY); + float t = usecTimestamp() / 1e6; + result = plan_go_to(&planner, data->relative, hover_pos, data->yaw, data->duration, t); + xSemaphoreGive(lockTraj); + } + return result; +} diff --git a/src/modules/src/planner.c b/src/modules/src/planner.c new file mode 100644 index 0000000000..387af6aaae --- /dev/null +++ b/src/modules/src/planner.c @@ -0,0 +1,145 @@ +/* + * ______ + * / ____/________ _____ __ ________ ______ __________ ___ + * / / / ___/ __ `/_ / / / / / ___/ | /| / / __ `/ ___/ __ `__ \ + * / /___/ / / /_/ / / /_/ /_/ (__ )| |/ |/ / /_/ / / / / / / / / + * \____/_/ \__,_/ /___/\__, /____/ |__/|__/\__,_/_/ /_/ /_/ /_/ + * /____/ + * + * Crazyswarm advanced control firmware for Crazyflie + * + +The MIT License (MIT) + +Copyright (c) 2018 Wolfgang Hoenig and James Alan Preiss + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. +*/ + +/* +implementation of planning state machine +*/ + +#include "planner.h" + +static struct piecewise_traj planned_trajectory; + +static void plan_takeoff_or_landing(struct planner *p, struct vec pos, float yaw, float height, float duration) +{ + struct vec takeoff_pos = pos; + takeoff_pos.z = height; + + piecewise_plan_7th_order_no_jerk(&planned_trajectory, duration, + pos, yaw, vzero(), 0, vzero(), + takeoff_pos, 0, vzero(), 0, vzero()); +} + +// ----------------- // +// public functions. // +// ----------------- // + +void plan_init(struct planner *p) +{ + p->state = TRAJECTORY_STATE_IDLE; + p->reversed = false; + p->trajectory = NULL; +} + +void plan_stop(struct planner *p) +{ + p->state = TRAJECTORY_STATE_IDLE; +} + +bool plan_is_stopped(struct planner *p) +{ + return p->state == TRAJECTORY_STATE_IDLE; +} + +struct traj_eval plan_current_goal(struct planner *p, float t) +{ + switch (p->state) { + case TRAJECTORY_STATE_LANDING: + if (piecewise_is_finished(p->trajectory, t)) { + p->state = TRAJECTORY_STATE_IDLE; + } + // intentional fall-thru + case TRAJECTORY_STATE_FLYING: + if (p->reversed) { + return piecewise_eval_reversed(p->trajectory, t); + } + else { + return piecewise_eval(p->trajectory, t); + } + + default: + return traj_eval_invalid(); + } +} + + +int plan_takeoff(struct planner *p, struct vec pos, float yaw, float height, float duration, float t) +{ + if (p->state != TRAJECTORY_STATE_IDLE) { + return 1; + } + + plan_takeoff_or_landing(p, pos, yaw, height, duration); + p->reversed = false; + p->state = TRAJECTORY_STATE_FLYING; + planned_trajectory.t_begin = t; + p->trajectory = &planned_trajectory; + return 0; +} + +int plan_land(struct planner *p, struct vec pos, float yaw, float height, float duration, float t) +{ + if ( p->state == TRAJECTORY_STATE_IDLE + || p->state == TRAJECTORY_STATE_LANDING) { + return 1; + } + + plan_takeoff_or_landing(p, pos, yaw, height, duration); + p->reversed = false; + p->state = TRAJECTORY_STATE_LANDING; + planned_trajectory.t_begin = t; + p->trajectory = &planned_trajectory; + return 0; +} + +int plan_go_to(struct planner *p, bool relative, struct vec hover_pos, float hover_yaw, float duration, float t) +{ + // allow in any state, i.e., can also be used to take-off or land + + struct traj_eval setpoint = plan_current_goal(p, t); + + if (relative) { + hover_pos = vadd(hover_pos, setpoint.pos); + hover_yaw += setpoint.yaw; + } + + piecewise_plan_7th_order_no_jerk(&planned_trajectory, duration, + setpoint.pos, setpoint.yaw, setpoint.vel, setpoint.omega.z, setpoint.acc, + hover_pos, hover_yaw, vzero(), 0, vzero()); + + p->reversed = false; + p->state = TRAJECTORY_STATE_FLYING; + planned_trajectory.t_begin = t; + p->trajectory = &planned_trajectory; + return 0; +} diff --git a/src/modules/src/pptraj.c b/src/modules/src/pptraj.c new file mode 100644 index 0000000000..be325beea7 --- /dev/null +++ b/src/modules/src/pptraj.c @@ -0,0 +1,383 @@ +/* + * ______ + * / ____/________ _____ __ ________ ______ __________ ___ + * / / / ___/ __ `/_ / / / / / ___/ | /| / / __ `/ ___/ __ `__ \ + * / /___/ / / /_/ / / /_/ /_/ (__ )| |/ |/ / /_/ / / / / / / / / + * \____/_/ \__,_/ /___/\__, /____/ |__/|__/\__,_/_/ /_/ /_/ /_/ + * /____/ + * + * Crazyswarm advanced control firmware for Crazyflie + * + +The MIT License (MIT) + +Copyright (c) 2018 Wolfgang Hoenig and James Alan Preiss + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. +*/ + +/* +implementation of piecewise polynomial trajectories +See Daniel Mellinger, Vijay Kumar: "Minimum snap trajectory generation and control for quadrotors". ICRA 2011: 2520-2525 +*/ + +#include "pptraj.h" + +#define GRAV (9.81f) + +// polynomials are stored with ascending degree + +void polylinear(float p[PP_SIZE], float duration, float x0, float x1) +{ + p[0] = x0; + p[1] = (x1 - x0) / duration; + for (int i = 2; i < PP_SIZE; ++i) { + p[i] = 0; + } +} + +void polyscale(float p[PP_SIZE], float s) +{ + for (int i = 0; i < PP_SIZE; ++i) { + p[i] *= s; + } +} + +// e.g. if s==2 the new polynomial will be stretched to take 2x longer +void polystretchtime(float p[PP_SIZE], float s) +{ + float recip = 1.0f / s; + float scale = recip; + for (int i = 1; i < PP_SIZE; ++i) { + p[i] *= scale; + scale *= recip; + } +} + +void polyreflect(float p[PP_SIZE]) +{ + for (int i = 1; i < PP_SIZE; i += 2) { + p[i] = -p[i]; + } +} + +// evaluate a polynomial using horner's rule. +float polyval(float const p[PP_SIZE], float t) +{ + float x = 0.0; + for (int i = PP_DEGREE; i >= 0; --i) { + x = x * t + p[i]; + } + return x; +} + +// compute derivative of a polynomial in place +void polyder(float p[PP_SIZE]) +{ + for (int i = 1; i <= PP_DEGREE; ++i) { + p[i-1] = i * p[i]; + } + p[PP_DEGREE] = 0; +} + +void poly5(float poly[PP_SIZE], float T, + float x0, float dx0, float ddx0, + float xf, float dxf, float ddxf) +{ + float T2 = T * T; + float T3 = T2 * T; + float T4 = T3 * T; + float T5 = T4 * T; + poly[0] = x0; + poly[1] = dx0; + poly[2] = ddx0 / 2; + poly[3] = (-12*dx0*T - 8*dxf*T - 3*ddx0*T2 + ddxf*T2 - 20*x0 + 20*xf)/(2*T3); + poly[4] = (16*dx0*T + 14*dxf*T + 3*ddx0*T2 - 2*ddxf*T2 + 30*x0 - 30*xf)/(2*T4); + poly[5] = (-6*dx0*T - 6*dxf*T - ddx0*T2 + ddxf*T2 - 12*x0 + 12*xf)/(2*T5); + for (int i = 6; i < PP_SIZE; ++i) { + poly[i] = 0; + } +}; + +static void poly7_nojerk(float poly[PP_SIZE], float T, + float x0, float dx0, float ddx0, + float xf, float dxf, float ddxf) +{ + float T2 = T * T; + float T3 = T2 * T; + float T4 = T3 * T; + float T5 = T4 * T; + float T6 = T5 * T; + float T7 = T6 * T; + poly[0] = x0; + poly[1] = dx0; + poly[2] = ddx0/2; + poly[3] = 0; + poly[4] = -(5*(14*x0 - 14*xf + 8*T*dx0 + 6*T*dxf + 2*T2*ddx0 - T2*ddxf))/(2*T4); + poly[5] = (84*x0 - 84*xf + 45*T*dx0 + 39*T*dxf + 10*T2*ddx0 - 7*T2*ddxf)/T5; + poly[6] = -(140*x0 - 140*xf + 72*T*dx0 + 68*T*dxf + 15*T2*ddx0 - 13*T2*ddxf)/(2*T6); + poly[7] = (2*(10*x0 - 10*xf + 5*T*dx0 + 5*T*dxf + T2*ddx0 - T2*ddxf))/T7; + for (int i = 8; i < PP_SIZE; ++i) { + poly[i] = 0; + } +} + + +// +// 4d single-piece polynomials +// + +// construct a 4d zero polynomial. +struct poly4d poly4d_zero(float duration) +{ + struct poly4d p = { + .p = {{0}}, + .duration = duration, + }; + return p; +} + +struct poly4d poly4d_linear(float duration, struct vec p0, struct vec p1, float yaw0, float yaw1) +{ + struct poly4d p; + p.duration = duration; + polylinear(p.p[0], duration, p0.x, p1.x); + polylinear(p.p[1], duration, p0.y, p1.y); + polylinear(p.p[2], duration, p0.z, p1.z); + polylinear(p.p[3], duration, yaw0, yaw1); + return p; +} + +void poly4d_scale(struct poly4d *p, float x, float y, float z, float yaw) +{ + polyscale(p->p[0], x); + polyscale(p->p[1], y); + polyscale(p->p[2], z); + polyscale(p->p[3], yaw); +} + +void poly4d_shift(struct poly4d *p, float x, float y, float z, float yaw) +{ + p->p[0][0] += x; + p->p[1][0] += y; + p->p[2][0] += z; + p->p[3][0] += yaw; +} + +void poly4d_stretchtime(struct poly4d *p, float s) +{ + for (int i = 0; i < 4; ++i) { + polystretchtime(p->p[i], s); + } + p->duration *= s; +} + +void polyder4d(struct poly4d *p) +{ + for (int i = 0; i < 4; ++i) { + polyder(p->p[i]); + } +} + +static struct vec polyval_xyz(struct poly4d const *p, float t) +{ + return mkvec(polyval(p->p[0], t), polyval(p->p[1], t), polyval(p->p[2], t)); +} + +static float polyval_yaw(struct poly4d const *p, float t) +{ + return polyval(p->p[3], t); +} + +// compute loose maximum of acceleration - +// uses L1 norm instead of Euclidean, evaluates polynomial instead of root-finding +float poly4d_max_accel_approx(struct poly4d const *p) +{ + static struct poly4d acc; + acc = *p; + polyder4d(&acc); + polyder4d(&acc); + int steps = 10 * p->duration; + float step = p->duration / (steps - 1); + float t = 0; + float amax = 0; + for (int i = 0; i < steps; ++i) { + struct vec ddx = polyval_xyz(&acc, t); + float ddx_minkowski = vminkowski(ddx); + if (ddx_minkowski > amax) amax = ddx_minkowski; + t += step; + } + return amax; +} + +struct traj_eval traj_eval_invalid() +{ + struct traj_eval ev; + ev.pos = vrepeat(NAN); + return ev; +} + +bool is_traj_eval_valid(struct traj_eval const *ev) +{ + return !visnan(ev->pos); +} + +struct traj_eval poly4d_eval(struct poly4d const *p, float t) +{ + // flat variables + struct traj_eval out; + out.pos = polyval_xyz(p, t); + out.yaw = polyval_yaw(p, t); + + // 1st derivative + static struct poly4d deriv; + deriv = *p; + polyder4d(&deriv); + out.vel = polyval_xyz(&deriv, t); + float dyaw = polyval_yaw(&deriv, t); + + // 2nd derivative + polyder4d(&deriv); + out.acc = polyval_xyz(&deriv, t); + + // 3rd derivative + polyder4d(&deriv); + struct vec jerk = polyval_xyz(&deriv, t); + + struct vec thrust = vadd(out.acc, mkvec(0, 0, GRAV)); + // float thrust_mag = mass * vmag(thrust); + + struct vec z_body = vnormalize(thrust); + struct vec x_world = mkvec(cos(out.yaw), sin(out.yaw), 0); + struct vec y_body = vnormalize(vcross(z_body, x_world)); + struct vec x_body = vcross(y_body, z_body); + + struct vec jerk_orth_zbody = vorthunit(jerk, z_body); + struct vec h_w = vscl(1.0f / vmag(thrust), jerk_orth_zbody); + + out.omega.x = -vdot(h_w, y_body); + out.omega.y = vdot(h_w, x_body); + out.omega.z = z_body.z * dyaw; + + return out; +} + +// +// piecewise 4d polynomials +// + +// piecewise eval +struct traj_eval piecewise_eval( + struct piecewise_traj const *traj, float t) +{ + int cursor = 0; + t = t - traj->t_begin; + while (cursor < traj->n_pieces) { + struct poly4d const *piece = &(traj->pieces[cursor]); + if (t <= piece->duration) { + return poly4d_eval(piece, t); + } + t -= piece->duration; + ++cursor; + } + // if we get here, the trajectory has ended + struct poly4d const *end_piece = &(traj->pieces[traj->n_pieces - 1]); + struct traj_eval ev = poly4d_eval(end_piece, end_piece->duration); + ev.vel = vzero(); + ev.acc = vzero(); + ev.omega = vzero(); + return ev; +} + +struct traj_eval piecewise_eval_reversed( + struct piecewise_traj const *traj, float t) +{ + int cursor = traj->n_pieces - 1; + t = t - traj->t_begin; + while (cursor >= 0) { + struct poly4d const *piece = &(traj->pieces[cursor]); + if (t <= piece->duration) { + struct poly4d piece_reversed = *piece; + for (int i = 0; i < 4; ++i) { + polyreflect(piece_reversed.p[i]); + } + t = t - piece->duration; + return poly4d_eval(&piece_reversed, t); + } + t -= piece->duration; + --cursor; + } + // if we get here, the trajectory has ended + struct poly4d const *end_piece = &(traj->pieces[0]); + struct traj_eval ev = poly4d_eval(end_piece, 0.0f); + ev.vel = vzero(); + ev.acc = vzero(); + ev.omega = vzero(); + return ev; +} + + +// y, dy == yaw, derivative of yaw +void piecewise_plan_5th_order(struct piecewise_traj *pp, float duration, + struct vec p0, float y0, struct vec v0, float dy0, struct vec a0, + struct vec p1, float y1, struct vec v1, float dy1, struct vec a1) +{ + struct poly4d *p = &pp->pieces[0]; + p->duration = duration; + pp->n_pieces = 1; + poly5(p->p[0], duration, p0.x, v0.x, a0.x, p1.x, v1.x, a1.x); + poly5(p->p[1], duration, p0.y, v0.y, a0.y, p1.y, v1.y, a1.y); + poly5(p->p[2], duration, p0.z, v0.z, a0.z, p1.z, v1.z, a1.z); + poly5(p->p[3], duration, y0, dy0, 0, y1, dy1, 0); +} + +// y, dy == yaw, derivative of yaw +void piecewise_plan_7th_order_no_jerk(struct piecewise_traj *pp, float duration, + struct vec p0, float y0, struct vec v0, float dy0, struct vec a0, + struct vec p1, float y1, struct vec v1, float dy1, struct vec a1) +{ + struct poly4d *p = &pp->pieces[0]; + p->duration = duration; + pp->n_pieces = 1; + poly7_nojerk(p->p[0], duration, p0.x, v0.x, a0.x, p1.x, v1.x, a1.x); + poly7_nojerk(p->p[1], duration, p0.y, v0.y, a0.y, p1.y, v1.y, a1.y); + poly7_nojerk(p->p[2], duration, p0.z, v0.z, a0.z, p1.z, v1.z, a1.z); + poly7_nojerk(p->p[3], duration, y0, dy0, 0, y1, dy1, 0); +} + +void piecewise_scale(struct piecewise_traj *pp, float x, float y, float z, float yaw) +{ + for (int i = 0; i < PP_MAX_PIECES; ++i) { + poly4d_scale(&pp->pieces[i], x, y, z, yaw); + } +} + +void piecewise_shift(struct piecewise_traj *pp, float x, float y, float z, float yaw) +{ + for (int i = 0; i < PP_MAX_PIECES; ++i) { + poly4d_shift(&pp->pieces[i], x, y, z, yaw); + } +} + +void piecewise_stretchtime(struct piecewise_traj *pp, float s) +{ + for (int i = 0; i < PP_MAX_PIECES; ++i) { + poly4d_stretchtime(&pp->pieces[i], s); + } +}