Skip to content

Commit

Permalink
Merge pull request #1166 from IMRCLab/refactor_ctrl_const_setpoint
Browse files Browse the repository at this point in the history
Refactor controller update to take const setpoint
  • Loading branch information
krichardsson authored Dec 9, 2022
2 parents d2be797 + baf753a commit c656b2c
Show file tree
Hide file tree
Showing 14 changed files with 30 additions and 26 deletions.
2 changes: 1 addition & 1 deletion src/modules/interface/controller.h
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,7 @@ typedef enum {

void controllerInit(ControllerType controller);
bool controllerTest(void);
void controller(control_t *control, setpoint_t *setpoint,
void controller(control_t *control, const setpoint_t *setpoint,
const sensorData_t *sensors,
const state_t *state,
const uint32_t tick);
Expand Down
2 changes: 1 addition & 1 deletion src/modules/interface/controller_brescianini.h
Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,7 @@
void controllerBrescianiniInit(void);
bool controllerBrescianiniTest(void);
void controllerBrescianini(control_t *control,
setpoint_t *setpoint,
const setpoint_t *setpoint,
const sensorData_t *sensors,
const state_t *state,
const uint32_t tick);
2 changes: 1 addition & 1 deletion src/modules/interface/controller_indi.h
Original file line number Diff line number Diff line change
Expand Up @@ -109,7 +109,7 @@ struct IndiVariables {

void controllerINDIInit(void);
bool controllerINDITest(void);
void controllerINDI(control_t *control, setpoint_t *setpoint,
void controllerINDI(control_t *control, const setpoint_t *setpoint,
const sensorData_t *sensors,
const state_t *state,
const uint32_t tick);
Expand Down
2 changes: 1 addition & 1 deletion src/modules/interface/controller_mellinger.h
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,7 @@

void controllerMellingerInit(void);
bool controllerMellingerTest(void);
void controllerMellinger(control_t *control, setpoint_t *setpoint,
void controllerMellinger(control_t *control, const setpoint_t *setpoint,
const sensorData_t *sensors,
const state_t *state,
const uint32_t tick);
Expand Down
2 changes: 1 addition & 1 deletion src/modules/interface/controller_pid.h
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,7 @@

void controllerPidInit(void);
bool controllerPidTest(void);
void controllerPid(control_t *control, setpoint_t *setpoint,
void controllerPid(control_t *control, const setpoint_t *setpoint,
const sensorData_t *sensors,
const state_t *state,
const uint32_t tick);
Expand Down
4 changes: 2 additions & 2 deletions src/modules/interface/position_controller.h
Original file line number Diff line number Diff line change
Expand Up @@ -33,9 +33,9 @@
void positionControllerInit();
void positionControllerResetAllPID();
void positionControllerResetAllfilters();
void positionController(float* thrust, attitude_t *attitude, setpoint_t *setpoint,
void positionController(float* thrust, attitude_t *attitude, const setpoint_t *setpoint,
const state_t *state);
void velocityController(float* thrust, attitude_t *attitude, setpoint_t *setpoint,
void velocityController(float* thrust, attitude_t *attitude, const Axis3f *setpoint_velocity,
const state_t *state);

#endif /* POSITION_CONTROLLER_H_ */
2 changes: 1 addition & 1 deletion src/modules/interface/position_controller_indi.h
Original file line number Diff line number Diff line change
Expand Up @@ -80,7 +80,7 @@ struct IndiOuterVariables {

void positionControllerINDIInit(void);
void positionControllerINDI(const sensorData_t *sensors,
setpoint_t *setpoint,
const setpoint_t *setpoint,
const state_t *state,
vector_t *refOuterINDI);

Expand Down
4 changes: 2 additions & 2 deletions src/modules/src/controller.c
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,7 @@ static void initController();
typedef struct {
void (*init)(void);
bool (*test)(void);
void (*update)(control_t *control, setpoint_t *setpoint, const sensorData_t *sensors, const state_t *state, const uint32_t tick);
void (*update)(control_t *control, const setpoint_t *setpoint, const sensorData_t *sensors, const state_t *state, const uint32_t tick);
const char* name;
} ControllerFcns;

Expand Down Expand Up @@ -77,7 +77,7 @@ bool controllerTest(void) {
return controllerFunctions[currentController].test();
}

void controller(control_t *control, setpoint_t *setpoint, const sensorData_t *sensors, const state_t *state, const uint32_t tick) {
void controller(control_t *control, const setpoint_t *setpoint, const sensorData_t *sensors, const state_t *state, const uint32_t tick) {
controllerFunctions[currentController].update(control, setpoint, sensors, state, tick);
}

Expand Down
2 changes: 1 addition & 1 deletion src/modules/src/controller_brescianini.c
Original file line number Diff line number Diff line change
Expand Up @@ -94,7 +94,7 @@ void controllerBrescianiniInit(void) {


void controllerBrescianini(control_t *control,
setpoint_t *setpoint,
const setpoint_t *setpoint,
const sensorData_t *sensors,
const state_t *state,
const uint32_t tick) {
Expand Down
2 changes: 1 addition & 1 deletion src/modules/src/controller_indi.c
Original file line number Diff line number Diff line change
Expand Up @@ -142,7 +142,7 @@ bool controllerINDITest(void)
return pass;
}

void controllerINDI(control_t *control, setpoint_t *setpoint,
void controllerINDI(control_t *control, const setpoint_t *setpoint,
const sensorData_t *sensors,
const state_t *state,
const uint32_t tick)
Expand Down
2 changes: 1 addition & 1 deletion src/modules/src/controller_mellinger.c
Original file line number Diff line number Diff line change
Expand Up @@ -121,7 +121,7 @@ bool controllerMellingerTest(void)
return true;
}

void controllerMellinger(control_t *control, setpoint_t *setpoint,
void controllerMellinger(control_t *control, const setpoint_t *setpoint,
const sensorData_t *sensors,
const state_t *state,
const uint32_t tick)
Expand Down
2 changes: 1 addition & 1 deletion src/modules/src/controller_pid.c
Original file line number Diff line number Diff line change
Expand Up @@ -53,7 +53,7 @@ static float capAngle(float angle) {
return result;
}

void controllerPid(control_t *control, setpoint_t *setpoint,
void controllerPid(control_t *control, const setpoint_t *setpoint,
const sensorData_t *sensors,
const state_t *state,
const uint32_t tick)
Expand Down
2 changes: 1 addition & 1 deletion src/modules/src/position_controller_indi.c
Original file line number Diff line number Diff line change
Expand Up @@ -116,7 +116,7 @@ void positionControllerINDIInit(void)


void positionControllerINDI(const sensorData_t *sensors,
setpoint_t *setpoint,
const setpoint_t *setpoint,
const state_t *state,
vector_t *refOuterINDI){

Expand Down
26 changes: 15 additions & 11 deletions src/modules/src/position_controller_pid.c
Original file line number Diff line number Diff line change
Expand Up @@ -190,7 +190,7 @@ static float runPid(float input, struct pidAxis_s *axis, float setpoint, float d

float state_body_x, state_body_y, state_body_vx, state_body_vy;

void positionController(float* thrust, attitude_t *attitude, setpoint_t *setpoint,
void positionController(float* thrust, attitude_t *attitude, const setpoint_t *setpoint,
const state_t *state)
{
this.pidX.pid.outputLimit = xVelMax * velMaxOverhead;
Expand All @@ -212,24 +212,28 @@ void positionController(float* thrust, attitude_t *attitude, setpoint_t *setpoin
float globalvy = setpoint->velocity.y;

//X, Y
Axis3f setpoint_velocity;
setpoint_velocity.x = setpoint->velocity.x;
setpoint_velocity.y = setpoint->velocity.y;
setpoint_velocity.z = setpoint->velocity.z;
if (setpoint->mode.x == modeAbs) {
setpoint->velocity.x = runPid(state_body_x, &this.pidX, setp_body_x, DT);
setpoint_velocity.x = runPid(state_body_x, &this.pidX, setp_body_x, DT);
} else if (!setpoint->velocity_body) {
setpoint->velocity.x = globalvx * cosyaw + globalvy * sinyaw;
setpoint_velocity.x = globalvx * cosyaw + globalvy * sinyaw;
}
if (setpoint->mode.y == modeAbs) {
setpoint->velocity.y = runPid(state_body_y, &this.pidY, setp_body_y, DT);
setpoint_velocity.y = runPid(state_body_y, &this.pidY, setp_body_y, DT);
} else if (!setpoint->velocity_body) {
setpoint->velocity.y = globalvy * cosyaw - globalvx * sinyaw;
setpoint_velocity.y = globalvy * cosyaw - globalvx * sinyaw;
}
if (setpoint->mode.z == modeAbs) {
setpoint->velocity.z = runPid(state->position.z, &this.pidZ, setpoint->position.z, DT);
setpoint_velocity.z = runPid(state->position.z, &this.pidZ, setpoint->position.z, DT);
}

velocityController(thrust, attitude, setpoint, state);
velocityController(thrust, attitude, &setpoint_velocity, state);
}

void velocityController(float* thrust, attitude_t *attitude, setpoint_t *setpoint,
void velocityController(float* thrust, attitude_t *attitude, const Axis3f* setpoint_velocity,
const state_t *state)
{
this.pidVX.pid.outputLimit = pLimit * rpLimitOverhead;
Expand All @@ -244,14 +248,14 @@ void velocityController(float* thrust, attitude_t *attitude, setpoint_t *setpoin
state_body_vy = -state->velocity.x * sinyaw + state->velocity.y * cosyaw;

// Roll and Pitch
attitude->pitch = -runPid(state_body_vx, &this.pidVX, setpoint->velocity.x, DT);
attitude->roll = -runPid(state_body_vy, &this.pidVY, setpoint->velocity.y, DT);
attitude->pitch = -runPid(state_body_vx, &this.pidVX, setpoint_velocity->x, DT);
attitude->roll = -runPid(state_body_vy, &this.pidVY, setpoint_velocity->y, DT);

attitude->roll = constrain(attitude->roll, -rLimit, rLimit);
attitude->pitch = constrain(attitude->pitch, -pLimit, pLimit);

// Thrust
float thrustRaw = runPid(state->velocity.z, &this.pidVZ, setpoint->velocity.z, DT);
float thrustRaw = runPid(state->velocity.z, &this.pidVZ, setpoint_velocity->z, DT);
// Scale the thrust and add feed forward term
*thrust = thrustRaw*thrustScale + this.thrustBase;
// Check for minimum thrust
Expand Down

0 comments on commit c656b2c

Please sign in to comment.