Skip to content

Commit

Permalink
drop pid_precalc, use single function
Browse files Browse the repository at this point in the history
  • Loading branch information
bkleiner committed Oct 22, 2023
1 parent 62cd633 commit e25d4f5
Show file tree
Hide file tree
Showing 8 changed files with 127 additions and 160 deletions.
2 changes: 2 additions & 0 deletions src/core/looptime.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -74,6 +74,8 @@ void looptime_update() {

state.looptime_us = CYCLES_TO_US(time_cycles() - last_loop_cycles);
state.looptime = state.looptime_us * 1e-6f;
// 0.0032f is there for legacy purposes, should be 0.001f = looptime
state.timefactor = 0.0032f / state.looptime;
state.loop_counter++;

last_loop_cycles = time_cycles();
Expand Down
6 changes: 2 additions & 4 deletions src/flight/angle_pid.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,8 +16,6 @@ static float apidoutput2[ANGLE_PID_SIZE];
static float apidoutput[ANGLE_PID_SIZE];
static float lasterror[ANGLE_PID_SIZE];

extern float timefactor;

float angle_pid(int x) {
const float angle_error_abs = fabsf(state.angleerror[x]);

Expand All @@ -28,10 +26,10 @@ float angle_pid(int x) {
apidoutput2[x] = angle_error_abs * state.angleerror[x] * profile.pid.big_angle.kp;

// D term 1 weighted + P term 1 weighted
apidoutput1[x] += (state.angleerror[x] - lasterror[x]) * profile.pid.small_angle.kd * (1 - angle_error_abs) * timefactor;
apidoutput1[x] += (state.angleerror[x] - lasterror[x]) * profile.pid.small_angle.kd * (1 - angle_error_abs) * state.timefactor;

// D term 2 weighted + P term 2 weighted
apidoutput2[x] += ((state.angleerror[x] - lasterror[x]) * profile.pid.big_angle.kd * angle_error_abs * timefactor);
apidoutput2[x] += ((state.angleerror[x] - lasterror[x]) * profile.pid.big_angle.kd * angle_error_abs * state.timefactor);

// apidoutput sum
apidoutput[x] = apidoutput1[x] + apidoutput2[x];
Expand Down
2 changes: 0 additions & 2 deletions src/flight/control.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -265,8 +265,6 @@ static void control_flight_mode() {
}

void control() {
pid_precalc();

if (rx_aux_on(AUX_TURTLE) && !rx_aux_on(AUX_MOTOR_TEST)) { // turtle active when aux high
turtle_mode_start();
} else {
Expand Down
10 changes: 7 additions & 3 deletions src/flight/control.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,7 @@ typedef struct {

uint16_t looptime_autodetect;
float looptime; // looptime in seconds
float timefactor; // timefactor for pid calc
uint32_t looptime_us; // looptime in us
uint32_t loop_counter; // number of loops ran
float uptime; // running sum of looptimes
Expand Down Expand Up @@ -85,9 +86,10 @@ typedef struct {
vec3_t accel_raw; // raw accel reading with rotation and scaling applied
vec3_t accel; // filtered accel readings

float gyro_temp; // gyro temparture reading
vec3_t gyro_raw; // raw gyro reading with rotation and scaling applied
vec3_t gyro; // filtered gyro reading
float gyro_temp; // gyro temparture reading
vec3_t gyro_raw; // raw gyro reading with rotation and scaling applied
vec3_t gyro; // filtered gyro reading
vec3_t gyro_delta_angle; // angle covered in last time interval

vec3_t GEstG; // gravity vector
vec3_t attitude;
Expand All @@ -109,6 +111,7 @@ typedef struct {
MEMBER(failloop, uint8_t) \
MEMBER(looptime_autodetect, uint16_t) \
MEMBER(looptime, float) \
MEMBER(timefactor, float) \
MEMBER(looptime_us, uint32_t) \
MEMBER(loop_counter, uint32_t) \
MEMBER(uptime, float) \
Expand Down Expand Up @@ -139,6 +142,7 @@ typedef struct {
MEMBER(gyro_temp, float) \
MEMBER(gyro_raw, vec3_t) \
MEMBER(gyro, vec3_t) \
MEMBER(gyro_delta_angle, vec3_t) \
MEMBER(GEstG, vec3_t) \
MEMBER(attitude, vec3_t) \
MEMBER(setpoint, vec3_t) \
Expand Down
36 changes: 12 additions & 24 deletions src/flight/imu.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -55,20 +55,14 @@ void imu_init() {

#ifdef SILVERWARE_IMU
void imu_calc() {
const float gyro_delta_angle[3] = {
state.gyro.axis[0] * state.looptime,
state.gyro.axis[1] * state.looptime,
state.gyro.axis[2] * state.looptime,
};
state.GEstG.axis[2] = state.GEstG.axis[2] - (state.gyro_delta_angle.axis[0]) * state.GEstG.axis[0];
state.GEstG.axis[0] = (state.gyro_delta_angle.axis[0]) * state.GEstG.axis[2] + state.GEstG.axis[0];

state.GEstG.axis[2] = state.GEstG.axis[2] - (gyro_delta_angle[0]) * state.GEstG.axis[0];
state.GEstG.axis[0] = (gyro_delta_angle[0]) * state.GEstG.axis[2] + state.GEstG.axis[0];
state.GEstG.axis[1] = state.GEstG.axis[1] + (state.gyro_delta_angle.axis[1]) * state.GEstG.axis[2];
state.GEstG.axis[2] = -(state.gyro_delta_angle.axis[1]) * state.GEstG.axis[1] + state.GEstG.axis[2];

state.GEstG.axis[1] = state.GEstG.axis[1] + (gyro_delta_angle[1]) * state.GEstG.axis[2];
state.GEstG.axis[2] = -(gyro_delta_angle[1]) * state.GEstG.axis[1] + state.GEstG.axis[2];

state.GEstG.axis[0] = state.GEstG.axis[0] - (gyro_delta_angle[2]) * state.GEstG.axis[1];
state.GEstG.axis[1] = (gyro_delta_angle[2]) * state.GEstG.axis[0] + state.GEstG.axis[1];
state.GEstG.axis[0] = state.GEstG.axis[0] - (state.gyro_delta_angle.axis[2]) * state.GEstG.axis[1];
state.GEstG.axis[1] = (state.gyro_delta_angle.axis[2]) * state.GEstG.axis[0] + state.GEstG.axis[1];

if (flags.on_ground) { // happyhour bartender - quad is ON GROUND and disarmed
// calc acc mag
Expand Down Expand Up @@ -127,20 +121,14 @@ void imu_calc() {

#ifdef QUICKSILVER_IMU
void imu_calc() {
const float gyro_delta_angle[3] = {
state.gyro.axis[0] * state.looptime,
state.gyro.axis[1] * state.looptime,
state.gyro.axis[2] * state.looptime,
};

state.GEstG.axis[2] = state.GEstG.axis[2] - (gyro_delta_angle[0]) * state.GEstG.axis[0];
state.GEstG.axis[0] = (gyro_delta_angle[0]) * state.GEstG.axis[2] + state.GEstG.axis[0];
state.GEstG.axis[2] = state.GEstG.axis[2] - (state.gyro_delta_angle.axis[0]) * state.GEstG.axis[0];
state.GEstG.axis[0] = (state.gyro_delta_angle.axis[0]) * state.GEstG.axis[2] + state.GEstG.axis[0];

state.GEstG.axis[1] = state.GEstG.axis[1] + (gyro_delta_angle[1]) * state.GEstG.axis[2];
state.GEstG.axis[2] = -(gyro_delta_angle[1]) * state.GEstG.axis[1] + state.GEstG.axis[2];
state.GEstG.axis[1] = state.GEstG.axis[1] + (state.gyro_delta_angle.axis[1]) * state.GEstG.axis[2];
state.GEstG.axis[2] = -(state.gyro_delta_angle.axis[1]) * state.GEstG.axis[1] + state.GEstG.axis[2];

state.GEstG.axis[0] = state.GEstG.axis[0] - (gyro_delta_angle[2]) * state.GEstG.axis[1];
state.GEstG.axis[1] = (gyro_delta_angle[2]) * state.GEstG.axis[0] + state.GEstG.axis[1];
state.GEstG.axis[0] = state.GEstG.axis[0] - (state.gyro_delta_angle.axis[2]) * state.GEstG.axis[1];
state.GEstG.axis[1] = (state.gyro_delta_angle.axis[2]) * state.GEstG.axis[0] + state.GEstG.axis[1];

filter_lp_pt1_coeff(&filter, PT1_FILTER_HZ);

Expand Down
Loading

0 comments on commit e25d4f5

Please sign in to comment.