Skip to content

Commit

Permalink
Merge pull request #1 from Flybrix/scale-factor-units
Browse files Browse the repository at this point in the history 
change units on pid scaling
  • Loading branch information
@rjwalters
rjwalters committed Mar 9, 2016
2 parents 62678dd + 88a35fc commit affcbab
Show file tree
Hide file tree
Showing 3 changed files with 27 additions and 26 deletions.
16 changes: 8 additions & 8 deletions config.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -66,63 +66,63 @@ void initializeEEPROM(void) { // Default Settings
CONFIG.data.thrustMasterPIDParameters[3] = 0.0f; // Windup guard
CONFIG.data.thrustMasterPIDParameters[4] = 0.005f; // D filter usec (15Hz)
CONFIG.data.thrustMasterPIDParameters[5] = 0.005f; // setpoint filter usec (30Hz)
CONFIG.data.thrustMasterPIDParameters[6] = 1.0f; // ppm to value scaling
CONFIG.data.thrustMasterPIDParameters[6] = 1.0f; // (meters / full stick action)

CONFIG.data.pitchMasterPIDParameters[0] = 12.0f; // P
CONFIG.data.pitchMasterPIDParameters[1] = 1.0f; // I
CONFIG.data.pitchMasterPIDParameters[2] = 0.2f; // D
CONFIG.data.pitchMasterPIDParameters[3] = 10.0f; // Windup guard
CONFIG.data.pitchMasterPIDParameters[4] = 0.005f; // D filter usec (15Hz)
CONFIG.data.pitchMasterPIDParameters[5] = 0.005f; // setpoint filter usec (30Hz)
CONFIG.data.pitchMasterPIDParameters[6] = 10.0f / 2047.0f; // ppm to value scaling
CONFIG.data.pitchMasterPIDParameters[6] = 10.0f; // (degrees / full stick action)

CONFIG.data.rollMasterPIDParameters[0] = 12.0f; // P
CONFIG.data.rollMasterPIDParameters[1] = 1.0f; // I
CONFIG.data.rollMasterPIDParameters[2] = 0.2f; // D
CONFIG.data.rollMasterPIDParameters[3] = 10.0f; // Windup guard
CONFIG.data.rollMasterPIDParameters[4] = 0.005f; // D filter usec (15Hz)
CONFIG.data.rollMasterPIDParameters[5] = 0.005f; // setpoint filter usec (30Hz)
CONFIG.data.rollMasterPIDParameters[6] = 10.0f / 2047.0f; // ppm to value scaling
CONFIG.data.rollMasterPIDParameters[6] = 10.0f; // (degrees / full stick action)

CONFIG.data.yawMasterPIDParameters[0] = 1.0f; // P
CONFIG.data.yawMasterPIDParameters[1] = 0.0f; // I
CONFIG.data.yawMasterPIDParameters[2] = 0.0f; // D
CONFIG.data.yawMasterPIDParameters[3] = 10.0f; // Windup guard
CONFIG.data.yawMasterPIDParameters[4] = 0.005f; // D filter usec (15Hz)
CONFIG.data.yawMasterPIDParameters[5] = 0.005f; // setpoint filter usec (30Hz)
CONFIG.data.yawMasterPIDParameters[6] = 10.0f / 2047.0f; // ppm to value scaling
CONFIG.data.yawMasterPIDParameters[6] = 180.0f; // (degrees / full stick action)

CONFIG.data.thrustSlavePIDParameters[0] = 1.0f; // P
CONFIG.data.thrustSlavePIDParameters[1] = 0.0f; // I
CONFIG.data.thrustSlavePIDParameters[2] = 0.0f; // D
CONFIG.data.thrustSlavePIDParameters[3] = 10.0f; // Windup guard
CONFIG.data.thrustSlavePIDParameters[4] = 0.001f; // D filter usec (150Hz)
CONFIG.data.thrustSlavePIDParameters[5] = 0.001f; // setpoint filter usec (300Hz)
CONFIG.data.thrustSlavePIDParameters[6] = 1.0f; // ppm to value scaling
CONFIG.data.thrustSlavePIDParameters[6] = 0.3f; // (meters/sec / full stick action)

CONFIG.data.pitchSlavePIDParameters[0] = 40.0f; // P
CONFIG.data.pitchSlavePIDParameters[1] = 5.0f; // I
CONFIG.data.pitchSlavePIDParameters[2] = 4.0f; // D
CONFIG.data.pitchSlavePIDParameters[3] = 30.0f; // Windup guard
CONFIG.data.pitchSlavePIDParameters[4] = 0.001f; // D filter usec (150Hz)
CONFIG.data.pitchSlavePIDParameters[5] = 0.001f; // setpoint filter usec (300Hz)
CONFIG.data.pitchSlavePIDParameters[6] = 360.0f / 2047.0f; // ppm to value scaling
CONFIG.data.pitchSlavePIDParameters[6] = 30.0f; // (deg/sec / full stick action)

CONFIG.data.rollSlavePIDParameters[0] = 40.0f; // P
CONFIG.data.rollSlavePIDParameters[1] = 5.0f; // I
CONFIG.data.rollSlavePIDParameters[2] = 4.0f; // D
CONFIG.data.rollSlavePIDParameters[3] = 30.0f; // Windup guard
CONFIG.data.rollSlavePIDParameters[4] = 0.001f; // D filter usec (150Hz)
CONFIG.data.rollSlavePIDParameters[5] = 0.001f; // setpoint filter usec (300Hz)
CONFIG.data.rollSlavePIDParameters[6] = 360.0f / 2047.0f; // ppm to value scaling
CONFIG.data.rollSlavePIDParameters[6] = 30.0f; // (deg/sec / full stick action)

CONFIG.data.yawSlavePIDParameters[0] = 20.0f; // P
CONFIG.data.yawSlavePIDParameters[1] = 1.0f; // I
CONFIG.data.yawSlavePIDParameters[2] = 0.0f; // D
CONFIG.data.yawSlavePIDParameters[3] = 20.0f; // Windup guard
CONFIG.data.yawSlavePIDParameters[4] = 0.001f; // D filter usec (150Hz)
CONFIG.data.yawSlavePIDParameters[5] = 0.001f; // setpoint filter usec (300Hz)
CONFIG.data.yawSlavePIDParameters[6] = 360.0f / 2047.0f; // ppm to value scaling
CONFIG.data.yawSlavePIDParameters[6] = 120.0f; // (deg/sec / full stick action)

CONFIG.data.pidBypass = 25; //AHRS/Horizon mode

Expand Down
33 changes: 16 additions & 17 deletions control.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,10 +37,9 @@ void Control::parseConfig(CONFIG_struct& config) {
yaw_pid = {config.yawMasterPIDParameters, config.yawSlavePIDParameters};

for (uint8_t i = 0; i < 8; ++i)
nonzero_input_[i] = ((config.pidBypass & (1 << i)) == 0);
pidEnabled[i] = ((config.pidBypass & (1 << i)) == 0);

// yaw controls a rate, not an angle!
// all slaves are rate controllers
// all slaves are rate controllers; set up the master pids as wrapped angle controllers
pitch_pid.isMasterWrapped();
roll_pid.isMasterWrapped();
yaw_pid.isMasterWrapped();
Expand All @@ -54,26 +53,26 @@ void Control::parseConfig(CONFIG_struct& config) {
void Control::calculateControlVectors() {
thrust_pid.setMasterInput(state->kinematicsAltitude);
thrust_pid.setSlaveInput(0.0f); // TODO: where would be get this data from?
pitch_pid.setMasterInput(state->kinematicsAngle[0] * 57.2957795);
pitch_pid.setSlaveInput(state->kinematicsRate[0] * 57.2957795);
roll_pid.setMasterInput(state->kinematicsAngle[1] * 57.2957795);
roll_pid.setSlaveInput(state->kinematicsRate[1] * 57.2957795);
yaw_pid.setMasterInput(state->kinematicsAngle[2] * 57.2957795);
yaw_pid.setSlaveInput(state->kinematicsRate[2] * 57.2957795);
pitch_pid.setMasterInput(state->kinematicsAngle[0] * 57.2957795f);
pitch_pid.setSlaveInput(state->kinematicsRate[0] * 57.2957795f);
roll_pid.setMasterInput(state->kinematicsAngle[1] * 57.2957795f);
roll_pid.setSlaveInput(state->kinematicsRate[1] * 57.2957795f);
yaw_pid.setMasterInput(state->kinematicsAngle[2] * 57.2957795f);
yaw_pid.setSlaveInput(state->kinematicsRate[2] * 57.2957795f);

thrust_pid.setSetpoint(state->command_throttle * thrust_pid.getScalingFactor(nonzero_input_[THRUST_MASTER], nonzero_input_[THRUST_SLAVE]));
pitch_pid.setSetpoint(state->command_pitch * pitch_pid.getScalingFactor(nonzero_input_[PITCH_MASTER], nonzero_input_[PITCH_SLAVE]));
roll_pid.setSetpoint(state->command_roll * roll_pid.getScalingFactor(nonzero_input_[ROLL_MASTER], nonzero_input_[ROLL_SLAVE]));
yaw_pid.setSetpoint(state->command_yaw * yaw_pid.getScalingFactor(nonzero_input_[YAW_MASTER], nonzero_input_[YAW_SLAVE]));
thrust_pid.setSetpoint(state->command_throttle * 4095.0f * thrust_pid.getScalingFactor(pidEnabled[THRUST_MASTER], pidEnabled[THRUST_SLAVE]));
pitch_pid.setSetpoint(state->command_pitch * 2047.0f * pitch_pid.getScalingFactor(pidEnabled[PITCH_MASTER], pidEnabled[PITCH_SLAVE]));
roll_pid.setSetpoint(state->command_roll * 2047.0f * roll_pid.getScalingFactor(pidEnabled[ROLL_MASTER], pidEnabled[ROLL_SLAVE]));
yaw_pid.setSetpoint(state->command_yaw * 2047.0f * yaw_pid.getScalingFactor(pidEnabled[YAW_MASTER], pidEnabled[YAW_SLAVE]));

uint32_t now = micros();

// compute new slave setpoints in the master PIDs
// and compute state control torques
state->Fz = thrust_pid.Compute(now, nonzero_input_[THRUST_MASTER], nonzero_input_[THRUST_SLAVE]);
state->Tx = pitch_pid.Compute(now, nonzero_input_[PITCH_MASTER], nonzero_input_[PITCH_SLAVE]);
state->Ty = roll_pid.Compute(now, nonzero_input_[ROLL_MASTER], nonzero_input_[ROLL_SLAVE]);
state->Tz = yaw_pid.Compute(now, nonzero_input_[YAW_MASTER], nonzero_input_[YAW_SLAVE]);
state->Fz = thrust_pid.Compute(now, pidEnabled[THRUST_MASTER], pidEnabled[THRUST_SLAVE]);
state->Tx = pitch_pid.Compute(now, pidEnabled[PITCH_MASTER], pidEnabled[PITCH_SLAVE]);
state->Ty = roll_pid.Compute(now, pidEnabled[ROLL_MASTER], pidEnabled[ROLL_SLAVE]);
state->Tz = yaw_pid.Compute(now, pidEnabled[YAW_MASTER], pidEnabled[YAW_SLAVE]);

// add in the feedforward torques
state->Tx += state->Tx_trim;
Expand Down
4 changes: 3 additions & 1 deletion control.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,9 @@ class Control {

State *state;
uint32_t lastUpdateMicros = 0; // 1.2 hrs should be enough
bool nonzero_input_[8]{false, false, false, false, false, false, false, false};

// unpack config.pidBypass for convenience
bool pidEnabled[8]{false, false, false, false, false, false, false, false};

// controllers
CascadedPID thrust_pid, pitch_pid, roll_pid, yaw_pid;
Expand Down

0 comments on commit affcbab

Please sign in to comment.