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..c51fdbd 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 {
@@ -174,11 +178,17 @@ typedef struct setpoint_s {
attitude_t attitudeRate; // deg/s
quaternion_t attitudeQuaternion;
float thrust;
+ 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
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
+++ 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..673b5cc 100644
--- a/src/modules/src/controller_pid.c
+++ b/src/modules/src/controller_pid.c
@@ -132,6 +132,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 +153,92 @@ void controllerPid(control_t *control, setpoint_t *setpoint,
// Reset the calculated YAW angle for rate control
attitudeDesired.yaw = state->attitude.yaw;
}
+ */
+
+ // 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
+ // if indicated by the respective mode
+ if (setpoint->mode.roll == modeDisable)
+ {
+ control->roll = 0;
+ cmd_roll = 0.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)
+ {
+ // 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->useDfallAdjustmentsToControllerPID == false"
+ 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 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
+ 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;
+
+ // 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..f6931a4 100644
--- a/src/modules/src/crtp_commander_generic.c
+++ b/src/modules/src/crtp_commander_generic.c
@@ -71,6 +71,7 @@ enum packet_type {
hoverType = 5,
fullStateType = 6,
positionType = 7,
+ dfallType = 8,
};
/* ---===== 2 - Decoding functions =====--- */
@@ -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;
}
/* 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;
+
+ // 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;
}
+
+ // 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;
+
+ // 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;
+
+ // 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;
+
+ // Turn off the "dfall mode adjustments"
+ setpoint->useDfallAdjustmentsToControllerPID = false;
+}
+
+/* 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}
+ */
+
+#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
+
+struct dfallPacket_s {
+ uint16_t controllerModes;
+ uint16_t cmd1;
+ uint16_t cmd2;
+ uint16_t cmd3;
+ uint16_t cmd4;
+ float xControllerSetpoint;
+ float yControllerSetpoint;
+ float zControllerSetpoint;
+ float yawControllerSetpoint;
+} __attribute__((packed));
+
+static void dfallDecoder(setpoint_t *setpoint, uint8_t type, const void *data, size_t datalen)
+{
+ 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;
+ }
+
+ // 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;
+ }
+
+ // Set the thrust setpoint to zero
+ setpoint->thrust = 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 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 =====--- */
@@ -377,6 +703,7 @@ const static packetDecoder_t packetDecoders[] = {
[hoverType] = hoverDecoder,
[fullStateType] = fullStateDecoder,
[positionType] = positionDecoder,
+ [dfallType] = dfallDecoder,
};
/* Decoder switch */
diff --git a/src/modules/src/power_distribution_stock.c b/src/modules/src/power_distribution_stock.c
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)
#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_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
--- 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