I had a look to Brad wii code.
Its code is very well documented, very clean and welle organized. easy to understand, even for a poor programmer like me.
A lot of thing have evolved between multiwii and bradwii, and i am not sure it is doable to port quickly his autotuning function:
Code: Select all
Index: src/MultiWii/lib_fp.h
===================================================================
--- src/MultiWii/lib_fp.h (revision 0)
+++ src/MultiWii/lib_fp.h (revision 0)
@@ -0,0 +1,77 @@
+#ifndef LIB_FP_H_
+#define LIB_FP_H_
+/*
+Copyright 2013 Brad Quick
+
+THIS LIBRARY IS DIRECTLY IMPORTED FROM BRADQUICK CODE:
+https://github.com/bradquick/bradwii_arduino/blob/master/bradwii/lib_fp.cpp
+revision date July 11, 2013
+
+This program is free software: you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+any later version.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with this program. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+
+// a fixedpointnum is a real number that's stored in a long that's shifted left FIXEDPOINTSHIFT bits
+// FIXEDPOINTSHIFT is currently 16, so 16 bits are used for the integer part of the number and
+// the other 16 bits are used for the fractional part of the number. I've tried using different
+// values, but the compliler likes shifting things by 16 bits (words at a time) so things run
+// faster with a FIXEDPOINTSHIFT of 16.
+
+// Therefore, the range of a fixedpointnum is -32768.0 to 32786.0 with an accuracy of 0.000015
+// There is no overflow or underflow protection, so it's up to the programmer to watch out.
+
+#define fixedpointnum long
+
+#define FIXEDPOINTSHIFT 16
+
+#define FIXEDPOINTONE (1L<<FIXEDPOINTSHIFT)
+
+#define FIXEDPOINTCONSTANT(number) ((fixedpointnum)(number*FIXEDPOINTONE))
+
+#define FIXEDPOINT45 (45L<<FIXEDPOINTSHIFT)
+#define FIXEDPOINT80 (80L<<FIXEDPOINTSHIFT)
+#define FIXEDPOINT90 (90L<<FIXEDPOINTSHIFT)
+#define FIXEDPOINT135 (135L<<FIXEDPOINTSHIFT)
+#define FIXEDPOINT180 (180L<<FIXEDPOINTSHIFT)
+#define FIXEDPOINT360 (360L<<FIXEDPOINTSHIFT)
+#define FIXEDPOINTTWO (2L<<FIXEDPOINTSHIFT)
+#define FIXEDPOINTTHREE (3L<<FIXEDPOINTSHIFT)
+#define FIXEDPOINTONEOVERONE FIXEDPOINTONE
+#define FIXEDPOINTONEOVERONEHALF FIXEDPOINTTWO
+#define FIXEDPOINTONEOVERTWO (FIXEDPOINTONE>>1)
+#define FIXEDPOINTONEOVERFOUR (FIXEDPOINTONE>>2)
+#define FIXEDPOINTONEOVERONEFOURTH (FIXEDPOINTONE<<2)
+#define FIXEDPOINTONEOVERONESIXTEENTH (FIXEDPOINTONE<<4)
+#define FIXEDPOINTONEOVERONESIXTYITH (60L<<FIXEDPOINTSHIFT)
+#define FIXEDPOINTONEFIFTIETH ((1L<<FIXEDPOINTSHIFT)/50)
+
+#define FIXEDPOINTPIOVER180 1144L // pi/180 for converting degrees to radians
+
+// since time slivers can be very small, we shift them an extra 8 bits to maintain accuracy
+#define TIMESLIVEREXTRASHIFT 8
+
+void lib_fp_constrain(fixedpointnum *lf,fixedpointnum low,fixedpointnum high);
+void lib_fp_constrain180(fixedpointnum *lf);
+fixedpointnum lib_fp_multiply(fixedpointnum x,fixedpointnum y);
+void lib_fp_lowpassfilter(fixedpointnum *variable,fixedpointnum newvalue,fixedpointnum timesliver,fixedpointnum oneoverperiod,int timesliverextrashift);
+fixedpointnum lib_fp_abs(fixedpointnum fp);
+fixedpointnum lib_fp_sine(fixedpointnum angle);
+fixedpointnum lib_fp_cosine(fixedpointnum angle);
+fixedpointnum lib_fp_atan2(fixedpointnum y, fixedpointnum x);
+fixedpointnum lib_fp_sqrt(fixedpointnum x);
+fixedpointnum lib_fp_stringtofixedpointnum(char *string);
+fixedpointnum lib_fp_invsqrt(fixedpointnum x);
+long lib_fp_stringtolong(char *string);
+
+#endif /* LIB_FP_H_ */
Index: src/MultiWii/config.h
===================================================================
--- src/MultiWii/config.h (revision 1538)
+++ src/MultiWii/config.h (working copy)
@@ -1079,6 +1079,14 @@
/* disable use of the POWER PIN (allready done if the option RCAUXPIN12 is selected) */
#define DISABLE_POWER_PIN
+// un-comment if you don't want to include autotune code
+//#define NO_AUTOTUNE
+// To adjust how agressive the tuning is, adjust the AUTOTUNEMAXOSCILLATION value. A larger
+// value will result in more agressive tuning. A lower value will result in softer tuning.
+// It will rock back and forth between -AUTOTUNE_TARGET_ANGLE and AUTOTUNE_TARGET_ANGLE degrees
+#define AUTOTUNE_MAX_OSCILLATION 4.0
+#define AUTOTUNE_TARGET_ANGLE 20.0
+
/*************************************************************************************************/
/**** END OF CONFIGURABLE PARAMETERS ****/
Index: src/MultiWii/autotune.cpp
===================================================================
--- src/MultiWii/autotune.cpp (revision 0)
+++ src/MultiWii/autotune.cpp (revision 0)
@@ -0,0 +1,195 @@
+/*
+Copyright 2013 Brad Quick
+
+THIS LIBRARY IS PORTED FROM BRADQUICK CODE:
+https://github.com/bradquick/bradwii_arduino/blob/master/bradwii/autotune.cpp
+revision date July 11, 2013
+
+This program is free software: you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+any later version.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with this program. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+// The theory behind auto tuning is that we generate a step in the setpoint, then analyze how the system reacts.
+// First, we set the I term to zero and work on only the P and D terms.
+// The target angle alternates from negative to posive FPAUTOTUNETARGETANGLE degrees. We set the target, then wait until
+// we cross the zero angle, just to make sure our angular velocity is in the right direction, then we look for the peak
+// which we recognize by observing the angular velocity switching direction.
+// Our goal is to get zero overshoot. By requiring zero overshoot, we can assume that any subsequent oscillations are
+// caused by our D term, since at zero overshoot, the P term doesn't have any effect. Once we see the first peak, we then
+// start a timer and look for a second peak in the negative direction. The difference between the first and second peak will
+// tell us the effect the D term is having. We will then try to keep this oscillation amplitude within a predeterrmined range.
+//
+// So, after we reach the target, then wait a little longer to record the peak oscillation, we have four choices:
+// if (overshoot)
+// {
+// if (too much oscillation) decrease P
+// else increase D
+// }
+// else // undershoot
+// {
+// if (too much oscillation) decrease D
+// else increase P
+// }
+
+
+#include "config.h"
+//#include "bradwii.h"
+#include "eeprom.h"
+#include "autotune.h"
+
+//extern globalstruct global;
+//extern usersettingsstruct usersettings;
+
+#ifndef NO_AUTOTUNE
+
+#define FPAUTOTUNEMAXOSCILLATION FIXEDPOINTCONSTANT(AUTOTUNE_MAX_OSCILLATION)
+#define FPAUTOTUNETARGETANGLE FIXEDPOINTCONSTANT(AUTOTUNE_TARGET_ANGLE)
+
+unsigned char rising;
+unsigned char sawfirstpeak;
+int autotuneindex=ROLL;
+fixedpointnum autotunetime;
+fixedpointnum autotunepeak1;
+fixedpointnum autotunepeak2;
+fixedpointnum targetangle=0;
+fixedpointnum targetangleatpeak;
+fixedpointnum currentivalue;
+
+char cyclecount=1;
+
+void autotune(fixedpointnum *angleerror,unsigned char startingorstopping)
+ {
+ if (!f.ARMED)
+ {
+ // we aren't armed. Don't do anything, but if autotuning is started and we have collected
+ // autotuning data, save our settings to eeprom
+ if (startingorstopping==AUTOTUNESTARTING && targetangle!=0)
+ writeusersettingstoeeprom(); // TODO: COMPLETE REWRITING....
+
+ return;
+ }
+
+ if (startingorstopping==AUTOTUNESTOPPING)
+ {
+ conf.pid[autotuneindex].I8=currentivalue;
+
+ conf.pid[YAW].I8=conf.pid[ROLL].I8;
+ conf.pid[YAW].D8=conf.pid[ROLL].D8;
+ conf.pid[YAW].P8=lib_fp_multiply(conf.pid[ROLL].P8,YAWGAINMULTIPLIER);
+
+ autotuneindex=!autotuneindex; // alternate between roll and pitch
+ return;
+ }
+
+ if (startingorstopping==AUTOTUNESTARTING)
+ {
+ currentivalue=conf.pid[autotuneindex].I8;
+ conf.pid[autotuneindex].I8=0;
+ cyclecount=1;
+ sawfirstpeak=0;
+ rising=0;
+ targetangle=-FPAUTOTUNETARGETANGLE;
+ }
+ else
+ {
+ if (!sawfirstpeak)
+ {
+ if ((rising && att.angle[autotuneindex]>0 && global.gyrorate[autotuneindex]<0) //TO CHECK : att.angle[autotuneindex] a value in degrees is expected... However not 100% sure it is the case. To be checked.
+ || (!rising && att.angle[autotuneindex]<0 && global.gyrorate[autotuneindex]>0)) // TODO : global.gyrorate[] : should be angular speed in degree/second
+ {
+ if (cyclecount==0) // we are checking the I value
+ {
+ autotunepeak1=att.angle[autotuneindex]-targetangle;
+ if (!rising) autotunepeak1=-autotunepeak1;
+
+ if (autotunepeak1<(FPAUTOTUNEMAXOSCILLATION>>1))
+ currentivalue=lib_fp_multiply(currentivalue,AUTOTUNEINCREASEMULTIPLIER);
+ else
+ {
+ currentivalue=lib_fp_multiply(currentivalue,AUTOTUNEDECREASEMULTIPLIER);
+ if (currentivalue<AUTOTUNEMINIMUMIVALUE) currentivalue=AUTOTUNEMINIMUMIVALUE;
+ }
+
+ // go back to checking P and D
+ cyclecount=1;
+ rising=!rising;
+ }
+ else // we are checking P and D values
+ {
+ targetangleatpeak=targetangle;
+ autotunepeak2=autotunepeak1=att.angle[autotuneindex];
+
+ sawfirstpeak=1;
+ autotunetime=0;
+ conf.pid[autotuneindex].I8=0;
+ }
+ }
+ }
+ else // we saw the first peak. looking for the second
+ {
+ if ((rising && att.angle[autotuneindex]<autotunepeak2)
+ || (!rising && att.angle[autotuneindex]>autotunepeak2))
+ autotunepeak2=att.angle[autotuneindex];
+
+ autotunetime+=(cycleTime*4295L)>>(FIXEDPOINTSHIFT-TIMESLIVEREXTRASHIFT);
+
+ if (autotunetime>AUTOTUNESETTLINGTIME)
+ {
+ if (!rising)
+ {
+ autotunepeak1=-autotunepeak1;
+ autotunepeak2=-autotunepeak2;
+ targetangleatpeak=-targetangleatpeak;
+ }
+
+ fixedpointnum oscillationamplitude=autotunepeak1-autotunepeak2;
+
+ // analyze the data
+ // Our goal is to have zero overshoot and to have AUTOTUNEMAXOSCILLATION amplitude
+ if (autotunepeak1>targetangleatpeak) // overshoot
+ {
+ if (oscillationamplitude>FPAUTOTUNEMAXOSCILLATION) // we have too much oscillation, so we can't increase D, so decrease P
+ conf.pid[autotuneindex].P8=lib_fp_multiply(conf.pid[autotuneindex].P8, AUTOTUNEDECREASEMULTIPLIER);
+ else // we don't have too much oscillation, so we can increase D
+ conf.pid[autotuneindex].D8=lib_fp_multiply(conf.pid[autotuneindex].D8, AUTOTUNEINCREASEMULTIPLIER);
+ }
+ else // undershoot
+ {
+ if (oscillationamplitude>FPAUTOTUNEMAXOSCILLATION) // we have too much oscillation, so we should lower D
+ conf.pid[autotuneindex].D8=lib_fp_multiply(conf.pid[autotuneindex].D8, AUTOTUNEDECREASEMULTIPLIER);
+ else // we don't have too much oscillation, so we increase P
+ conf.pid[autotuneindex].P8=lib_fp_multiply(conf.pid[autotuneindex].P8, AUTOTUNEINCREASEMULTIPLIER);
+ }
+
+ // switch to the other direction and start a new cycle
+ rising=!rising;
+ sawfirstpeak=0;
+
+ if (++cyclecount==3)
+ { // switch to testing I value
+ cyclecount=0;
+
+ conf.pid[autotuneindex].I8=currentivalue;
+ }
+ }
+ }
+ }
+
+ if (rising) targetangle=global.rxvalues[autotuneindex]*20L+FPAUTOTUNETARGETANGLE; // TODO : global.rxvalues[autotuneindex] is The values of the RX inputs, ranging from -1.0 to 1.0
+ else targetangle=global.rxvalues[autotuneindex]*20L-FPAUTOTUNETARGETANGLE;
+
+ // override the angle error for the axis we are tuning
+ angleerror[autotuneindex]=targetangle-att.angle[autotuneindex];
+ }
+
+#endif
Index: src/MultiWii/autotune.h
===================================================================
--- src/MultiWii/autotune.h (revision 0)
+++ src/MultiWii/autotune.h (revision 0)
@@ -0,0 +1,48 @@
+#ifndef AUTOTUNE_H_
+#define AUTOTUNE_H_
+
+/*
+Copyright 2013 Brad Quick
+
+THIS LIBRARY IS PORTED FROM BRADQUICK CODE:
+https://github.com/bradquick/bradwii_arduino/blob/master/bradwii/autotune.h
+revision date July 11, 2013
+
+This program is free software: you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+any later version.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with this program. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+
+#include "lib_fp.h"
+
+
+#define AUTOTUNESTARTING 1
+#define AUTOTUNESTOPPING 2
+#define AUTOTUNETUNING 0
+
+#define AUTOTUNEGOINGTOWARDTARGET 0
+#define AUTOTUNEGOINGAWAYFROMTARGET 1
+
+#define AUTOTUNESETTLINGTIME (FIXEDPOINTCONSTANT(.25)<<TIMESLIVEREXTRASHIFT) // 1/4 second
+
+#define AUTOTUNEINCREASEMULTIPLIER FIXEDPOINTCONSTANT(1.05)
+#define AUTOTUNEDECREASEMULTIPLIER FIXEDPOINTCONSTANT(.95)
+#define AUTOTUNEMINIMUMIVALUE (1L<<3)
+
+#define YAWGAINMULTIPLIER FIXEDPOINTCONSTANT(2.0) // yaw p gain is simply set to a multiple of the roll p gain
+
+void autotune(fixedpointnum *angleerror,unsigned char startingorstopping);
+
+
+
+#endif /* AUTOTUNE_H_ */
Index: src/MultiWii/lib_fp.cpp
===================================================================
--- src/MultiWii/lib_fp.cpp (revision 0)
+++ src/MultiWii/lib_fp.cpp (revision 0)
@@ -0,0 +1,444 @@
+/*
+Copyright 2013 Brad Quick
+
+THIS LIBRARY IS DIRECTLY IMPORTED FROM BRADQUICK CODE:
+https://github.com/bradquick/bradwii_arduino/blob/master/bradwii/lib_fp.cpp
+revision date July 11, 2013
+
+
+This program is free software: you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+any later version.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with this program. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+
+#include "lib_fp.h"
+
+void lib_fp_constrain(fixedpointnum *lf,fixedpointnum low,fixedpointnum high)
+ {
+ if (*lf<low) *lf=low;
+ else if (*lf>high) *lf=high;
+ }
+
+// returns the equivilent angle within the range -180 and 180 degrees
+void lib_fp_constrain180(fixedpointnum *lf)
+ {
+ while (*lf<-FIXEDPOINT180) *lf+=FIXEDPOINT360;
+ while (*lf>FIXEDPOINT180) *lf-=FIXEDPOINT360;
+ }
+
+// D1 C1 B1 A1 (x)
+// x D2 C2 B2 A2 (y)
+// -----------------------
+// z0 y0 D0 C0 B0 A0 x0 w0
+
+
+// A2*A1 -> w0,x0 (skip)
+// A2*B1 -> x0,A0 (unsigned, unsigned)
+// A2*C1 -> A0,B0 (unsigned, unsigned) // do first
+// A2*D1 -> B0,C0 (unsigned, signed)
+// B2*A1 -> x0,A0 (unsigned, unsigned)
+// B2*B1 -> A0,B0 (unsigned, unsigned)
+// B2*C1 -> B0,C0 (unsigned, unsigned)
+// B2*D1 -> C0,D0 (unsigned, signed)
+// C2*A1 -> A0,B0 (unsigned, unsigned)
+// C2*B1 -> B0,C0 (unsigned, unsigned)
+// C2*C1 -> C0,D0 (unsigned, unsigned) // do second
+// C2*D1 -> D0,y0 (unsigned, signed)
+// D2*A1 -> B0,C0 (signed, unsigned)
+// D2*B1 -> C0,D0 (signed, unsigned)
+// D2*C1 -> D0,y0 (signed, unsigned)
+// D2*D1 -> y0,z0 (skip)
+
+
+// A2*C1 -> A0,B0 (unsigned, unsigned) // do first
+// C2*C1 -> C0,D0 (unsigned, unsigned) // do second
+// B2*C1 -> B0,C0 (unsigned, unsigned)
+// D2*C1 -> D0,y0 (signed, unsigned)
+// B2*A1 -> x0,A0 (unsigned, unsigned)
+// C2*A1 -> A0,B0 (unsigned, unsigned)
+// D2*A1 -> B0,C0 (signed, unsigned)
+// A2*B1 -> x0,A0 (unsigned, unsigned)
+// B2*B1 -> A0,B0 (unsigned, unsigned)
+// C2*B1 -> B0,C0 (unsigned, unsigned)
+// D2*B1 -> C0,D0 (signed, unsigned)
+// A2*D1 -> B0,C0 (unsigned, signed)
+// B2*D1 -> C0,D0 (unsigned, signed)
+// C2*D1 -> D0,y0 (unsigned, signed)
+
+//#define FPPROCESSORDOESNTSUPPORTMULSU
+#ifndef FPPROCESSORDOESNTSUPPORTMULSU
+
+fixedpointnum lib_fp_multiply(fixedpointnum x,fixedpointnum y)
+ { // multiplies two fixed point numbers without overflowing and returns the result.
+ // doing this in assembly increased the speed tremendously.
+ fixedpointnum result;
+
+ asm volatile (
+ "clr r26 \n\t"
+ "mov r16,%C1 \n\t"
+ "mul %A2, r16 \n\t" // A2*C1 -> A0,B0 (unsigned, unsigned) // do first
+ "movw %A0, r0 \n\t"
+ "mul %C2, r16 \n\t" // C2*C1 -> C0,D0 (unsigned, unsigned) // do second
+ "movw %C0, r0 \n\t"
+ "mul %B2, r16 \n\t" // B2*C1 -> B0,C0 (unsigned, unsigned)
+ "add %B0, r0 \n\t"
+ "adc %C0, r1 \n\t"
+ "adc %D0, r26 \n\t"
+ "mulsu %D2, r16 \n\t" // D2*C1 -> D0,y0 (signed, unsigned)
+ "add %D0, r0 \n\t"
+ "mov r16,%A1 \n\t"
+ "mul %B2, r16 \n\t" // B2*A1 -> x0,A0 (unsigned, unsigned)
+ "add %A0, r1 \n\t"
+ "adc %B0, r26 \n\t"
+ "adc %C0, r26 \n\t"
+ "adc %D0, r26 \n\t"
+ "mul %C2, r16 \n\t" // C2*A1 -> A0,B0 (unsigned, unsigned)
+ "add %A0, r0 \n\t"
+ "adc %B0, r1 \n\t"
+ "adc %C0, r26 \n\t"
+ "adc %D0, r26 \n\t"
+ "mulsu %D2, r16 \n\t" // D2*A1 -> B0,C0 (signed, unsigned)
+ "sbc %D0, r26 \n\t"
+ "add %B0, r0 \n\t"
+ "adc %C0, r1 \n\t"
+ "adc %D0, r26 \n\t"
+ "mov r16,%B1 \n\t"
+ "mul %A2, r16 \n\t" // A2*B1 -> x0,A0 (unsigned, unsigned)
+ "add %A0, r1 \n\t"
+ "adc %B0, r26 \n\t"
+ "adc %C0, r26 \n\t"
+ "adc %D0, r26 \n\t"
+ "mul %B2, r16 \n\t" // B2*B1 -> A0,B0 (unsigned, unsigned)
+ "add %A0, r0 \n\t"
+ "adc %B0, r1 \n\t"
+ "adc %C0, r26 \n\t"
+ "adc %D0, r26 \n\t"
+ "mul %C2, r16 \n\t" // C2*B1 -> B0,C0 (unsigned, unsigned)
+ "add %B0, r0 \n\t"
+ "adc %C0, r1 \n\t"
+ "adc %D0, r26 \n\t"
+ "mulsu %D2, r16 \n\t" // D2*B1 -> C0,D0 (signed, unsigned)
+ "add %C0, r0 \n\t"
+ "adc %D0, r1 \n\t"
+ "mov r16,%D1 \n\t"
+ "mulsu r16, %A2 \n\t" // A2*D1 -> B0,C0 (unsigned, signed)
+ "sbc %D0, r26 \n\t"
+ "add %B0, r0 \n\t"
+ "adc %C0, r1 \n\t"
+ "adc %D0, r26 \n\t"
+ "mulsu r16, %B2 \n\t" // B2*D1 -> C0,D0 (unsigned, signed)
+ "add %C0, r0 \n\t"
+ "adc %D0, r1 \n\t"
+ "mulsu r16, %C2 \n\t" // C2*D1 -> D0,y0 (unsigned, signed)
+ "add %D0, r0 \n\t"
+
+ "clr r1 \n\t"
+ :
+ "=&r" (result)
+ :
+ "r" (x),
+ "a" (y)
+ :
+ "r26","r16"
+ );
+
+ return(result);
+ }
+
+#else // this will work on all processors, but it's slower
+fixedpointnum lib_fp_multiply(fixedpointnum x,fixedpointnum y)
+ { // multiplies two fixed point numbers without overflowing and returns the result
+ long xh=x>>FIXEDPOINTSHIFT;
+ unsigned long xl=x & 0xffff;
+ long yh=y>>FIXEDPOINTSHIFT;
+ unsigned long yl=y & 0xffff;
+ return((xh*yh)<<FIXEDPOINTSHIFT)+xh*yl+yh*xl+((xl*yl)>>FIXEDPOINTSHIFT);
+ }
+#endif
+
+
+void lib_fp_lowpassfilter(fixedpointnum *variable,fixedpointnum newvalue,fixedpointnum timesliver,fixedpointnum oneoverperiod,int timesliverextrashift)
+ { // updates a low pass filter variable. It uses timesliver. oneoverperiod is one over the time period
+ // over which the filter is averaged. We use oneoverperiod to avoid having to use division.
+ // this does the equivilent of:
+ // float fraction=timesliver/period;
+ // variable=fraction*newvalue+(1-fraction)*variable;
+ // except it does it using fixed point arithmatic
+ // If timesliver is very small, resolution can be gained by keeping timesliver shifted left by some extra bits.
+ // Make sure variable can be also shifted left this same number of bits or else the following will overflow!
+ fixedpointnum fraction=lib_fp_multiply(timesliver,oneoverperiod);
+
+ *variable=(lib_fp_multiply(fraction,newvalue)+lib_fp_multiply((FIXEDPOINTONE<<timesliverextrashift)-fraction,*variable))>>timesliverextrashift;
+
+ // the adder combines adding .5 for rounding error and adding .5 in the direction the newvalue is trying to pull us
+ // So we can zero in on the desired value.
+ if (newvalue>*variable) ++*variable;
+ }
+
+fixedpointnum lib_fp_abs(fixedpointnum fp)
+ {
+ if (fp<0) return(-fp);
+ else return(fp);
+ }
+
+// we may be able to make these unsigned ints and save space
+fixedpointnum biganglesinelookup[]={ // every 8 degrees
+ 0, // sine(0)
+ 9121, // sine(8)
+ 18064, // sine(16)
+ 26656, // sine(24)
+ 34729, // sine(32)
+ 42126,
+ 48703,
+ 54332,
+ 58903,
+ 62328,
+ 64540,
+ 65496,
+ };
+
+fixedpointnum biganglecosinelookup[]={ // every 8 degrees
+ 65536, // cosine(0)
+ 64898, // cosine(8)
+ 62997, // cosine(16)
+ 59870, // cosine(32)
+ 55578,
+ 50203,
+ 43852,
+ 36647,
+ 28729,
+ 20252,
+ 11380,
+ 2287
+ };
+
+fixedpointnum lib_fp_sine(fixedpointnum angle)
+ { // returns sine of angle where angle is in degrees
+ // manipulate so that we only have to work in a range from 0 to 90 degrees
+ char negate=0;
+
+ while (angle>FIXEDPOINT180)
+ angle-=FIXEDPOINT360;
+ while (angle<-FIXEDPOINT180)
+ angle+=FIXEDPOINT360;
+
+ if (angle<0)
+ {
+ angle=-angle;
+ negate=!negate;
+ }
+ if (angle>FIXEDPOINT90)
+ {
+ angle=FIXEDPOINT180-angle;
+ }
+
+ // below works from 0 to 90 degrees
+ int bigangle=angle>>(3+FIXEDPOINTSHIFT); // big angle is angle/8
+ angle-=((long)bigangle)<<(3+FIXEDPOINTSHIFT); // the small angle remainder
+
+ // sine(x+y)=cosine(x)*sine(y)+sine(x)*cosine(y);
+ // for small angles, sine(x)=x where x is in radians
+ // and cosine of x=1 for small angles
+ fixedpointnum result=lib_fp_multiply(biganglecosinelookup[bigangle],(angle*143)>>13)+biganglesinelookup[bigangle];
+ if (negate) result=-result;
+ return(result);
+ }
+
+fixedpointnum lib_fp_cosine(fixedpointnum angle)
+ {
+ return(lib_fp_sine(angle+FIXEDPOINT90));
+ }
+
+fixedpointnum atanlist[]=
+ {
+ 2949120L, // atan(2^-i), in fixedpointnum
+ 1740967L,
+ 919879L,
+ 466945L,
+ 234379L,
+ 117304L,
+ 58666L,
+ 29335L,
+ 14668L,
+ 7334L,
+ 3667L,
+ 1833L,
+ 917L
+ };
+
+// cordic arctan2 using no division!
+// http://www.coranac.com/documents/arctangent/
+
+fixedpointnum lib_fp_atan2(fixedpointnum y, fixedpointnum x)
+ { // returns angle from -180 to 180 degrees
+ if (y==0) return (x>=0 ? 0 : FIXEDPOINT180);
+
+ fixedpointnum returnvalue=0;
+ fixedpointnum tmp;
+
+ if (y< 0)
+ {
+ x= -x;
+ y= -y;
+ returnvalue -= FIXEDPOINT180;
+ }
+ if (x<= 0)
+ {
+ tmp= x;
+ x= y;
+ y= -tmp;
+ returnvalue += FIXEDPOINT90;
+ }
+ if (x<=y)
+ {
+ tmp=y-x;
+ x= x+y;
+ y= tmp;
+ returnvalue += FIXEDPOINT45;
+ }
+
+ // Scale up a bit for greater accuracy.
+ if (x < 0x10000)
+ {
+ x *= 0x1000;
+ y *= 0x1000;
+ }
+
+ for (int i=1; i<11; i++) // increase the accuracy by increasing the number of cycles (was 12)
+ {
+ if (y>=0)
+ {
+ tmp= x + (y>>i);
+ y = y - (x>>i);
+ x = tmp;
+ returnvalue += atanlist[i];
+ }
+ else
+ {
+ tmp= x - (y>>i);
+ y = y + (x>>i);
+ x = tmp;
+ returnvalue -= atanlist[i];
+ }
+ }
+ return (returnvalue);
+ }
+
+fixedpointnum lib_fp_sqrt(fixedpointnum x)
+ {
+ return(lib_fp_multiply(x, lib_fp_invsqrt(x)));
+ }
+
+long lib_fp_stringtolong(char *string)
+ { // converts a string to an integer. Stops at any character other than 0-9 or '-'. Stops at decimal places too.
+ long value=0;
+ char negative=0;
+ if (*string=='-')
+ {
+ negative=1;
+ ++string;
+ }
+ while (*string>='0' && *string<='9')
+ {
+ value=value*10+*string++-'0';
+ }
+ if (negative) return(-value);
+ else return(value);
+ }
+
+fixedpointnum lib_fp_stringtofixedpointnum(char *string)
+ {
+ char negative=0;
+ if (*string=='-')
+ {
+ negative=1;
+ ++string;
+ }
+
+ fixedpointnum value=(long)lib_fp_stringtolong(string)<<FIXEDPOINTSHIFT;
+
+ // find the decimal place
+ while (*string !='\0' && *string !='.') ++string;
+ if (*string=='\0') return(value);
+
+ // use six digits after the decimal
+ ++string;
+ int fractionallength=0;
+ while (string[fractionallength]>='0' && string[fractionallength]<='9') ++fractionallength;
+
+ if (fractionallength>6) string[6]='\0';
+ fixedpointnum fractional=lib_fp_stringtolong(string);
+ while (fractionallength++<6)
+ {
+ fractional*=10;
+ }
+ value=value+(lib_fp_multiply(fractional,137439L)>>5); // 137439L is .000001 * 2^16 * 2^16 * 2^5
+
+ if (negative) return(-value);
+ else return(value);
+ }
+
+fixedpointnum invsqrtstartingpoint[12]=
+ { // =(1/((x+4.5)/16)^0.5)
+ // add 4 because range of xysquared starts at .25.
+ // add .5 to get a value in the middle of the range.
+ // 16 is the number of steps we break down into (hence the shift by 4 when calling)
+ 123575L,
+ 111778L,
+ 102821L,
+ 95721L,
+ 89914L,
+ 85050L,
+ 80899L,
+ 77302L,
+ 74145L,
+ 71346L,
+ 68842L,
+ 66584L
+ };
+
+fixedpointnum lib_fp_invsqrt(fixedpointnum x)
+ {
+ if (x<=0) return(0);
+
+ fixedpointnum y=FIXEDPOINTONE;
+ fixedpointnum xysquared=x;
+
+ // find the starting point
+ while (xysquared<FIXEDPOINTONEOVERFOUR)
+ {
+ y=y<<1;
+ xysquared=xysquared<<2;
+ }
+ while (xysquared>=FIXEDPOINTONE)
+ {
+ y=y>>1;
+ xysquared=xysquared>>2;
+ }
+
+ // at this point .25 <= xysquared < 1
+ // the idea is to get xysquared as close to 1 as possible. Start by using
+ // the lookup table.
+
+ y=lib_fp_multiply(y,invsqrtstartingpoint[(xysquared>>(FIXEDPOINTSHIFT-4))-4]);
+
+ // now use newton's method to increase accuracy
+ // increase iterations if you want more accuracy
+ // this one iteration should get us to within .5% error
+ xysquared=lib_fp_multiply(y,lib_fp_multiply(x,y));
+ y = lib_fp_multiply(y,(FIXEDPOINTTHREE - xysquared))>>1;
+// xysquared=lib_fp_multiply(y,lib_fp_multiply(x,y));
+// y = lib_fp_multiply(y,(FIXEDPOINTTHREE - xysquared))>>1;
+
+ return(y);
+ }
- All the Bradwii code is using variable type "fixedpointnum" . I am pretty sure, we ill meet some trouble by mixing with the conventionnal types used by multiwii.
- writeusersettingstoeeprom(); function, should probably be completely rewritten for multiwii... I have not loked at the details yet.
- global.gyrorate[autotuneindex] is a variable giving the angular speed in deg/s. I have not been able to find the equivalent variable in multiwii. Does it exist ?
- global.rxvalues[autotuneindex] is The values of the RX inputs, ranging from -1.0 to 1.0. As far as now, The RCvalues are stored in multiwii as 1000-2000 values. Some simple math to do.