PID controller in C

PID controller in C

Asked about it several times I decided to dig out this old piece of header code of a PID controller and publish it here with some example codes. The explanation what a closed loop PID controller is, how to set it up and what to take into account is written down here.

The disassembly is to give you the gist how much CPU power is needed to run it on different architectures and how the compiler can optimise.

Diesen Quelltext habe ich aus der Kiste gekramt und ein wenig mit Beispielen aufbereitet da ich öfter mal gefragt wurde - vielleicht ist er auch für Dich hilfreich. Die gewünschte Erklärung (im Plauderton), was ein PID-Regler ist, habe ich hier geschrieben).

Die weiterhin angehängten Disassembly Listen sollten eine Idee vermitteln wie viele CPU-Anweisungen für die Berechnung eines neuen Stellwertes (unter drei verschiedenen Architekturen) erforderlich sind.

Header

/*
 * @file pid_ctrl.h
 * @author stfwi
 * @license (what you want it to be)
 * @date 2005-09-07
 * ---
 * 2013-07-03 Removed D prefilter, added more documentation
 */
#ifndef __PID_CTRL_H__
#define __PID_CTRL_H__
#ifdef  __cplusplus
extern "C" {
#endif
 
/**
 * You should define pidctl_n_t before you include this file.
 * This way you can decide which number data type you like to
 * use for the controller.
 */
#ifndef pidctl_n_t
#define pidctl_n_t float
#endif
 
/**
 * The structure used to configure and run the closed loop
 * controller.
 */
typedef struct {
  pidctl_n_t Kp;            /* Proportional constant                */
  pidctl_n_t Ki;            /* Integral constant                    */
  pidctl_n_t Kd;            /* Differential constant                */
  pidctl_n_t offset;        /* Output offset                        */
  pidctl_n_t saturation;    /* Output saturation                    */
  pidctl_n_t i_saturation;  /* Saturation of the intrgrator         */
  pidctl_n_t i;             /* Buffer of the intrgrator             */
  pidctl_n_t e_last;        /* Last error for the differentiator    */
} pidctl_t;                 /* (filter for D removed)               */
 
 
/**
 * Resets the current values (integrator and last error).
 * It does not change the configuration.
 */
#define pidctl_reset(PID, E) {  \
    (PID).i = 0;                \
    (PID).e_last = (E);         \
}
 
/**
 * Calculates the new output value (saved in O) dependent on the
 * actual error (E) and the controller configuration/state (PID).
 */
#define pidctl(PID, E, O) {                     \
  (PID).i += (PID).Ki * (E);                    \
  if((PID).i > (PID).i_saturation) {            \
    (PID).i = (PID).i_saturation;               \
  } else if((PID).i < -(PID).i_saturation) {    \
    (PID).i = -(PID).i_saturation;              \
  }                                             \
  (O) = (E) - (PID).e_last;                     \
  /* stfwi: removed D input filter  */          \
  (O) *= (PID).Kd;                              \
  (O) += (PID).Kp * (E);                        \
  (O) += (PID).i;                               \
  (O) += (PID).offset;                          \
  if((O) > (PID).saturation) {                  \
    (O) = (PID).saturation;                     \
  } else if((O) < -(PID).saturation) {          \
    (O) = -(PID).saturation;                    \
  }                                             \
    (PID).e_last = (E);                           \
}
 
#ifdef  __cplusplus
}
#endif
#endif

Beispielprogramm

Example program

/**
 * @file main.c
 * @author stfwi
 *
 * Test program to simulate the behaviour of the PID controller defined in
 * pid_ctrl.h. To make it interactively callable you the program saves the
 * actual controller state and the configuration in a specified file. Every time
 * you execute the program it is as e.g. a timer interrupt would occur calling
 * the PID controller. Of cause loading and saving from/to file makes this
 * example program absolutely useless for real world use, but it can help to
 * see what the algorithm does.
 *
 * Program usage:
 *
 *  pidctl <state-file> init [SAMPLE_RATE] [P] [I] [D] [OFFSET] [I-SAT] [SAT]
 *
 *      Initialises the state file, sets :
 *          pid.Kp = P
 *          pid.Ki = I
 *          pid.Kd = D
 *          pid.offset = OFFSET
 *          pid.i_saturation = I-SAT
 *          pid.saturation = SAT
 *          pid.i = 0.0
 *          pid.e_last = 0.0
 *
 *      The SAMPLE RATE will be stored as well, but only to display the actual
 *      time. The time will be incremented every time the program is called.
 *
 *  pidctl <state-file> reset
 *
 *      Resets t=0, pid.i = 0, pid.e_last = 0
 *
 *  pidctl <state-file> state
 *
 *      Displays the actual state saved in the state-file to STDOUT
 *
 *  pidctl <state-file> run <error-value>
 *
 *      Runs one cycle. This is as the PID algorighm would be called e.g.
 *      in a timer interrupt. You specify the actual error-signal as third
 *      command line parameter. It prints the time, actual input and controller
 *      output to STDOUT and saves the state for next call.
 *
 */
 
#define pidctl_n_t double
#include "pid_ctrl.h"
 
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
 
 
/**
 * We load and save this structure as binary data.
 */
typedef struct {
  pidctl_t pid;
  double sample_rate;
  double t;
} program_state_t;
 
 
/**
 * Main - input, do, save state
 */
int main(int argc, char** argv)
{
  // Variables used
  program_state_t state;
  const char *filename;
  const char *what;
  FILE *p_file;
  double e, o, e_last;   // PID input ("error"), output
 
  memset(&state, 0, sizeof(program_state_t));
 
  // Input check and open state file for write or read/write
  if(argc < 3) {
    fprintf(stderr, "Usage:\n");
    fprintf(stderr, "  pidctl <state-file> init [SAMPLE_RATE] [P] [I] [D] "
                    "[OFFSET] [I-SAT] [OUTPUT_SAT]\n");
    fprintf(stderr, "  pidctl <state-file> reset\n");
    fprintf(stderr, "  pidctl <state-file> run <error-value>\n");
    fprintf(stderr, "  pidctl <state-file> state\n");
    fprintf(stderr, "\n");
    exit(1);
  } else if(!(filename = argv[1]) || !(what = argv[2])) { // should be caught by argc<3 already.
    fprintf(stderr, "No state file specified\n", filename);
    exit(2);
  } else if(strcmp(what, "init") && !(p_file = fopen(filename, "rb+"))) {
    fprintf(stderr, "Could find state file '%s' (did you already init?)\n", filename);
    exit(2);
  } else if(!strcmp(what, "init") && !(p_file = fopen(filename, "wb+"))) {
    fprintf(stderr, "Failed to open state file for init '%s'\n", filename);
    exit(2);
  } else if(strcmp(what, "init") && !fread(&state, sizeof(program_state_t), 1, p_file)) {
    fclose(p_file);
    fprintf(stderr, "Failed to read state file '%s'\n", filename);
    exit(2);
  }
 
  // Do
  if(!strcmp(what, "init")) {
    if(argc < 10
    || isnan(state.sample_rate = atof(argv[3]))
    || isnan(state.pid.Kp = atof(argv[4]))
    || isnan(state.pid.Ki = atof(argv[5]))
    || isnan(state.pid.Kd = atof(argv[6]))
    || isnan(state.pid.offset = atof(argv[7]))
    || isnan(state.pid.i_saturation = atof(argv[8]))
    || isnan(state.pid.saturation = atof(argv[9]))
    ) {
      if(p_file) fclose(p_file);
      fprintf(stderr, "At least one of your init parameters is not numeric.\n");
      return 3;
    } else if(state.sample_rate == 0.0) {
      if(p_file) fclose(p_file);
      fprintf(stderr, "Sample rate 0 is no good idea.\n");
      return 4;
    } else {
      state.t = 0;
      //
      // Reset controller macro is used like this:
      //
      pidctl_reset(state.pid, 0);
      //
      //
      //
    }
  } else if(!strcmp(what, "reset")) {
    state.t = 0;
    pidctl_reset(state.pid, 0);
  } else if(!strcmp(what, "run")) {
    if(argc < 4 || isnan(e = atof(argv[3]))) {
      fprintf(stderr, "Your error input signal value is not a number\n");
      if(p_file) fclose(p_file);
      return 5;
    } else {
      //
      // Controller "run a cycle". Used similar to a function, except that it
      // changes the value of 'o' and the internal variables of the integrator!
      // You don't see that because functions normally get a pointer, this
      // macro not.
      //
      // pidctl(controller structure, actual error value, output variable);
      //
      e_last = state.pid.e_last;
      pidctl(state.pid, e, o);
      //
      //
      //
      state.t += 1.0 / state.sample_rate;
      printf("%7.3f: %+7.3f -> %+7.3f | I=%+7.3f | D=%+7.3f\n",
         state.t, e, o,
         state.pid.i,
         state.pid.Kd * (e - e_last)
      );
    }
  } else if(!strcmp(what, "state")) {
        // handled below
  } else {
    fprintf(stderr, "Unknown command '%s'\n", what);
    if(p_file) fclose(p_file);
    return 5;
  }
 
  if(!strcmp(what, "state") || !strcmp(what, "init")) {
    printf("t            : %6.3f\n", state.t);
    printf("integrator   : %6.3f\n", state.pid.i);
    printf("last error   : %6.3f\n", state.pid.e_last);
    printf("\n");
    printf("Config\n");
    printf(" sample rate : %6.3f\n", state.sample_rate);
    printf(" P           : %6.3f\n", state.pid.Kp);
    printf(" I           : %6.3f\n", state.pid.Ki);
    printf(" D           : %6.3f\n", state.pid.Ki);
    printf(" OFFSET      : %6.3f\n", state.pid.offset);
    printf(" SATURATION  : %6.3f\n", state.pid.saturation);
    printf(" I-SATURATION: %6.3f\n", state.pid.i_saturation);
  }
 
  // Save and close
  if(p_file) {
    fseek(p_file, 0, SEEK_SET);
    if(!fwrite(&state, sizeof(program_state_t), 1, p_file)) {
      fprintf(stderr, "Failed to save the actual program state!\n");
    }
    fclose(p_file);
  }
  return 0;
}

Makefile

CC=gcc
CFLAGS=-c -O3
LDFLAGS=

all: pidctl clean

clean:
    @rm -f *.b

pidctl: main.b
    @$(CC) $(LDFLAGS) main.b -o pidctl

disassembly: main.c
    ($CC) -c -g -Wa,-a,-ad -O3 main.c -o main.b > disasm.txt

main.b: main.c
    @$(CC) $(CFLAGS) main.c -o main.b

run: all
    @echo "\n---- full PID -----------------------------"
    @./pidctl state.b init 1000 0.7 0.1 0.25 0.0 2.5 10
    @echo "---------------------------------------------"
    @./pidctl state.b run 3
    @./pidctl state.b run 5
    @./pidctl state.b run 5
    @./pidctl state.b run 5
    @./pidctl state.b run 5
    @./pidctl state.b run 5
    @./pidctl state.b run 5
    @./pidctl state.b run 7
    @./pidctl state.b run 7
    @./pidctl state.b run 7
    @./pidctl state.b run 7
    @./pidctl state.b run 6
    @./pidctl state.b run 5
    @./pidctl state.b run 4
    @./pidctl state.b run 3
    @./pidctl state.b run 2
    @./pidctl state.b run 1
    @./pidctl state.b run 0
    @./pidctl state.b run 0
    @./pidctl state.b run -1
    @./pidctl state.b run -2
    @./pidctl state.b run -4
    @./pidctl state.b run -3
    @./pidctl state.b run -2
    @./pidctl state.b run -2
    @./pidctl state.b run -2
    @./pidctl state.b run -2
    @./pidctl state.b run -2
    @./pidctl state.b run -2
    @./pidctl state.b run -2
    @./pidctl state.b run -2
    @./pidctl state.b run -2
    @./pidctl state.b run -2
    @./pidctl state.b run -1
    @./pidctl state.b run 10
    @./pidctl state.b run -10
    @./pidctl state.b run 10
    @./pidctl state.b run -10
    @./pidctl state.b run 10
    @./pidctl state.b run -10
    @./pidctl state.b run 0 
    @rm -f state.b

run-p: all  
    @echo "\n----P PART ONLY----------------------------"
    @./pidctl state.b init 1000 2.0 0 0 0.0 0 10
    @echo "---------------------------------------------"
    @./pidctl state.b run 1
    @./pidctl state.b run 2
    @./pidctl state.b run 3
    @./pidctl state.b run 4
    @./pidctl state.b run 5
    @./pidctl state.b run 6
    @./pidctl state.b run 5
    @./pidctl state.b run 4
    @./pidctl state.b run 3
    @./pidctl state.b run 2
    @./pidctl state.b run 1
    @./pidctl state.b run 0
    @./pidctl state.b run -1
    @./pidctl state.b run -2
    @./pidctl state.b run -3
    @./pidctl state.b run -4
    @rm -f state.b

run-i: all  
    @echo "\n----I PART ONLY----------------------------"
    @./pidctl state.b init 1000 0.0 0.5 0.0 0.0 10 10
    @echo "---------------------------------------------"
    @./pidctl state.b run 1
    @./pidctl state.b run 2
    @./pidctl state.b run 3
    @./pidctl state.b run 4
    @./pidctl state.b run 5
    @./pidctl state.b run 6
    @./pidctl state.b run 5
    @./pidctl state.b run 5
    @./pidctl state.b run 5
    @./pidctl state.b run 5
    @./pidctl state.b run 5
    @./pidctl state.b run 5
    @./pidctl state.b run -5
    @./pidctl state.b run -5
    @./pidctl state.b run -4
    @./pidctl state.b run -3
    @./pidctl state.b run -2
    @./pidctl state.b run -1
    @./pidctl state.b run -1
    @./pidctl state.b run 0

run-d: all  
    @echo "\n----D PART ONLY----------------------------"
    @./pidctl state.b init 1000 0.0 0.0 1.0 0.0 0.0 10
    @echo "---------------------------------------------"
    @./pidctl state.b run 1
    @./pidctl state.b run 2
    @./pidctl state.b run 3
    @./pidctl state.b run 4
    @./pidctl state.b run 5
    @./pidctl state.b run 5
    @./pidctl state.b run 5
    @./pidctl state.b run 5
    @./pidctl state.b run 0
    @./pidctl state.b run -1
    @./pidctl state.b run 1
    @./pidctl state.b run -2
    @./pidctl state.b run 3
    @./pidctl state.b run -4
    @./pidctl state.b run 5
    @./pidctl state.b run -6
    @./pidctl state.b run 7
    @./pidctl state.b run -8
    @./pidctl state.b run 9
    @./pidctl state.b run -10
    @./pidctl state.b run 10

Beispiel-Ausgabe

Example program output

stfwi$ make run

---- full PID -----------------------------------
t            :  0.000
integrator   :  0.000
last error   :  0.000

Config
 sample rate : 1000.000
 P           :  0.700
 I           :  0.100
 D           :  0.100
 OFFSET      :  0.000
 SATURATION  : 10.000
 I-SATURATION:  2.500
-------------------------------------------------
0.001:  +3.000 ->  +3.150 | I= +0.300 | D= +0.750
0.002:  +5.000 ->  +4.800 | I= +0.800 | D= +0.500
0.003:  +5.000 ->  +4.800 | I= +1.300 | D= +0.000
0.004:  +5.000 ->  +5.300 | I= +1.800 | D= +0.000
0.005:  +5.000 ->  +5.800 | I= +2.300 | D= +0.000
0.006:  +5.000 ->  +6.000 | I= +2.500 | D= +0.000
0.007:  +5.000 ->  +6.000 | I= +2.500 | D= +0.000
0.008:  +7.000 ->  +7.900 | I= +2.500 | D= +0.500
0.009:  +7.000 ->  +7.400 | I= +2.500 | D= +0.000
0.010:  +7.000 ->  +7.400 | I= +2.500 | D= +0.000
0.011:  +7.000 ->  +7.400 | I= +2.500 | D= +0.000
0.012:  +6.000 ->  +6.450 | I= +2.500 | D= -0.250
0.013:  +5.000 ->  +5.750 | I= +2.500 | D= -0.250
0.014:  +4.000 ->  +5.050 | I= +2.500 | D= -0.250
0.015:  +3.000 ->  +4.350 | I= +2.500 | D= -0.250
0.016:  +2.000 ->  +3.650 | I= +2.500 | D= -0.250
0.017:  +1.000 ->  +2.950 | I= +2.500 | D= -0.250
0.018:  +0.000 ->  +2.250 | I= +2.500 | D= -0.250
0.019:  +0.000 ->  +2.500 | I= +2.500 | D= +0.000
0.020:  -1.000 ->  +1.450 | I= +2.400 | D= -0.250
0.021:  -2.000 ->  +0.550 | I= +2.200 | D= -0.250
0.022:  -4.000 ->  -1.500 | I= +1.800 | D= -0.500
0.023:  -3.000 ->  -0.350 | I= +1.500 | D= +0.250
0.024:  -2.000 ->  +0.150 | I= +1.300 | D= +0.250
0.025:  -2.000 ->  -0.300 | I= +1.100 | D= +0.000
0.026:  -2.000 ->  -0.500 | I= +0.900 | D= +0.000
0.027:  -2.000 ->  -0.700 | I= +0.700 | D= +0.000
0.028:  -2.000 ->  -0.900 | I= +0.500 | D= +0.000
0.029:  -2.000 ->  -1.100 | I= +0.300 | D= +0.000
0.030:  -2.000 ->  -1.300 | I= +0.100 | D= +0.000
0.031:  -2.000 ->  -1.500 | I= -0.100 | D= +0.000
0.032:  -2.000 ->  -1.700 | I= -0.300 | D= +0.000
0.033:  -2.000 ->  -1.900 | I= -0.500 | D= +0.000
0.034:  -1.000 ->  -1.050 | I= -0.600 | D= +0.250
0.035: +10.000 -> +10.000 | I= +0.400 | D= +2.750
0.036: -10.000 -> -10.000 | I= -0.600 | D= -5.000
0.037: +10.000 -> +10.000 | I= +0.400 | D= +5.000
0.038: -10.000 -> -10.000 | I= -0.600 | D= -5.000
0.039: +10.000 -> +10.000 | I= +0.400 | D= +5.000
0.040: -10.000 -> -10.000 | I= -0.600 | D= -5.000
0.041:  +0.000 ->  +1.900 | I= -0.600 | D= +2.500
-------------------------------------------------

Beispielprogramm für Disassembly

Example program used for disassembly

/**
 * @file main-min.c
 * @author stfwi
 *
 * Small main program used for disassembly purposes.
 *
 */
#define pidctl_n_t short
#include "pid_ctrl.h"
 
/**
 * ! NOTE these volatile variables simulate i/o
 * registers. They is not related to a specific
 * microcontroller !
 */
volatile unsigned short adc0;       // let's say 10 bit
volatile unsigned short adc1;       // let's say 10 bit
volatile unsigned short pwm0;       // let's say 10 bit
#define dint()
#define eint()
 
/**
 * Our PID structure
 */
static pidctl_t pid;
 
/**
 * Such routines are normally defined with
 * __naked and __interrupt. We just pretend
 * to have them
 */
void pwm_timer_interrupt_service_routine()
{
  dint();
  // This is what we like to do:
  // ADC1 shall be our reference (e.g. potentioneter)
  // ADC0 shall be our measured value (e.g. a current)
  // PWM0 shall be our output channel to change the current
  // pid  is the structure we defined before
  unsigned short o;
  unsigned short e = adc0 - adc1;
 
  // To see the lines in the disassembly the macro
  // is extracted here
  pid.i += pid.Ki * e;
  if(pid.i > pid.i_saturation) {
    pid.i = pid.i_saturation;
  } else if(pid.i < -pid.i_saturation) {
    pid.i = -pid.i_saturation;
  }
  o = e - pid.e_last;
  o *= pid.Kd;
  o += pid.Kp * e;
  o += pid.i;
  o += pid.offset;
  if(o > pid.saturation) {
    o = pid.saturation;
  } else if(o < -pid.saturation) {
    o = -pid.saturation;
  }
  pwm0 = o;
  eint();
}
 
 
int main()
{
  short i;
 
  // Dummy PID settings
  pid.Kp = 10;
  pid.Kd = 3;
  pid.Ki = 1;
  pid.offset = 0;
  pid.saturation = 100;
  pid.i_saturation = 25;
  pidctl_reset(pid, 0);
 
  while(1) {
    // That is normally nonsense, but we call
    // this function here so that the compiler
    // dies not optimise it away. Otherwise we
    // don't see much in the disassembly.
    pwm_timer_interrupt_service_routine();
  }
 
  return 0;
}

Disassembly

Main-min.c wurde mit GCC für drei Architekturen disassembliert: amd64, arm und 8-bit AVR (Atmel-Controller). Die Ausgabe habe ich auf das Wesentlicht reduziert, damit die ungefähre Anzahl an Instructions ersichtlich wird. Als Datentyp für den Regler wurde int16 (short) verwendet.

Disassembly

The disassembly was done using GCC for three architectures: amd64, arm, and 8-bit AVR (Atmel). I stripped the disassembled files to the bare asm codes to give you the gist how much instructions the CPU will roughly need to calculate it. As data type short (int16) was used.

stfwi$  gcc -c -O3 -g -Wall -Wa,-a,-ad main-min.c
 
[...]
 
movzwl  adc0(%rip), %edx
movzwl  adc1(%rip), %eax
movzwl  pid+10(%rip), %ecx
subw    %ax, %dx
movl    %edx, %eax
imulw   pid+2(%rip), %ax
addw    pid+12(%rip), %ax
cmpw    %cx, %ax
movw    %ax, pid+12(%rip)
jle .L2
movw    %cx, pid+12(%rip)
movl    %ecx, %eax
movl    %edx, %ecx
subw    pid+14(%rip), %cx
imulw   pid(%rip), %dx
imulw   pid+4(%rip), %cx
addw    pid+6(%rip), %dx
addl    %ecx, %edx
addl    %edx, %eax
movzwl  pid+8(%rip), %edx
movzwl  %ax, %esi
movswl  %dx, %ecx
cmpl    %ecx, %esi
cmovg   %edx, %eax
movw    %ax, pwm0(%rip)
ret
.L2:
movswl  %cx, %esi
movswl  %ax, %edi
negl    %esi
cmpl    %esi, %edi
jge .L3
movl    %ecx, %eax
negl    %eax
movw    %ax, pid+12(%rip)
jmp .L3
 
[...]

stfwi$ arm-bcm2708hardfp-linux-gnueabi-gcc -c -O3 -g -Wa,-a,-ad main-min.c
 
[...]
 
ldr r3, .L6
ldr r2, .L6+4
ldrh    r1, [r3, #0]
ldr r3, .L6+8
ldrh    r2, [r2, #0]
ldrh    r0, [r3, #2]
rsb r1, r2, r1
ldrh    r2, [r3, #12]
uxth    r1, r1
mla r2, r0, r1, r2
ldrh    r0, [r3, #10]
uxth    r2, r2
str r4, [sp, #-4]!
sxth    ip, r2
sxth    r4, r0
cmp ip, r4
strh    r2, [r3, #12]   @ movhi
strgth  r0, [r3, #12]   @ movhi
movgt   r2, r0
bgt .L3
rsb r4, r4, #0
cmp ip, r4
rsblt   r2, r0, #0
uxthlt  r2, r2
strlth  r2, [r3, #12]   @ movhi
.L3:
ldrh    ip, [r3, #0]
ldrh    r0, [r3, #6]
ldrh    r4, [r3, #14]
mla r0, ip, r1, r0
ldrh    ip, [r3, #4]
rsb r1, r4, r1
mla r1, r1, ip, r0
ldrh    r3, [r3, #8]
uxtah   r2, r2, r1
uxth    r2, r2
sxth    r1, r3
cmp r2, r1
uxthgt  r2, r3
ldr r3, .L6+12
strh    r2, [r3, #0]    @ movhi
 
[...]

stfwi$ avr-gcc -mmcu=atmega169 -c -O3 -g -Wa,-a,-ad main-min.c
 
[...]
 
lds r20,adc0
lds r21,adc0+1
lds r24,adc1
lds r25,adc1+1
sub r20,r24
sbc r21,r25
lds r24,pid+2
lds r25,pid+2+1
mul r20,r24
movw r18,r0
mul r20,r25
add r19,r0
mul r21,r24
add r19,r0
clr r1
lds r24,pid+12
lds r25,pid+12+1
add r18,r24
adc r19,r25
sts pid+12+1,r19
sts pid+12,r18
lds r24,pid+10
lds r25,pid+10+1
cp r24,r18
cpc r25,r19
brge .+2
rjmp .L6
com r25
neg r24
sbci r25,lo8(-1)
cp r18,r24
cpc r19,r25
brlt .L6
.L3:
lds r24,pid+14
lds r25,pid+14+1
movw r30,r20
sub r30,r24
sbc r31,r25
lds r24,pid+4
lds r25,pid+4+1
mul r30,r24
movw r22,r0
mul r30,r25
add r23,r0
mul r31,r24
add r23,r0
clr r1
lds r30,pid
lds r31,pid+1
mul r20,r30
movw r24,r0
mul r20,r31
add r25,r0
mul r21,r30
add r25,r0
clr r1
lds r20,pid+6
lds r21,pid+6+1
add r24,r20
adc r25,r21
add r24,r22
adc r25,r23
add r24,r18
adc r25,r19
lds r18,pid+8
lds r19,pid+8+1
cp r18,r24
cpc r19,r25
brlo .L4
clr r20
clr r21
sub r20,r18
sbc r21,r19
movw r18,r24
cp r24,r20
cpc r25,r21
brlo .L8
.L4:
sts pwm0+1,r19
sts pwm0,r18
ret
.L6:
sts pid+12+1,r25
sts pid+12,r24
movw r18,r24
rjmp .L3
.L8:
movw r18,r20
sts pwm0+1,r19
sts pwm0,r18
ret
 
[...]