ARM7 based quadrocopter
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

45 lines
1.8KB

  1. /***************************************************************************
  2. * Copyright (C) 03/2008 by Olaf Rempel *
  3. * razzor@kopf-tisch.de *
  4. * *
  5. * This program is free software; you can redistribute it and/or modify *
  6. * it under the terms of the GNU General Public License as published by *
  7. * the Free Software Foundation; version 2 of the License *
  8. * *
  9. * This program is distributed in the hope that it will be useful, *
  10. * but WITHOUT ANY WARRANTY; without even the implied warranty of *
  11. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
  12. * GNU General Public License for more details. *
  13. * *
  14. * You should have received a copy of the GNU General Public License *
  15. * along with this program; if not, write to the *
  16. * Free Software Foundation, Inc., *
  17. * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
  18. ***************************************************************************/
  19. #include <stdint.h>
  20. #include "pidctrl.h"
  21. #include "board.h"
  22. int32_t pid_ctrl(struct pid_data *pid, int32_t error)
  23. {
  24. int32_t out = 0;
  25. if (pid->kp != 0)
  26. out += pid->kp * error;
  27. if (pid->ki != 0) {
  28. pid->err_sum += error;
  29. pid->err_sum = LIMIT(pid->err_sum, pid->err_sum_min, pid->err_sum_max);
  30. out += (pid->err_sum / pid->ki);
  31. }
  32. if (pid->kd != 0) {
  33. out += pid->kd * (error - pid->err_old);
  34. pid->err_old = error;
  35. }
  36. return LIMIT(out, pid->out_min, pid->out_max);
  37. }