diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/interface/motors.h b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/interface/motors.h
index a62d121bd522c9fa9280c34f01b7367efa18843a..c65de17c50536e5ad64076b7580dcf178173846d 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/interface/motors.h
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/interface/motors.h
@@ -116,6 +116,18 @@
#define MOTORS_BL_POLARITY TIM_OCPolarity_Low
#endif
+// ------------- uCart 23 Drone ---------------------------
+#define BLMC_PERIOD_uCart23 0.0025 // 2.5ms = 400Hz
+#define MOTORS_HIGH_PERIOD_ZERO_uCart23 0.001 // 1ms for zero throttle
+
+#define MOTORS_BL_PWM_PRESCALE_RAW_uCart23 (uint32_t)((TIM_CLOCK_HZ/0xFFFF) * BLMC_PERIOD_uCart23 + 1) // +1 is to not end up above 0xFFFF in the end
+#define MOTORS_BL_PWM_CNT_FOR_PERIOD_uCart23 (uint32_t)(TIM_CLOCK_HZ * BLMC_PERIOD_uCart23 / MOTORS_BL_PWM_PRESCALE_RAW_uCart23)
+#define MOTORS_BL_PWM_CNT_FOR_HIGH_uCart23 (uint32_t)(TIM_CLOCK_HZ * MOTORS_HIGH_PERIOD_ZERO_uCart23 / MOTORS_BL_PWM_PRESCALE_RAW_uCart23)
+#define MOTORS_BL_PWM_PERIOD_uCart23 MOTORS_BL_PWM_CNT_FOR_PERIOD_uCart23
+#define MOTORS_BL_PWM_PRESCALE_uCart23 (uint16_t)(MOTORS_BL_PWM_PRESCALE_RAW_uCart23 - 1)
+#define MOTORS_BL_POLARITY_uCart23 TIM_OCPolarity_Low
+// ---------------------------------------------------------
+
#define NBR_OF_MOTORS 4
// Motors IDs define
#define MOTOR_M1 0
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/motors.c b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/motors.c
index 24bccfb7349e8a532374994104dc5438ace968cd..47ba32d1d793456bd58735327f7fb8f23cd2037a 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/motors.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/motors.c
@@ -46,12 +46,14 @@ static uint16_t motorsBLConvBitsTo16(uint16_t bits);
static uint16_t motorsBLConv16ToBits(uint16_t bits);
static uint16_t motorsConvBitsTo16(uint16_t bits);
static uint16_t motorsConv16ToBits(uint16_t bits);
+static uint16_t motorsConv16ToBits_uCart23(uint16_t bits);
uint32_t motor_ratios[] = {0, 0, 0, 0};
void motorsPlayTone(uint16_t frequency, uint16_t duration_msec);
void motorsPlayMelody(uint16_t *notes);
void motorsBeep(int id, bool enable, uint16_t frequency, uint16_t ratio);
+void motorsConfigureESC();
#include "motors_def_cf2.c"
@@ -67,26 +69,31 @@ static bool isInit = false;
static uint16_t motorsBLConvBitsTo16(uint16_t bits)
{
- return (0xFFFF * (bits - MOTORS_BL_PWM_CNT_FOR_HIGH) / MOTORS_BL_PWM_CNT_FOR_HIGH);
+ return (0xFFFF * (bits - MOTORS_BL_PWM_CNT_FOR_HIGH) / MOTORS_BL_PWM_CNT_FOR_HIGH);
}
static uint16_t motorsBLConv16ToBits(uint16_t bits)
{
- return (MOTORS_BL_PWM_CNT_FOR_HIGH + ((bits * MOTORS_BL_PWM_CNT_FOR_HIGH) / 0xFFFF));
+ return (MOTORS_BL_PWM_CNT_FOR_HIGH + ((bits * MOTORS_BL_PWM_CNT_FOR_HIGH) / 0xFFFF));
}
static uint16_t motorsConvBitsTo16(uint16_t bits)
{
- return ((bits) << (16 - MOTORS_PWM_BITS));
+ return ((bits) << (16 - MOTORS_PWM_BITS));
}
static uint16_t motorsConv16ToBits(uint16_t bits)
{
- return ((bits) >> (16 - MOTORS_PWM_BITS) & ((1 << MOTORS_PWM_BITS) - 1));
+ return ((bits) >> (16 - MOTORS_PWM_BITS) & ((1 << MOTORS_PWM_BITS) - 1));
+}
+
+static uint16_t motorsConv16ToBits_uCart23(uint16_t bits)
+{
+ return (MOTORS_BL_PWM_CNT_FOR_HIGH_uCart23 + ((bits * MOTORS_BL_PWM_CNT_FOR_HIGH_uCart23) / 0xFFFF));
}
// We have data that maps PWM to thrust at different supply voltage levels.
-// However, it is not the PWM that drives the motors but the voltage and
+// However, it is not the PWM that drives the moStors but the voltage and
// amps (= power). With the PWM it is possible to simulate different
// voltage levels. The assumption is that the voltage used will be an
// procentage of the supply voltage, we assume that 50% PWM will result in
@@ -257,10 +264,12 @@ bool motorsTest(void)
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));
+ vTaskDelay(M2T(MOTORS_TEST_DELAY_TIME_MS));*/
+ motorsConfigureESC();
#else
motorsSetRatio(MOTORS[i], MOTORS_TEST_RATIO);
vTaskDelay(M2T(MOTORS_TEST_ON_TIME_MS));
@@ -346,6 +355,45 @@ void motorsBeep(int id, bool enable, uint16_t frequency, uint16_t ratio)
}
+void motorsConfigureESC() {
+
+ // Set maximum throttle, 100% duty cycle
+ motorMap[0]->setCompare(motorMap[0]->tim, 0);
+ motorMap[1]->setCompare(motorMap[1]->tim, 0);
+ motorMap[2]->setCompare(motorMap[2]->tim, 0);
+ motorMap[3]->setCompare(motorMap[3]->tim, 0);
+ vTaskDelay(M2T(20)); // wait 2 ms
+
+
+
+
+ motorMap[0]->setCompare(motorMap[0]->tim, motorsConv16ToBits_uCart23(65535));
+ vTaskDelay(M2T(2)); // wait 2 ms
+ motorMap[0]->setCompare(motorMap[0]->tim, motorsConv16ToBits_uCart23(0));
+ vTaskDelay(M2T(20)); // wait 2 msS
+
+ motorMap[1]->setCompare(motorMap[1]->tim, motorsConv16ToBits_uCart23(65535));
+ vTaskDelay(M2T(2)); // wait 2 ms
+ motorMap[1]->setCompare(motorMap[1]->tim, motorsConv16ToBits_uCart23(0));
+ vTaskDelay(M2T(20)); // wait 2 ms
+
+ motorMap[2]->setCompare(motorMap[2]->tim, motorsConv16ToBits_uCart23(65535));
+ vTaskDelay(M2T(2)); // wait 2 ms
+ motorMap[2]->setCompare(motorMap[2]->tim, motorsConv16ToBits_uCart23(0));
+ vTaskDelay(M2T(20)); // wait 2 ms
+
+ motorMap[3]->setCompare(motorMap[3]->tim, motorsConv16ToBits_uCart23(65535));
+ vTaskDelay(M2T(2)); // wait 2 ms
+ motorMap[3]->setCompare(motorMap[3]->tim, motorsConv16ToBits_uCart23(0));
+ vTaskDelay(M2T(20)); // wait 2 ms
+ // set min throttle: duty cycle 1 ms for 2ms
+
+
+
+
+}
+
+
// Play a tone with a given frequency and a specific duration in milliseconds (ms)
void motorsPlayTone(uint16_t frequency, uint16_t duration_msec)
{
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/modules/src/controller_student.c b/crazyflie_software/crazyflie-firmware-2021.06/src/modules/src/controller_student.c
index 634bfcc0b532d32b2c8bb939f4341219d15af4d1..86b26c5620616e343dd186cc44dfed62a96f828f 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/modules/src/controller_student.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/modules/src/controller_student.c
@@ -118,7 +118,7 @@ void controllerStudent(control_t *control, setpoint_t *setpoint, const sensorDat
}
//set desired thrust
- thrustDesired = setpoint->thrust;
+ thrustDesired = setpoint->thrust/4;
// Run the attitude controller with the measured attitude and desired attitude
@@ -144,6 +144,7 @@ void controllerStudent(control_t *control, setpoint_t *setpoint, const sensorDat
if(tuning_mode == true){
if (setpoint->mode.yaw == modeVelocity) {
rateDesired.yaw = setpoint->attitudeRate.yaw;
+ attitudeDesired.roll = 3;
studentAttitudeControllerResetYawAttitudePID();
}
}
@@ -151,9 +152,9 @@ void controllerStudent(control_t *control, setpoint_t *setpoint, const sensorDat
//update the attitude rate PID, given the current angular rate
//read by the gyro and the desired rate
studentAttitudeControllerCorrectRatePID(sensors->gyro.x, -sensors->gyro.y, sensors->gyro.z,
- rateDesired.roll, rateDesired.pitch, rateDesired.yaw,
+ rateDesired.roll, rateDesired.pitch, setpoint->attitudeRate.yaw,
&(control->roll), &(control->pitch), &(control->yaw));
-
+ rateDesired.yaw = setpoint->attitudeRate.yaw;
//invert yaw control
control->yaw = -control->yaw;
}
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/modules/src/student_attitude_controller.c b/crazyflie_software/crazyflie-firmware-2021.06/src/modules/src/student_attitude_controller.c
index 0f4c6b3001069adf89226bae540f611fbb6904ad..6285f129e58e8cbb22c58f2346f1ff50f7420d7d 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/modules/src/student_attitude_controller.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/modules/src/student_attitude_controller.c
@@ -134,13 +134,14 @@ void studentAttitudeControllerCorrectRatePID(
)
{
studentPidSetDesired(&pidRollRate, rollRateDesired);
- *rollCommand = saturateSignedInt16(studentPidUpdate(&pidRollRate, rollRateMeasured, true));
+
+ *rollCommand = saturateSignedInt16(studentPidUpdate(&pidRollRate, rollRateMeasured, true)/4);
studentPidSetDesired(&pidPitchRate, pitchRateDesired);
- *pitchCommand = saturateSignedInt16(studentPidUpdate(&pidPitchRate, pitchRateMeasured, true));
+ *pitchCommand = saturateSignedInt16(studentPidUpdate(&pidPitchRate, pitchRateMeasured, true)/4);
studentPidSetDesired(&pidYawRate, yawRateDesired);
- *yawCommand = saturateSignedInt16(studentPidUpdate(&pidYawRate, yawRateMeasured, true));
+ *yawCommand = saturateSignedInt16(studentPidUpdate(&pidYawRate, yawRateMeasured, true)/4);
}
void studentAttitudeControllerResetRollAttitudePID(void)
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/tools/make/config.mk.example b/crazyflie_software/crazyflie-firmware-2021.06/tools/make/config.mk.example
deleted file mode 100644
index 9f4b7c452ec60800306cfcb52011980564c45144..0000000000000000000000000000000000000000
--- a/crazyflie_software/crazyflie-firmware-2021.06/tools/make/config.mk.example
+++ /dev/null
@@ -1,113 +0,0 @@
-## Copy this file to config.mk and modify to get you personal build configuration
-
-## Weight of the Crazyflie, including decks. The default setting is a Crazyflie 2.X without decks.
-# CFLAGS += -DCF_MASS=0.027f // in kg
-
-## Force device type string
-# CFLAGS += -DDEVICE_TYPE_STRING_FORCE="CF20"
-
-## Force a sensor implementation to be used
-# SENSORS=bosch
-
-## Set CRTP link to E-SKY receiver
-# CFLAGS += -DUSE_ESKYLINK
-
-## Redirect the console output to the UART
-# CFLAGS += -DDEBUG_PRINT_ON_UART
-
-## Redirect the console output to JLINK (using SEGGER RTT)
-# DEBUG_PRINT_ON_SEGGER_RTT = 1
-
-## Load a deck driver that has no OW memory
-# CFLAGS += -DDECK_FORCE=bcBuzzer
-
-## Load multiple deck drivers that has no OW memory
-# CFLAGS += -DDECK_FORCE=bcBuzzer:bcLedRing
-
-## 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
-
-## Disable LEDs from turning on/flashing
-# CFLAGS += -DTURN_OFF_LEDS
-
-## Set the default LED Ring effect (if not set, effect 6 will be used)
-# CFLAGS += -DLEDRING_DEFAULT_EFFECT=0
-
-## Set LED Rings to use less LEDs
-# CFLAGS += -DLED_RING_NBR_LEDS=6
-
-## Set LED Rings to use less more LEDs (only if board is modified)
-# CFLAGS += -DLED_RING_NBR_LEDS=24
-
-## Do not send CRTP messages when parameter values are updated
-# CFLAGS += -DSILENT_PARAM_UPDATES
-
-## 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
-
-## Set other pin for reset on the LOCO deck. Only works when LOCODECK_USE_ALT_PINS is set.
-# For instance useful with Loco + Lighhouse + Flow decks where IO_3 collides with the Flow deck
-# CFLAGS += -DLOCODECK_ALT_PIN_RESET=DECK_GPIO_IO4
-
-## Disable Low Interference Mode when using Loco Deck
-# CFLAGS += -DLOCODECK_NO_LOW_INTERFERENCE
-
-## Low interference communication
-# Set the 'low interference' 2.4GHz TX power. This power is set when the loco deck is initialized
-# Possible power are: +4, 0, -4, -8, -12, -16, -20, -30
-# CFLAGS += -DPLATFORM_NRF51_LOW_INTERFERENCE_TX_POWER_DBM="(-12)"
-
-## Set alternative pins for uSD-deck. Patch soldering required (CS->RX2(PA3), SCLK->TX1(PC10), MISO->RX1(PC11), MOSI->IO_4(PC12))
-# CFLAGS += -DUSDDECK_USE_ALT_PINS_AND_SPI
-
-## Use J-Link as Debugger/flasher
-# OPENOCD_INTERFACE ?= interface/jlink.cfg
-# OPENOCD_TARGET ?= target/stm32f4x.cfg
-# OPENOCD_CMDS ?= -c "transport select swd"
-
-## LPS settings ----------------------------------------------------
-## Set operation mode of the LPS system (auto is default)
-## TWR
-# CFLAGS += -DLPS_TWR_ENABLE=1
-## TDoA2
-# LPS_TDOA_ENABLE=1
-## TDoA3
-# LPS_TDOA3_ENABLE=1
-
-## TDoA 3 - experimental
-# Enable 2D positioning. The value (1.2) is the height that the tag will move at. Only use in TDoA 3
-# CFLAGS += -DLPS_2D_POSITION_HEIGHT=1.2
-
-## Enable longer range (lower bit rate). Only use in TDoA 3
-# Note: Anchors must also be built with this flag
-# CFLAGS += -DLPS_LONGER_RANGE
-
-## Full LPS TX power.
-# CFLAGS += -DLPS_FULL_TX_POWER
-
-## SDCard test configuration ------------------------------------
-# FATFS_DISKIO_TESTS = 1 # Set to 1 to enable FatFS diskio function tests. Erases card.
-
-## Brushless handling
-# Start disarmed, needs to be armed before being able to fly
-# CFLAGS += -DSTART_DISARMED
-# IDLE motor drive when armed, 0 = 0%, 65535 = 100% (the motors runs as long as the Crazyflie is armed)
-# CFLAGS += -DDEFAULT_IDLE_THRUST=5000
-
-## Lighthouse handling
-# If lighthouse will need to act as a ground truth (so not entered in the kalman filter)
-# CFLAGS += -DLIGHTHOUSE_AS_GROUNDTRUTH