diff --git a/crazyflie-firmware/README.md b/crazyflie-firmware/README.md
index c6e4a032ea30cd55ee9d62052c9f767107f2c9f4..40219f33fd4da81a2821048a67f42c3ac2fc6a08 100644
--- a/crazyflie-firmware/README.md
+++ b/crazyflie-firmware/README.md
@@ -6,7 +6,7 @@ https://github.com/bitcraze/crazyflie-firmware
However, it is easiest to locate the official realease versions of the firmware from here:<br>
https://github.com/bitcraze/crazyflie-release/releases
-The complete instructions for applying the patch are given separately for each firmware version.
+The complete instructions for applying the patch are given separately for each firmware version (currently only one firmware version).
@@ -38,9 +38,11 @@ To apply the patch, use the following sequence of commands:
`git clone --recursive https://github.com/bitcraze/crazyflie-firmware.git`<br>
`cd crazyflie-firmware`<br>
`git checkout e27cd170b7eb8be7ed1cbbc9f5622b469d335d97`<br>
+Note that it is also possible to checkout this commit using the commit tag, i.e., using `git checkout 2020.02`
+
**Step 2:** Apply the patch:<br>
-`git apply <folder_to_dfall-system_repository>/crazyflie_firmware/crazyflie-firmware-version-2019-09/firmware_modifications_for_version_2019-09.patch`<br>
+`git apply <folder_to_dfall-system_repository>/crazyflie_firmware/crazyflie-firmware-version-2020-20/firmware_modifications_for_version_2020-20.patch`<br>
Now the repository is ready to compile. To do this, first make sure that you have
installed the necessary toolchain as described above ([Install Compilation Toolchain](#install-compilation-toolchain)).
@@ -63,47 +65,4 @@ The pre-compiled binary files are already included in this repository. To update
`<folder_to_dfall-system_repository>/crazyflie_firmware/crazyflie-firmware-version-2020-02/cf2-2020.02_withDfallPatch.bin`<br>
To update the firmware of both the main processor and the nrf processor, select the file:<br>
-`<folder_to_dfall-system_repository>/crazyflie_firmware/crazyflie-firmware-version-2020-02/firmware-cf2-2020.02_withDfallPatch.zip`<br>
-
-
-
-
-
-## Firware Version 2017-06
-
-The hash of the exact commit to which this patch applies is:<br>
-`f4d21213d7ce37d7ed7338fad0f9a94a96cec191`
-As commited on Mon Feb 3 15:04:52 2020 +0100
-
-To apply the patch, use the following sequence of commands:
-
-**Step 1:** Clone the repository and checkout the appropriate commit:<br>
-`git clone --recursive https://github.com/bitcraze/crazyflie-firmware.git`<br>
-`cd crazyflie-firmware`<br>
-`git checkout f4d21213d7ce37d7ed7338fad0f9a94a96cec191`<br>
-
-**Step 2:** Apply the patch:<br>
-`git apply <folder_to_dfall-system_repository>/crazyflie_firmware/crazyflie-firmware-version-2017-06/firmware_modifications_for_version_2017-06.patch`<br>
-
-Now the repository is ready to compile. To do this, first make sure that you have
-installed the necessary toolchain as described above ([Install Compilation Toolchain](#install-compilation-toolchain)).
-
-**Step 3:** In the `crazyflie-firmware` folder where you applied the patch, compile the firmware using the following command:<br>
-`make PLATFORM=CF2`
-
-If everything is successful, you will see something like this:<br>
-
-<img src="success_building_for_version_2017-06.png" style="width: 400px;"/> <br><br>
-
-This should have created the binary file `cf2.bin`.
-
-**Step 4:** upload the binary to the crazyflie processor by following the instructions given here:<br>
-https://www.bitcraze.io/documentation/tutorials/getting-started-with-crazyflie-2-x/#update-fw<br>
-Instructions are also given in the wiki of this repository under:
-`/wiki/setup.md#firmware`
-
-The pre-compiled binary files are already included in this repository. To update the firmware of just the main processor, select the file:
-`<folder_to_dfall-system_repository>/crazyflie_firmware/crazyflie-firmware-version-2017-06/cf2-2017.06_withDfallPatch.bin`<br>
-
-To update the firmware of both the main processor and the nrf processor, select the file:<br>
-`<folder_to_dfall-system_repository>/crazyflie_firmware/crazyflie-firmware-version-2017-06/firmware-cf2-2017.06_withDfallPatch.zip`<br>
\ No newline at end of file
+`<folder_to_dfall-system_repository>/crazyflie_firmware/crazyflie-firmware-version-2020-02/firmware-cf2-2020.02_withDfallPatch.zip`<br>
\ No newline at end of file
diff --git a/crazyflie-firmware/crazyflie-firmware-version-2017-06/cf1-2017.06.bin b/crazyflie-firmware/crazyflie-firmware-version-2017-06/cf1-2017.06.bin
deleted file mode 100644
index 8fe23d6d7142ee791214be735a5bbe51bdea8024..0000000000000000000000000000000000000000
Binary files a/crazyflie-firmware/crazyflie-firmware-version-2017-06/cf1-2017.06.bin and /dev/null differ
diff --git a/crazyflie-firmware/crazyflie-firmware-version-2017-06/cf2-2017.06_withDfallPatch.bin b/crazyflie-firmware/crazyflie-firmware-version-2017-06/cf2-2017.06_withDfallPatch.bin
deleted file mode 100755
index 5f2a63f7b230e616de18d2fe1f5df25f36b6a7b3..0000000000000000000000000000000000000000
Binary files a/crazyflie-firmware/crazyflie-firmware-version-2017-06/cf2-2017.06_withDfallPatch.bin and /dev/null differ
diff --git a/crazyflie-firmware/crazyflie-firmware-version-2017-06/cf2_nrf-2017.05.bin b/crazyflie-firmware/crazyflie-firmware-version-2017-06/cf2_nrf-2017.05.bin
deleted file mode 100644
index e73ab57124abde79c75fe14a754a7ad40f0a7c59..0000000000000000000000000000000000000000
Binary files a/crazyflie-firmware/crazyflie-firmware-version-2017-06/cf2_nrf-2017.05.bin and /dev/null differ
diff --git a/crazyflie-firmware/crazyflie-firmware-version-2017-06/firmware-cf2-2017.06_withDfallPatch.zip b/crazyflie-firmware/crazyflie-firmware-version-2017-06/firmware-cf2-2017.06_withDfallPatch.zip
deleted file mode 100644
index 79ac384102115983868f12721c3e79117775817d..0000000000000000000000000000000000000000
Binary files a/crazyflie-firmware/crazyflie-firmware-version-2017-06/firmware-cf2-2017.06_withDfallPatch.zip and /dev/null differ
diff --git a/crazyflie-firmware/crazyflie-firmware-version-2017-06/firmware_modifications_for_version_2017-06.patch b/crazyflie-firmware/crazyflie-firmware-version-2017-06/firmware_modifications_for_version_2017-06.patch
deleted file mode 100644
index 0718a5654065db8dff377ef1f1a7568a524cc88d..0000000000000000000000000000000000000000
--- a/crazyflie-firmware/crazyflie-firmware-version-2017-06/firmware_modifications_for_version_2017-06.patch
+++ /dev/null
@@ -1,435 +0,0 @@
-diff --git a/src/modules/interface/attitude_controller.h b/src/modules/interface/attitude_controller.h
-index ad23ef0..f79b7d1 100644
---- a/src/modules/interface/attitude_controller.h
-+++ b/src/modules/interface/attitude_controller.h
-@@ -63,6 +63,11 @@ void attitudeControllerResetRollAttitudePID(void);
- */
- void attitudeControllerResetPitchAttitudePID(void);
-
-+/**
-+ * Reset controller yaw attitude PID
-+ */
-+void attitudeControllerResetYawAttitudePID(void);
-+
- /**
- * Reset controller roll, pitch and yaw PID's.
- */
-diff --git a/src/modules/interface/stabilizer_types.h b/src/modules/interface/stabilizer_types.h
-index 418cc0b..30d031b 100644
---- a/src/modules/interface/stabilizer_types.h
-+++ b/src/modules/interface/stabilizer_types.h
-@@ -144,6 +144,10 @@ typedef struct control_s {
- int16_t pitch;
- int16_t yaw;
- float thrust;
-+ uint16_t cmd1;
-+ uint16_t cmd2;
-+ uint16_t cmd3;
-+ uint16_t cmd4;
- } control_t;
-
- typedef enum mode_e {
-@@ -152,12 +156,24 @@ typedef enum mode_e {
- modeVelocity
- } stab_mode_t;
-
-+typedef enum pps_mode_e {
-+ modeDisabled = 0,
-+ modeMotors,
-+ modeRate,
-+ modeAngle
-+} pps_mode_t;
-+
- typedef struct setpoint_s {
- uint32_t timestamp;
-
- attitude_t attitude;
- attitude_t attitudeRate;
- float thrust;
-+ pps_mode_t pps_mode;
-+ uint16_t cmd1;
-+ uint16_t cmd2;
-+ uint16_t cmd3;
-+ uint16_t cmd4;
- point_t position;
- velocity_t velocity;
- bool velocity_body;
-diff --git a/src/modules/src/attitude_pid_controller.c b/src/modules/src/attitude_pid_controller.c
-index 8b5f4aa..f16288f 100644
---- a/src/modules/src/attitude_pid_controller.c
-+++ b/src/modules/src/attitude_pid_controller.c
-@@ -142,9 +142,14 @@ void attitudeControllerResetRollAttitudePID(void)
-
- void attitudeControllerResetPitchAttitudePID(void)
- {
-- pidReset(&pidRoll);
-+ pidReset(&pidPitch);
- }
-
-+void attitudeControllerResetYawAttitudePID(void)
-+{
-+ pidReset(&pidYaw);
-+}
-+
- void attitudeControllerResetAllPID(void)
- {
- pidReset(&pidRoll);
-diff --git a/src/modules/src/controller_pid.c b/src/modules/src/controller_pid.c
-index 0e5e62d..516315a 100644
---- a/src/modules/src/controller_pid.c
-+++ b/src/modules/src/controller_pid.c
-@@ -51,9 +51,13 @@ void stateController(control_t *control, setpoint_t *setpoint,
- }
-
- if (RATE_DO_EXECUTE(POSITION_RATE, tick)) {
-- positionController(&actuatorThrust, &attitudeDesired, setpoint, state);
-+ if(setpoint->pps_mode != modeMotors && setpoint->pps_mode != modeRate && setpoint->pps_mode != modeAngle)
-+ {
-+ positionController(&actuatorThrust, &attitudeDesired, setpoint, state);
-+ }
- }
-
-+
- if (RATE_DO_EXECUTE(ATTITUDE_RATE, tick)) {
- // Switch between manual and automatic position control
- if (setpoint->mode.z == modeDisable) {
-@@ -63,7 +67,7 @@ void stateController(control_t *control, setpoint_t *setpoint,
- attitudeDesired.roll = setpoint->attitude.roll;
- attitudeDesired.pitch = setpoint->attitude.pitch;
- }
--
-+ /* if everything is modeVelocity, skip this */
- attitudeControllerCorrectAttitudePID(state->attitude.roll, state->attitude.pitch, state->attitude.yaw,
- attitudeDesired.roll, attitudeDesired.pitch, attitudeDesired.yaw,
- &rateDesired.roll, &rateDesired.pitch, &rateDesired.yaw);
-@@ -80,6 +84,12 @@ void stateController(control_t *control, setpoint_t *setpoint,
- attitudeControllerResetPitchAttitudePID();
- }
-
-+ //added for PPSSafeController to make the Onboard Controller using the external YawRate. Pure speed controller, no position/500 small steps
-+ if (setpoint->mode.yaw == modeVelocity) {
-+ rateDesired.yaw = (-1) * setpoint->attitudeRate.yaw;
-+ attitudeControllerResetYawAttitudePID();
-+ }
-+
- // TODO: Investigate possibility to subtract gyro drift.
- attitudeControllerCorrectRatePID(sensors->gyro.x, -sensors->gyro.y, sensors->gyro.z,
- rateDesired.roll, rateDesired.pitch, rateDesired.yaw);
-@@ -100,19 +110,76 @@ void stateController(control_t *control, setpoint_t *setpoint,
- control->thrust = actuatorThrust;
- }
-
-- if (control->thrust == 0)
-+ if(setpoint->pps_mode == modeMotors)
- {
-- control->thrust = 0;
-- control->roll = 0;
-- control->pitch = 0;
-- control->yaw = 0;
--
-- attitudeControllerResetAllPID();
-- positionControllerResetAllPID();
--
-- // Reset the calculated YAW angle for rate control
-- attitudeDesired.yaw = state->attitude.yaw;
-+ control->roll = 0;
-+ control->pitch = 0;
-+ control->yaw = 0;
-+ control->cmd1 = setpoint->cmd1;
-+ control->cmd2 = setpoint->cmd2;
-+ control->cmd3 = setpoint->cmd3;
-+ control->cmd4 = setpoint->cmd4;
-+ if ((control->cmd1 == 0 && control->cmd2 == 0 && control->cmd3 == 0 && control->cmd4 == 0))
-+ {
-+ control->cmd1 = 0;
-+ control->cmd2 = 0;
-+ control->cmd3 = 0;
-+ control->cmd4 = 0;
-+ control->roll = 0;
-+ control->pitch = 0;
-+ control->yaw = 0;
-+
-+ attitudeControllerResetAllPID();
-+ positionControllerResetAllPID();
-+
-+ // Reset the calculated YAW angle for rate control
-+ attitudeDesired.yaw = state->attitude.yaw;
-+ }
-+ }
-+ else if(setpoint->pps_mode == modeRate || setpoint->pps_mode == modeAngle)
-+ {
-+ control->cmd1 = setpoint->cmd1;
-+ control->cmd2 = setpoint->cmd2;
-+ control->cmd3 = setpoint->cmd3;
-+ control->cmd4 = setpoint->cmd4;
-+ if ((control->cmd1 == 0 && control->cmd2 == 0 && control->cmd3 == 0 && control->cmd4 == 0))
-+ {
-+ control->cmd1 = 0;
-+ control->cmd2 = 0;
-+ control->cmd3 = 0;
-+ control->cmd4 = 0;
-+ control->roll = 0;
-+ control->pitch = 0;
-+ control->yaw = 0;
-+
-+ attitudeControllerResetAllPID();
-+ positionControllerResetAllPID();
-+
-+ // Reset the calculated YAW angle for rate control
-+ attitudeDesired.yaw = state->attitude.yaw;
-+ }
- }
-+ else /* pps_mode == modeDisabled */
-+ {
-+ control->cmd1 = control->thrust;
-+ control->cmd2 = control->thrust;
-+ control->cmd3 = control->thrust;
-+ control->cmd4 = control->thrust;
-+ if (control->thrust == 0)
-+ {
-+ control->thrust = 0;
-+ control->roll = 0;
-+ control->pitch = 0;
-+ control->yaw = 0;
-+
-+ attitudeControllerResetAllPID();
-+ positionControllerResetAllPID();
-+
-+ // Reset the calculated YAW angle for rate control
-+ attitudeDesired.yaw = state->attitude.yaw;
-+ }
-+ }
-+
- }
-
-
-diff --git a/src/modules/src/crtp_commander_generic.c b/src/modules/src/crtp_commander_generic.c
-index 993dead..4370101 100644
---- a/src/modules/src/crtp_commander_generic.c
-+++ b/src/modules/src/crtp_commander_generic.c
-@@ -34,6 +34,8 @@
- #include "num.h"
- #include "FreeRTOS.h"
-
-+/* #include "motors.h" */
-+
- /* The generic commander format contains a packet type and data that has to be
- * decoded into a setpoint_t structure. The aim is to make it future-proof
- * by easily allowing the addition of new packets for future use cases.
-@@ -67,6 +69,9 @@ enum packet_type {
- cppmEmuType = 3,
- altHoldType = 4,
- hoverType = 5,
-+ PPSMotorsType = 8,
-+ PPSRateType = 9,
-+ PPSAngleType = 10,
- };
-
- /* ---===== 2 - Decoding functions =====--- */
-@@ -292,6 +297,152 @@ static void hoverDecoder(setpoint_t *setpoint, uint8_t type, const void *data, s
- setpoint->velocity_body = true;
- }
-
-+struct PPSMotorsPacket_s {
-+ uint16_t cmd1;
-+ uint16_t cmd2;
-+ uint16_t cmd3;
-+ uint16_t cmd4;
-+} __attribute__((packed));
-+
-+static void PPSMotorsDecoder(setpoint_t *setpoint, uint8_t type, const void *data, size_t datalen)
-+{
-+ const struct PPSMotorsPacket_s *values = data;
-+ ASSERT(datalen == sizeof(struct PPSMotorsPacket_s));
-+
-+ /* we dont use position controller onboard */
-+ setpoint->mode.x = modeDisable;
-+ setpoint->mode.y = modeDisable;
-+ setpoint->mode.z = modeDisable;
-+
-+ setpoint->thrust = 0;
-+ setpoint->position.x = 0;
-+ setpoint->position.y = 0;
-+ setpoint->position.z = 0;
-+ setpoint->velocity.x = 0;
-+ setpoint->velocity.y = 0;
-+ setpoint->velocity.z = 0;
-+
-+ /* we dont use rate controller onboard either */
-+ setpoint->mode.roll = modeDisable;
-+ setpoint->mode.pitch = modeDisable;
-+ setpoint->mode.yaw = modeDisable;
-+
-+ setpoint->attitude.roll = 0;
-+ setpoint->attitude.pitch = 0;
-+ setpoint->attitude.yaw = 0;
-+ setpoint->attitudeRate.roll = 0;
-+ setpoint->attitudeRate.pitch = 0;
-+ setpoint->attitudeRate.yaw = 0;
-+
-+ /* update cmds */
-+
-+ setpoint->cmd1 = limitUint16(values->cmd1);
-+ setpoint->cmd2 = limitUint16(values->cmd2);
-+ setpoint->cmd3 = limitUint16(values->cmd3);
-+ setpoint->cmd4 = limitUint16(values->cmd4);
-+
-+ setpoint->pps_mode = modeMotors;
-+}
-+
-+struct PPSRatePacket_s {
-+ uint16_t cmd1;
-+ uint16_t cmd2;
-+ uint16_t cmd3;
-+ uint16_t cmd4;
-+ float roll_rate;
-+ float pitch_rate;
-+ float yaw_rate;
-+} __attribute__((packed));
-+
-+static void PPSRateDecoder(setpoint_t *setpoint, uint8_t type, const void *data, size_t datalen)
-+{
-+ const struct PPSRatePacket_s *values = data;
-+ ASSERT(datalen == sizeof(struct PPSRatePacket_s));
-+
-+ /* we dont use position controller onboard */
-+ setpoint->mode.x = modeDisable;
-+ setpoint->mode.y = modeDisable;
-+ setpoint->mode.z = modeDisable;
-+
-+ setpoint->thrust = 0;
-+ setpoint->position.x = 0;
-+ setpoint->position.y = 0;
-+ setpoint->position.z = 0;
-+ setpoint->velocity.x = 0;
-+ setpoint->velocity.y = 0;
-+ setpoint->velocity.z = 0;
-+
-+ /* we use rate controller onboard either */
-+ setpoint->mode.roll = modeVelocity;
-+ setpoint->mode.pitch = modeVelocity;
-+ setpoint->mode.yaw = modeVelocity;
-+
-+ setpoint->attitude.roll = 0;
-+ setpoint->attitude.pitch = 0;
-+ setpoint->attitude.yaw = 0;
-+ setpoint->attitudeRate.roll = values->roll_rate;
-+ setpoint->attitudeRate.pitch = values->pitch_rate;
-+ setpoint->attitudeRate.yaw = values->yaw_rate;
-+
-+ /* update cmds */
-+
-+ setpoint->cmd1 = limitUint16(values->cmd1);
-+ setpoint->cmd2 = limitUint16(values->cmd2);
-+ setpoint->cmd3 = limitUint16(values->cmd3);
-+ setpoint->cmd4 = limitUint16(values->cmd4);
-+
-+ setpoint->pps_mode = modeRate;
-+}
-+
-+struct PPSAnglePacket_s {
-+ uint16_t cmd1;
-+ uint16_t cmd2;
-+ uint16_t cmd3;
-+ uint16_t cmd4;
-+ float roll;
-+ float pitch;
-+ float yaw;
-+} __attribute__((packed));
-+
-+static void PPSAngleDecoder(setpoint_t *setpoint, uint8_t type, const void *data, size_t datalen)
-+{
-+ const struct PPSAnglePacket_s *values = data;
-+ ASSERT(datalen == sizeof(struct PPSRatePacket_s));
-+
-+ /* we dont use position controller onboard */
-+ setpoint->mode.x = modeDisable;
-+ setpoint->mode.y = modeDisable;
-+ setpoint->mode.z = modeDisable;
-+
-+ setpoint->thrust = 0;
-+ setpoint->position.x = 0;
-+ setpoint->position.y = 0;
-+ setpoint->position.z = 0;
-+ setpoint->velocity.x = 0;
-+ setpoint->velocity.y = 0;
-+ setpoint->velocity.z = 0;
-+
-+ /* we use position angle controller onboard */
-+ setpoint->mode.roll = modeAbs;
-+ setpoint->mode.pitch = modeAbs;
-+ setpoint->mode.yaw = modeAbs;
-+
-+ setpoint->attitude.roll = values->roll;
-+ setpoint->attitude.pitch = values->pitch;
-+ setpoint->attitude.yaw = values->yaw;
-+ setpoint->attitudeRate.roll = 0;
-+ setpoint->attitudeRate.pitch = 0;
-+ setpoint->attitudeRate.yaw = 0;
-+
-+ /* update cmds */
-+ setpoint->cmd1 = limitUint16(values->cmd1);
-+ setpoint->cmd2 = limitUint16(values->cmd2);
-+ setpoint->cmd3 = limitUint16(values->cmd3);
-+ setpoint->cmd4 = limitUint16(values->cmd4);
-+
-+ setpoint->pps_mode = modeAngle;
-+}
-+
- /* ---===== 3 - packetDecoders array =====--- */
- const static packetDecoder_t packetDecoders[] = {
- [stopType] = stopDecoder,
-@@ -300,6 +451,9 @@ const static packetDecoder_t packetDecoders[] = {
- [cppmEmuType] = cppmEmuDecoder,
- [altHoldType] = altHoldDecoder,
- [hoverType] = hoverDecoder,
-+ [PPSMotorsType] = PPSMotorsDecoder,
-+ [PPSRateType] = PPSRateDecoder,
-+ [PPSAngleType] = PPSAngleDecoder,
- };
-
- /* Decoder switch */
-diff --git a/src/modules/src/power_distribution_stock.c b/src/modules/src/power_distribution_stock.c
-index 0e66c0b..79d5f2c 100644
---- a/src/modules/src/power_distribution_stock.c
-+++ b/src/modules/src/power_distribution_stock.c
-@@ -76,18 +76,19 @@ void powerDistribution(const control_t *control)
- #ifdef QUAD_FORMATION_X
- int16_t r = control->roll / 2.0f;
- int16_t p = control->pitch / 2.0f;
-- motorPower.m1 = limitThrust(control->thrust - r + p + control->yaw);
-- motorPower.m2 = limitThrust(control->thrust - r - p - control->yaw);
-- motorPower.m3 = limitThrust(control->thrust + r - p + control->yaw);
-- motorPower.m4 = limitThrust(control->thrust + r + p - control->yaw);
-+ motorPower.m1 = limitThrust(control->cmd1 - r + p + control->yaw);
-+ motorPower.m2 = limitThrust(control->cmd2 - r - p - control->yaw);
-+ motorPower.m3 = limitThrust(control->cmd3 + r - p + control->yaw);
-+ motorPower.m4 = limitThrust(control->cmd4 + r + p - control->yaw);
-+
- #else // QUAD_FORMATION_NORMAL
-- motorPower.m1 = limitThrust(control->thrust + control->pitch +
-+ motorPower.m1 = limitThrust(control->cmd1 + control->pitch +
- control->yaw);
-- motorPower.m2 = limitThrust(control->thrust - control->roll -
-+ motorPower.m2 = limitThrust(control->cmd2 - control->roll -
- control->yaw);
-- motorPower.m3 = limitThrust(control->thrust - control->pitch +
-+ motorPower.m3 = limitThrust(control->cmd3 - control->pitch +
- control->yaw);
-- motorPower.m4 = limitThrust(control->thrust + control->roll -
-+ motorPower.m4 = limitThrust(control->cmd4 + control->roll -
- control->yaw);
- #endif
-
-diff --git a/src/modules/src/stabilizer.c b/src/modules/src/stabilizer.c
-index eccc709..6b9fcc3 100644
---- a/src/modules/src/stabilizer.c
-+++ b/src/modules/src/stabilizer.c
-@@ -130,7 +130,6 @@ static void stabilizerTask(void* param)
- commanderGetSetpoint(&setpoint, &state);
-
- sitAwUpdateSetpoint(&setpoint, &sensorData, &state);
--
- stateController(&control, &setpoint, &sensorData, &state, tick);
-
- checkEmergencyStopTimeout();
diff --git a/crazyflie-firmware/crazyflie-firmware-version-2017-06/manifest.json b/crazyflie-firmware/crazyflie-firmware-version-2017-06/manifest.json
deleted file mode 100644
index 58f97a9ac6d1174015a31ee746aab63d97dba97a..0000000000000000000000000000000000000000
--- a/crazyflie-firmware/crazyflie-firmware-version-2017-06/manifest.json
+++ /dev/null
@@ -1,28 +0,0 @@
-{
- "version": 1,
- "subversion": 1,
- "release": "2017.06",
- "files": {
- "cf2-2017.06_withDfallPatch.bin": {
- "platform": "cf2",
- "target": "stm32",
- "type": "fw",
- "release": "2017.06",
- "repository": "crazyflie-firmware"
- },
- "cf2_nrf-2017.05.bin": {
- "platform": "cf2",
- "target": "nrf51",
- "type": "fw",
- "release": "2017.05",
- "repository": "crazyflie2-nrf-firmware"
- },
- "cf1-2017.06.bin": {
- "platform": "cf1",
- "target": "stm32",
- "type": "fw",
- "release": "2017.06",
- "repository": "crazyflie-firmware"
- }
- }
-}
diff --git a/crazyflie-firmware/crazyflie-firmware-version-2020-02/cf2-2020.02_withDfallPatch.bin b/crazyflie-firmware/crazyflie-firmware-version-2020-02/cf2-2020.02_withDfallPatch.bin
index 01fe563354fc0d4ecf38feabf6440f162f703ee3..04fddf6eca9a4b1b9cac7f9c1162e74ae2a84f55 100755
Binary files a/crazyflie-firmware/crazyflie-firmware-version-2020-02/cf2-2020.02_withDfallPatch.bin and b/crazyflie-firmware/crazyflie-firmware-version-2020-02/cf2-2020.02_withDfallPatch.bin differ
diff --git a/crazyflie-firmware/crazyflie-firmware-version-2020-02/firmware-cf2-2020.02_withDfallPatch.zip b/crazyflie-firmware/crazyflie-firmware-version-2020-02/firmware-cf2-2020.02_withDfallPatch.zip
index 5b372f7f3eef8a006ae39c8c994eedd03ade5e98..fea06a8cc08ee07cdeeba670ba7d17ae49667a89 100644
Binary files a/crazyflie-firmware/crazyflie-firmware-version-2020-02/firmware-cf2-2020.02_withDfallPatch.zip and b/crazyflie-firmware/crazyflie-firmware-version-2020-02/firmware-cf2-2020.02_withDfallPatch.zip differ
diff --git a/crazyflie-firmware/crazyflie-firmware-version-2020-02/firmware_modifications_for_version_2020-02.patch b/crazyflie-firmware/crazyflie-firmware-version-2020-02/firmware_modifications_for_version_2020-02.patch
index 776fa60d4faf6946623680e6b87091a96c573656..6b2d1151426995f11f155d2bb8904bc54fe14748 100644
--- a/crazyflie-firmware/crazyflie-firmware-version-2020-02/firmware_modifications_for_version_2020-02.patch
+++ b/crazyflie-firmware/crazyflie-firmware-version-2020-02/firmware_modifications_for_version_2020-02.patch
@@ -15,7 +15,7 @@ index fe25dc0..fd69bdc 100644
* Reset controller roll, pitch and yaw PID's.
*/
diff --git a/src/modules/interface/stabilizer_types.h b/src/modules/interface/stabilizer_types.h
-index fc3e378..272cc88 100644
+index fc3e378..c51fdbd 100644
--- a/src/modules/interface/stabilizer_types.h
+++ b/src/modules/interface/stabilizer_types.h
@@ -159,6 +159,10 @@ typedef struct control_s {
@@ -29,25 +29,10 @@ index fc3e378..272cc88 100644
} control_t;
typedef enum mode_e {
-@@ -167,6 +171,13 @@ typedef enum mode_e {
- modeVelocity
- } stab_mode_t;
-
-+typedef enum dfall_mode_e {
-+ modeDfallDisable = 0,
-+ modeDfallMotors,
-+ modeDfallRate,
-+ modeDfallAngle,
-+} dfall_mode_t;
-+
- typedef struct setpoint_s {
- uint32_t timestamp;
-
-@@ -174,6 +185,11 @@ typedef struct setpoint_s {
+@@ -174,11 +178,17 @@ typedef struct setpoint_s {
attitude_t attitudeRate; // deg/s
quaternion_t attitudeQuaternion;
float thrust;
-+ dfall_mode_t dfall_mode;
+ uint16_t cmd1;
+ uint16_t cmd2;
+ uint16_t cmd3;
@@ -55,6 +40,13 @@ index fc3e378..272cc88 100644
point_t position; // m
velocity_t velocity; // m/s
acc_t acceleration; // m/s^2
+ bool velocity_body; // true if velocity is given in body frame; false if velocity is given in world frame
+
++ bool useDfallAdjustmentsToControllerPID;
++
+ struct {
+ stab_mode_t x;
+ stab_mode_t y;
diff --git a/src/modules/src/attitude_pid_controller.c b/src/modules/src/attitude_pid_controller.c
index b7d707e..bcb02bc 100644
--- a/src/modules/src/attitude_pid_controller.c
@@ -72,25 +64,10 @@ index b7d707e..bcb02bc 100644
{
pidReset(&pidRoll);
diff --git a/src/modules/src/controller_pid.c b/src/modules/src/controller_pid.c
-index f4ecb75..4a9047c 100644
+index f4ecb75..673b5cc 100644
--- a/src/modules/src/controller_pid.c
+++ b/src/modules/src/controller_pid.c
-@@ -74,7 +74,13 @@ void controllerPid(control_t *control, setpoint_t *setpoint,
- }
-
- if (RATE_DO_EXECUTE(POSITION_RATE, tick)) {
-- positionController(&actuatorThrust, &attitudeDesired, setpoint, state);
-+ // Check if the "dfall_mode" needs the position controller to run
-+ bool posControllIsNeeded_for_dfall_mode = (setpoint->dfall_mode != modeDfallMotors && setpoint->dfall_mode != modeDfallRate && setpoint->dfall_mode != modeDfallAngle);
-+ // Hence run the position controller only if needed
-+ if (posControllIsNeeded_for_dfall_mode)
-+ {
-+ positionController(&actuatorThrust, &attitudeDesired, setpoint, state);
-+ }
- }
-
- if (RATE_DO_EXECUTE(ATTITUDE_RATE, tick)) {
-@@ -132,6 +138,9 @@ void controllerPid(control_t *control, setpoint_t *setpoint,
+@@ -132,6 +132,9 @@ void controllerPid(control_t *control, setpoint_t *setpoint,
control->thrust = actuatorThrust;
}
@@ -100,57 +77,57 @@ index f4ecb75..4a9047c 100644
if (control->thrust == 0)
{
control->thrust = 0;
-@@ -150,6 +159,89 @@ void controllerPid(control_t *control, setpoint_t *setpoint,
+@@ -150,6 +153,92 @@ void controllerPid(control_t *control, setpoint_t *setpoint,
// Reset the calculated YAW angle for rate control
attitudeDesired.yaw = state->attitude.yaw;
}
+ */
+
-+ // This set of "if:then:else" statements is used to specify
-+ // the "control->cmd{1,2,3,4}" depending on the "setpoint->dfall_mode"
-+ // And in all cases perform the check if either:
-+ // >> "control->thrust==0", or
-+ // >> "control->cmd{1,2,3,4}==0"
-+ // where the check performed is appropriate to the case.
-+ // The result of the check is used to set all commands to zero and
-+ // to reset all PIDs, if necessary.
-+
-+ // Default the check to false
-+ bool isZeroThrust = false;
-+
-+ if (setpoint->dfall_mode == modeDfallMotors)
++ // The following "if:then:else" statements is used to
++ // specify the "control->cmd{1,2,3,4}" depending on
++ // the boolean:
++ // >> "setpoint->useDfallAdjustmentsToControllerPID"
++ if (setpoint->useDfallAdjustmentsToControllerPID)
+ {
+ // Set the {roll,pitch,yaw} commands to zero
-+ control->roll = 0;
-+ control->pitch = 0;
-+ control->yaw = 0;
-+ // Copy across the per motor commands
-+ control->cmd1 = setpoint->cmd1;
-+ control->cmd2 = setpoint->cmd2;
-+ control->cmd3 = setpoint->cmd3;
-+ control->cmd4 = setpoint->cmd4;
-+ // Perform check for all thrust being zero
-+ if ((control->cmd1 == 0 && control->cmd2 == 0 && control->cmd3 == 0 && control->cmd4 == 0))
++ // if indicated by the respective mode
++ if (setpoint->mode.roll == modeDisable)
+ {
-+ // Set the flag to true that thrusts are zero
-+ isZeroThrust = true;
++ control->roll = 0;
++ cmd_roll = 0.0;
+ }
-+ }
-+ else if(setpoint->dfall_mode == modeDfallRate || setpoint->dfall_mode == modeDfallAngle)
-+ {
-+ // Copy across the per motor commands
-+ control->cmd1 = setpoint->cmd1;
-+ control->cmd2 = setpoint->cmd2;
-+ control->cmd3 = setpoint->cmd3;
-+ control->cmd4 = setpoint->cmd4;
-+ // Perform check for all thrust being zero
-+ if ((control->cmd1 == 0 && control->cmd2 == 0 && control->cmd3 == 0 && control->cmd4 == 0))
++ if (setpoint->mode.pitch == modeDisable)
++ {
++ control->pitch = 0;
++ cmd_pitch = 0.0;
++ }
++ if (setpoint->mode.yaw == modeDisable)
++ {
++ control->yaw = 0;
++ cmd_yaw = 0.0;
++ }
++ // Apply the per-motor commands differently
++ // depending on the Z-controller mode
++ if (setpoint->mode.z == modeDisable)
+ {
-+ // Set the flag to true that thrusts are zero
-+ isZeroThrust = true;
++ // Copy across the per motor commands
++ control->cmd1 = setpoint->cmd1;
++ control->cmd2 = setpoint->cmd2;
++ control->cmd3 = setpoint->cmd3;
++ control->cmd4 = setpoint->cmd4;
++ }
++ else
++ {
++ // Add the setpoints on top of the
++ // "control thrust" computed by the
++ // PID controllers above
++ control->cmd1 = setpoint->cmd1 + control->thrust;
++ control->cmd2 = setpoint->cmd2 + control->thrust;
++ control->cmd3 = setpoint->cmd3 + control->thrust;
++ control->cmd4 = setpoint->cmd4 + control->thrust;
+ }
+ }
-+ // Otherwise, "setpoint->dfall_mode == modeDfallDisable"
++ // Otherwise, "setpoint->useDfallAdjustmentsToControllerPID == false"
+ else
+ {
+ // Put the thrust value onto each motor
@@ -158,16 +135,13 @@ index f4ecb75..4a9047c 100644
+ control->cmd2 = control->thrust;
+ control->cmd3 = control->thrust;
+ control->cmd4 = control->thrust;
-+ // If thrust is zero, then:
-+ if (control->thrust == 0)
-+ {
-+ // Set the flag to true that thrusts are zero
-+ isZeroThrust = true;
-+ }
+ }
+
-+ // If the flag idicates that thrusts are zero,
-+ // then set all commands to zero and reset the PIDs
++ // If all control commands are zero, i.e., if:
++ // >> "control->cmd{1,2,3,4}==0"
++ // then set all commands to zero and to reset
++ // all PIDs.
++ bool isZeroThrust = ((control->cmd1 == 0 && control->cmd2 == 0 && control->cmd3 == 0 && control->cmd4 == 0));
+ if (isZeroThrust)
+ {
+ // Set all commands to zero
@@ -186,217 +160,412 @@ index f4ecb75..4a9047c 100644
+
+ // Reset the calculated YAW angle for rate control
+ attitudeDesired.yaw = state->attitude.yaw;
++
++ // Update also the log variables to reflect this
++ cmd_thrust = 0.0;
++ cmd_roll = 0.0;
++ cmd_pitch = 0.0;
++ cmd_yaw = 0.0;
+ }
}
diff --git a/src/modules/src/crtp_commander_generic.c b/src/modules/src/crtp_commander_generic.c
-index 1662fe4..84d0631 100644
+index 1662fe4..f6931a4 100644
--- a/src/modules/src/crtp_commander_generic.c
+++ b/src/modules/src/crtp_commander_generic.c
-@@ -71,6 +71,9 @@ enum packet_type {
+@@ -71,6 +71,7 @@ enum packet_type {
hoverType = 5,
fullStateType = 6,
positionType = 7,
-+ dfallMotorsType = 8,
-+ dfallRateType = 9,
-+ dfallAngleType = 10,
++ dfallType = 8,
};
/* ---===== 2 - Decoding functions =====--- */
-@@ -367,6 +370,180 @@ static void positionDecoder(setpoint_t *setpoint, uint8_t type, const void *data
- setpoint->attitude.yaw = values->yaw;
+@@ -112,6 +113,9 @@ static void velocityDecoder(setpoint_t *setpoint, uint8_t type, const void *data
+ setpoint->mode.yaw = modeVelocity;
+
+ setpoint->attitudeRate.yaw = -values->yawrate;
++
++ // Turn off the "dfall mode adjustments"
++ setpoint->useDfallAdjustmentsToControllerPID = false;
}
-+/* dfall motors mode decoder
-+ * disable all controllers, the motor commands provided are
-+ * set directly to the motors
-+ */
-+struct dfallMotorsPacket_s {
-+ uint16_t cmd1;
-+ uint16_t cmd2;
-+ uint16_t cmd3;
-+ uint16_t cmd4;
-+} __attribute__((packed));
+ /* zDistanceDecoder
+@@ -145,6 +149,9 @@ static void zDistanceDecoder(setpoint_t *setpoint, uint8_t type, const void *dat
+
+ setpoint->attitude.roll = values->roll;
+ setpoint->attitude.pitch = values->pitch;
+
-+static void dfallMotorsDecoder(setpoint_t *setpoint, uint8_t type, const void *data, size_t datalen)
-+{
-+ const struct dfallMotorsPacket_s *values = data;
-+ ASSERT(datalen == sizeof(struct dfallMotorsPacket_s));
++ // Turn off the "dfall mode adjustments"
++ setpoint->useDfallAdjustmentsToControllerPID = false;
+ }
+
+ /* cppmEmuDecoder
+@@ -230,6 +237,9 @@ static void cppmEmuDecoder(setpoint_t *setpoint, uint8_t type, const void *data,
+ {
+ setpoint->thrust = 0;
+ }
+
-+ // Disable the onboard position controllers
-+ setpoint->mode.x = modeDisable;
-+ setpoint->mode.y = modeDisable;
-+ setpoint->mode.z = modeDisable;
++ // Turn off the "dfall mode adjustments"
++ setpoint->useDfallAdjustmentsToControllerPID = false;
+ }
+
+ /* altHoldDecoder
+@@ -263,6 +273,9 @@ static void altHoldDecoder(setpoint_t *setpoint, uint8_t type, const void *data,
+
+ setpoint->attitude.roll = values->roll;
+ setpoint->attitude.pitch = values->pitch;
+
-+ // And set their setpoint values to zero
-+ setpoint->thrust = 0;
-+ setpoint->position.x = 0;
-+ setpoint->position.y = 0;
-+ setpoint->position.z = 0;
-+ setpoint->velocity.x = 0;
-+ setpoint->velocity.y = 0;
-+ setpoint->velocity.z = 0;
-+
-+ // Disable the onboard orientation controllers
-+ setpoint->mode.roll = modeDisable;
-+ setpoint->mode.pitch = modeDisable;
-+ setpoint->mode.yaw = modeDisable;
-+
-+ // And set their setpoint values to zero
-+ setpoint->attitude.roll = 0;
-+ setpoint->attitude.pitch = 0;
-+ setpoint->attitude.yaw = 0;
-+ setpoint->attitudeRate.roll = 0;
-+ setpoint->attitudeRate.pitch = 0;
-+ setpoint->attitudeRate.yaw = 0;
++ // Turn off the "dfall mode adjustments"
++ setpoint->useDfallAdjustmentsToControllerPID = false;
+ }
+
+ /* hoverDecoder
+@@ -294,6 +307,9 @@ static void hoverDecoder(setpoint_t *setpoint, uint8_t type, const void *data, s
+ setpoint->velocity.y = values->vy;
+
+ setpoint->velocity_body = true;
+
-+ // Tranfer the per-motor commands from the packet
-+ // to the respective setpoints
-+ setpoint->cmd1 = limitUint16(values->cmd1);
-+ setpoint->cmd2 = limitUint16(values->cmd2);
-+ setpoint->cmd3 = limitUint16(values->cmd3);
-+ setpoint->cmd4 = limitUint16(values->cmd4);
++ // Turn off the "dfall mode adjustments"
++ setpoint->useDfallAdjustmentsToControllerPID = false;
+ }
+
+ struct fullStatePacket_s {
+@@ -338,6 +354,9 @@ static void fullStateDecoder(setpoint_t *setpoint, uint8_t type, const void *dat
+ setpoint->mode.roll = modeDisable;
+ setpoint->mode.pitch = modeDisable;
+ setpoint->mode.yaw = modeDisable;
++
++ // Turn off the "dfall mode adjustments"
++ setpoint->useDfallAdjustmentsToControllerPID = false;
+ }
+
+ /* positionDecoder
+@@ -365,6 +384,313 @@ static void positionDecoder(setpoint_t *setpoint, uint8_t type, const void *data
+ setpoint->mode.yaw = modeAbs;
+
+ setpoint->attitude.yaw = values->yaw;
+
-+ // Set the "dfall_mode" accordingly
-+ setpoint->dfall_mode = modeDfallMotors;
++ // Turn off the "dfall mode adjustments"
++ setpoint->useDfallAdjustmentsToControllerPID = false;
+}
+
-+/* dfall rate mode decoder
-+ * disable the position controllers and use the attitude controllers
-+ * in "velocity mode", i.e., use the onboard "rate controllers"
++/* dfall mode decoder
++ * set the mode of the onboard {x,y,z,roll,pitch,yaw}
++ * controllers based on the mode specification
++ * contained in the packet. A separate mode is
++ * specified for each of the four independent
++ * coordinates of {x,y,z,yaw}
+ */
-+struct dfallRatePacket_s {
-+ uint16_t cmd1;
-+ uint16_t cmd2;
-+ uint16_t cmd3;
-+ uint16_t cmd4;
-+ float roll_rate;
-+ float pitch_rate;
-+ float yaw_rate;
-+} __attribute__((packed));
-+
-+static void dfallRateDecoder(setpoint_t *setpoint, uint8_t type, const void *data, size_t datalen)
-+{
-+ const struct dfallRatePacket_s *values = data;
-+ ASSERT(datalen == sizeof(struct dfallRatePacket_s));
-+
-+ // Disable the onboard position controllers
-+ setpoint->mode.x = modeDisable;
-+ setpoint->mode.y = modeDisable;
-+ setpoint->mode.z = modeDisable;
+
-+ // And set their setpoint values to zero
-+ setpoint->thrust = 0;
-+ setpoint->position.x = 0;
-+ setpoint->position.y = 0;
-+ setpoint->position.z = 0;
-+ setpoint->velocity.x = 0;
-+ setpoint->velocity.y = 0;
-+ setpoint->velocity.z = 0;
-+
-+ // Enable the onboard orientation controllers in
-+ // velocity mode, i.e., as rate controllers
-+ setpoint->mode.roll = modeVelocity;
-+ setpoint->mode.pitch = modeVelocity;
-+ setpoint->mode.yaw = modeVelocity;
-+
-+ // Set the angle setpoints to zero
-+ setpoint->attitude.roll = 0;
-+ setpoint->attitude.pitch = 0;
-+ setpoint->attitude.yaw = 0;
-+ // Transfer the angular rate commands from the packet
-+ // to the respective setpoints
-+ setpoint->attitudeRate.roll = values->roll_rate;
-+ setpoint->attitudeRate.pitch = values->pitch_rate;
-+ setpoint->attitudeRate.yaw = values->yaw_rate;
++#define DFALL_CONTROLLER_MODE_OFF 0
++#define DFALL_CONTROLLER_MODE_ANGULAR_RATE 1
++#define DFALL_CONTROLLER_MODE_ANGLE 2
++#define DFALL_CONTROLLER_MODE_VELOCITY 3
++#define DFALL_CONTROLLER_MODE_POSITION 4
+
-+ // Tranfer the per-motor commands from the packet
-+ // to the respective setpoints
-+ setpoint->cmd1 = limitUint16(values->cmd1);
-+ setpoint->cmd2 = limitUint16(values->cmd2);
-+ setpoint->cmd3 = limitUint16(values->cmd3);
-+ setpoint->cmd4 = limitUint16(values->cmd4);
-+
-+ // Set the "dfall_mode" accordingly
-+ setpoint->dfall_mode = modeDfallRate;
-+}
-+
-+/* dfall angle mode decoder
-+ * disable the position controllers and use the attitude controllers
-+ * in "absolute mode", i.e., use the onboard "angle controllers"
-+ */
-+struct dfallAnglePacket_s {
++struct dfallPacket_s {
++ uint16_t controllerModes;
+ uint16_t cmd1;
+ uint16_t cmd2;
+ uint16_t cmd3;
+ uint16_t cmd4;
-+ float roll;
-+ float pitch;
-+ float yaw;
++ float xControllerSetpoint;
++ float yControllerSetpoint;
++ float zControllerSetpoint;
++ float yawControllerSetpoint;
+} __attribute__((packed));
+
-+static void dfallAngleDecoder(setpoint_t *setpoint, uint8_t type, const void *data, size_t datalen)
++static void dfallDecoder(setpoint_t *setpoint, uint8_t type, const void *data, size_t datalen)
+{
-+ const struct dfallAnglePacket_s *values = data;
-+ ASSERT(datalen == sizeof(struct dfallRatePacket_s));
++ const struct dfallPacket_s *values = data;
++ ASSERT(datalen == sizeof(struct dfallPacket_s));
++
++ // DECODE THE 16-BIT INTEGER THAT ENCODES
++ // THE REQUESTED {x,y,z,yaw} CONTROLLER MODES
++ // > Each mode is encoded using 3 bits, stacked together as follows:
++ // ---------------------------------------------------------------
++ // |BIT | 15 14 13 12 | 11 10 9 | 8 7 6 | 5 4 3 | 2 1 0 |
++ // |DESC. | free bits | yaw_mode | z_mode | y_mode | x_mode |
++ // ---------------------------------------------------------------
++
++ uint16_t mode_selector_bits = 7;
++
++ // EXTRACT THE MODE FOR EACH CONTROLLER
++ uint8_t x_controller_mode = (values->controllerModes >> 0) & mode_selector_bits;
++ uint8_t y_controller_mode = (values->controllerModes >> 3) & mode_selector_bits;
++ uint8_t z_controller_mode = (values->controllerModes >> 6) & mode_selector_bits;
++ uint8_t yaw_controller_mode = (values->controllerModes >> 9) & mode_selector_bits;
++
++ // INITIALISE A BOOLEAN FOR TRACKING IF ANY
++ // OF THE CONTROLLER MODES ARE INVALID
++ bool invalid_controller_mode_requested = false;
++
++ // PROCESS THE X CONTROLLER
++ switch (x_controller_mode)
++ {
++ case DFALL_CONTROLLER_MODE_ANGULAR_RATE:
++ // Set the modes
++ setpoint->mode.x = modeDisable;
++ setpoint->mode.pitch = modeVelocity;
++ // Set the setpoint
++ setpoint->position.x = 0.0;
++ setpoint->velocity.x = 0.0;
++ setpoint->attitude.pitch = 0.0;
++ setpoint->attitudeRate.pitch = values->xControllerSetpoint;
++ break;
++
++ case DFALL_CONTROLLER_MODE_ANGLE:
++ // Set the modes
++ setpoint->mode.x = modeDisable;
++ setpoint->mode.pitch = modeAbs;
++ // Set the setpoints
++ setpoint->position.x = 0.0;
++ setpoint->velocity.x = 0.0;
++ setpoint->attitude.pitch = values->xControllerSetpoint;
++ setpoint->attitudeRate.pitch = 0.0;
++ break;
++
++ case DFALL_CONTROLLER_MODE_VELOCITY:
++ // Set the modes
++ setpoint->mode.x = modeVelocity;
++ setpoint->mode.pitch = modeAbs;
++ // Set the setpoints
++ setpoint->position.x = 0.0;
++ setpoint->velocity.x = values->xControllerSetpoint;
++ setpoint->attitude.pitch = 0.0;
++ setpoint->attitudeRate.pitch = 0.0;
++ break;
++
++ case DFALL_CONTROLLER_MODE_POSITION:
++ // Set the modes
++ setpoint->mode.x = modeAbs;
++ setpoint->mode.pitch = modeAbs;
++ // Set the setpoints
++ setpoint->position.x = values->xControllerSetpoint;
++ setpoint->velocity.x = 0.0;
++ setpoint->attitude.pitch = 0.0;
++ setpoint->attitudeRate.pitch = 0.0;
++ break;
++
++ case DFALL_CONTROLLER_MODE_OFF:
++ // Set the modes
++ setpoint->mode.x = modeDisable;
++ setpoint->mode.pitch = modeDisable;
++ // Set the setpoints
++ setpoint->position.x = 0.0;
++ setpoint->velocity.x = 0.0;
++ setpoint->attitude.pitch = 0.0;
++ setpoint->attitudeRate.pitch = 0.0;
++ break;
++
++ default:
++ // Set the flag that an invalid mode
++ // was requested
++ invalid_controller_mode_requested = true;
++ break;
++ }
++
++ // PROCESS THE Y CONTROLLER
++ switch (y_controller_mode)
++ {
++ case DFALL_CONTROLLER_MODE_ANGULAR_RATE:
++ // Set the modes
++ setpoint->mode.y = modeDisable;
++ setpoint->mode.roll = modeVelocity;
++ // Set the setpoint
++ setpoint->position.y = 0.0;
++ setpoint->velocity.y = 0.0;
++ setpoint->attitude.roll = 0.0;
++ setpoint->attitudeRate.roll = values->yControllerSetpoint;
++ break;
++
++ case DFALL_CONTROLLER_MODE_ANGLE:
++ // Set the modes
++ setpoint->mode.y = modeDisable;
++ setpoint->mode.roll = modeAbs;
++ // Set the setpoints
++ setpoint->position.y = 0.0;
++ setpoint->velocity.y = 0.0;
++ setpoint->attitude.roll = values->yControllerSetpoint;
++ setpoint->attitudeRate.roll = 0.0;
++ break;
++
++ case DFALL_CONTROLLER_MODE_VELOCITY:
++ // Set the modes
++ setpoint->mode.y = modeVelocity;
++ setpoint->mode.roll = modeAbs;
++ // Set the setpoints
++ setpoint->position.y = 0.0;
++ setpoint->velocity.y = values->yControllerSetpoint;
++ setpoint->attitude.roll = 0.0;
++ setpoint->attitudeRate.roll = 0.0;
++ break;
++
++ case DFALL_CONTROLLER_MODE_POSITION:
++ // Set the modes
++ setpoint->mode.y = modeAbs;
++ setpoint->mode.roll = modeAbs;
++ // Set the setpoints
++ setpoint->position.y = values->yControllerSetpoint;
++ setpoint->velocity.y = 0.0;
++ setpoint->attitude.roll = 0.0;
++ setpoint->attitudeRate.roll = 0.0;
++ break;
++
++ case DFALL_CONTROLLER_MODE_OFF:
++ // Set the modes
++ setpoint->mode.y = modeDisable;
++ setpoint->mode.roll = modeDisable;
++ // Set the setpoints
++ setpoint->position.y = 0.0;
++ setpoint->velocity.y = 0.0;
++ setpoint->attitude.roll = 0.0;
++ setpoint->attitudeRate.roll = 0.0;
++ break;
++
++ default:
++ // Set the flag that an invalid mode
++ // was requested
++ invalid_controller_mode_requested = true;
++ break;
++ }
++
++ // PROCESS THE Z CONTROLLER
++ switch (z_controller_mode)
++ {
++ case DFALL_CONTROLLER_MODE_VELOCITY:
++ // Set the mode
++ setpoint->mode.z = modeVelocity;
++ // Set the setpoints
++ setpoint->position.z = 0.0;
++ setpoint->velocity.z = values->zControllerSetpoint;
++ break;
++
++ case DFALL_CONTROLLER_MODE_POSITION:
++ // Set the mode
++ setpoint->mode.z = modeAbs;
++ // Set the setpoints
++ setpoint->position.z = values->zControllerSetpoint;
++ setpoint->velocity.z = 0.0;
++ break;
++
++ case DFALL_CONTROLLER_MODE_OFF:
++ // Set the mode
++ setpoint->mode.z = modeDisable;
++ // Set the setpoints
++ setpoint->position.z = 0.0;
++ setpoint->velocity.z = 0.0;
++ break;
++
++ case DFALL_CONTROLLER_MODE_ANGULAR_RATE:
++ case DFALL_CONTROLLER_MODE_ANGLE:
++ default:
++ // Set the flag that an invalid mode
++ // was requested
++ invalid_controller_mode_requested = true;
++ break;
++ }
+
-+ // Disable the onboard position controllers
-+ setpoint->mode.x = modeDisable;
-+ setpoint->mode.y = modeDisable;
-+ setpoint->mode.z = modeDisable;
++ // PROCESS THE YAW CONTROLLER
++ switch (yaw_controller_mode)
++ {
++ case DFALL_CONTROLLER_MODE_ANGULAR_RATE:
++ // Set the mode
++ setpoint->mode.yaw = modeVelocity;
++ // Set the setpoint
++ setpoint->attitude.yaw = 0.0;
++ setpoint->attitudeRate.yaw = values->yawControllerSetpoint;
++ break;
++
++ case DFALL_CONTROLLER_MODE_ANGLE:
++ // Set the modes
++ setpoint->mode.yaw = modeAbs;
++ // Set the setpoints
++ setpoint->attitude.roll = values->yawControllerSetpoint;
++ setpoint->attitudeRate.roll = 0.0;
++ break;
++
++ case DFALL_CONTROLLER_MODE_OFF:
++ // Set the modes
++ setpoint->mode.yaw = modeDisable;
++ // Set the setpoints
++ setpoint->attitude.yaw = 0.0;
++ setpoint->attitudeRate.yaw = 0.0;
++ break;
++
++ case DFALL_CONTROLLER_MODE_VELOCITY:
++ case DFALL_CONTROLLER_MODE_POSITION:
++ default:
++ // Set the flag that an invalid mode
++ // was requested
++ invalid_controller_mode_requested = true;
++ break;
++ }
+
-+ // And set their setpoint values to zero
++ // Set the thrust setpoint to zero
+ setpoint->thrust = 0;
-+ setpoint->position.x = 0;
-+ setpoint->position.y = 0;
-+ setpoint->position.z = 0;
-+ setpoint->velocity.x = 0;
-+ setpoint->velocity.y = 0;
-+ setpoint->velocity.z = 0;
-+
-+ // Enable the onboard orientation controllers in
-+ // absolute mode, i.e., as angle controllers
-+ setpoint->mode.roll = modeAbs;
-+ setpoint->mode.pitch = modeAbs;
-+ setpoint->mode.yaw = modeAbs;
-+
-+ // Transfer the angle commands from the packet
-+ // to the respective setpoints
-+ setpoint->attitude.roll = values->roll;
-+ setpoint->attitude.pitch = values->pitch;
-+ setpoint->attitude.yaw = values->yaw;
-+ // Set the angular rate setpoints to zero
-+ setpoint->attitudeRate.roll = 0;
-+ setpoint->attitudeRate.pitch = 0;
-+ setpoint->attitudeRate.yaw = 0;
+
+ // Tranfer the per-motor commands from the packet
+ // to the respective setpoints
++ // NOTES:
++ // (1) this is performed regardless of the
++ // "z_controller_mode" because the per motor
++ // commands can be used for correcting steady-
++ // state offsets in {x,y,yaw}.
++ // (2) if the "z_controller_mode" is in "POSITION"
++ // or "VELOCITY" mode, then onboard controllers
++ // will compute the main thurst controler, and
++ // the user is expected to fill in cmd{1,2,3,4}
++ // with only adjustment values.
+ setpoint->cmd1 = limitUint16(values->cmd1);
+ setpoint->cmd2 = limitUint16(values->cmd2);
+ setpoint->cmd3 = limitUint16(values->cmd3);
+ setpoint->cmd4 = limitUint16(values->cmd4);
+
-+ // Set the "dfall_mode" accordingly
-+ setpoint->dfall_mode = modeDfallAngle;
-+}
++ // Set everything to off and zero if an invlaid
++ // controller mode was requested
++ if (invalid_controller_mode_requested)
++ {
++ // Set the modes
++ setpoint->mode.x = modeDisable;
++ setpoint->mode.pitch = modeDisable;
++ setpoint->mode.y = modeDisable;
++ setpoint->mode.roll = modeDisable;
++ setpoint->mode.z = modeDisable;
++ setpoint->mode.yaw = modeDisable;
++
++ // Set the setpoints
++ setpoint->position.x = 0.0;
++ setpoint->velocity.x = 0.0;
++ setpoint->attitude.pitch = 0.0;
++ setpoint->attitudeRate.pitch = 0.0;
++ setpoint->position.y = 0.0;
++ setpoint->velocity.y = 0.0;
++ setpoint->attitude.roll = 0.0;
++ setpoint->attitudeRate.roll = 0.0;
++ setpoint->position.z = 0.0;
++ setpoint->velocity.z = 0.0;
++ setpoint->attitude.yaw = 0.0;
++ setpoint->attitudeRate.yaw = 0.0;
++ }
+
++ // Turn on the "dfall mode adjustments"
++ setpoint->useDfallAdjustmentsToControllerPID = true;
+ }
+
/* ---===== 3 - packetDecoders array =====--- */
- const static packetDecoder_t packetDecoders[] = {
- [stopType] = stopDecoder,
-@@ -377,6 +554,9 @@ const static packetDecoder_t packetDecoders[] = {
+@@ -377,6 +703,7 @@ const static packetDecoder_t packetDecoders[] = {
[hoverType] = hoverDecoder,
[fullStateType] = fullStateDecoder,
[positionType] = positionDecoder,
-+ [dfallMotorsType] = dfallMotorsDecoder,
-+ [dfallRateType] = dfallRateDecoder,
-+ [dfallAngleType] = dfallAngleDecoder,
++ [dfallType] = dfallDecoder,
};
/* Decoder switch */
diff --git a/src/modules/src/power_distribution_stock.c b/src/modules/src/power_distribution_stock.c
-index d3b7d8d..4e7f311 100644
+index d3b7d8d..072a699 100644
--- a/src/modules/src/power_distribution_stock.c
+++ b/src/modules/src/power_distribution_stock.c
@@ -80,18 +80,18 @@ void powerDistribution(const control_t *control)
@@ -435,13 +604,10 @@ index d3b7d8d..4e7f311 100644
LOG_GROUP_START(motor)
-LOG_ADD(LOG_INT32, m4, &motorPower.m4)
--LOG_ADD(LOG_INT32, m1, &motorPower.m1)
--LOG_ADD(LOG_INT32, m2, &motorPower.m2)
--LOG_ADD(LOG_INT32, m3, &motorPower.m3)
-+LOG_ADD(LOG_UINT32, m1, &motorPower.m1)
-+LOG_ADD(LOG_UINT32, m2, &motorPower.m2)
-+LOG_ADD(LOG_UINT32, m3, &motorPower.m3)
-+LOG_ADD(LOG_UINT32, m4, &motorPower.m4)
+ LOG_ADD(LOG_INT32, m1, &motorPower.m1)
+ LOG_ADD(LOG_INT32, m2, &motorPower.m2)
+ LOG_ADD(LOG_INT32, m3, &motorPower.m3)
++LOG_ADD(LOG_INT32, m4, &motorPower.m4)
LOG_GROUP_STOP(motor)
diff --git a/src/modules/src/stabilizer.c b/src/modules/src/stabilizer.c
index 372ec94..cb4f848 100644
diff --git a/crazyflie-firmware/success_building_for_version_2017-06.png b/crazyflie-firmware/success_building_for_version_2017-06.png
deleted file mode 100644
index f48fca5634c490355d66642170d49b5fe537af3f..0000000000000000000000000000000000000000
Binary files a/crazyflie-firmware/success_building_for_version_2017-06.png and /dev/null differ
diff --git a/crazyflie-firmware/success_building_for_version_2020-02.png b/crazyflie-firmware/success_building_for_version_2020-02.png
index 854299c68d6d3d49277d711d0e8608732f943949..62c3e54314a0fd24a7fbe4597521d22146530d38 100644
Binary files a/crazyflie-firmware/success_building_for_version_2020-02.png and b/crazyflie-firmware/success_building_for_version_2020-02.png differ
diff --git a/dfall_ws/src/dfall_pkg/crazyradio/CrazyRadio.py b/dfall_ws/src/dfall_pkg/crazyradio/CrazyRadio.py
index 1aae76782598ebb0753b7fc5d887bf786889796c..6828e3df1b4800ddb4988082e4f6066bea572baf 100755
--- a/dfall_ws/src/dfall_pkg/crazyradio/CrazyRadio.py
+++ b/dfall_ws/src/dfall_pkg/crazyradio/CrazyRadio.py
@@ -91,6 +91,16 @@ CF_ONBOARD_CONTROLLER_MODE_ANGLE 2
CF_ONBOARD_CONTROLLER_MODE_VELOCITY 3
CF_ONBOARD_CONTROLLER_MODE_POSITION 4
+# CF_PACKET_DECODER_STOP_TYPE 0
+# CF_PACKET_DECODER_VELOCITY_WORLD_TYPE 1
+# CF_PACKET_DECODER_Z_DISTANCE_TYEP 2
+# CF_PACKET_DECODER_CPPM_EMU_TYPE 3
+# CF_PACKET_DECODER_ALT_HOLD_TYPE 4
+# CF_PACKET_DECODER_HOVER_TYPE 5
+# CF_PACKET_DECODER_FULL_STATE_TYPE 6
+# CF_PACKET_DECODER_POSITION_TYPE 7
+CF_PACKET_DECODER_DFALL_TYPE 8
+
RAD_TO_DEG = 57.296
# CrazyRadio states:
@@ -381,7 +391,7 @@ class CrazyRadioClient:
def _send_to_commander(self, cmd1, cmd2, cmd3, cmd4, x_mode , x_value, y_mode, y_value, z_mode, z_value, yaw_mode, yaw_value ):
- # CONSTRUCT THE CONDENSE 16-BIT INTEGER THAT ENCODES
+ # CONSTRUCT THE CONDENSED 16-BIT INTEGER THAT ENCODES
# THE REQUESTED {x,y,z,yaw} MODES
# > Each mode is encoded using 3 bits, stacked together as follows:
# ---------------------------------------------------------------
@@ -403,16 +413,56 @@ class CrazyRadioClient:
# |O or 1 | 1 0 0 | 0 1 0 | 0 0 1 | 1 1 1 |
# |Value | mode=4 | mode=2 | mode=1 | mode=7 |
# --------------------------------------------------
+ #
+ # > To extract a mode, an example code snippet is:
+ # >>> yaw_mode = (modes >> 9) & 7
+ # > The bitwise & operator with the number 7 serves to
+ # select only the right-hand most 3 bits of (modes >> 9),
+ # the last 3 bits of the variable modes after is has been
+ # shifted to the right by 9 bits.
+ # > This syntax works in both python and plain-c
+
# CONSTRUCT AND SEND THE COMMANDER PACKET
# > See hints at end of this file for an explanation
# of the struct packing format, i.e., of '<HHHHHffff'
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER_GENERIC
- pk.data = struct.pack('<HHHHHffff', modes_as_uint16, cmd1, cmd2, cmd3, cmd4, x_value, y_value, z_value, yaw_value)
+ pk.data = struct.pack('<BHHHHHffff', CF_PACKET_DECODER_DFALL_TYPE, modes_as_uint16, cmd1, cmd2, cmd3, cmd4, x_value, y_value, z_value, yaw_value)
self._cf.send_packet(pk)
+ # NOTES ON THE PACKING OF THE "pk.data" STRUCT
+ # > The "B" is the decoder type packed into 1 btye
+ # > The first "H" is the condense 2-byte integer
+ # of four onboard controller modes
+ # > The next four "H" are the 2-byte per motor commands
+ # > The four "f" are the setpoints for each onboard controller
+ # > Hence the total size of the data is:
+ # sizs('HHHHHffff') = 5*2bytes + 4*4bytes = 26bytes
+ # > This leaves 3 spare bytes before reaching the maximum
+ # of 29 bytes as specified in the comment below
+
+ # DESCRIPTION OF THE CRAZYFLIE PACKETS
+ # > This is taken directly from the file
+ # "crtp_commander_generic.c" in the
+ # Crazyflie firmware
+ #
+ # The generic commander format contains a packet type and data that has to be
+ # decoded into a setpoint_t structure. The aim is to make it future-proof
+ # by easily allowing the addition of new packets for future use cases.
+ #
+ # The packet format is:
+ # +------+==========================+
+ # | TYPE | DATA |
+ # +------+==========================+
+ #
+ # The type is defined bellow together with a decoder function that should take
+ # the data buffer in and fill up a setpoint_t structure.
+ # The maximum data size is 29 bytes.
+
+
+
def crazyRadioCommandCallback(self, msg):