Something went wrong on our end
firmware_modifications.patch 14.21 KiB
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..64447d6 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 = 6,
+ PPSRateType = 7,
+ PPSAngleType = 8,
};
/* ---===== 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();