From 69a5e95827cafbb70984782c3786e64a612295c9 Mon Sep 17 00:00:00 2001 From: Tobias Antonsson Date: Wed, 5 Aug 2015 10:06:05 +0200 Subject: [PATCH 01/11] Removed constant in the battery compensated thrust so that 0 thrust outputs 0 PWM. --- drivers/src/motors.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/drivers/src/motors.c b/drivers/src/motors.c index 03e510a5a7..d6afb8758b 100644 --- a/drivers/src/motors.c +++ b/drivers/src/motors.c @@ -458,7 +458,7 @@ void motorsSetRatio(int id, uint16_t ithrust) ASSERT(id < NBR_OF_MOTORS); float thrust = ((float)ithrust / 65536.0f) * 60; - float volts = -0.0006239 * thrust * thrust + 0.088 * thrust + 0.069; + float volts = -0.0006239 * thrust * thrust + 0.088 * thrust; float supply_voltage = pmGetBatteryVoltage(); float percentage = volts / supply_voltage; percentage = percentage > 1.0 ? 1.0 : percentage; From 8035cd9784c4c66459f14177db1b520b9fcebb89 Mon Sep 17 00:00:00 2001 From: Fredrik Gratte Date: Sun, 9 Aug 2015 23:09:47 +0200 Subject: [PATCH 02/11] Adding automatic take-off function based on the sitaw framework --- modules/interface/sitaw.h | 2 +- modules/src/stabilizer.c | 89 +++++++++++++++++++++++++++++++++++++-- 2 files changed, 87 insertions(+), 4 deletions(-) diff --git a/modules/interface/sitaw.h b/modules/interface/sitaw.h index 9e5983aba8..dd708fc0cc 100644 --- a/modules/interface/sitaw.h +++ b/modules/interface/sitaw.h @@ -46,7 +46,7 @@ void sitAwInit(void); #define SITAW_FF_TRIGGER_COUNT 15 /* The number of consecutive tests for Free Fall to be detected. Configured for 250Hz testing. */ /* Configuration options for the 'At Rest' detection. */ -#define SITAW_AR_THRESHOLD 0.02 /* The default tolerance for AccZ deviations from 1 and AccX, AccY deviations from 0, indicating At Rest. */ +#define SITAW_AR_THRESHOLD 0.05 /* The default tolerance for AccZ deviations from 1 and AccX, AccY deviations from 0, indicating At Rest. */ #define SITAW_AR_TRIGGER_COUNT 500 /* The number of consecutive tests for At Rest to be detected. Configured for 250Hz testing. */ /* Configuration options for the 'Tumbled' detection. */ diff --git a/modules/src/stabilizer.c b/modules/src/stabilizer.c index f9becd61cd..fbe4be9531 100644 --- a/modules/src/stabilizer.c +++ b/modules/src/stabilizer.c @@ -122,6 +122,16 @@ static uint16_t altHoldMinThrust = 00000; // minimum hover thrust - not used static uint16_t altHoldBaseThrust = 43000; // approximate throttle needed when in perfect hover. More weight/older battery can use a higher value static uint16_t altHoldMaxThrust = 60000; // max altitude hold thrust +#if defined(SITAW_ENABLED) +// Automatic take-off variables +static bool autoTOActive = false; // Flag indicating if automatic take-off is active / deactive. +static float autoTOAltBase = 0.0f; // Base altitude for the automatic take-off. Set to altHoldTarget when automatic take-off is activated. +static float autoTOAltCurrent = 0.0f; // Current target altitude adjustment. Equals 0 when function is activated, increases to autoTOThresh when function is deactivated. +// Automatic take-off parameters +static float autoTOAlpha = 0.98f; // Smoothing factor when adjusting the altHoldTarget altitude. +static float autoTOTargetAdjust = 1.5f; // Meters to add to altHoldTarget to reach auto take-off altitude. +static float autoTOThresh = 0.97f; // Threshold for when to deactivate auto Take-Off. A value of 0.97 means 97% of the target altitude adjustment. +#endif RPYType rollType; RPYType pitchType; @@ -184,7 +194,7 @@ bool stabilizerTest(void) static void stabilizerPostAttitudeUpdateCallOut(void) { - /* Code that shall run AFTER each attitude update should be placed here. */ + /* Code that shall run AFTER each attitude update, should be placed here. */ #if defined(SITAW_ENABLED) /* Test values for Free Fall detection. */ @@ -196,11 +206,13 @@ static void stabilizerPostAttitudeUpdateCallOut(void) /* Test values for At Rest detection. */ sitAwARTest(acc.x, acc.y, acc.z); + /* Enable altHold mode if free fall is detected. */ if(sitAwFFDetected() && !sitAwTuDetected()) { commanderSetAltHoldMode(true); } - if(sitAwARDetected() || sitAwTuDetected()) { + /* Disable altHold mode if a Tumbled situation is detected. */ + if(sitAwTuDetected()) { commanderSetAltHoldMode(false); } #endif @@ -208,10 +220,11 @@ static void stabilizerPostAttitudeUpdateCallOut(void) static void stabilizerPreThrustUpdateCallOut(void) { - /* Code that shall run BEFORE each thrust distribution update should be placed here. */ + /* Code that shall run BEFORE each thrust distribution update, should be placed here. */ #if defined(SITAW_ENABLED) if(sitAwTuDetected()) { + /* Kill the thrust to the motors if a Tumbled situation is detected. */ actuatorThrust = 0; } #endif @@ -323,6 +336,55 @@ static void stabilizerTask(void* param) } } +static void stabilizerPreAltHoldComputeThrustCallOut(void) +{ + /* Code that shall run BEFORE each altHold thrust computation, should be placed here. */ + +#if defined(SITAW_ENABLED) + /* + * The number of variables used for automatic Take-Off could be reduced, however that would + * cause debugging and tuning to become more difficult. The variables currently used ensure + * that tuning can easily be done through the LOG and PARAM frameworks. + * + * Note that while the automatic take-off function is active, it will overrule any other + * changes to altHoldTarget by the user. + * + * The automatic take-off function will automatically deactivate once the take-off has been + * conducted. + */ + if(!autoTOActive){ + /* + * Enabling automatic take-off: When At Rest, Not Tumbled, and the user pressing the AltHold button + */ + if(sitAwARDetected() && !sitAwTuDetected() && setAltHold) { + /* Enable automatic take-off. */ + autoTOActive = true; + autoTOAltBase = altHoldTarget; + autoTOAltCurrent = 0.0f; + } + } + + if(autoTOActive) { + /* + * Automatic take-off is quite simple: Slowly increase altHoldTarget until reaching the target altitude. + */ + + /* Calculate the new current setpoint for altHoldTarget. autoTOAltCurrent is normalized to values from 0 to 1. */ + autoTOAltCurrent = autoTOAltCurrent * autoTOAlpha + (1 - autoTOAlpha); + + /* Update the altHoldTarget variable. */ + altHoldTarget = autoTOAltBase + autoTOAltCurrent * autoTOTargetAdjust; + + if((autoTOAltCurrent >= autoTOThresh)) { + /* Disable the automatic take-off mode if target altitude has been reached. */ + autoTOActive = false; + autoTOAltBase = 0.0f; + autoTOAltCurrent = 0.0f; + } + } +#endif +} + static void stabilizerAltHoldUpdate(void) { // Get altitude hold commands from pilot @@ -376,6 +438,9 @@ static void stabilizerAltHoldUpdate(void) altHoldPIDVal = pidUpdate(&altHoldPID, asl, false); } + /* Call out before performing altHold thrust regulation. */ + stabilizerPreAltHoldComputeThrustCallOut(); + // In altitude hold mode if (altHold) { @@ -529,6 +594,15 @@ LOG_ADD(LOG_FLOAT, vSpeedASL, &vSpeedASL) LOG_ADD(LOG_FLOAT, vSpeedAcc, &vSpeedAcc) LOG_GROUP_STOP(altHold) +#if defined(SITAW_ENABLED) +// Automatic take-off parameters +LOG_GROUP_START(autoTO) +LOG_ADD(LOG_UINT8, Active, &autoTOActive) +LOG_ADD(LOG_FLOAT, AltBase, &autoTOAltBase) +LOG_ADD(LOG_FLOAT, AltCurrent, &autoTOAltCurrent) +LOG_GROUP_STOP(autoTO) +#endif + // Params for altitude hold PARAM_GROUP_START(altHold) PARAM_ADD(PARAM_FLOAT, aslAlpha, &aslAlpha) @@ -551,3 +625,12 @@ PARAM_ADD(PARAM_UINT16, baseThrust, &altHoldBaseThrust) PARAM_ADD(PARAM_UINT16, maxThrust, &altHoldMaxThrust) PARAM_ADD(PARAM_UINT16, minThrust, &altHoldMinThrust) PARAM_GROUP_STOP(altHold) + +#if defined(SITAW_ENABLED) +// Automatic take-off parameters +PARAM_GROUP_START(autoTO) +PARAM_ADD(PARAM_FLOAT, TargetAdjust, &autoTOTargetAdjust) +PARAM_ADD(PARAM_FLOAT, Thresh, &autoTOThresh) +PARAM_ADD(PARAM_FLOAT, Alpha, &autoTOAlpha) +PARAM_GROUP_STOP(autoTO) +#endif From d167028a51f32e6cb42d5f79afa4819571a2d913 Mon Sep 17 00:00:00 2001 From: Tobias Antonsson Date: Wed, 5 Aug 2015 14:28:11 +0200 Subject: [PATCH 03/11] Added initial BigQuad support. --- Makefile | 2 +- drivers/interface/motors.h | 7 +++++ drivers/src/motors.c | 46 +++++++++++++++++----------- hal/src/ow_syslink.c | 10 +++++- modules/interface/bigquad.h | 35 +++++++++++++++++++++ modules/interface/expbrd.h | 1 + modules/src/bigquad.c | 61 +++++++++++++++++++++++++++++++++++++ modules/src/expbrd.c | 6 ++++ 8 files changed, 148 insertions(+), 20 deletions(-) create mode 100644 modules/interface/bigquad.h create mode 100644 modules/src/bigquad.c diff --git a/Makefile b/Makefile index c894fb664c..f4f2755390 100644 --- a/Makefile +++ b/Makefile @@ -136,7 +136,7 @@ PROJ_OBJ_CF2 += imu_cf2.o pm_f405.o syslink.o radiolink.o ow_syslink.o proximity PROJ_OBJ += system.o comm.o console.o pid.o crtpservice.o param.o mem.o PROJ_OBJ += commander.o controller.o sensfusion6.o stabilizer.o PROJ_OBJ += log.o worker.o trigger.o sitaw.o -PROJ_OBJ_CF2 += neopixelring.o expbrd.o platformservice.o +PROJ_OBJ_CF2 += neopixelring.o expbrd.o platformservice.o bigquad.o # Expansion boards PROJ_OBJ_CF2 += exptest.o diff --git a/drivers/interface/motors.h b/drivers/interface/motors.h index 7c38097d7f..e772753f6c 100644 --- a/drivers/interface/motors.h +++ b/drivers/interface/motors.h @@ -163,8 +163,15 @@ #define FULL 1000 #define STOP 0 +typedef enum +{ + BRUSHED, + BRUSHLESS +} motorsDrvType; + typedef struct { + motorsDrvType drvType; uint32_t gpioPerif; GPIO_TypeDef* gpioPort; uint32_t gpioPin; diff --git a/drivers/src/motors.c b/drivers/src/motors.c index d6afb8758b..b08f9859a4 100644 --- a/drivers/src/motors.c +++ b/drivers/src/motors.c @@ -49,6 +49,7 @@ void motorsBeep(int id, bool enable, uint16_t frequency, uint16_t ratio); #ifdef PLATFORM_CF1 static const MotorPerifDef CONN_M1 = { + .drvType = BRUSHED, .gpioPerif = RCC_APB2Periph_GPIOB, .gpioPort = GPIOB, .gpioPin = GPIO_Pin_1, @@ -70,6 +71,7 @@ static const MotorPerifDef CONN_M1 = static const MotorPerifDef CONN_M2 = { + .drvType = BRUSHED, .gpioPerif = RCC_APB2Periph_GPIOB, .gpioPort = GPIOB, .gpioPin = GPIO_Pin_0, @@ -91,6 +93,7 @@ static const MotorPerifDef CONN_M2 = static const MotorPerifDef CONN_M3 = { + .drvType = BRUSHED, .gpioPerif = RCC_APB2Periph_GPIOB, .gpioPort = GPIOB, .gpioPin = GPIO_Pin_9, @@ -112,6 +115,7 @@ static const MotorPerifDef CONN_M3 = static const MotorPerifDef CONN_M4 = { + .drvType = BRUSHED, .gpioPerif = RCC_APB2Periph_GPIOB, .gpioPort = GPIOB, .gpioPin = GPIO_Pin_8, @@ -133,6 +137,7 @@ static const MotorPerifDef CONN_M4 = #else static const MotorPerifDef CONN_M1 = { + .drvType = BRUSHED, .gpioPerif = RCC_AHB1Periph_GPIOA, .gpioPort = GPIOA, .gpioPin = GPIO_Pin_1, @@ -154,6 +159,7 @@ static const MotorPerifDef CONN_M1 = static const MotorPerifDef CONN_M2 = { + .drvType = BRUSHED, .gpioPerif = RCC_AHB1Periph_GPIOB, .gpioPort = GPIOB, .gpioPin = GPIO_Pin_11, @@ -176,6 +182,7 @@ static const MotorPerifDef CONN_M2 = static const MotorPerifDef CONN_M3 = { + .drvType = BRUSHED, .gpioPerif = RCC_AHB1Periph_GPIOA, .gpioPort = GPIOA, .gpioPin = GPIO_Pin_15, @@ -197,6 +204,7 @@ static const MotorPerifDef CONN_M3 = static const MotorPerifDef CONN_M4 = { + .drvType = BRUSHED, .gpioPerif = RCC_AHB1Periph_GPIOB, .gpioPort = GPIOB, .gpioPin = GPIO_Pin_9, @@ -218,6 +226,7 @@ static const MotorPerifDef CONN_M4 = static const MotorPerifDef DECK_TX2 = { + .drvType = BRUSHLESS, .gpioPerif = RCC_AHB1Periph_GPIOA, .gpioPort = GPIOA, .gpioPin = GPIO_Pin_2, @@ -239,6 +248,7 @@ static const MotorPerifDef DECK_TX2 = static const MotorPerifDef DECK_RX2 = { + .drvType = BRUSHLESS, .gpioPerif = RCC_AHB1Periph_GPIOA, .gpioPort = GPIOA, .gpioPin = GPIO_Pin_3, @@ -260,6 +270,7 @@ static const MotorPerifDef DECK_RX2 = static const MotorPerifDef DECK_IO2 = { + .drvType = BRUSHLESS, .gpioPerif = RCC_AHB1Periph_GPIOB, .gpioPort = GPIOB, .gpioPin = GPIO_Pin_5, @@ -281,6 +292,7 @@ static const MotorPerifDef DECK_IO2 = static const MotorPerifDef DECK_IO3 = { + .drvType = BRUSHLESS, .gpioPerif = RCC_AHB1Periph_GPIOB, .gpioPort = GPIOB, .gpioPin = GPIO_Pin_4, @@ -307,6 +319,9 @@ const MotorPerifDef* motorMapBrushed[NBR_OF_MOTORS] = {&CONN_M1, &CONN_M2, &CONN const MotorPerifDef** motorMap; /* Current map configuration */ const int MOTORS[] = { MOTOR_M1, MOTOR_M2, MOTOR_M3, MOTOR_M4 }; + +static const uint16_t testsound[NBR_OF_MOTORS] = {A4, A5, F5, D5 }; + static bool isInit = false; /* Private functions */ @@ -423,30 +438,25 @@ void motorsDeInit(const MotorPerifDef** motorMapSelect) bool motorsTest(void) { - #ifndef BRUSHLESS_MOTORCONTROLLER int i; -#ifdef ACTIVATE_STARTUP_SOUND - uint16_t testsound[NBR_OF_MOTORS] = {A4, A5, F5, D5 }; - for (i = 0; i < sizeof(MOTORS) / sizeof(*MOTORS); i++) { - motorsBeep(MOTORS[i], true, testsound[i], (uint16_t)(MOTORS_TIM_BEEP_CLK_FREQ / A4)/ 20); - vTaskDelay(M2T(MOTORS_TEST_ON_TIME_MS)); - motorsBeep(MOTORS[i], false, 0, 0); - vTaskDelay(M2T(MOTORS_TEST_DELAY_TIME_MS)); - } + if (motorMap[i]->drvType == BRUSHED) + { +#ifdef ACTIVATE_STARTUP_SOUND + motorsBeep(MOTORS[i], true, testsound[i], (uint16_t)(MOTORS_TIM_BEEP_CLK_FREQ / A4)/ 20); + vTaskDelay(M2T(MOTORS_TEST_ON_TIME_MS)); + motorsBeep(MOTORS[i], false, 0, 0); + vTaskDelay(M2T(MOTORS_TEST_DELAY_TIME_MS)); #else - for (i = 0; i < sizeof(MOTORS) / sizeof(*MOTORS); i++) - { - motorsSetRatio(MOTORS[i], MOTORS_TEST_RATIO); - vTaskDelay(M2T(MOTORS_TEST_ON_TIME_MS)); - motorsSetRatio(MOTORS[i], 0); - vTaskDelay(M2T(MOTORS_TEST_DELAY_TIME_MS)); - } - -#endif + motorsSetRatio(MOTORS[i], MOTORS_TEST_RATIO); + vTaskDelay(M2T(MOTORS_TEST_ON_TIME_MS)); + motorsSetRatio(MOTORS[i], 0); + vTaskDelay(M2T(MOTORS_TEST_DELAY_TIME_MS)); #endif + } + } return isInit; } diff --git a/hal/src/ow_syslink.c b/hal/src/ow_syslink.c index b43724e046..eb3072e712 100644 --- a/hal/src/ow_syslink.c +++ b/hal/src/ow_syslink.c @@ -61,8 +61,16 @@ static uint8_t tbData[] = 0xeb, 0x00, 0x00, 0x00, 0x00, 0xbc, 0xff, 0xaa, 0x00, 0x0c, 0x01, 0x07, 0x62, 0x63, 0x63, 0x66, 0x32, 0x74, 0x62, 0x02, 0x01, 0x61, 0xbb, 0x00 }; + +static uint8_t bqData[] = +{ + 0xeb, 0x00, 0x00, 0x00, 0x00, 0xbc, 0x03, 0x9d, 0x00, 0x0e, 0x01, 0x09, 0x62, 0x63, + 0x42, 0x69, 0x67, 0x51, 0x75, 0x61, 0x64, 0x02, 0x01, 0x61, 0xe9, 0x00 +}; + #endif + void owInit() { syslinkInit(); @@ -118,7 +126,7 @@ bool owTest() } #endif #ifdef OW_WRITE_TEST - if (owWrite(0, 0, sizeof(tbData), tbData)) + if (owWrite(0, 0, sizeof(bqData), bqData)) { DEBUG_PRINT("Write [OK].\n"); } diff --git a/modules/interface/bigquad.h b/modules/interface/bigquad.h new file mode 100644 index 0000000000..164e450628 --- /dev/null +++ b/modules/interface/bigquad.h @@ -0,0 +1,35 @@ +/** + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ + * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ + * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ + * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ + * + * Crazyflie control firmware + * + * Copyright (C) 2011-2012 Bitcraze AB + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, in version 3. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * bigquad.h - Big-Quad-Deck + */ +#ifndef __BIGQUAD_H__ +#define __BIGQUAD_H__ + +#include + +void bigquadInit(); +bool bigquadTest(); + +#endif //__BIGQUAD_H__ + diff --git a/modules/interface/expbrd.h b/modules/interface/expbrd.h index 0a85664466..74c04e9a02 100644 --- a/modules/interface/expbrd.h +++ b/modules/interface/expbrd.h @@ -38,6 +38,7 @@ // Definition of expansion board Product ID #define EXPBRD_PID_LEDRING 0x01 #define EXPBRD_PID_QI 0x02 +#define EXPBRD_PID_BIGQUAD 0x03 #define EXPBRD_PID_ET 0xFF diff --git a/modules/src/bigquad.c b/modules/src/bigquad.c new file mode 100644 index 0000000000..82bb817b35 --- /dev/null +++ b/modules/src/bigquad.c @@ -0,0 +1,61 @@ +/* + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ + * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ + * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ + * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ + * + * Crazyflie control firmware + * + * Copyright (C) 2011-2012 Bitcraze AB + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, in version 3. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * exptest.c - Testing of expansion port. + */ +#define DEBUG_MODULE "BIGQUAD" + +#include +#include + +#include "stm32fxxx.h" +#include "config.h" +#include "bigquad.h" +#include "motors.h" +#include "debug.h" + +//Hardware configuration +static bool isInit; + +void bigquadInit() +{ + if(isInit) + return; + + DEBUG_PRINT("Switching to brushless.\n"); + motorsInit(motorMapBigQuadDeck); + + isInit = true; +} + +bool bigquadTest() +{ + bool status = true; + + if(!isInit) + return false; + + status = motorsTest(); + + return status; +} diff --git a/modules/src/expbrd.c b/modules/src/expbrd.c index f837a551f6..84d76aa820 100644 --- a/modules/src/expbrd.c +++ b/modules/src/expbrd.c @@ -35,6 +35,7 @@ #include "crc.h" #include "exptest.h" #include "neopixelring.h" +#include "bigquad.h" #include "imu.h" #include "config.h" @@ -67,6 +68,11 @@ void expbrdInit() #endif } + if (expbrdIsPresent(EXPBRD_VID_BITCRAZE, EXPBRD_PID_BIGQUAD)) + { +// bigquadInit(); + } + isInit = true; } From 526a8b776eda48522196e2d04cd1145b88aad5b6 Mon Sep 17 00:00:00 2001 From: Tobias Antonsson Date: Wed, 5 Aug 2015 19:24:59 +0200 Subject: [PATCH 04/11] Removed BAT compensated thrust for brushless setup. Fixed motorsDeInit which reset all pins. --- drivers/src/motors.c | 37 +++++++++++++++++++++++-------------- modules/src/expbrd.c | 2 +- 2 files changed, 24 insertions(+), 15 deletions(-) diff --git a/drivers/src/motors.c b/drivers/src/motors.c index b08f9859a4..d703a7895f 100644 --- a/drivers/src/motors.c +++ b/drivers/src/motors.c @@ -371,8 +371,15 @@ void motorsInit(const MotorPerifDef** motorMapSelect) MOTORS_RCC_TIM_CMD(motorMap[i]->timPerif, ENABLE); // Configure the GPIO for the timer output + GPIO_StructInit(&GPIO_InitStructure); GPIO_InitStructure.GPIO_Mode = MOTORS_GPIO_MODE; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; +#ifdef PLATFORM_CF2 + if (motorMap[i]->drvType == BRUSHLESS) + { + GPIO_InitStructure.GPIO_OType = GPIO_OType_OD; + } +#endif GPIO_InitStructure.GPIO_Pin = motorMap[i]->gpioPin; GPIO_Init(motorMap[i]->gpioPort, &GPIO_InitStructure); @@ -421,6 +428,7 @@ void motorsDeInit(const MotorPerifDef** motorMapSelect) { // Configure default GPIO_StructInit(&GPIO_InitStructure); + GPIO_InitStructure.GPIO_Pin = motorMap[i]->gpioPin; GPIO_Init(motorMap[i]->gpioPort, &GPIO_InitStructure); #ifdef PLATFORM_CF1 @@ -461,28 +469,29 @@ bool motorsTest(void) return isInit; } -#ifdef ENABLE_THRUST_BAT_COMPENSATED // Ithrust is thrust mapped for 65536 <==> 60g void motorsSetRatio(int id, uint16_t ithrust) { + uint16_t ratio; + ASSERT(id < NBR_OF_MOTORS); - float thrust = ((float)ithrust / 65536.0f) * 60; - float volts = -0.0006239 * thrust * thrust + 0.088 * thrust; - float supply_voltage = pmGetBatteryVoltage(); - float percentage = volts / supply_voltage; - percentage = percentage > 1.0 ? 1.0 : percentage; - uint16_t ratio = percentage * UINT16_MAX; + ratio = ithrust; + +#ifdef ENABLE_THRUST_BAT_COMPENSATED + if (motorMap[id]->drvType == BRUSHED) + { + float thrust = ((float)ithrust / 65536.0f) * 60; + float volts = -0.0006239 * thrust * thrust + 0.088 * thrust; + float supply_voltage = pmGetBatteryVoltage(); + float percentage = volts / supply_voltage; + percentage = percentage > 1.0 ? 1.0 : percentage; + ratio = percentage * UINT16_MAX; + } +#endif motorMap[id]->setCompare(motorMap[id]->tim, motorMap[id]->conv16ToBits(ratio)); } -#else -void motorsSetRatio(int id, uint16_t ratio) -{ - ASSERT(id < NBR_OF_MOTORS); - motorMap[id]->setCompare(motorMap[id]->tim, motorMap[id]->conv16ToBits(ratio)); -} -#endif int motorsGetRatio(int id) { diff --git a/modules/src/expbrd.c b/modules/src/expbrd.c index 84d76aa820..d6b08e2099 100644 --- a/modules/src/expbrd.c +++ b/modules/src/expbrd.c @@ -70,7 +70,7 @@ void expbrdInit() if (expbrdIsPresent(EXPBRD_VID_BITCRAZE, EXPBRD_PID_BIGQUAD)) { -// bigquadInit(); + bigquadInit(); } isInit = true; From 626a688a610ae4bc56773b6b585be3fa17388ba1 Mon Sep 17 00:00:00 2001 From: Tobias Antonsson Date: Fri, 7 Aug 2015 11:05:39 +0200 Subject: [PATCH 05/11] Cleaned up the motor definitions by putting them i separate .c files. Added brushless config for motor connectors. --- drivers/interface/motors.h | 51 ++-- drivers/src/motors.c | 306 +++-------------------- drivers/src/motors_def_cf1.c | 217 +++++++++++++++++ drivers/src/motors_def_cf2.c | 461 +++++++++++++++++++++++++++++++++++ modules/src/stabilizer.c | 2 +- 5 files changed, 734 insertions(+), 303 deletions(-) create mode 100644 drivers/src/motors_def_cf1.c create mode 100644 drivers/src/motors_def_cf2.c diff --git a/drivers/interface/motors.h b/drivers/interface/motors.h index e772753f6c..a49ffbbc71 100644 --- a/drivers/interface/motors.h +++ b/drivers/interface/motors.h @@ -40,27 +40,10 @@ #include "stm32fxxx.h" /******** Defines ********/ -/** - * *VARNING* Flashing the brushless driver on the Crazyflie with normal brushed motors connected - * will turn it on at full speed when it is powered on! - * - * Generates a PWM wave (50 - 200 Hz update rate with 1-2 ms high pulse) using the timer. That way we can use the same - * base as for the regular PWM driver. This means it will be a PWM with a period of the update rate configured to be high - * only in the 1-2 ms range. - * The BLMC input signal are meant to be connected to the Crazyflie round motor solder pad (open-drain output). A resistor - * around 470 ohm needs to pull the signal high to the voltage level of the BLMC (normally 5V). - */ - #define BLMC_PERIOD 0.0025 // 2.5ms = 400Hz - #define TIM_CLOCK_HZ 84000000 - #define MOTORS_BL_PWM_PRESCALE_RAW (uint32_t)((TIM_CLOCK_HZ/0xFFFF) * BLMC_PERIOD + 1) // +1 is to not end up above 0xFFFF in the end - #define MOTORS_BL_PWM_CNT_FOR_PERIOD (uint32_t)(TIM_CLOCK_HZ * BLMC_PERIOD / MOTORS_BL_PWM_PRESCALE_RAW) - #define MOTORS_BL_PWM_CNT_FOR_1MS (uint32_t)(TIM_CLOCK_HZ * 0.001 / MOTORS_BL_PWM_PRESCALE_RAW) - #define MOTORS_BL_PWM_PERIOD MOTORS_BL_PWM_CNT_FOR_PERIOD - #define MOTORS_BL_PWM_PRESCALE (uint16_t)(MOTORS_BL_PWM_PRESCALE_RAW - 1) - #define MOTORS_BL_POLARITY TIM_OCPolarity_Low #ifdef PLATFORM_CF1 // The following defines gives a PWM of 9 bits at ~140KHz for a sysclock of 72MHz + #define TIM_CLOCK_HZ 72000000 #define MOTORS_PWM_BITS 9 #define MOTORS_PWM_PERIOD ((1<drvType == BRUSHLESS) - { - GPIO_InitStructure.GPIO_OType = GPIO_OType_OD; - } + GPIO_InitStructure.GPIO_OType = motorMap[i]->gpioOType; #endif GPIO_InitStructure.GPIO_Pin = motorMap[i]->gpioPin; GPIO_Init(motorMap[i]->gpioPort, &GPIO_InitStructure); @@ -470,7 +203,7 @@ bool motorsTest(void) } // Ithrust is thrust mapped for 65536 <==> 60g -void motorsSetRatio(int id, uint16_t ithrust) +void motorsSetRatio(uint32_t id, uint16_t ithrust) { uint16_t ratio; @@ -489,14 +222,31 @@ void motorsSetRatio(int id, uint16_t ithrust) ratio = percentage * UINT16_MAX; } #endif - - motorMap[id]->setCompare(motorMap[id]->tim, motorMap[id]->conv16ToBits(ratio)); + if (motorMap[id]->drvType == BRUSHLESS) + { + motorMap[id]->setCompare(motorMap[id]->tim, motorsBLConv16ToBits(ratio)); + } + else + { + motorMap[id]->setCompare(motorMap[id]->tim, motorsConv16ToBits(ratio)); + } } -int motorsGetRatio(int id) +int motorsGetRatio(uint32_t id) { + int ratio; + ASSERT(id < NBR_OF_MOTORS); - return motorMap[id]->convBitsTo16(motorMap[id]->getCompare(motorMap[id]->tim)); + if (motorMap[id]->drvType == BRUSHLESS) + { + ratio = motorsBLConvBitsTo16(motorMap[id]->getCompare(motorMap[id]->tim)); + } + else + { + ratio = motorsConvBitsTo16(motorMap[id]->getCompare(motorMap[id]->tim)); + } + + return ratio; } /* Set PWM frequency for motor controller diff --git a/drivers/src/motors_def_cf1.c b/drivers/src/motors_def_cf1.c new file mode 100644 index 0000000000..10d12b5898 --- /dev/null +++ b/drivers/src/motors_def_cf1.c @@ -0,0 +1,217 @@ +/** + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ + * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ + * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ + * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ + * + * Crazyflie control firmware + * + * Copyright (C) 2011-2012 Bitcraze AB + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, in version 3. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * motors.c - Motor driver + * + * This code mainly interfacing the PWM peripheral lib of ST. + */ + +static const MotorPerifDef CONN_M1 = +{ + .drvType = BRUSHED, + .gpioPerif = RCC_APB2Periph_GPIOB, + .gpioPort = GPIOB, + .gpioPin = GPIO_Pin_1, + .gpioPinSource = GPIO_PinSource1, + .gpioOType = 0, + .gpioAF = GPIO_PartialRemap_TIM3, + .timPerif = RCC_APB1Periph_TIM3, + .tim = TIM3, + .timPolarity = TIM_OCPolarity_High, + .timDbgStop = DBGMCU_TIM3_STOP, + .timPeriod = MOTORS_PWM_PERIOD, + .timPrescaler = MOTORS_PWM_PRESCALE, + .setCompare = TIM_SetCompare4, + .getCompare = TIM_GetCapture4, + .ocInit = TIM_OC4Init, + .preloadConfig = TIM_OC4PreloadConfig, +}; + +static const MotorPerifDef CONN_M2 = +{ + .drvType = BRUSHED, + .gpioPerif = RCC_APB2Periph_GPIOB, + .gpioPort = GPIOB, + .gpioPin = GPIO_Pin_0, + .gpioPinSource = GPIO_PinSource0, + .gpioOType = 0, + .gpioAF = GPIO_PartialRemap_TIM3, + .timPerif = RCC_APB1Periph_TIM3, + .tim = TIM3, + .timPolarity = TIM_OCPolarity_High, + .timDbgStop = DBGMCU_TIM3_STOP, + .timPeriod = MOTORS_PWM_PERIOD, + .timPrescaler = MOTORS_PWM_PRESCALE, + .setCompare = TIM_SetCompare3, + .getCompare = TIM_GetCapture3, + .ocInit = TIM_OC3Init, + .preloadConfig = TIM_OC3PreloadConfig, +}; + +static const MotorPerifDef CONN_M3 = +{ + .drvType = BRUSHED, + .gpioPerif = RCC_APB2Periph_GPIOB, + .gpioPort = GPIOB, + .gpioPin = GPIO_Pin_9, + .gpioPinSource = GPIO_PinSource9, + .gpioOType = 0, + .gpioAF = 0, + .timPerif = RCC_APB1Periph_TIM4, + .tim = TIM4, + .timPolarity = TIM_OCPolarity_High, + .timDbgStop = DBGMCU_TIM4_STOP, + .timPeriod = MOTORS_PWM_PERIOD, + .timPrescaler = MOTORS_PWM_PRESCALE, + .setCompare = TIM_SetCompare4, + .getCompare = TIM_GetCapture4, + .ocInit = TIM_OC4Init, + .preloadConfig = TIM_OC4PreloadConfig, +}; + +static const MotorPerifDef CONN_M4 = +{ + .drvType = BRUSHED, + .gpioPerif = RCC_APB2Periph_GPIOB, + .gpioPort = GPIOB, + .gpioPin = GPIO_Pin_8, + .gpioPinSource = GPIO_PinSource8, + .gpioOType = 0, + .gpioAF = 0, + .timPerif = RCC_APB1Periph_TIM4, + .tim = TIM4, + .timPolarity = TIM_OCPolarity_High, + .timDbgStop = DBGMCU_TIM4_STOP, + .timPeriod = MOTORS_PWM_PERIOD, + .timPrescaler = MOTORS_PWM_PRESCALE, + .setCompare = TIM_SetCompare3, + .getCompare = TIM_GetCapture3, + .ocInit = TIM_OC3Init, + .preloadConfig = TIM_OC3PreloadConfig, +}; + +static const MotorPerifDef CONN_M1_BL = +{ + .drvType = BRUSHLESS, + .gpioPerif = RCC_APB2Periph_GPIOB, + .gpioPort = GPIOB, + .gpioPin = GPIO_Pin_1, + .gpioPinSource = GPIO_PinSource1, + .gpioOType = 0, + .gpioAF = GPIO_PartialRemap_TIM3, + .timPerif = RCC_APB1Periph_TIM3, + .tim = TIM3, + .timPolarity = TIM_OCPolarity_Low, + .timDbgStop = DBGMCU_TIM3_STOP, + .timPeriod = MOTORS_BL_PWM_PERIOD, + .timPrescaler = MOTORS_BL_PWM_PRESCALE, + .setCompare = TIM_SetCompare4, + .getCompare = TIM_GetCapture4, + .ocInit = TIM_OC4Init, + .preloadConfig = TIM_OC4PreloadConfig, +}; + +static const MotorPerifDef CONN_M2_BL = +{ + .drvType = BRUSHLESS, + .gpioPerif = RCC_APB2Periph_GPIOB, + .gpioPort = GPIOB, + .gpioPin = GPIO_Pin_0, + .gpioPinSource = GPIO_PinSource0, + .gpioOType = 0, + .gpioAF = GPIO_PartialRemap_TIM3, + .timPerif = RCC_APB1Periph_TIM3, + .tim = TIM3, + .timPolarity = TIM_OCPolarity_Low, + .timDbgStop = DBGMCU_TIM3_STOP, + .timPeriod = MOTORS_BL_PWM_PERIOD, + .timPrescaler = MOTORS_BL_PWM_PRESCALE, + .setCompare = TIM_SetCompare3, + .getCompare = TIM_GetCapture3, + .ocInit = TIM_OC3Init, + .preloadConfig = TIM_OC3PreloadConfig, +}; + +static const MotorPerifDef CONN_M3_BL = +{ + .drvType = BRUSHLESS, + .gpioPerif = RCC_APB2Periph_GPIOB, + .gpioPort = GPIOB, + .gpioPin = GPIO_Pin_9, + .gpioPinSource = GPIO_PinSource9, + .gpioOType = 0, + .gpioAF = 0, + .timPerif = RCC_APB1Periph_TIM4, + .tim = TIM4, + .timPolarity = TIM_OCPolarity_Low, + .timDbgStop = DBGMCU_TIM4_STOP, + .timPeriod = MOTORS_BL_PWM_PERIOD, + .timPrescaler = MOTORS_BL_PWM_PRESCALE, + .setCompare = TIM_SetCompare4, + .getCompare = TIM_GetCapture4, + .ocInit = TIM_OC4Init, + .preloadConfig = TIM_OC4PreloadConfig, +}; + +static const MotorPerifDef CONN_M4_BL = +{ + .drvType = BRUSHLESS, + .gpioPerif = RCC_APB2Periph_GPIOB, + .gpioPort = GPIOB, + .gpioPin = GPIO_Pin_8, + .gpioPinSource = GPIO_PinSource8, + .gpioOType = 0, + .gpioAF = 0, + .timPerif = RCC_APB1Periph_TIM4, + .tim = TIM4, + .timPolarity = TIM_OCPolarity_Low, + .timDbgStop = DBGMCU_TIM4_STOP, + .timPeriod = MOTORS_BL_PWM_PERIOD, + .timPrescaler = MOTORS_BL_PWM_PRESCALE, + .setCompare = TIM_SetCompare3, + .getCompare = TIM_GetCapture3, + .ocInit = TIM_OC3Init, + .preloadConfig = TIM_OC3PreloadConfig, +}; + +/** + * Default brushed mapping to M1-M4 connectors. + */ +const MotorPerifDef* motorMapDefaultBrushed[NBR_OF_MOTORS] = +{ + &CONN_M1, + &CONN_M2, + &CONN_M3, + &CONN_M4 +}; + +/** + * Brushless motors mapped to the standard motor connectors with pull-ups (~1K) to VBAT soldered. + */ +const MotorPerifDef* motorMapDefaltConBrushless[NBR_OF_MOTORS] = +{ + &CONN_M1_BL, + &CONN_M2_BL, + &CONN_M3_BL, + &CONN_M4_BL +}; diff --git a/drivers/src/motors_def_cf2.c b/drivers/src/motors_def_cf2.c new file mode 100644 index 0000000000..af3fc8e677 --- /dev/null +++ b/drivers/src/motors_def_cf2.c @@ -0,0 +1,461 @@ +/** + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ + * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ + * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ + * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ + * + * Crazyflie control firmware + * + * Copyright (C) 2011-2012 Bitcraze AB + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, in version 3. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * motors.c - Motor driver + * + * This code mainly interfacing the PWM peripheral lib of ST. + */ +// Connector M1, PA1, TIM2_CH2 +static const MotorPerifDef CONN_M1 = +{ + .drvType = BRUSHED, + .gpioPerif = RCC_AHB1Periph_GPIOA, + .gpioPort = GPIOA, + .gpioPin = GPIO_Pin_1, + .gpioPinSource = GPIO_PinSource1, + .gpioOType = GPIO_OType_PP, + .gpioAF = GPIO_AF_TIM2, + .timPerif = RCC_APB1Periph_TIM2, + .tim = TIM2, + .timPolarity = TIM_OCPolarity_High, + .timDbgStop = DBGMCU_TIM2_STOP, + .timPeriod = MOTORS_PWM_PERIOD, + .timPrescaler = MOTORS_PWM_PRESCALE, + .setCompare = TIM_SetCompare2, + .getCompare = TIM_GetCapture2, + .ocInit = TIM_OC2Init, + .preloadConfig = TIM_OC2PreloadConfig, +}; + +// Connector M2, PB11, TIM2_CH4 +static const MotorPerifDef CONN_M2 = +{ + .drvType = BRUSHED, + .gpioPerif = RCC_AHB1Periph_GPIOB, + .gpioPort = GPIOB, + .gpioPin = GPIO_Pin_11, + .gpioPinSource = GPIO_PinSource11, + .gpioOType = GPIO_OType_PP, + .gpioAF = GPIO_AF_TIM2, + .timPerif = RCC_APB1Periph_TIM2, + .tim = TIM2, + .timPolarity = TIM_OCPolarity_High, + .timDbgStop = DBGMCU_TIM2_STOP, + .timPeriod = MOTORS_PWM_PERIOD, + .timPrescaler = MOTORS_PWM_PRESCALE, + .setCompare = TIM_SetCompare4, + .getCompare = TIM_GetCapture4, + .ocInit = TIM_OC4Init, + .preloadConfig = TIM_OC4PreloadConfig, +}; + +// Connector M3, PA15, TIM2_CH1 +static const MotorPerifDef CONN_M3 = +{ + .drvType = BRUSHED, + .gpioPerif = RCC_AHB1Periph_GPIOA, + .gpioPort = GPIOA, + .gpioPin = GPIO_Pin_15, + .gpioPinSource = GPIO_PinSource15, + .gpioOType = GPIO_OType_PP, + .gpioAF = GPIO_AF_TIM2, + .timPerif = RCC_APB1Periph_TIM2, + .tim = TIM2, + .timPolarity = TIM_OCPolarity_High, + .timDbgStop = DBGMCU_TIM2_STOP, + .timPeriod = MOTORS_PWM_PERIOD, + .timPrescaler = MOTORS_PWM_PRESCALE, + .setCompare = TIM_SetCompare1, + .getCompare = TIM_GetCapture1, + .ocInit = TIM_OC1Init, + .preloadConfig = TIM_OC1PreloadConfig, +}; + +// Connector M4, PB9, TIM4_CH4 +static const MotorPerifDef CONN_M4 = +{ + .drvType = BRUSHED, + .gpioPerif = RCC_AHB1Periph_GPIOB, + .gpioPort = GPIOB, + .gpioPin = GPIO_Pin_9, + .gpioPinSource = GPIO_PinSource9, + .gpioOType = GPIO_OType_PP, + .gpioAF = GPIO_AF_TIM4, + .timPerif = RCC_APB1Periph_TIM4, + .tim = TIM4, + .timPolarity = TIM_OCPolarity_High, + .timDbgStop = DBGMCU_TIM4_STOP, + .timPeriod = MOTORS_PWM_PERIOD, + .timPrescaler = MOTORS_PWM_PRESCALE, + .setCompare = TIM_SetCompare4, + .getCompare = TIM_GetCapture4, + .ocInit = TIM_OC4Init, + .preloadConfig = TIM_OC4PreloadConfig, +}; + +// Connector M1, PA1, TIM2_CH2, Brushless config +static const MotorPerifDef CONN_M1_BL = +{ + .drvType = BRUSHLESS, + .gpioPerif = RCC_AHB1Periph_GPIOA, + .gpioPort = GPIOA, + .gpioPin = GPIO_Pin_1, + .gpioPinSource = GPIO_PinSource1, + .gpioOType = GPIO_OType_PP, + .gpioAF = GPIO_AF_TIM2, + .timPerif = RCC_APB1Periph_TIM2, + .tim = TIM2, + .timPolarity = TIM_OCPolarity_Low, + .timDbgStop = DBGMCU_TIM2_STOP, + .timPeriod = MOTORS_BL_PWM_PERIOD, + .timPrescaler = MOTORS_BL_PWM_PRESCALE, + .setCompare = TIM_SetCompare2, + .getCompare = TIM_GetCapture2, + .ocInit = TIM_OC2Init, + .preloadConfig = TIM_OC2PreloadConfig, +}; + +// Connector M2, PB11, TIM2_CH4, Brushless config +static const MotorPerifDef CONN_M2_BL = +{ + .drvType = BRUSHLESS, + .gpioPerif = RCC_AHB1Periph_GPIOB, + .gpioPort = GPIOB, + .gpioPin = GPIO_Pin_11, + .gpioPinSource = GPIO_PinSource11, + .gpioOType = GPIO_OType_PP, + .gpioAF = GPIO_AF_TIM2, + .timPerif = RCC_APB1Periph_TIM2, + .tim = TIM2, + .timPolarity = TIM_OCPolarity_Low, + .timDbgStop = DBGMCU_TIM2_STOP, + .timPeriod = MOTORS_BL_PWM_PERIOD, + .timPrescaler = MOTORS_BL_PWM_PRESCALE, + .setCompare = TIM_SetCompare4, + .getCompare = TIM_GetCapture4, + .ocInit = TIM_OC4Init, + .preloadConfig = TIM_OC4PreloadConfig, +}; + +// Connector M3, PA15, TIM2_CH1, Brushless config +static const MotorPerifDef CONN_M3_BL = +{ + .drvType = BRUSHLESS, + .gpioPerif = RCC_AHB1Periph_GPIOA, + .gpioPort = GPIOA, + .gpioPin = GPIO_Pin_15, + .gpioPinSource = GPIO_PinSource15, + .gpioOType = GPIO_OType_PP, + .gpioAF = GPIO_AF_TIM2, + .timPerif = RCC_APB1Periph_TIM2, + .tim = TIM2, + .timPolarity = TIM_OCPolarity_Low, + .timDbgStop = DBGMCU_TIM2_STOP, + .timPeriod = MOTORS_BL_PWM_PERIOD, + .timPrescaler = MOTORS_BL_PWM_PRESCALE, + .setCompare = TIM_SetCompare1, + .getCompare = TIM_GetCapture1, + .ocInit = TIM_OC1Init, + .preloadConfig = TIM_OC1PreloadConfig, +}; + +// Connector M4, PB9, TIM4_CH4, Brushless config +static const MotorPerifDef CONN_M4_BL = +{ + .drvType = BRUSHLESS, + .gpioPerif = RCC_AHB1Periph_GPIOB, + .gpioPort = GPIOB, + .gpioPin = GPIO_Pin_9, + .gpioPinSource = GPIO_PinSource9, + .gpioOType = GPIO_OType_PP, + .gpioAF = GPIO_AF_TIM4, + .timPerif = RCC_APB1Periph_TIM4, + .tim = TIM4, + .timPolarity = TIM_OCPolarity_Low, + .timDbgStop = DBGMCU_TIM4_STOP, + .timPeriod = MOTORS_BL_PWM_PERIOD, + .timPrescaler = MOTORS_BL_PWM_PRESCALE, + .setCompare = TIM_SetCompare4, + .getCompare = TIM_GetCapture4, + .ocInit = TIM_OC4Init, + .preloadConfig = TIM_OC4PreloadConfig, +}; + +// Deck TX2, PA2, TIM2_CH3 +static const MotorPerifDef DECK_TX2_TIM2 = +{ + .drvType = BRUSHLESS, + .gpioPerif = RCC_AHB1Periph_GPIOA, + .gpioPort = GPIOA, + .gpioPin = GPIO_Pin_2, + .gpioPinSource = GPIO_PinSource2, + .gpioOType = GPIO_OType_OD, + .gpioAF = GPIO_AF_TIM2, + .timPerif = RCC_APB1Periph_TIM2, + .tim = TIM2, + .timPolarity = TIM_OCPolarity_High, + .timDbgStop = DBGMCU_TIM2_STOP, + .timPeriod = MOTORS_BL_PWM_PERIOD, + .timPrescaler = MOTORS_BL_PWM_PRESCALE, + .setCompare = TIM_SetCompare3, + .getCompare = TIM_GetCapture3, + .ocInit = TIM_OC3Init, + .preloadConfig = TIM_OC3PreloadConfig, +}; + +// Deck TX2, PA2, TIM5_CH3 +static const MotorPerifDef DECK_TX2_TIM5 = +{ + .drvType = BRUSHLESS, + .gpioPerif = RCC_AHB1Periph_GPIOA, + .gpioPort = GPIOA, + .gpioPin = GPIO_Pin_2, + .gpioPinSource = GPIO_PinSource2, + .gpioOType = GPIO_OType_OD, + .gpioAF = GPIO_AF_TIM5, + .timPerif = RCC_APB1Periph_TIM5, + .tim = TIM5, + .timPolarity = TIM_OCPolarity_High, + .timDbgStop = DBGMCU_TIM5_STOP, + .timPeriod = MOTORS_BL_PWM_PERIOD, + .timPrescaler = MOTORS_BL_PWM_PRESCALE, + .setCompare = TIM_SetCompare3, + .getCompare = TIM_GetCapture3, + .ocInit = TIM_OC3Init, + .preloadConfig = TIM_OC3PreloadConfig, +}; + +// Deck RX2, PA3, TIM2_CH4 +static const MotorPerifDef DECK_RX2_TIM2 = +{ + .drvType = BRUSHLESS, + .gpioPerif = RCC_AHB1Periph_GPIOA, + .gpioPort = GPIOA, + .gpioPin = GPIO_Pin_3, + .gpioPinSource = GPIO_PinSource3, + .gpioOType = GPIO_OType_OD, + .gpioAF = GPIO_AF_TIM2, + .timPerif = RCC_APB1Periph_TIM2, + .tim = TIM2, + .timPolarity = TIM_OCPolarity_High, + .timDbgStop = DBGMCU_TIM2_STOP, + .timPeriod = MOTORS_BL_PWM_PERIOD, + .timPrescaler = MOTORS_BL_PWM_PRESCALE, + .setCompare = TIM_SetCompare4, + .getCompare = TIM_GetCapture4, + .ocInit = TIM_OC4Init, + .preloadConfig = TIM_OC4PreloadConfig, +}; + +// Deck RX2, PA3, TIM5_CH4 +static const MotorPerifDef DECK_RX2_TIM5 = +{ + .drvType = BRUSHLESS, + .gpioPerif = RCC_AHB1Periph_GPIOA, + .gpioPort = GPIOA, + .gpioPin = GPIO_Pin_3, + .gpioPinSource = GPIO_PinSource3, + .gpioOType = GPIO_OType_OD, + .gpioAF = GPIO_AF_TIM5, + .timPerif = RCC_APB1Periph_TIM5, + .tim = TIM5, + .timPolarity = TIM_OCPolarity_High, + .timDbgStop = DBGMCU_TIM5_STOP, + .timPeriod = MOTORS_BL_PWM_PERIOD, + .timPrescaler = MOTORS_BL_PWM_PRESCALE, + .setCompare = TIM_SetCompare4, + .getCompare = TIM_GetCapture4, + .ocInit = TIM_OC4Init, + .preloadConfig = TIM_OC4PreloadConfig, +}; + +// Deck IO1, PB8, TIM4_CH3 +static const MotorPerifDef DECK_IO1_TIM4 = +{ + .drvType = BRUSHLESS, + .gpioPerif = RCC_AHB1Periph_GPIOB, + .gpioPort = GPIOB, + .gpioPin = GPIO_Pin_8, + .gpioPinSource = GPIO_PinSource8, + .gpioOType = GPIO_OType_OD, + .gpioAF = GPIO_AF_TIM4, + .timPerif = RCC_APB1Periph_TIM4, + .tim = TIM4, + .timPolarity = TIM_OCPolarity_High, + .timDbgStop = DBGMCU_TIM4_STOP, + .timPeriod = MOTORS_BL_PWM_PERIOD, + .timPrescaler = MOTORS_BL_PWM_PRESCALE, + .setCompare = TIM_SetCompare3, + .getCompare = TIM_GetCapture3, + .ocInit = TIM_OC3Init, + .preloadConfig = TIM_OC3PreloadConfig, +}; + +// Deck IO2, PB5, TIM3_CH2 +static const MotorPerifDef DECK_IO2 = +{ + .drvType = BRUSHLESS, + .gpioPerif = RCC_AHB1Periph_GPIOB, + .gpioPort = GPIOB, + .gpioPin = GPIO_Pin_5, + .gpioPinSource = GPIO_PinSource5, + .gpioOType = GPIO_OType_OD, + .gpioAF = GPIO_AF_TIM3, + .timPerif = RCC_APB1Periph_TIM3, + .tim = TIM3, + .timPolarity = TIM_OCPolarity_High, + .timDbgStop = DBGMCU_TIM3_STOP, + .timPeriod = MOTORS_BL_PWM_PERIOD, + .timPrescaler = MOTORS_BL_PWM_PRESCALE, + .setCompare = TIM_SetCompare2, + .getCompare = TIM_GetCapture2, + .ocInit = TIM_OC2Init, + .preloadConfig = TIM_OC2PreloadConfig, +}; + +// Deck IO3, PB4, TIM3_CH1 +static const MotorPerifDef DECK_IO3 = +{ + .drvType = BRUSHLESS, + .gpioPerif = RCC_AHB1Periph_GPIOB, + .gpioPort = GPIOB, + .gpioPin = GPIO_Pin_4, + .gpioPinSource = GPIO_PinSource4, + .gpioOType = GPIO_OType_OD, + .gpioAF = GPIO_AF_TIM3, + .timPerif = RCC_APB1Periph_TIM3, + .tim = TIM3, + .timPolarity = TIM_OCPolarity_High, + .timDbgStop = DBGMCU_TIM3_STOP, + .timPeriod = MOTORS_BL_PWM_PERIOD, + .timPrescaler = MOTORS_BL_PWM_PRESCALE, + .setCompare = TIM_SetCompare1, + .getCompare = TIM_GetCapture1, + .ocInit = TIM_OC1Init, + .preloadConfig = TIM_OC1PreloadConfig, +}; + +// Deck SCK, PA5, TIM2_CH1 +static const MotorPerifDef DECK_SCK = +{ + .drvType = BRUSHLESS, + .gpioPerif = RCC_AHB1Periph_GPIOA, + .gpioPort = GPIOA, + .gpioPin = GPIO_Pin_5, + .gpioPinSource = GPIO_PinSource5, + .gpioOType = GPIO_OType_OD, + .gpioAF = GPIO_AF_TIM2, + .timPerif = RCC_APB1Periph_TIM2, + .tim = TIM2, + .timPolarity = TIM_OCPolarity_High, + .timDbgStop = DBGMCU_TIM2_STOP, + .timPeriod = MOTORS_BL_PWM_PERIOD, + .timPrescaler = MOTORS_BL_PWM_PRESCALE, + .setCompare = TIM_SetCompare1, + .getCompare = TIM_GetCapture1, + .ocInit = TIM_OC1Init, + .preloadConfig = TIM_OC1PreloadConfig, +}; + +// Deck MISO, PA6, TIM3_CH1 +static const MotorPerifDef DECK_MISO = +{ + .drvType = BRUSHLESS, + .gpioPerif = RCC_AHB1Periph_GPIOA, + .gpioPort = GPIOA, + .gpioPin = GPIO_Pin_6, + .gpioPinSource = GPIO_PinSource6, + .gpioOType = GPIO_OType_OD, + .gpioAF = GPIO_AF_TIM3, + .timPerif = RCC_APB1Periph_TIM3, + .tim = TIM3, + .timPolarity = TIM_OCPolarity_High, + .timDbgStop = DBGMCU_TIM3_STOP, + .timPeriod = MOTORS_BL_PWM_PERIOD, + .timPrescaler = MOTORS_BL_PWM_PRESCALE, + .setCompare = TIM_SetCompare1, + .getCompare = TIM_GetCapture1, + .ocInit = TIM_OC1Init, + .preloadConfig = TIM_OC1PreloadConfig, +}; + +// Deck MOSI, PA7, TIM3_CH1 +static const MotorPerifDef DECK_MOSI = +{ + .drvType = BRUSHLESS, + .gpioPerif = RCC_AHB1Periph_GPIOA, + .gpioPort = GPIOA, + .gpioPin = GPIO_Pin_7, + .gpioPinSource = GPIO_PinSource7, + .gpioOType = GPIO_OType_OD, + .gpioAF = GPIO_AF_TIM3, + .timPerif = RCC_APB1Periph_TIM3, + .tim = TIM3, + .timPolarity = TIM_OCPolarity_High, + .timDbgStop = DBGMCU_TIM3_STOP, + .timPeriod = MOTORS_BL_PWM_PERIOD, + .timPrescaler = MOTORS_BL_PWM_PRESCALE, + .setCompare = TIM_SetCompare2, + .getCompare = TIM_GetCapture2, + .ocInit = TIM_OC2Init, + .preloadConfig = TIM_OC2PreloadConfig, +}; + +/** + * Default brushed mapping to M1-M4 connectors. + */ +const MotorPerifDef* motorMapDefaultBrushed[NBR_OF_MOTORS] = +{ + &CONN_M1, + &CONN_M2, + &CONN_M3, + &CONN_M4 +}; + +/** + * Brushless motors mapped as on the Big-Quad deck + * M1 -> IO3 + * M2 -> TX2 + * M3 -> RX2 + * M4 -> IO2 + */ +const MotorPerifDef* motorMapBigQuadDeck[NBR_OF_MOTORS] = +{ + &DECK_IO3, + &DECK_TX2_TIM2, + &DECK_RX2_TIM2, + &DECK_IO2 +}; + +/** + * Brushless motors mapped to the standard motor connectors with pull-ups (~1K) to VBAT soldered. + */ +const MotorPerifDef* motorMapDefaltConBrushless[NBR_OF_MOTORS] = +{ + &CONN_M1_BL, + &CONN_M2_BL, + &CONN_M3_BL, + &CONN_M4_BL +}; + + diff --git a/modules/src/stabilizer.c b/modules/src/stabilizer.c index fbe4be9531..65d4670018 100644 --- a/modules/src/stabilizer.c +++ b/modules/src/stabilizer.c @@ -162,7 +162,7 @@ void stabilizerInit(void) if(isInit) return; - motorsInit(motorMapBrushed); + motorsInit(motorMapDefaultBrushed); imu6Init(); sensfusion6Init(); controllerInit(); From e830f594ab814e6df035be4e6f3f571b34b56027 Mon Sep 17 00:00:00 2001 From: Tobias Antonsson Date: Tue, 11 Aug 2015 14:53:51 +0200 Subject: [PATCH 06/11] Adjusted motor mapping for Big-quad-deck proto-v2. --- drivers/src/motors_def_cf2.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/drivers/src/motors_def_cf2.c b/drivers/src/motors_def_cf2.c index af3fc8e677..7f7895cd1e 100644 --- a/drivers/src/motors_def_cf2.c +++ b/drivers/src/motors_def_cf2.c @@ -441,10 +441,10 @@ const MotorPerifDef* motorMapDefaultBrushed[NBR_OF_MOTORS] = */ const MotorPerifDef* motorMapBigQuadDeck[NBR_OF_MOTORS] = { - &DECK_IO3, &DECK_TX2_TIM2, - &DECK_RX2_TIM2, - &DECK_IO2 + &DECK_IO3, + &DECK_IO2, + &DECK_RX2_TIM2 }; /** From 74c31f1c4163c93b1ba41bcd99c1f4c4c6ba4942 Mon Sep 17 00:00:00 2001 From: Tobias Antonsson Date: Tue, 11 Aug 2015 16:44:46 +0200 Subject: [PATCH 07/11] Added clarification comments to log variables. Closes #50. --- hal/src/imu_cf2.c | 2 ++ modules/src/stabilizer.c | 66 ++++++++++++++++++++-------------------- 2 files changed, 35 insertions(+), 33 deletions(-) diff --git a/hal/src/imu_cf2.c b/hal/src/imu_cf2.c index d2c96b250c..b604a890ed 100644 --- a/hal/src/imu_cf2.c +++ b/hal/src/imu_cf2.c @@ -70,8 +70,10 @@ #define GYRO_NBR_OF_AXES 3 #define GYRO_MIN_BIAS_TIMEOUT_MS M2T(1*1000) +// Number of samples used in variance calculation. Changing this effects the threshold #define IMU_NBR_OF_BIAS_SAMPLES 128 +// Variance threshold to take zero bias for gyro #define GYRO_VARIANCE_BASE 2000 #define GYRO_VARIANCE_THRESHOLD_X (GYRO_VARIANCE_BASE) #define GYRO_VARIANCE_THRESHOLD_Y (GYRO_VARIANCE_BASE) diff --git a/modules/src/stabilizer.c b/modules/src/stabilizer.c index 65d4670018..0852a1fd28 100644 --- a/modules/src/stabilizer.c +++ b/modules/src/stabilizer.c @@ -70,34 +70,34 @@ static Axis3f gyro; // Gyro axis data in deg/s static Axis3f acc; // Accelerometer axis data in mG static Axis3f mag; // Magnetometer axis data in testla -static float eulerRollActual; -static float eulerPitchActual; -static float eulerYawActual; -static float eulerRollDesired; -static float eulerPitchDesired; -static float eulerYawDesired; -static float rollRateDesired; -static float pitchRateDesired; -static float yawRateDesired; +static float eulerRollActual; // Measured roll angle in deg +static float eulerPitchActual; // Measured pitch angle in deg +static float eulerYawActual; // Measured yaw angle in deg +static float eulerRollDesired; // Desired roll angle in deg +static float eulerPitchDesired; // Desired ptich angle in deg +static float eulerYawDesired; // Desired yaw angle in deg +static float rollRateDesired; // Desired roll rate in deg/s +static float pitchRateDesired; // Desired pitch rate in deg/s +static float yawRateDesired; // Desired yaw rate in deg/s // Baro variables -static float temperature; // temp from barometer -static float pressure; // pressure from barometer -static float asl; // smoothed asl -static float aslRaw; // raw asl -static float aslLong; // long term asl +static float temperature; // temp from barometer in celcius +static float pressure; // pressure from barometer in bar +static float asl; // smoothed asl +static float aslRaw; // raw asl +static float aslLong; // long term asl // Altitude hold variables -static PidObject altHoldPID; // Used for altitute hold mode. I gets reset when the bat status changes -bool altHold = false; // Currently in altitude hold mode +static PidObject altHoldPID; // Used for altitute hold mode. I gets reset when the bat status changes +bool altHold = false; // Currently in altitude hold mode bool setAltHold = false; // Hover mode has just been activated -static float accWZ = 0.0; -static float accMAG = 0.0; -static float vSpeedASL = 0.0; -static float vSpeedAcc = 0.0; +static float accWZ = 0.0; // Acceleration Without gravity along Z axis. +static float accMAG = 0.0; // Acceleration magnitude +static float vSpeedASL = 0.0; // Vertical speed (world frame) derived from barometer ASL +static float vSpeedAcc = 0.0; // Vertical speed (world frame) integrated from vertical acceleration static float vSpeed = 0.0; // Vertical speed (world frame) integrated from vertical acceleration -static float altHoldPIDVal; // Output of the PID controller -static float altHoldErr; // Different between target and current altitude +static float altHoldPIDVal; // Output of the PID controller +static float altHoldErr; // Different between target and current altitude // Altitude hold & Baro Params static float altHoldKp = 0.5; // PID gain constants, used everytime we reinitialise the PID controller @@ -133,19 +133,19 @@ static float autoTOTargetAdjust = 1.5f; // Meters to add to altHoldTarget to static float autoTOThresh = 0.97f; // Threshold for when to deactivate auto Take-Off. A value of 0.97 means 97% of the target altitude adjustment. #endif -RPYType rollType; -RPYType pitchType; -RPYType yawType; +RPYType rollType; // Current configuration type of roll (rate or angle) +RPYType pitchType; // Current configuration type of pitch (rate or angle) +RPYType yawType; // Current configuration type of yaw (rate or angle) -uint16_t actuatorThrust; -int16_t actuatorRoll; -int16_t actuatorPitch; -int16_t actuatorYaw; +uint16_t actuatorThrust; // Actuator output for thrust base +int16_t actuatorRoll; // Actuator output roll compensation +int16_t actuatorPitch; // Actuator output pitch compensation +int16_t actuatorYaw; // Actuator output yaw compensation -uint32_t motorPowerM4; -uint32_t motorPowerM2; -uint32_t motorPowerM1; -uint32_t motorPowerM3; +uint32_t motorPowerM1; // Motor 1 power output (16bit value used: 0 - 65535) +uint32_t motorPowerM2; // Motor 2 power output (16bit value used: 0 - 65535) +uint32_t motorPowerM3; // Motor 3 power output (16bit value used: 0 - 65535) +uint32_t motorPowerM4; // Motor 4 power output (16bit value used: 0 - 65535) static bool isInit; From 446c1e7a14c980b9eb9cab47911aa7495157689d Mon Sep 17 00:00:00 2001 From: Tobias Antonsson Date: Wed, 12 Aug 2015 14:16:46 +0200 Subject: [PATCH 08/11] Fixed CF1 configblock flash address. --- config/config.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/config/config.h b/config/config.h index de97e18b9a..f5af3c7f2d 100644 --- a/config/config.h +++ b/config/config.h @@ -58,7 +58,7 @@ #else #define P_NAME "Crazyflie Rev.F" - #define CONFIG_BLOCK_ADDRESS (1024 * (64-1)) + #define CONFIG_BLOCK_ADDRESS (1024 * (128-1)) #define MCU_ID_ADDRESS 0x1FFFF7E8 #define MCU_FLASH_SIZE_ADDRESS 0x1FFFF7E0 #define FREERTOS_HEAP_SIZE 15000 From 7cc43495522b55036631c8bc66b0033375f83816 Mon Sep 17 00:00:00 2001 From: Tobias Antonsson Date: Wed, 12 Aug 2015 16:59:12 +0200 Subject: [PATCH 09/11] Added Yaw flight modes Carefree, Plus, X. Added possibility to reset carefree heading. Thanks to capriele for the base code. --- modules/interface/commander.h | 26 +++++++++- modules/src/commander.c | 18 ++++++- modules/src/stabilizer.c | 89 +++++++++++++++++++++++++++++++++++ 3 files changed, 130 insertions(+), 3 deletions(-) diff --git a/modules/interface/commander.h b/modules/interface/commander.h index 0769b69934..4f673acb7e 100644 --- a/modules/interface/commander.h +++ b/modules/interface/commander.h @@ -27,16 +27,36 @@ #define COMMANDER_H_ #include #include +#include "config.h" + +#ifdef PLATFORM_CF1 + #define DEFUALT_YAW_MODE PLUSMODE +#else + #define DEFUALT_YAW_MODE XMODE +#endif #define COMMANDER_WDT_TIMEOUT_STABALIZE M2T(500) #define COMMANDER_WDT_TIMEOUT_SHUTDOWN M2T(2000) +/** + * Stabilization modes for Roll, Pitch, Yaw. + */ typedef enum { - RATE, - ANGLE + RATE = 0, + ANGLE = 1, } RPYType; +/** + * Yaw flight Modes + */ +typedef enum +{ + CAREFREE = 0, // Yaw is locked to world coordinates thus heading stays the same when yaw rotates + PLUSMODE = 1, // Plus-mode. Motor M1 is defined as front + XMODE = 2, // X-mode. M1 & M4 is defined as front +} YawModeType; + void commanderInit(void); bool commanderTest(void); void commanderWatchdog(void); @@ -47,5 +67,7 @@ void commanderGetThrust(uint16_t* thrust); void commanderGetAltHold(bool* altHold, bool* setAltHold, float* altHoldChange); bool commanderGetAltHoldMode(void); void commanderSetAltHoldMode(bool altHoldModeNew); +YawModeType commanderGetYawMode(void); +bool commanderGetYawModeCarefreeResetFront(void); #endif /* COMMANDER_H_ */ diff --git a/modules/src/commander.c b/modules/src/commander.c index 083a87a57c..5f7226687e 100644 --- a/modules/src/commander.c +++ b/modules/src/commander.c @@ -51,6 +51,10 @@ static bool thrustLocked; static bool altHoldMode = false; static bool altHoldModeOld = false; +static YawModeType yawMode = DEFUALT_YAW_MODE; // Yaw mode configuration +static bool carefreeResetFront; // Reset what is front in carefree mode + + static void commanderCrtpCB(CRTPPacket* pk); static void commanderWatchdogReset(void); @@ -142,7 +146,7 @@ void commanderGetAltHold(bool* altHold, bool* setAltHold, float* altHoldChange) bool commanderGetAltHoldMode(void) { - return(altHoldMode); + return (altHoldMode); } void commanderSetAltHoldMode(bool altHoldModeNew) @@ -199,8 +203,20 @@ void commanderGetThrust(uint16_t* thrust) commanderWatchdog(); } +YawModeType commanderGetYawMode(void) +{ + return yawMode; +} + +bool commanderGetYawModeCarefreeResetFront(void) +{ + return carefreeResetFront; +} + // Params for flight modes PARAM_GROUP_START(flightmode) PARAM_ADD(PARAM_UINT8, althold, &altHoldMode) +PARAM_ADD(PARAM_UINT8, yawMode, &yawMode) +PARAM_ADD(PARAM_UINT8, yawRst, &carefreeResetFront) PARAM_GROUP_STOP(flightmode) diff --git a/modules/src/stabilizer.c b/modules/src/stabilizer.c index 0852a1fd28..e9c9c6b43d 100644 --- a/modules/src/stabilizer.c +++ b/modules/src/stabilizer.c @@ -137,6 +137,8 @@ RPYType rollType; // Current configuration type of roll (rate or angle) RPYType pitchType; // Current configuration type of pitch (rate or angle) RPYType yawType; // Current configuration type of yaw (rate or angle) +static float carefreeFrontAngle = 0; // carefree front angle that is set + uint16_t actuatorThrust; // Actuator output for thrust base int16_t actuatorRoll; // Actuator output roll compensation int16_t actuatorPitch; // Actuator output pitch compensation @@ -149,7 +151,11 @@ uint32_t motorPowerM4; // Motor 4 power output (16bit value used: 0 - 65535) static bool isInit; + static void stabilizerAltHoldUpdate(void); +static void stabilizerRotateYaw(float yawRad); +static void stabilizerRotateYawCarefree(bool reset); +static void stabilizerYawModeUpdate(void); static void distributePower(const uint16_t thrust, const int16_t roll, const int16_t pitch, const int16_t yaw); static uint16_t limitThrust(int32_t value); @@ -266,6 +272,9 @@ static void stabilizerTask(void* param) // Estimate speed from acc (drifts) vSpeed += deadband(accWZ, vAccDeadband) * FUSION_UPDATE_DT; + // Adjust yaw if configured to do so + stabilizerYawModeUpdate(); + controllerCorrectAttitudePID(eulerRollActual, eulerPitchActual, eulerYawActual, eulerRollDesired, eulerPitchDesired, -eulerYawDesired, &rollRateDesired, &pitchRateDesired, &yawRateDesired); @@ -473,6 +482,86 @@ static void stabilizerAltHoldUpdate(void) altHoldPIDVal = 0.0; } } +/** + * Rotate Yaw so that the Crazyflie will change what is considered front. + * + * @param yawRad Amount of radians to rotate yaw. + */ +static void stabilizerRotateYaw(float yawRad) +{ + float cosy; + float siny; + float originalRoll = eulerRollDesired; + float originalPitch = eulerPitchDesired; + + cosy = cosf(yawRad); + siny = sinf(yawRad); + eulerRollDesired = originalRoll * cosy - originalPitch * siny; + eulerPitchDesired = originalPitch * cosy + originalRoll * siny; +} + +/** + * Yaw carefree mode means yaw will stay in world coordinates. So even though + * the Crazyflie rotates around the yaw, front will stay the same as when it started. + * This makes makes it a bit easier for beginners + */ +static void stabilizerRotateYawCarefree(bool reset) +{ + float yawRad; + float cosy; + float siny; + float originalRoll = eulerRollDesired; + + if (reset) + { + carefreeFrontAngle = eulerYawActual; + } + + yawRad = (eulerYawActual - carefreeFrontAngle) * (float)M_PI / 180; + cosy = cosf(yawRad); + siny = sinf(yawRad); + eulerRollDesired = eulerRollDesired * cosy - eulerPitchDesired * siny; + eulerPitchDesired = eulerPitchDesired * cosy + originalRoll * siny; +} + +/** + * Update Yaw according to current setting + */ +#ifdef PLATFORM_CF1 +static void stabilizerYawModeUpdate(void) +{ + switch (commanderGetYawMode()) + { + case CAREFREE: + stabilizerRotateYawCarefree(commanderGetYawModeCarefreeResetFront()); + break; + case PLUSMODE: + // Default in plus mode. Do nothing + break; + case XMODE: // Fall though + default: + stabilizerRotateYaw(-45 * M_PI / 180); + break; + } +} +#else +static void stabilizerYawModeUpdate(void) +{ + switch (commanderGetYawMode()) + { + case CAREFREE: + stabilizerRotateYawCarefree(commanderGetYawModeCarefreeResetFront()); + break; + case PLUSMODE: + stabilizerRotateYaw(45 * M_PI / 180); + break; + case XMODE: // Fall though + default: + // Default in x-mode. Do nothing + break; + } +} +#endif static void distributePower(const uint16_t thrust, const int16_t roll, const int16_t pitch, const int16_t yaw) From 9ff50aed4d5eaa2307e83f3f106087c9dfc21fc7 Mon Sep 17 00:00:00 2001 From: Tobias Antonsson Date: Wed, 12 Aug 2015 17:02:23 +0200 Subject: [PATCH 10/11] Made console and crtp init earlier so that DEBUG_PRINT can be used earlier (it is buffered though until link is opened). Added console output for config block init. --- config/config.h | 4 ++-- modules/src/comm.c | 6 ++++-- modules/src/crtp.c | 15 +++++++++++---- modules/src/system.c | 21 ++++++++++++--------- utils/src/configblockeeprom.c | 3 +++ utils/src/configblockflash.c | 14 +++++++++++--- 6 files changed, 43 insertions(+), 20 deletions(-) diff --git a/config/config.h b/config/config.h index f5af3c7f2d..c07bf3c138 100644 --- a/config/config.h +++ b/config/config.h @@ -46,7 +46,7 @@ #define PROTOCOL_VERSION 2 #ifdef STM32F4XX - #define P_NAME "Crazyflie 2.0 Rev.C" + #define P_NAME "Crazyflie 2.0" #define QUAD_FORMATION_X #define CONFIG_BLOCK_ADDRESS (2048 * (64-1)) @@ -57,7 +57,7 @@ #define FREERTOS_MCU_CLOCK_HZ 168000000 #else - #define P_NAME "Crazyflie Rev.F" + #define P_NAME "Crazyflie 1.0" #define CONFIG_BLOCK_ADDRESS (1024 * (128-1)) #define MCU_ID_ADDRESS 0x1FFFF7E8 #define MCU_FLASH_SIZE_ADDRESS 0x1FFFF7E0 diff --git a/modules/src/comm.c b/modules/src/comm.c index 0dd26dc3b6..14b1c3220f 100644 --- a/modules/src/comm.c +++ b/modules/src/comm.c @@ -58,7 +58,10 @@ void commInit(void) radiolinkInit(); #endif - crtpInit(); + /* These functions are moved to be initialized early so + * that DEBUG_PRINT can be used early */ + // crtpInit(); + // consoleInit(); #ifdef USE_RADIOLINK_CRTP crtpSetLink(radiolinkGetLink()); @@ -73,7 +76,6 @@ void commInit(void) platformserviceInit(); #endif logInit(); - consoleInit(); paramInit(); //setup CRTP communication channel diff --git a/modules/src/crtp.c b/modules/src/crtp.c index e3cdbd0c50..4565a1c31b 100644 --- a/modules/src/crtp.c +++ b/modules/src/crtp.c @@ -130,11 +130,18 @@ void crtpTxTask(void *param) while (true) { - if (xQueueReceive(txQueue, &p, portMAX_DELAY) == pdTRUE) + if (link != &nopLink) { - // Keep testing, if the link changes to USB it will go though - while (link->sendPacket(&p) == false) - ; + if (xQueueReceive(txQueue, &p, portMAX_DELAY) == pdTRUE) + { + // Keep testing, if the link changes to USB it will go though + while (link->sendPacket(&p) == false) + ; + } + } + else + { + vTaskDelay(M2T(10)); } } } diff --git a/modules/src/system.c b/modules/src/system.c index 795f08c833..e3eac18a06 100644 --- a/modules/src/system.c +++ b/modules/src/system.c @@ -87,6 +87,18 @@ void systemInit(void) canStartMutex = xSemaphoreCreateMutex(); xSemaphoreTake(canStartMutex, portMAX_DELAY); + /* Initialized hear and early so that DEBUG_PRINT (buffered) can be used early */ + crtpInit(); + consoleInit(); + + DEBUG_PRINT("----------------------------\n"); + DEBUG_PRINT(P_NAME " is up and running!\n"); + DEBUG_PRINT("Build %s:%s (%s) %s\n", V_SLOCAL_REVISION, + V_SREVISION, V_STAG, (V_MODIFIED)?"MODIFIED":"CLEAN"); + DEBUG_PRINT("I am 0x%X%X%X and I have %dKB of flash!\n", + *((int*)(MCU_ID_ADDRESS+8)), *((int*)(MCU_ID_ADDRESS+4)), + *((int*)(MCU_ID_ADDRESS+0)), *((short*)(MCU_FLASH_SIZE_ADDRESS))); + configblockInit(); workerInit(); adcInit(); @@ -144,15 +156,6 @@ void systemTask(void *arg) #endif //ndef USE_RADIOLINK_CRTP commInit(); - - DEBUG_PRINT("----------------------------\n"); - DEBUG_PRINT("Crazyflie is up and running!\n"); - DEBUG_PRINT("Build %s:%s (%s) %s\n", V_SLOCAL_REVISION, - V_SREVISION, V_STAG, (V_MODIFIED)?"MODIFIED":"CLEAN"); - DEBUG_PRINT("I am 0x%X%X%X and I have %dKB of flash!\n", - *((int*)(MCU_ID_ADDRESS+8)), *((int*)(MCU_ID_ADDRESS+4)), - *((int*)(MCU_ID_ADDRESS+0)), *((short*)(MCU_FLASH_SIZE_ADDRESS))); - commanderInit(); stabilizerInit(); #ifdef PLATFORM_CF2 diff --git a/utils/src/configblockeeprom.c b/utils/src/configblockeeprom.c index 40eb604f5b..80d482ec4b 100644 --- a/utils/src/configblockeeprom.c +++ b/utils/src/configblockeeprom.c @@ -31,6 +31,7 @@ #include #include "config.h" +#include "debug.h" #include "i2cdev.h" #include "configblock.h" #include "eeprom.h" @@ -136,10 +137,12 @@ int configblockInit(void) if (configblockCheckChecksum(&configblock)) { // Everything is fine + DEBUG_PRINT("v%d, verification [OK]\n", configblock.version); cb_ok = true; } else { + DEBUG_PRINT("Verification [FAIL]\n"); cb_ok = false; } } diff --git a/utils/src/configblockflash.c b/utils/src/configblockflash.c index ffa3e162a2..70d05de404 100644 --- a/utils/src/configblockflash.c +++ b/utils/src/configblockflash.c @@ -23,14 +23,15 @@ * * configblock.c - Simple static implementation of the config block */ +#define DEBUG_MODULE "CFGBLK" #include #include #include -#include "configblock.h" - #include "config.h" +#include "configblock.h" +#include "debug.h" /* Internal format of the config block */ #define MAGIC 0x43427830 @@ -71,9 +72,16 @@ int configblockInit(void) //Verify the config block if (configblock->magic!=MAGIC || configblock->version!= VERSION || calculate_cksum(configblock, sizeof(*configblock)) ) + { + DEBUG_PRINT("Verification [FAIL]\n"); return -1; + } + else + { + DEBUG_PRINT("v%d, verification [OK]\n", configblock->version); + cb_ok = true; + } - cb_ok = true; return 0; } From db20e5254445474315606a10192d27f33d991fb2 Mon Sep 17 00:00:00 2001 From: Tobias Antonsson Date: Wed, 12 Aug 2015 18:24:09 +0200 Subject: [PATCH 11/11] Added parameters to configure rate/angle mode. --- modules/src/commander.c | 14 ++++++++++---- modules/src/stabilizer.c | 7 +++---- 2 files changed, 13 insertions(+), 8 deletions(-) diff --git a/modules/src/commander.c b/modules/src/commander.c index 5f7226687e..0e836196d9 100644 --- a/modules/src/commander.c +++ b/modules/src/commander.c @@ -51,6 +51,10 @@ static bool thrustLocked; static bool altHoldMode = false; static bool altHoldModeOld = false; +static RPYType stabilizationModeRoll = ANGLE; // Current stabilization type of roll (rate or angle) +static RPYType stabilizationModePitch = ANGLE; // Current stabilization type of pitch (rate or angle) +static RPYType stabilizationModeYaw = RATE; // Current stabilization type of yaw (rate or angle) + static YawModeType yawMode = DEFUALT_YAW_MODE; // Yaw mode configuration static bool carefreeResetFront; // Reset what is front in carefree mode @@ -169,9 +173,9 @@ void commanderSetAltHoldMode(bool altHoldModeNew) void commanderGetRPYType(RPYType* rollType, RPYType* pitchType, RPYType* yawType) { - *rollType = ANGLE; - *pitchType = ANGLE; - *yawType = RATE; + *rollType = stabilizationModeRoll; + *pitchType = stabilizationModePitch; + *yawType = stabilizationModeYaw; } void commanderGetThrust(uint16_t* thrust) @@ -218,5 +222,7 @@ PARAM_GROUP_START(flightmode) PARAM_ADD(PARAM_UINT8, althold, &altHoldMode) PARAM_ADD(PARAM_UINT8, yawMode, &yawMode) PARAM_ADD(PARAM_UINT8, yawRst, &carefreeResetFront) +PARAM_ADD(PARAM_UINT8, stabModeRoll, &stabilizationModeRoll) +PARAM_ADD(PARAM_UINT8, stabModePitch, &stabilizationModePitch) +PARAM_ADD(PARAM_UINT8, stabModeYaw, &stabilizationModeYaw) PARAM_GROUP_STOP(flightmode) - diff --git a/modules/src/stabilizer.c b/modules/src/stabilizer.c index e9c9c6b43d..ef35f19ed9 100644 --- a/modules/src/stabilizer.c +++ b/modules/src/stabilizer.c @@ -133,10 +133,6 @@ static float autoTOTargetAdjust = 1.5f; // Meters to add to altHoldTarget to static float autoTOThresh = 0.97f; // Threshold for when to deactivate auto Take-Off. A value of 0.97 means 97% of the target altitude adjustment. #endif -RPYType rollType; // Current configuration type of roll (rate or angle) -RPYType pitchType; // Current configuration type of pitch (rate or angle) -RPYType yawType; // Current configuration type of yaw (rate or angle) - static float carefreeFrontAngle = 0; // carefree front angle that is set uint16_t actuatorThrust; // Actuator output for thrust base @@ -238,6 +234,9 @@ static void stabilizerPreThrustUpdateCallOut(void) static void stabilizerTask(void* param) { + RPYType rollType; + RPYType pitchType; + RPYType yawType; uint32_t attitudeCounter = 0; uint32_t altHoldCounter = 0; uint32_t lastWakeTime;