firmware_modifications_for_version_2020-02.patch

diff --git a/src/modules/interface/attitude_controller.h b/src/modules/interface/attitude_controller.h
index fe25dc0..fd69bdc 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 fc3e378..272cc88 100644
--- a/src/modules/interface/stabilizer_types.h
+++ b/src/modules/interface/stabilizer_types.h
@@ -159,6 +159,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 {
@@ -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 {
   attitude_t attitudeRate;  // deg/s
   quaternion_t attitudeQuaternion;
   float thrust;
+  dfall_mode_t dfall_mode;
+  uint16_t cmd1;
+  uint16_t cmd2;
+  uint16_t cmd3;
+  uint16_t cmd4;
   point_t position;         // m
   velocity_t velocity;      // m/s
   acc_t acceleration;       // m/s^2
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
+++ b/src/modules/src/attitude_pid_controller.c
@@ -145,6 +145,11 @@ void attitudeControllerResetPitchAttitudePID(void)
     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 f4ecb75..4a9047c 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,
     control->thrust = actuatorThrust;
   }
 
+  // The following "if (control->thrust == 0)" block is commented
+  // out because it is replaced with what follows after.
+  /*
   if (control->thrust == 0)
   {
     control->thrust = 0;
@@ -150,6 +159,89 @@ 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)
+  {
+    // 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))
+    {
+      // Set the flag to true that thrusts are zero
+      isZeroThrust = true;
+    }
+  }
+  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))
+    {
+      // Set the flag to true that thrusts are zero
+      isZeroThrust = true;
+    }
+  }
+  // Otherwise, "setpoint->dfall_mode == modeDfallDisable"
+  else
+  {
+    // Put the thrust value onto each motor
+    control->cmd1 = control->thrust;
+    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 (isZeroThrust)
+  {
+    // Set all commands to zero
+    control->cmd1 = 0;
+    control->cmd2 = 0;
+    control->cmd3 = 0;
+    control->cmd4 = 0;
+    control->thrust = 0;
+    control->roll = 0;
+    control->pitch = 0;
+    control->yaw = 0;
+
+    // Reset all PIDs
+    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 1662fe4..84d0631 100644
--- a/src/modules/src/crtp_commander_generic.c
+++ b/src/modules/src/crtp_commander_generic.c
@@ -71,6 +71,9 @@ enum packet_type {
   hoverType         = 5,
   fullStateType     = 6,
   positionType      = 7,
+  dfallMotorsType   = 8,
+  dfallRateType     = 9,
+  dfallAngleType    = 10,
 };
 
 /* ---===== 2 - Decoding functions =====--- */
@@ -367,6 +370,180 @@ static void positionDecoder(setpoint_t *setpoint, uint8_t type, const void *data
   setpoint->attitude.yaw = values->yaw;
 }
 
+/* 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));
+
+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));
+
+  // 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;
+
+  // 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;
+
+  // 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 = modeDfallMotors;
+}
+
+/* dfall rate mode decoder
+ * disable the position controllers and use the attitude controllers
+ * in "velocity mode", i.e., use the onboard "rate controllers"
+ */
+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;
+
+  // 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 {
+    uint16_t cmd1;
+    uint16_t cmd2;
+    uint16_t cmd3;
+    uint16_t cmd4;
+    float roll;
+    float pitch;
+    float yaw;
+} __attribute__((packed));
+
+static void dfallAngleDecoder(setpoint_t *setpoint, uint8_t type, const void *data, size_t datalen)
+{
+  const struct dfallAnglePacket_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
+  // 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
+  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;
+}
+
  /* ---===== 3 - packetDecoders array =====--- */
 const static packetDecoder_t packetDecoders[] = {
   [stopType]          = stopDecoder,
@@ -377,6 +554,9 @@ const static packetDecoder_t packetDecoders[] = {
   [hoverType]         = hoverDecoder,
   [fullStateType]     = fullStateDecoder,
   [positionType]      = positionDecoder,
+  [dfallMotorsType]   = dfallMotorsDecoder,
+  [dfallRateType]     = dfallRateDecoder,
+  [dfallAngleType]    = dfallAngleDecoder,
 };
 
 /* Decoder switch */
diff --git a/src/modules/src/power_distribution_stock.c b/src/modules/src/power_distribution_stock.c
index d3b7d8d..4e7f311 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)
   #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
 
@@ -117,11 +117,11 @@ PARAM_ADD(PARAM_UINT16, m1, &motorPowerSet.m1)
 PARAM_ADD(PARAM_UINT16, m2, &motorPowerSet.m2)
 PARAM_ADD(PARAM_UINT16, m3, &motorPowerSet.m3)
 PARAM_ADD(PARAM_UINT16, m4, &motorPowerSet.m4)
-PARAM_GROUP_STOP(ring)
+PARAM_GROUP_STOP(motorPowerSet)
 
 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_GROUP_STOP(motor)
diff --git a/src/modules/src/stabilizer.c b/src/modules/src/stabilizer.c
index 372ec94..cb4f848 100644
--- a/src/modules/src/stabilizer.c
+++ b/src/modules/src/stabilizer.c
@@ -92,6 +92,10 @@ static struct {
   int16_t ax;
   int16_t ay;
   int16_t az;
+  // attitude - milliradians
+  int16_t roll;
+  int16_t pitch;
+  int16_t yaw;
   // compressed quaternion, see quatcompress.h
   int32_t quat;
   // angular velocity - milliradians / sec
@@ -151,6 +155,11 @@ static void compressState()
   stateCompressed.ay = state.acc.y * 9.81f * 1000.0f;
   stateCompressed.az = (state.acc.z + 1) * 9.81f * 1000.0f;
 
+  float const deg2millirad = ((float)M_PI * 1000.0f) / 180.0f;
+  stateCompressed.roll = state.attitude.roll * deg2millirad;
+  stateCompressed.pitch = state.attitude.pitch * deg2millirad;
+  stateCompressed.yaw = state.attitude.yaw * deg2millirad;
+
   float const q[4] = {
     state.attitudeQuaternion.x,
     state.attitudeQuaternion.y,
@@ -158,7 +167,7 @@ static void compressState()
     state.attitudeQuaternion.w};
   stateCompressed.quat = quatcompress(q);
 
-  float const deg2millirad = ((float)M_PI * 1000.0f) / 180.0f;
+  //float const deg2millirad = ((float)M_PI * 1000.0f) / 180.0f;
   stateCompressed.rateRoll = sensorData.gyro.x * deg2millirad;
   stateCompressed.ratePitch = -sensorData.gyro.y * deg2millirad;
   stateCompressed.rateYaw = sensorData.gyro.z * deg2millirad;
@@ -670,6 +679,10 @@ LOG_ADD(LOG_INT16, ax, &stateCompressed.ax)               // acceleration - mm /
 LOG_ADD(LOG_INT16, ay, &stateCompressed.ay)
 LOG_ADD(LOG_INT16, az, &stateCompressed.az)
 
+LOG_ADD(LOG_INT16, roll, &stateCompressed.roll)           // attitude - milliradians
+LOG_ADD(LOG_INT16, pitch, &stateCompressed.pitch)
+LOG_ADD(LOG_INT16, yaw, &stateCompressed.yaw)
+
 LOG_ADD(LOG_UINT32, quat, &stateCompressed.quat)           // compressed quaternion, see quatcompress.h
 
 LOG_ADD(LOG_INT16, rateRoll, &stateCompressed.rateRoll)   // angular velocity - milliradians / sec