From f2a7758761cf129dcad4469553c2df929231d8b1 Mon Sep 17 00:00:00 2001 From: Daniele Palossi Date: Thu, 3 Oct 2019 18:34:25 +0200 Subject: [PATCH] PULP-Shield/PULP-DroNet integration --- .gitignore | 1 - Makefile | 5 +- src/config/config.h | 3 + src/deck/api/deck_spi.c | 80 ++++ src/deck/drivers/interface/pulp_shield.h | 88 +++++ src/deck/drivers/src/flowdeck_v1v2.c | 6 +- src/deck/drivers/src/pulp_shield.c | 423 ++++++++++++++++++++++ src/deck/interface/deck_spi.h | 2 + src/modules/src/commander.c | 137 +++++++ src/modules/src/estimator_kalman.c | 4 + src/modules/src/position_controller_pid.c | 4 +- tools/make/config.mk | 44 +++ 12 files changed, 790 insertions(+), 7 deletions(-) create mode 100644 src/deck/drivers/interface/pulp_shield.h create mode 100644 src/deck/drivers/src/pulp_shield.c create mode 100644 tools/make/config.mk diff --git a/.gitignore b/.gitignore index 9c6b24f012..23222d9938 100644 --- a/.gitignore +++ b/.gitignore @@ -8,7 +8,6 @@ bin/* .settings/* .vscode/* *~ -tools/make/config.mk cflie.* version.c tags diff --git a/Makefile b/Makefile index 7bb12d5ce6..28712c769b 100644 --- a/Makefile +++ b/Makefile @@ -35,8 +35,8 @@ CFLAGS += -DCRAZYFLIE_FW ######### Stabilizer configuration ########## ## These are set by the platform (see tools/make/platforms/*.mk), can be overwritten here -ESTIMATOR ?= any -CONTROLLER ?= Any # one of Any, PID, Mellinger +ESTIMATOR ?= Kalman # any +CONTROLLER ?= PID # one of Any, PID, Mellinger POWER_DISTRIBUTION ?= stock #OpenOCD conf @@ -173,6 +173,7 @@ PROJ_OBJ += deck_analog.o PROJ_OBJ += deck_spi.o # Decks +PROJ_OBJ += pulp_shield.o PROJ_OBJ += bigquad.o PROJ_OBJ += rzr.o PROJ_OBJ += ledring12.o diff --git a/src/config/config.h b/src/config/config.h index e363cdfe3f..684da06f68 100644 --- a/src/config/config.h +++ b/src/config/config.h @@ -85,6 +85,7 @@ #define USDWRITE_TASK_PRI 0 #define PCA9685_TASK_PRI 3 #define CMD_HIGH_LEVEL_TASK_PRI 2 +#define PULP_SHIELD_TASK_PRI 1 #define SYSLINK_TASK_PRI 3 #define USBLINK_TASK_PRI 3 @@ -123,6 +124,7 @@ #define PCA9685_TASK_NAME "PCA9685" #define CMD_HIGH_LEVEL_TASK_NAME "CMDHL" #define MULTIRANGER_TASK_NAME "MR" +#define PULP_SHIELD_TASK_NAME "PULP-SHIELD" //Task stack sizes #define SYSTEM_TASK_STACKSIZE (2* configMINIMAL_STACK_SIZE) @@ -151,6 +153,7 @@ #define PCA9685_TASK_STACKSIZE (2 * configMINIMAL_STACK_SIZE) #define CMD_HIGH_LEVEL_TASK_STACKSIZE configMINIMAL_STACK_SIZE #define MULTIRANGER_TASK_STACKSIZE (2 * configMINIMAL_STACK_SIZE) +#define PULP_SHIELD_TASK_STACKSIZE (3 * configMINIMAL_STACK_SIZE) //The radio channel. From 0 to 125 #define RADIO_CHANNEL 80 diff --git a/src/deck/api/deck_spi.c b/src/deck/api/deck_spi.c index 7591715137..93094aee53 100644 --- a/src/deck/api/deck_spi.c +++ b/src/deck/api/deck_spi.c @@ -88,6 +88,7 @@ static SemaphoreHandle_t spiMutex; static void spiDMAInit(); static void spiConfigureWithSpeed(uint16_t baudRatePrescaler); +static void spiConfigureWithSpeedSlave(uint16_t baudRatePrescaler); void spiBegin(void) { @@ -146,6 +147,59 @@ void spiBegin(void) isInit = true; } +void spiBeginSlave(void) +{ + GPIO_InitTypeDef GPIO_InitStructure; + + // binary semaphores created using xSemaphoreCreateBinary() are created in a state + // such that the the semaphore must first be 'given' before it can be 'taken' + txComplete = xSemaphoreCreateBinary(); + rxComplete = xSemaphoreCreateBinary(); + spiMutex = xSemaphoreCreateMutex(); + + /*!< Enable the SPI clock */ + SPI_CLK_INIT(SPI_CLK, ENABLE); + + /*!< Enable GPIO clocks */ + RCC_AHB1PeriphClockCmd(SPI_SCK_GPIO_CLK | SPI_MISO_GPIO_CLK | + SPI_MOSI_GPIO_CLK, ENABLE); + + /*!< Enable DMA Clocks */ + SPI_DMA_CLK_INIT(SPI_DMA_CLK, ENABLE); + + /*!< SPI pins configuration *************************************************/ + + /*!< Connect SPI pins to AF5 */ + GPIO_PinAFConfig(SPI_SCK_GPIO_PORT, SPI_SCK_SOURCE, SPI_SCK_AF); + GPIO_PinAFConfig(SPI_MISO_GPIO_PORT, SPI_MISO_SOURCE, SPI_MISO_AF); + GPIO_PinAFConfig(SPI_MOSI_GPIO_PORT, SPI_MOSI_SOURCE, SPI_MOSI_AF); + + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; + GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; + GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_DOWN; + + /*!< SPI SCK pin configuration */ + GPIO_InitStructure.GPIO_Pin = SPI_SCK_PIN; + GPIO_Init(SPI_SCK_GPIO_PORT, &GPIO_InitStructure); + + /*!< SPI MOSI pin configuration */ + GPIO_InitStructure.GPIO_Pin = SPI_MOSI_PIN; + GPIO_Init(SPI_MOSI_GPIO_PORT, &GPIO_InitStructure); + + /*!< SPI MISO pin configuration */ + GPIO_InitStructure.GPIO_Pin = SPI_MISO_PIN; + GPIO_Init(SPI_MISO_GPIO_PORT, &GPIO_InitStructure); + + /*!< SPI DMA Initialization */ + spiDMAInit(); + + /*!< SPI configuration */ + spiConfigureWithSpeedSlave(SPI_BAUDRATE_2MHZ); + + isInit = true; +} + static void spiDMAInit() { DMA_InitTypeDef DMA_InitStructure; @@ -214,6 +268,26 @@ static void spiConfigureWithSpeed(uint16_t baudRatePrescaler) SPI_Init(SPI, &SPI_InitStructure); } +static void spiConfigureWithSpeedSlave(uint16_t baudRatePrescaler) +{ + SPI_InitTypeDef SPI_InitStructure; + + SPI_I2S_DeInit(SPI); + + SPI_InitStructure.SPI_Direction = SPI_Direction_2Lines_FullDuplex; + SPI_InitStructure.SPI_Mode = SPI_Mode_Slave; + SPI_InitStructure.SPI_DataSize = SPI_DataSize_8b; + SPI_InitStructure.SPI_CPOL = SPI_CPOL_Low; + SPI_InitStructure.SPI_CPHA = SPI_CPHA_1Edge; + SPI_InitStructure.SPI_NSS = SPI_NSS_Soft; + SPI_InitStructure.SPI_FirstBit = SPI_FirstBit_MSB; + SPI_InitStructure.SPI_CRCPolynomial = 0; // Not used + + SPI_InitStructure.SPI_BaudRatePrescaler = baudRatePrescaler; + SPI_Init(SPI, &SPI_InitStructure); +} + + bool spiTest(void) { return isInit; @@ -262,6 +336,12 @@ void spiBeginTransaction(uint16_t baudRatePrescaler) spiConfigureWithSpeed(baudRatePrescaler); } +void spiBeginTransactionSlave(uint16_t baudRatePrescaler) +{ + xSemaphoreTake(spiMutex, portMAX_DELAY); + spiConfigureWithSpeedSlave(baudRatePrescaler); +} + void spiEndTransaction() { xSemaphoreGive(spiMutex); diff --git a/src/deck/drivers/interface/pulp_shield.h b/src/deck/drivers/interface/pulp_shield.h new file mode 100644 index 0000000000..cc42c6a330 --- /dev/null +++ b/src/deck/drivers/interface/pulp_shield.h @@ -0,0 +1,88 @@ +/*----------------------------------------------------------------------------* + * Copyright (C) 2018-2019 ETH Zurich, Switzerland * + * All rights reserved. * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); * + * you may not use this file except in compliance with the License. * + * See LICENSE.apache.md in the top directory for details. * + * You may obtain a copy of the License at * + * * + * http://www.apache.org/licenses/LICENSE-2.0 * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + * * + * File: pulp_shield.h * + * Author: Daniele Palossi * + * Date: 04.09.2019 * + *----------------------------------------------------------------------------*/ + +#ifndef _PULPSHIELD_H_ +#define _PULPSHIELD_H_ + +#include "FreeRTOS.h" +#include "task.h" +#include "stabilizer_types.h" +#include "deck_core.h" +#include "deck.h" +#include "arm_math.h" +#include "log.h" +#include "param.h" +#include "debug.h" + +#define PULP_SHIELD +#define WATCH_DOG_EN +//#define COLLISION_TEST + +#define PULP_TARGET_H 0.50f +#define SLEEP_THRESHOLD 20000 +#define ERROR_THRESHOLD 750 + +#define PULP_TAKEOFF_RATE 10 +#define PULP_NAVIGATE_RATE 100 +#define PULP_LANDING_RATE 10 + +#define PULP_STEPS_TAKEOFF 20 +#define PULP_STEPS_HOVERING 200 +#define PULP_STEPS_LANDING 200 + +/* For conversion from fixed-point to float */ +#define FIXEDPT_FBITS 11 /* Q5.11 */ +#define FIXED_BITVALUE (1.0f / (1<= 1-CRITICAL_PROB_COLL +#define MAX_FORWARD_INDEX 1.0f // Default: 1.0f +#define CRITICAL_PROB_COLL 0.7f // Default: 0.7f + +void PULPShieldOn(); + +void PULPShieldOff(); + +void GPIOInit(); + +void GPIOOn(); + +void GPIOOff(); + +void DroNetTakeOff(); + +void DroNetGetSetpoint(); + +void DroNetResetSetpoint(); + +float fixed2float(int16_t *x); + +uint32_t getLastPULPUpdate(); + +setpoint_t PULPShieldGetSetpoint() ; + + +#endif /* _PULPSHIELD_H_ */ \ No newline at end of file diff --git a/src/deck/drivers/src/flowdeck_v1v2.c b/src/deck/drivers/src/flowdeck_v1v2.c index 8d0ee2e829..30af1434c9 100644 --- a/src/deck/drivers/src/flowdeck_v1v2.c +++ b/src/deck/drivers/src/flowdeck_v1v2.c @@ -85,14 +85,16 @@ static void flowdeckTask(void *param) // (might need to be changed if mounted differently) int16_t accpx = -currentMotion.deltaY; int16_t accpy = -currentMotion.deltaX; + currentMotion.deltaY = accpy; + currentMotion.deltaX = accpx; // Outlier removal if (abs(accpx) < OULIER_LIMIT && abs(accpy) < OULIER_LIMIT) { // Form flow measurement struct and push into the EKF flowMeasurement_t flowData; - flowData.stdDevX = 0.25; // [pixels] should perhaps be made larger? - flowData.stdDevY = 0.25; // [pixels] should perhaps be made larger? + flowData.stdDevX = 2.0f; // Default: 0.25; // [pixels] should perhaps be made larger? + flowData.stdDevY = 2.0f; // Default: 0.25; // [pixels] should perhaps be made larger? flowData.dt = 0.01; #if defined(USE_MA_SMOOTHING) diff --git a/src/deck/drivers/src/pulp_shield.c b/src/deck/drivers/src/pulp_shield.c new file mode 100644 index 0000000000..45d40f111b --- /dev/null +++ b/src/deck/drivers/src/pulp_shield.c @@ -0,0 +1,423 @@ +/*----------------------------------------------------------------------------* + * Copyright (C) 2018-2019 ETH Zurich, Switzerland * + * All rights reserved. * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); * + * you may not use this file except in compliance with the License. * + * See LICENSE.apache.md in the top directory for details. * + * You may obtain a copy of the License at * + * * + * http://www.apache.org/licenses/LICENSE-2.0 * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + * * + * File: pulp_shield.c * + * Author: Daniele Palossi * + * Date: 04.09.2019 * + *----------------------------------------------------------------------------*/ + + +#include "pulp_shield.h" + +static float alpha_velocity_ = ALPHA_VEL; +static float alpha_yaw_ = ALPHA_YAW; +static float max_forward_index_ = MAX_FORWARD_INDEX; +static float critical_prob_coll_ = CRITICAL_PROB_COLL; +static float yaw_scaling = YAW_SCALING; + +static float steering_angle_; +static float probability_of_collision_; +static float desired_forward_velocity_; +static float desired_angular_velocity_; + +static setpoint_t setpoint_ = { + .position.x = 0.0f, + .position.y = 0.0f, + .position.z = 0.0f, + + .velocity.x = 0.0f, + .velocity.y = 0.0f, + .velocity.z = 0.0f, + + .acceleration.x = 0.0f, + .acceleration.y = 0.0f, + .acceleration.z = 0.0f, + + .attitude.pitch = 0.0f, + .attitude.roll = 0.0f, + .attitude.yaw = 0.0f, + + .attitudeRate.pitch = 0.0f, + .attitudeRate.roll = 0.0f, + .attitudeRate.yaw = 0.0f, + + .mode.x = modeDisable, + .mode.y = modeDisable, + .mode.z = modeDisable, + .mode.pitch = modeDisable, + .mode.roll = modeDisable, + .mode.yaw = modeDisable, + .mode.quat = modeDisable, + + .velocity_body = true, +}; + +static uint32_t step, lastUpdate, sleep; +static bool isInit = false; +static bool isEnabled = false; +#ifdef COLLISION_TEST +static bool stop = false; +#endif + + +static struct { + float32_t samples[MA_HISTORY]; + size_t count; +} steeringAverages; + +static struct { + float32_t samples[MA_HISTORY]; + size_t count; +} velocityAverages; + + +setpoint_t PULPShieldGetSetpoint() { + return setpoint_; +} + + +static void PULPShieldTask(void *param) { + + systemWaitStart(); + + while(1) { + + uint32_t currentTime = xTaskGetTickCount(); + /**** INITIAL TIMER ****/ + if(currentTime-sleep PULP_TAKEOFF_RATE) { + DroNetTakeOff(); + lastUpdate = xTaskGetTickCount(); + step++; + } + } + /**** AUTONOMOUS NAVIGATION ****/ + else { + if((currentTime-lastUpdate) > PULP_NAVIGATE_RATE) { + DroNetGetSetpoint(); + lastUpdate = xTaskGetTickCount(); + step++; + } + } + } else { + vTaskDelay(200); + } + } + } // close while +} + + +static void PULPShieldInit() { + + if(isInit) return; + + xTaskCreate(PULPShieldTask, // pointer to the task entry function + PULP_SHIELD_TASK_NAME, // name for the task + PULP_SHIELD_TASK_STACKSIZE, // number of words for task's stack + NULL, // the task's parameter + PULP_SHIELD_TASK_PRI, // task priority + NULL); // pass a handle + + GPIOInit(); + GPIOOff(); + + isInit = true; + + DEBUG_PRINT("PULP-Shield Initialized\n"); +} + + +static bool PULPShieldTest() { + + DEBUG_PRINT("First PULP-Shield test passed!\n"); + // GPIO/SPI test should go here + return true; +} + + +void PULPShieldOn() { + + steering_angle_ = 0.0f; + probability_of_collision_ = 0.0f; + desired_forward_velocity_ = 0.0f; //max_forward_index_; + desired_angular_velocity_ = 0.0f; + lastUpdate = xTaskGetTickCount(); + + // Set GPIO on (power on the shield) + GPIOOn(); + // Enable SPI + spiBeginSlave(); + + // Reset task's step counter + step = 1; + + // Reset internal state + DroNetResetSetpoint(); + for(int i=0; i 1.0f) steering_angle_ = 1.0f; + if(probability_of_collision_ < 0.1f) probability_of_collision_ = 0.0f; + +#ifdef COLLISION_TEST + steering_angle_ = 0.0f; +#endif + +} + + +void DroNetGetSetpoint() { + + // update the output from DroNet + getDronetOutputs(); + float desired_forward_velocity_m; + + /************************ FORWARD VELOCITY ************************/ + + desired_forward_velocity_m = (1.0f - probability_of_collision_) * max_forward_index_; + + if(desired_forward_velocity_m < 0.0f) { + DEBUG_PRINT("Detected negative forward velocity! Drone will now stop!\n"); + desired_forward_velocity_m = 0.0f; + } + + // Low pass filter the velocity + desired_forward_velocity_ = (1.0f - alpha_velocity_) * desired_forward_velocity_ + + alpha_velocity_ * desired_forward_velocity_m; + + // Stop if velocity is prob of collision is too high + if(desired_forward_velocity_ < ((1.0f - critical_prob_coll_) * max_forward_index_)) { + desired_forward_velocity_ = 0.0f; + } + + /******************************************************************/ + + + /************************ ANGULAR VELOCITY ************************/ + + // Low pass filter the angular_velocity + desired_angular_velocity_ = (1.0f - alpha_yaw_) * desired_angular_velocity_ + + alpha_yaw_ * steering_angle_; + + /******************************************************************/ + + + /*********************** STATE UPDATE RATE ************************/ + + setpoint_.mode.x = modeVelocity; + setpoint_.mode.y = modeVelocity; + setpoint_.mode.z = modeAbs; + setpoint_.mode.yaw = modeVelocity; + setpoint_.position.z = PULP_TARGET_H; + setpoint_.velocity.x = desired_forward_velocity_; + setpoint_.velocity.y = 0.0f; + setpoint_.attitudeRate.yaw = desired_angular_velocity_*yaw_scaling; + + /******************************************************************/ +} + + +void DroNetResetSetpoint() { + + setpoint_.position.x = 0.0f; + setpoint_.position.y = 0.0f; + setpoint_.position.z = 0.0f; + + setpoint_.velocity.x = 0.0f; + setpoint_.velocity.y = 0.0f; + setpoint_.velocity.z = 0.0f; + + setpoint_.acceleration.x = 0.0f; + setpoint_.acceleration.y = 0.0f; + setpoint_.acceleration.z = 0.0f; + + setpoint_.attitude.pitch = 0.0f; + setpoint_.attitude.roll = 0.0f; + setpoint_.attitude.yaw = 0.0f; + + setpoint_.attitudeRate.pitch= 0.0f; + setpoint_.attitudeRate.roll = 0.0f; + setpoint_.attitudeRate.yaw = 0.0f; + + setpoint_.mode.x = modeDisable; + setpoint_.mode.y = modeDisable; + setpoint_.mode.z = modeDisable; + setpoint_.mode.roll = modeDisable; + setpoint_.mode.pitch = modeDisable; + setpoint_.mode.yaw = modeDisable; + setpoint_.mode.quat = modeDisable; + + setpoint_.velocity_body = true; +} + + +void DroNetTakeOff() { + + setpoint_.position.x = 0.0f; + setpoint_.position.y = 0.0f; + if(step <= PULP_STEPS_TAKEOFF) + setpoint_.position.z = (PULP_TARGET_H/PULP_STEPS_TAKEOFF*1.0f)*(step*1.0f); + else + setpoint_.position.z = PULP_TARGET_H; + setpoint_.velocity.x = 0.0f; + setpoint_.velocity.y = 0.0f; + setpoint_.velocity.z = 0.0f; + + setpoint_.acceleration.x = 0.0f; + setpoint_.acceleration.y = 0.0f; + setpoint_.acceleration.z = 0.0f; + + setpoint_.attitude.pitch = 0.0f; + setpoint_.attitude.roll = 0.0f; + setpoint_.attitude.yaw = 0.0f; + + setpoint_.attitudeRate.pitch= 0.0f; + setpoint_.attitudeRate.roll = 0.0f; + setpoint_.attitudeRate.yaw = 0.0f; + + setpoint_.mode.x = modeVelocity; + setpoint_.mode.y = modeVelocity; + setpoint_.mode.z = modeAbs; + setpoint_.mode.roll = modeDisable; + setpoint_.mode.pitch = modeDisable; + setpoint_.mode.yaw = modeVelocity; + setpoint_.mode.quat = modeDisable; + + setpoint_.velocity_body = true; +} + + +float fixed2float(int16_t *x) { + if((~x[0]) < x[0]) + return -FIXED_BITVALUE * ((~x[0])+1); + else + return FIXED_BITVALUE * x[0]; +} + + +uint32_t getLastPULPUpdate() { + return lastUpdate; +} + + +static const DeckDriver PULPShieldDriver = { + .name = "PULPShield", + .init = PULPShieldInit, + .test = PULPShieldTest, +}; + + +DECK_DRIVER(PULPShieldDriver); + +PARAM_GROUP_START(dronet) +PARAM_ADD(PARAM_FLOAT, alphaVel, &alpha_velocity_) +PARAM_ADD(PARAM_FLOAT, alphaYaw, &alpha_yaw_) +PARAM_ADD(PARAM_FLOAT, maxVel, &max_forward_index_) +PARAM_ADD(PARAM_FLOAT, criticalProb, &critical_prob_coll_) +PARAM_ADD(PARAM_FLOAT, yawScaling, &yaw_scaling) +PARAM_GROUP_STOP(dronet) + +LOG_GROUP_START(DroNet) +LOG_ADD(LOG_FLOAT, Steering, &steering_angle_) +LOG_ADD(LOG_FLOAT, Collision, &probability_of_collision_) +LOG_GROUP_STOP(DroNet) \ No newline at end of file diff --git a/src/deck/interface/deck_spi.h b/src/deck/interface/deck_spi.h index c68b7677c0..0486289ec6 100644 --- a/src/deck/interface/deck_spi.h +++ b/src/deck/interface/deck_spi.h @@ -41,7 +41,9 @@ * Initialize the SPI. */ void spiBegin(void); +void spiBeginSlave(void); void spiBeginTransaction(uint16_t baudRatePrescaler); +void spiBeginTransactionSlave(uint16_t baudRatePrescaler); void spiEndTransaction(); /* Send the data_tx buffer and receive into the data_rx buffer */ diff --git a/src/modules/src/commander.c b/src/modules/src/commander.c index cc0e29b861..40f7d20280 100644 --- a/src/modules/src/commander.c +++ b/src/modules/src/commander.c @@ -32,8 +32,12 @@ #include "commander.h" #include "crtp_commander.h" #include "crtp_commander_high_level.h" +#include "pulp_shield.h" +#include "stm32f4xx_rcc.h" #include "param.h" +#include "debug.h" +#include "log.h" static bool isInit; const static setpoint_t nullSetpoint; @@ -42,6 +46,13 @@ const static int priorityDisable = COMMANDER_PRIORITY_DISABLE; static uint32_t lastUpdate; static bool enableHighLevel = false; +/* PULP-Shield variables */ +static bool enablePULPShield = false; +static bool PULPRunning = false; +static bool landing = false; +static uint32_t landingStep, lastLandingUpdate; +static float PULP_vel_x, PULP_pos_z, PULP_att_yaw; + QueueHandle_t setpointQueue; QueueHandle_t priorityQueue; @@ -59,6 +70,12 @@ void commanderInit(void) crtpCommanderInit(); crtpCommanderHighLevelInit(); lastUpdate = xTaskGetTickCount(); + lastLandingUpdate = xTaskGetTickCount(); + + /* PULP-Shield initialization */ + PULP_vel_x = 0.0f; + PULP_pos_z = 0.0f; + PULP_att_yaw = 0.0f; isInit = true; } @@ -102,6 +119,119 @@ void commanderGetSetpoint(setpoint_t *setpoint, const state_t *state) setpoint->attitudeRate.yaw = 0; // Keep Z as it is } + + #ifdef PULP_SHIELD + + if(enablePULPShield) { + + if(!PULPRunning) { + //Turning on the Power-Enable on the Shield + PULPShieldOn(); + PULPRunning = true; + landing = true; + landingStep = 1; + } + + /* Watchdog triggering emergency landing in case of missing SPI + * communication from PULP. Typically due to FLL issue on GAP8 SoC */ +#ifdef WATCH_DOG_EN + if(xTaskGetTickCount()-getLastPULPUpdate()>ERROR_THRESHOLD) { + enablePULPShield = false; + PULPRunning = false; + DEBUG_PRINT("EMERGENCY STOP!\n"); + } +#endif + + setpoint->mode.x = PULPShieldGetSetpoint().mode.x; + setpoint->mode.y = PULPShieldGetSetpoint().mode.y; + setpoint->mode.z = PULPShieldGetSetpoint().mode.z; + setpoint->mode.pitch = modeDisable; + setpoint->mode.roll = modeDisable; + setpoint->mode.yaw = PULPShieldGetSetpoint().mode.yaw; + setpoint->mode.quat = modeDisable; + setpoint->velocity_body = true; + setpoint->position.z = PULPShieldGetSetpoint().position.z; + setpoint->velocity.x = PULPShieldGetSetpoint().velocity.x; + setpoint->velocity.y = PULPShieldGetSetpoint().velocity.y; + setpoint->attitudeRate.yaw = PULPShieldGetSetpoint().attitudeRate.yaw; + } + else { + if(landing) { + if(landingStep==PULP_STEPS_LANDING) { + landing = false; + landingStep = 1; + } + else { + + setpoint->mode.x = modeDisable; + setpoint->mode.y = modeDisable; + setpoint->mode.z = modeAbs; + setpoint->mode.roll = modeAbs; + setpoint->mode.pitch = modeAbs; + setpoint->mode.yaw = modeVelocity; + setpoint->mode.quat = modeDisable; + setpoint->velocity_body = true; + setpoint->position.x = 0.0f; + setpoint->position.y = 0.0f; + setpoint->position.z = PULP_TARGET_H-((PULP_TARGET_H/PULP_STEPS_LANDING*1.0f)*(landingStep*1.0f)); + setpoint->velocity.x = 0.0f; + setpoint->velocity.y = 0.0f; + setpoint->velocity.z = 0.0f; + setpoint->acceleration.x = 0.0f; + setpoint->acceleration.y = 0.0f; + setpoint->acceleration.z = 0.0f; + setpoint->attitude.pitch = 0.0f; + setpoint->attitude.roll = 0.0f; + setpoint->attitude.yaw = 0.0f; + setpoint->attitudeRate.pitch = 0.0f; + setpoint->attitudeRate.roll = 0.0f; + setpoint->attitudeRate.yaw = 0.0f; + + if((currentTime-lastLandingUpdate) > PULP_LANDING_RATE) { + landingStep++; + lastLandingUpdate = xTaskGetTickCount(); + } + } + } + else { + + PULPShieldOff(); + + setpoint->mode.x = modeDisable; + setpoint->mode.y = modeDisable; + setpoint->mode.z = modeDisable; + setpoint->mode.roll = modeDisable; + setpoint->mode.pitch = modeDisable; + setpoint->mode.yaw = modeDisable; + setpoint->mode.quat = modeDisable; + setpoint->velocity_body = true; + setpoint->position.x = 0.0f; + setpoint->position.y = 0.0f; + setpoint->position.z = 0.0f; + setpoint->velocity.x = 0.0f; + setpoint->velocity.y = 0.0f; + setpoint->velocity.z = 0.0f; + setpoint->acceleration.x = 0.0f; + setpoint->acceleration.y = 0.0f; + setpoint->acceleration.z = 0.0f; + setpoint->attitude.pitch = 0.0f; + setpoint->attitude.roll = 0.0f; + setpoint->attitude.yaw = 0.0f; + setpoint->attitudeRate.pitch = 0.0f; + setpoint->attitudeRate.roll = 0.0f; + setpoint->attitudeRate.yaw = 0.0f; + + PULPRunning = false; + } + } + + // only for logging + PULP_vel_x = setpoint->velocity.x; + PULP_pos_z = setpoint->position.z; + PULP_att_yaw = setpoint->attitudeRate.yaw/YAW_SCALING; + +#endif // PULP_SHIELD + } bool commanderTest(void) @@ -123,4 +253,11 @@ int commanderGetActivePriority(void) PARAM_GROUP_START(commander) PARAM_ADD(PARAM_UINT8, enHighLevel, &enableHighLevel) +PARAM_ADD(PARAM_UINT8, enPULPShield, &enablePULPShield) PARAM_GROUP_STOP(commander) + +LOG_GROUP_START(PULP) +LOG_ADD(LOG_FLOAT, PULP_x, &PULP_vel_x) +LOG_ADD(LOG_FLOAT, PULP_z, &PULP_pos_z) +LOG_ADD(LOG_FLOAT, PULP_yaw, &PULP_att_yaw) +LOG_GROUP_STOP(PULP) \ No newline at end of file diff --git a/src/modules/src/estimator_kalman.c b/src/modules/src/estimator_kalman.c index dbd820abda..fb464253dd 100644 --- a/src/modules/src/estimator_kalman.c +++ b/src/modules/src/estimator_kalman.c @@ -540,6 +540,10 @@ void estimatorKalmanGetEstimatedPos(point_t* pos) { pos->z = coreData.S[KC_STATE_Z]; } +void setResetEstimation(bool val) { + coreData.resetEstimation = val; +} + // Temporary development groups LOG_GROUP_START(kalman_states) LOG_ADD(LOG_FLOAT, ox, &coreData.S[KC_STATE_X]) diff --git a/src/modules/src/position_controller_pid.c b/src/modules/src/position_controller_pid.c index e9efd0773a..7c96a2082b 100644 --- a/src/modules/src/position_controller_pid.c +++ b/src/modules/src/position_controller_pid.c @@ -82,7 +82,7 @@ static struct this_s this = { .init = { .kp = 25.0f, .ki = 1.0f, - .kd = 0.0f, + .kd = 0.5f, }, .pid.dt = DT, }, @@ -91,7 +91,7 @@ static struct this_s this = { .init = { .kp = 25.0f, .ki = 1.0f, - .kd = 0.0f, + .kd = 0.5f, }, .pid.dt = DT, }, diff --git a/tools/make/config.mk b/tools/make/config.mk new file mode 100644 index 0000000000..761a21aae3 --- /dev/null +++ b/tools/make/config.mk @@ -0,0 +1,44 @@ +## Copy this file to config.mk and modify to get you personal build configuration + +## Set CRTP link to E-SKY receiver +# CFLAGS += -DUSE_ESKYLINK + +## Redirect the console output to the UART +# CFLAGS += -DDEBUG_PRINT_ON_UART + +## Load a deck driver that has no OW memory +# CFLAGS += -DDECK_FORCE=bcBuzzer + +## Enable biq quad deck features +# CFLAGS += -DENABLE_BQ_DECK +# CFLAGS += -DBQ_DECK_ENABLE_PM +# CFLAGS += -DBQ_DECK_ENABLE_OSD + +## Use morse when flashing the LED to indicate that the Crazyflie is calibrated +# CFLAGS += -DCALIBRATED_LED_MORSE + +## Turn on monitoring of queue usages +# CFLAGS += -DDEBUG_QUEUE_MONITOR + +## Automatically reboot to bootloader before flashing +# CLOAD_CMDS = -w radio://0/100/2M/E7E7E7E7E7 + +## Set number of anchor in LocoPositioningSystem +# CFLAGS += -DLOCODECK_NR_OF_ANCHORS=8 + +## Set alternative pins for LOCO deck (IRQ=IO_2, RESET=IO_3, default are RX1 and TX1) +# CFLAGS += -DLOCODECK_USE_ALT_PINS + +## Compile positioning system for TDoA mode +# LPS_TDOA_ENABLE=1 + +## Use J-Link as Debugger/flasher +# OPENOCD_INTERFACE ?= interface/jlink.cfg +# OPENOCD_TARGET ?= target/stm32f4x.cfg +# OPENOCD_CMDS ?= -c "transport select swd" + +#ESTIMATOR = Kalman +#CFLAGS += -DUPDATE_KALMAN_WITH_RANGING -DKALMAN_DECOUPLE_XY +CFLAGS += -DDECK_FORCE=PULPShield + +DEBUG=1 \ No newline at end of file