control.cpp

#include "control.h"
#include "config.h"
#include "modes.h"

// State
float pitch_i = 0, roll_i = 0;
float pitch_prev = 0, roll_prev = 0;

int last_fl = SERVO_NEUTRAL, last_fr = SERVO_NEUTRAL;
int last_rl = SERVO_NEUTRAL, last_rr = SERVO_NEUTRAL;

void controlInit() {
  pitch_i = roll_i = 0;
  pitch_prev = roll_prev = 0;
}

int clamp(int val, int minVal, int maxVal) {
  if (val < minVal) return minVal;
  if (val > maxVal) return maxVal;
  return val;
}

ControlOutput controlUpdate(ImuData imu) {
  ControlOutput out;
  ModeConfig cfg = modesGetConfig();

  // --- Roll PID ---
  float error_roll = 0 - imu.roll;
  roll_i += error_roll;
  float d_roll = imu.roll - roll_prev;
  float out_roll = cfg.Kp_roll * error_roll + cfg.Ki_roll * roll_i - cfg.Kd_roll * d_roll;
  roll_prev = imu.roll;

  // --- Pitch PID ---
  float error_pitch = 0 - imu.pitch;
  pitch_i += error_pitch;
  float d_pitch = imu.pitch - pitch_prev;
  float out_pitch = cfg.Kp_pitch * error_pitch + cfg.Ki_pitch * pitch_i - cfg.Kd_pitch * d_pitch;
  pitch_prev = imu.pitch;

  // Servo targets
  int target_fl = SERVO_NEUTRAL - out_roll - out_pitch;
  int target_fr = SERVO_NEUTRAL + out_roll - out_pitch;
  int target_rl = SERVO_NEUTRAL - out_roll + out_pitch;
  int target_rr = SERVO_NEUTRAL + out_roll + out_pitch;

  // Clamp & smooth
  target_fl = clamp(target_fl, SERVO_NEUTRAL - cfg.outputLimit, SERVO_NEUTRAL + cfg.outputLimit);
  target_fr = clamp(target_fr, SERVO_NEUTRAL - cfg.outputLimit, SERVO_NEUTRAL + cfg.outputLimit);
  target_rl = clamp(target_rl, SERVO_NEUTRAL - cfg.outputLimit, SERVO_NEUTRAL + cfg.outputLimit);
  target_rr = clamp(target_rr, SERVO_NEUTRAL - cfg.outputLimit, SERVO_NEUTRAL + cfg.outputLimit);

  last_fl += clamp(target_fl - last_fl, -cfg.slewRate, cfg.slewRate);
  last_fr += clamp(target_fr - last_fr, -cfg.slewRate, cfg.slewRate);
  last_rl += clamp(target_rl - last_rl, -cfg.slewRate, cfg.slewRate);
  last_rr += clamp(target_rr - last_rr, -cfg.slewRate, cfg.slewRate);

  out.fl = last_fl;
  out.fr = last_fr;
  out.rl = last_rl;
  out.rr = last_rr;

  return out;
}