PULP-Shield/PULP-DroNet integration

.gitignore

@@ -8,7 +8,6 @@ bin/*
 .settings/*
 .vscode/*
 *~
-tools/make/config.mk
 cflie.*
 version.c
 tags

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

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

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

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 <dpalossi@iis.ee.ethz.ch>                         *
+ * 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<<FIXEDPT_FBITS))
+
+#define MA_HISTORY			16
+
+#define ALPHA_YAW			0.5f	// Default: 0.5f
+#define YAW_SCALING			-180.0f	// Default: -180.0f
+
+#define ALPHA_VEL			0.3f	// Default: 0.3f >= 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_ */

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)