UAVX Multicopter Flight Controller.

Dependencies:   mbed

Files at this revision

API Documentation at this revision

Comitter:
gke
Date:
Tue Apr 26 12:12:29 2011 +0000
Parent:
1:1e3318a30ddd
Commit message:
Not flightworthy. Posted for others to make use of the I2C SW code.

Changed in this revision

NXP1768pins.c Show annotated file Show diff for this revision Revisions of this file
SerialBuffered.h Show annotated file Show diff for this revision Revisions of this file
UAVXArm.c Show annotated file Show diff for this revision Revisions of this file
UAVXArm.h Show annotated file Show diff for this revision Revisions of this file
UAVXRevision.h Show annotated file Show diff for this revision Revisions of this file
UAVXRevisionSVN.h Show annotated file Show diff for this revision Revisions of this file
accel.c Show annotated file Show diff for this revision Revisions of this file
analog.c Show annotated file Show diff for this revision Revisions of this file
attitude.c Show annotated file Show diff for this revision Revisions of this file
autonomous.c Show annotated file Show diff for this revision Revisions of this file
baro.c Show annotated file Show diff for this revision Revisions of this file
compass.c Show annotated file Show diff for this revision Revisions of this file
control.c Show annotated file Show diff for this revision Revisions of this file
gps.c Show annotated file Show diff for this revision Revisions of this file
gyro.c Show annotated file Show diff for this revision Revisions of this file
harness.c Show annotated file Show diff for this revision Revisions of this file
i2c.c Show annotated file Show diff for this revision Revisions of this file
ir.c Show annotated file Show diff for this revision Revisions of this file
irq.c Show annotated file Show diff for this revision Revisions of this file
leds.c Show annotated file Show diff for this revision Revisions of this file
math.c Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
menu.c Show annotated file Show diff for this revision Revisions of this file
nonvolatile.c Show annotated file Show diff for this revision Revisions of this file
outputs.c Show annotated file Show diff for this revision Revisions of this file
outputs_conventional.h Show annotated file Show diff for this revision Revisions of this file
outputs_copter.h Show annotated file Show diff for this revision Revisions of this file
outputs_y6.h Show annotated file Show diff for this revision Revisions of this file
params.c Show annotated file Show diff for this revision Revisions of this file
rc.c Show annotated file Show diff for this revision Revisions of this file
sb_globals.cpp Show annotated file Show diff for this revision Revisions of this file
serial.c Show annotated file Show diff for this revision Revisions of this file
stats.c Show annotated file Show diff for this revision Revisions of this file
telemetry.c Show annotated file Show diff for this revision Revisions of this file
temperature.c Show annotated file Show diff for this revision Revisions of this file
uavx_aileron.h Show annotated file Show diff for this revision Revisions of this file
uavx_elevon.h Show annotated file Show diff for this revision Revisions of this file
uavx_helicopter.h Show annotated file Show diff for this revision Revisions of this file
uavx_multicopter.h Show annotated file Show diff for this revision Revisions of this file
utils.c Show annotated file Show diff for this revision Revisions of this file
diff -r 1e3318a30ddd -r 90292f8bd179 NXP1768pins.c
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/NXP1768pins.c	Tue Apr 26 12:12:29 2011 +0000
@@ -0,0 +1,150 @@
+// ===============================================================================================
+// =                              UAVXArm Quadrocopter Controller                                =
+// =                           Copyright (c) 2008 by Prof. Greg Egan                             =
+// =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
+// =                           http://code.google.com/p/uavp-mods/                               =
+// ===============================================================================================
+
+//    This is part of UAVXArm.
+
+//    UAVXArm 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 (at your option) any later version.
+
+//    UAVXArm 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/
+
+//    IO functions for NXP LPC1768.
+
+//    See also: http://bitbucket.org/jpc/lpc1768/
+
+//    Original Copyright (c) 2010 LoEE - Jakub Piotr Cłapa
+//    This program was released under the new BSD license.
+
+//    Rewritten for UAVXArm by G.K Egan 2011
+
+#ifdef USE_NXP_PIN_MAP // NOT COMMISSIONED
+
+#include "UAVXArm.h"
+
+boolean PinRead(uint8);
+void PinWrite(uint8, boolean);
+void PinSetOutput(uint8, boolean);
+
+//extern __inline__ __attribute__((always_inline))
+
+
+boolean PinRead(uint8 pn) {
+
+    static uint8 p,m;
+    p = pn >> 5;
+    m = 1 << ( pn & 0x1f );
+
+    switch ( p ) {
+        case 0:
+            return ( LPC_GPIO0->FIOPIN & m ) != 0;
+        case 1:
+            return ( LPC_GPIO1->FIOPIN & m ) != 0;
+        case 2:
+            return ( LPC_GPIO2->FIOPIN & m ) != 0;
+        case 3:
+            return ( LPC_GPIO3->FIOPIN & m ) != 0;
+        case 4:
+            return ( LPC_GPIO4->FIOPIN & m ) != 0;
+        default:
+            return (0);
+    }
+} // PinRead
+
+void PinWrite(uint8 pn, boolean v) {
+
+    static uint8 p, m;
+    p = pn >> 5;
+    m = 1 << ( pn & 0x1f );
+
+    switch ( p ) {
+        case 0:
+            if ( v )
+                LPC_GPIO0->FIOSET = m;
+            else
+                LPC_GPIO0->FIOCLR = m;
+            break;
+        case 1:
+            if ( v )
+                LPC_GPIO1->FIOSET = m;
+            else
+                LPC_GPIO1->FIOCLR = m;
+            break;
+        case 2:
+            if ( v )
+                LPC_GPIO2->FIOSET = m;
+            else
+                LPC_GPIO2->FIOCLR = m;
+            break;
+        case 3:
+            if ( v )
+                LPC_GPIO3->FIOSET = m;
+            else
+                LPC_GPIO3->FIOCLR = m;
+            break;
+        case 4:
+            if ( v )
+                LPC_GPIO4->FIOSET = m;
+            else
+                LPC_GPIO4->FIOCLR = m;
+            break;
+        default:
+            break;
+    }
+} // PinWrite
+
+void PinSetOutput(uint8 pn, boolean PinIsOutput) {
+
+    static uint8 p,m;
+    p = pn >> 5;
+    m = 1 << ( pn & 0x1f );
+
+    switch ( p ) {
+        case 0:
+            if ( PinIsOutput )
+                LPC_GPIO0->FIODIR |= m;
+            else
+                LPC_GPIO0->FIODIR &= ~m;
+            break;
+        case 1:
+            if ( PinIsOutput )
+                LPC_GPIO1->FIODIR |= m;
+            else
+                LPC_GPIO1->FIODIR &= ~m;
+            break;
+        case 2:
+            if ( PinIsOutput )
+                LPC_GPIO2->FIODIR |= m;
+            else
+                LPC_GPIO2->FIODIR &= ~m;
+            break;
+        case 3:
+            if ( PinIsOutput )
+                LPC_GPIO3->FIODIR |= m;
+            else
+                LPC_GPIO3->FIODIR &= ~m;
+            break;
+        case 4:
+            if ( PinIsOutput )
+                LPC_GPIO4->FIODIR |= m;
+            else
+                LPC_GPIO4->FIODIR &= ~m;
+            break;
+        default:
+            break;
+    }
+
+} // PinSetOutput
+ 
+#endif // USE_NXP_PIN_MAP
+
+
diff -r 1e3318a30ddd -r 90292f8bd179 SerialBuffered.h
--- a/SerialBuffered.h	Fri Feb 25 01:35:24 2011 +0000
+++ b/SerialBuffered.h	Tue Apr 26 12:12:29 2011 +0000
@@ -1,303 +1,303 @@
-/*
-    Copyright (c) 2010 Andy Kirkham
- 
-    Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
- 
-    The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
- 
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
-*/
-
-#ifndef SERIALBUFFERED_H
-#define SERIALBUFFERED_H
-
-#include "mbed.h"
-
-
-
-/** SerialBuffered based on Serial but fully buffered IO
- *
- * Example:
- * @code
- * #include "mbed.h"
- * #include "SerialBuffered.h"
- *
- * SerialBuffered serial1 (p13, p14); 
- * SerialBuffered serial2 (p28, p27);
- *
- * int main() {
- *     while(1) {
- *         if (serial1.readable()) {
- *             while (!serial2.writeable());
- *             serial2.putc(serial1.getch());
- *         }
- *         if (serial2.readable()) {
- *             while (!serial1.writeable());
- *             serial1.putc(serial2.getc());
- *         }
- *     }
- * }
- * @endcode
- *
- * <b>Note</b>, because this system "traps" the interrupts for the UART
- * being used <b>do not</b> use the .attach() method, otherwise the buffers
- * will cease functioning. Or worse, behaviour becomes unpredictable.
- */
-
-class SerialBuffered : public Serial {
-
-    public:
-        enum { None = 0, One = 1, Two = 2 };
-        enum { WordLength5 = 0, WordLength6 = 1, WordLength7 = 2, WordLength8 = 3 };
-        enum { NoParity = 0, OddParity = (1UL << 3), EvenParity = (3UL << 3), Forced1 = (5UL << 3), Forced0 = (7UL << 3) };
-        enum { StopBit1 = (0UL << 2), StopBit2 = (1UL << 2) };
-        
-        /** Create a SerialBuffered object connected to the specified pins
-         *
-         * @param PinName tx The Mbed TX pin for the uart port.
-         * @param PinName rx The Mbed RX pin for the uart port.
-         */
-        SerialBuffered(PinName tx, PinName rx);
-        
-        virtual ~SerialBuffered();
-        
-        /** Get a character from the serial stream.
-         *
-         * @return char A char value of the character read.
-         */
-        char getc(void);
-        
-        /** Gets a character from the serial stream with optional blocking.
-         *
-         * This method allows for getting a character from the serial stream
-         * if one is available. If <b>blocking</b> is true, the method will
-         * wait for serial input if the RX buffer is empty. If <b>blocking</b>
-         * is false, the method will return immediately if the RX buffer is empty.
-         * On return, if not blocking and the buffer was empty, -1 is returned.
-         *
-         * @param blocking true or false, when true will block.
-         * @return int An int representation of the 8bit char or -1 on buffer empty.
-         */
-        int  getc(bool blocking);
-        
-        /** Puts a characher to the serial stream.
-         *
-         * This sends a character out of the uart port or, if no room in the
-         * TX FIFO, will place the character into the TX buffer. <b>Note</b>, if the
-         * TX buffer is also full, this method will <b>block</b> (wait) until
-         * there is room in the buffer.
-         *
-         * @param int An int representation of the 8bit character to send.
-         * @return int Always returns zero.
-         */
-        int  putc(int c);
-        
-        /** Puts a characher to the serial stream.
-         *
-         * As with putc(int c) this function allows for a character to be sent
-         * to the uart TX port. However, an extra parameter is added to allow
-         * the caller to decide if the method should block or not. If blocking
-         * is disabled then this method returns -1 to signal there was no room 
-         * in the TX FIFO or the TX buffer. The character c passed is has not
-         * therefore been sent.
-         *
-         * @param int An int representation of the 8bit character to send.
-         * @param bool true if blocking required, false to disable blocking.
-         * @return int Zero on success, -1 if no room in TX FIFO or TX buffer.
-         */
-        int  putc(int c, bool blocking);
-        
-        /** Are there characters in the RX buffer we can read?
-         *
-         * @return int 1 if characters are available, 0 otherwise.
-         */
-        int  readable(void);
-        
-        /** Is there room in the TX buffer to send a character?
-         *
-         * @return int 1 if room available, 0 otherwise.
-         */
-        int  writeable(void);
-        
-        /** Set's the UART baud rate.
-         *
-         * Any allowed baudrate may be passed. However, you should
-         * ensure it matches the far end of the serial link.
-         * 
-         * @param int The baudrate to set.
-         */
-        void baud(int baudrate);
-        
-        /** Sets the serial format.
-         *
-         * Valid serial bit lengths are:-
-         * <ul>
-         *  <li>SerialBuffered::WordLength5</li>
-         *  <li>SerialBuffered::WordLength6</li>
-         *  <li>SerialBuffered::WordLength7</li>
-         *  <li>SerialBuffered::WordLength8</li>
-         * </ul>
-         *
-         * Valid serial parity are:-
-         * <ul>
-         *  <li>SerialBuffered::NoParity</li>
-         *  <li>SerialBuffered::OddParity</li>
-         *  <li>SerialBuffered::EvenParity</li>
-         *  <li>SerialBuffered::Forced1</li>
-         *  <li>SerialBuffered::Forced0</li>
-         * </ul>
-         *
-         * Valid stop bits are:-
-         * <ul>
-         *  <li>SerialBuffered::None</li>
-         *  <li>SerialBuffered::One</li>
-         *  <li>SerialBuffered::Two</li>
-         * </ul>
-         *
-         * @param int bits
-         * @param int parity
-         * @param int stopbits
-         */
-        void format(int bits, int parity, int stopbits);
-        
-        /** Change the TX buffer size
-         *
-         * By default, when the SerialBuffer object is created, a default
-         * TX buffer of 256 bytes in size is created. If you need a bigger
-         * (or smaller) buffer then use this function to change the TX buffer
-         * size. 
-         *
-         * <b>Note</b>, when a buffer is resized, any previous buffer
-         * in operation is discarded (destroyed and lost).
-         *
-         * @param int The size of the TX buffer required.
-         */
-        void set_tx_buffer_size(int buffer_size);
-        
-        /** Change the TX buffer size and provide your own allocation.
-         *
-         * This methos allows for the buffer size to be changed and for the
-         * caller to pass a pointer to an area of RAM they have already set
-         * aside to hold the buffer. The standard method is to malloc space
-         * from the heap. This method allows that to be overriden and use a
-         * user supplied buffer. 
-         *
-         * <b>Note</b>, the buffer you create must be of the size you specify!
-         * <b>Note</b>, when a buffer is resized, any previous buffer
-         * in operation is discarded (destroyed and lost).
-         *
-         * @param int The size of the TX buffer required.
-         * @param char* A pointer to a buffer area you previously allocated.
-         */
-        void set_tx_buffer_size(int buffer_size, char *buffer);
-        
-        /** Change the RX buffer size
-         *
-         * By default, when the SerialBuffer object is created, a default
-         * RX buffer of 256 bytes in size is created. If you need a bigger
-         * (or smaller) buffer then use this function to change the RX buffer
-         * size. 
-         *
-         * <b>Note</b>, when a buffer is resized, any previous buffer
-         * in operation is discarded (destroyed and lost).
-         *
-         * @param int The size of the RX buffer required.
-         */
-        void set_rx_buffer_size(int buffer_size);
-        
-        /** Change the RX buffer size and provide your own allocation.
-         *
-         * This methos allows for the buffer size to be changed and for the
-         * caller to pass a pointer to an area of RAM they have already set
-         * aside to hold the buffer. The standard method is to malloc space
-         * from the heap. This method allows that to be overriden and use a
-         * user supplied buffer. 
-         *
-         * <b>Note</b>, the buffer you create must be of the size you specify!
-         * <b>Note</b>, when a buffer is resized, any previous buffer
-         * in operation is discarded (destroyed and lost).
-         *
-         * @param int The size of the RX buffer required.
-         * @param char* A pointer to a buffer area you previously allocated.
-         */
-        void set_rx_buffer_size(int buffer_size, char *buffer);
-        
-    protected:
-        /** Calculate the divisors for a UART's baud
-         *
-         * @param int The desired baud rate
-         * @param int The UART in use to calculate for
-         */
-        uint16_t calculate_baud(int baud, int uart);
-            
-    private:
-        /** set_uart0
-         *
-         * Sets up the hardware and interrupts for the UART.
-         *
-         * @param PinName tx
-         * @param PinName rx
-         */
-        void set_uart0(PinName tx, PinName rx);
-        
-        /** set_uart1
-         *
-         * Sets up the hardware and interrupts for the UART.
-         *
-         * @param PinName tx
-         * @param PinName rx
-         */
-        void set_uart1(PinName tx, PinName rx);
-        
-        /** set_uart2
-         *
-         * Sets up the hardware and interrupts for the UART.
-         *
-         * @param PinName tx
-         * @param PinName rx
-         */
-        void set_uart2(PinName tx, PinName rx);
-        
-        /** set_uart3
-         *
-         * Sets up the hardware and interrupts for the UART.
-         *
-         * @param PinName tx
-         * @param PinName rx
-         */
-        void set_uart3(PinName tx, PinName rx);
-        
-        /** Reset the TX Buffer
-         *
-         * @param int The UART buffer to reset.
-         */
-        void reset_uart_tx(int uart_number);
-        
-        /** Reset the RX Buffer
-         *
-         * @param int The UART buffer to reset.
-         */
-        void reset_uart_rx(int uart_number);
-        
-        /** LPC1768 UART peripheral base address for UART in use.
-         */
-        unsigned long uart_base;
-        
-        /** LPC1768 UART number for UART in use.
-         */
-        int uart_number;
-};
-
-#endif
+/*
+    Copyright (c) 2010 Andy Kirkham
+ 
+    Permission is hereby granted, free of charge, to any person obtaining a copy
+    of this software and associated documentation files (the "Software"), to deal
+    in the Software without restriction, including without limitation the rights
+    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+    copies of the Software, and to permit persons to whom the Software is
+    furnished to do so, subject to the following conditions:
+ 
+    The above copyright notice and this permission notice shall be included in
+    all copies or substantial portions of the Software.
+ 
+    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+    THE SOFTWARE.
+*/
+
+#ifndef SERIALBUFFERED_H
+#define SERIALBUFFERED_H
+
+#include "mbed.h"
+
+
+
+/** SerialBuffered based on Serial but fully buffered IO
+ *
+ * Example:
+ * @code
+ * #include "mbed.h"
+ * #include "SerialBuffered.h"
+ *
+ * SerialBuffered serial1 (p13, p14); 
+ * SerialBuffered serial2 (p28, p27);
+ *
+ * int main() {
+ *     while(1) {
+ *         if (serial1.readable()) {
+ *             while (!serial2.writeable());
+ *             serial2.putc(serial1.getch());
+ *         }
+ *         if (serial2.readable()) {
+ *             while (!serial1.writeable());
+ *             serial1.putc(serial2.getc());
+ *         }
+ *     }
+ * }
+ * @endcode
+ *
+ * <b>Note</b>, because this system "traps" the interrupts for the UART
+ * being used <b>do not</b> use the .attach() method, otherwise the buffers
+ * will cease functioning. Or worse, behaviour becomes unpredictable.
+ */
+
+class SerialBuffered : public Serial {
+
+    public:
+        enum { None = 0, One = 1, Two = 2 };
+        enum { WordLength5 = 0, WordLength6 = 1, WordLength7 = 2, WordLength8 = 3 };
+        enum { NoParity = 0, OddParity = (1UL << 3), EvenParity = (3UL << 3), Forced1 = (5UL << 3), Forced0 = (7UL << 3) };
+        enum { StopBit1 = (0UL << 2), StopBit2 = (1UL << 2) };
+        
+        /** Create a SerialBuffered object connected to the specified pins
+         *
+         * @param PinName tx The Mbed TX pin for the uart port.
+         * @param PinName rx The Mbed RX pin for the uart port.
+         */
+        SerialBuffered(PinName tx, PinName rx);
+        
+        virtual ~SerialBuffered();
+        
+        /** Get a character from the serial stream.
+         *
+         * @return char A char value of the character read.
+         */
+        char getc(void);
+        
+        /** Gets a character from the serial stream with optional blocking.
+         *
+         * This method allows for getting a character from the serial stream
+         * if one is available. If <b>blocking</b> is true, the method will
+         * wait for serial input if the RX buffer is empty. If <b>blocking</b>
+         * is false, the method will return immediately if the RX buffer is empty.
+         * On return, if not blocking and the buffer was empty, -1 is returned.
+         *
+         * @param blocking true or false, when true will block.
+         * @return int An int representation of the 8bit char or -1 on buffer empty.
+         */
+        int  getc(bool blocking);
+        
+        /** Puts a characher to the serial stream.
+         *
+         * This sends a character out of the uart port or, if no room in the
+         * TX FIFO, will place the character into the TX buffer. <b>Note</b>, if the
+         * TX buffer is also full, this method will <b>block</b> (wait) until
+         * there is room in the buffer.
+         *
+         * @param int An int representation of the 8bit character to send.
+         * @return int Always returns zero.
+         */
+        int  putc(int c);
+        
+        /** Puts a characher to the serial stream.
+         *
+         * As with putc(int c) this function allows for a character to be sent
+         * to the uart TX port. However, an extra parameter is added to allow
+         * the caller to decide if the method should block or not. If blocking
+         * is disabled then this method returns -1 to signal there was no room 
+         * in the TX FIFO or the TX buffer. The character c passed is has not
+         * therefore been sent.
+         *
+         * @param int An int representation of the 8bit character to send.
+         * @param bool true if blocking required, false to disable blocking.
+         * @return int Zero on success, -1 if no room in TX FIFO or TX buffer.
+         */
+        int  putc(int c, bool blocking);
+        
+        /** Are there characters in the RX buffer we can read?
+         *
+         * @return int 1 if characters are available, 0 otherwise.
+         */
+        int  readable(void);
+        
+        /** Is there room in the TX buffer to send a character?
+         *
+         * @return int 1 if room available, 0 otherwise.
+         */
+        int  writeable(void);
+        
+        /** Set's the UART baud rate.
+         *
+         * Any allowed baudrate may be passed. However, you should
+         * ensure it matches the far end of the serial link.
+         * 
+         * @param int The baudrate to set.
+         */
+        void baud(int baudrate);
+        
+        /** Sets the serial format.
+         *
+         * Valid serial bit lengths are:-
+         * <ul>
+         *  <li>SerialBuffered::WordLength5</li>
+         *  <li>SerialBuffered::WordLength6</li>
+         *  <li>SerialBuffered::WordLength7</li>
+         *  <li>SerialBuffered::WordLength8</li>
+         * </ul>
+         *
+         * Valid serial parity are:-
+         * <ul>
+         *  <li>SerialBuffered::NoParity</li>
+         *  <li>SerialBuffered::OddParity</li>
+         *  <li>SerialBuffered::EvenParity</li>
+         *  <li>SerialBuffered::Forced1</li>
+         *  <li>SerialBuffered::Forced0</li>
+         * </ul>
+         *
+         * Valid stop bits are:-
+         * <ul>
+         *  <li>SerialBuffered::None</li>
+         *  <li>SerialBuffered::One</li>
+         *  <li>SerialBuffered::Two</li>
+         * </ul>
+         *
+         * @param int bits
+         * @param int parity
+         * @param int stopbits
+         */
+        void format(int bits, int parity, int stopbits);
+        
+        /** Change the TX buffer size
+         *
+         * By default, when the SerialBuffer object is created, a default
+         * TX buffer of 256 bytes in size is created. If you need a bigger
+         * (or smaller) buffer then use this function to change the TX buffer
+         * size. 
+         *
+         * <b>Note</b>, when a buffer is resized, any previous buffer
+         * in operation is discarded (destroyed and lost).
+         *
+         * @param int The size of the TX buffer required.
+         */
+        void set_tx_buffer_size(int buffer_size);
+        
+        /** Change the TX buffer size and provide your own allocation.
+         *
+         * This methos allows for the buffer size to be changed and for the
+         * caller to pass a pointer to an area of RAM they have already set
+         * aside to hold the buffer. The standard method is to malloc space
+         * from the heap. This method allows that to be overriden and use a
+         * user supplied buffer. 
+         *
+         * <b>Note</b>, the buffer you create must be of the size you specify!
+         * <b>Note</b>, when a buffer is resized, any previous buffer
+         * in operation is discarded (destroyed and lost).
+         *
+         * @param int The size of the TX buffer required.
+         * @param char* A pointer to a buffer area you previously allocated.
+         */
+        void set_tx_buffer_size(int buffer_size, char *buffer);
+        
+        /** Change the RX buffer size
+         *
+         * By default, when the SerialBuffer object is created, a default
+         * RX buffer of 256 bytes in size is created. If you need a bigger
+         * (or smaller) buffer then use this function to change the RX buffer
+         * size. 
+         *
+         * <b>Note</b>, when a buffer is resized, any previous buffer
+         * in operation is discarded (destroyed and lost).
+         *
+         * @param int The size of the RX buffer required.
+         */
+        void set_rx_buffer_size(int buffer_size);
+        
+        /** Change the RX buffer size and provide your own allocation.
+         *
+         * This methos allows for the buffer size to be changed and for the
+         * caller to pass a pointer to an area of RAM they have already set
+         * aside to hold the buffer. The standard method is to malloc space
+         * from the heap. This method allows that to be overriden and use a
+         * user supplied buffer. 
+         *
+         * <b>Note</b>, the buffer you create must be of the size you specify!
+         * <b>Note</b>, when a buffer is resized, any previous buffer
+         * in operation is discarded (destroyed and lost).
+         *
+         * @param int The size of the RX buffer required.
+         * @param char* A pointer to a buffer area you previously allocated.
+         */
+        void set_rx_buffer_size(int buffer_size, char *buffer);
+        
+    protected:
+        /** Calculate the divisors for a UART's baud
+         *
+         * @param int The desired baud rate
+         * @param int The UART in use to calculate for
+         */
+        uint16_t calculate_baud(int baud, int uart);
+            
+    private:
+        /** set_uart0
+         *
+         * Sets up the hardware and interrupts for the UART.
+         *
+         * @param PinName tx
+         * @param PinName rx
+         */
+        void set_uart0(PinName tx, PinName rx);
+        
+        /** set_uart1
+         *
+         * Sets up the hardware and interrupts for the UART.
+         *
+         * @param PinName tx
+         * @param PinName rx
+         */
+        void set_uart1(PinName tx, PinName rx);
+        
+        /** set_uart2
+         *
+         * Sets up the hardware and interrupts for the UART.
+         *
+         * @param PinName tx
+         * @param PinName rx
+         */
+        void set_uart2(PinName tx, PinName rx);
+        
+        /** set_uart3
+         *
+         * Sets up the hardware and interrupts for the UART.
+         *
+         * @param PinName tx
+         * @param PinName rx
+         */
+        void set_uart3(PinName tx, PinName rx);
+        
+        /** Reset the TX Buffer
+         *
+         * @param int The UART buffer to reset.
+         */
+        void reset_uart_tx(int uart_number);
+        
+        /** Reset the RX Buffer
+         *
+         * @param int The UART buffer to reset.
+         */
+        void reset_uart_rx(int uart_number);
+        
+        /** LPC1768 UART peripheral base address for UART in use.
+         */
+        unsigned long uart_base;
+        
+        /** LPC1768 UART number for UART in use.
+         */
+        int uart_number;
+};
+
+#endif
diff -r 1e3318a30ddd -r 90292f8bd179 UAVXArm.c
--- a/UAVXArm.c	Fri Feb 25 01:35:24 2011 +0000
+++ b/UAVXArm.c	Tue Apr 26 12:12:29 2011 +0000
@@ -2,7 +2,7 @@
 // =                              UAVXArm Quadrocopter Controller                                =
 // =                           Copyright (c) 2008 by Prof. Greg Egan                             =
 // =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
-// =                     http://code.google.com/p/uavp-mods/ http://uavp.ch                      =
+// =                           http://code.google.com/p/uavp-mods/                               =
 // ===============================================================================================
 
 //    This is part of UAVXArm.
@@ -21,16 +21,30 @@
 #include "UAVXArm.h"
 
 volatile Flags     F;
+int8 r; // global I2C
 
 int main(void) {
 
     InitMisc();
+    InitI2C();
     InitHarness();
 
     InitRC();
     InitTimersAndInterrupts();
     InitLEDs();
-            
+
+    /*
+    Delay1mS(500);
+            InitADXL345Acc();
+                MCP4725_ID_Actual = FORCE_BARO_ID;
+        while (1) {
+               DebugPin = 1;
+           SetFreescaleMCP4725(1);
+           DebugPin = 0;
+            Delay1mS(1);
+            ReadADXL345Acc();
+        }
+    */
     InitParameters();
     ReadStatsPX();
     InitMotors();
@@ -40,7 +54,7 @@
     InitAccelerometers();
     InitGyros();
     InitIRSensors();
- 
+
     InitCompass();
     InitRangefinder();
 
@@ -60,7 +74,7 @@
         StopMotors();
 
         LightsAndSirens();    // Check for Rx signal, disarmed on power up, throttle closed, gyros ONLINE
-        
+
         GetAttitude();
         MixAndLimitCam();
 
@@ -87,25 +101,31 @@
                         }
 
                         InitControl();
+                        DesiredHeading = Heading;
+                        Angle[Yaw] = 0.0;
                         CaptureTrims();
                         InitGPS();
                         InitNavigation();
 
                         DesiredThrottle = 0;
                         ErectGyros(); // DO NOT MOVE AIRCRAFT!
+                        InitAttitude();
                         ZeroStats();
                         DoStartingBeeps(3);
-                        
+
                         SendParameters(ParamSet);
 
                         mS[ArmedTimeout] = mSClock() + ARMED_TIMEOUT_MS;
                         mS[RxFailsafeTimeout] = mSClock() + RC_NO_CHANGE_TIMEOUT_MS;
+                        ControlUpdateTimeuS = uSClock();
                         F.ForceFailsafe = F.LostModel = false;
 
                         State = Landed;
                         break;
                     case Landed:
                         DesiredThrottle = 0;
+                        DesiredHeading = Heading;
+                        Angle[Yaw] = 0.0;
                         if ( mSClock() > mS[ArmedTimeout] )
                             DoShutdown();
                         else
@@ -129,6 +149,8 @@
                             }
                         break;
                     case Landing:
+                        DesiredHeading = Heading;
+                        Angle[Yaw] = 0.0;
                         if ( StickThrottle > IdleThrottle ) {
                             DesiredThrottle = 0;
                             State = InFlight;
@@ -170,16 +192,20 @@
             } else
                 DoPPMFailsafe();
 
+            while ( uSClock() < ControlUpdateTimeuS ) {}; // CAUTION: uS clock wraps at about an hour
+            ControlUpdateTimeuS = uSClock() + PID_CYCLE_US;
+
             DoControl();
 
             MixAndLimitMotors();
             OutSignals();
-            
+
             MixAndLimitCam();
-            
-            GetTemperature();  
+
+            GetTemperature();
             GetBattery();
             CheckAlarms();
+
             CheckTelemetry();
 
             SensorTrace();
diff -r 1e3318a30ddd -r 90292f8bd179 UAVXArm.h
--- a/UAVXArm.h	Fri Feb 25 01:35:24 2011 +0000
+++ b/UAVXArm.h	Tue Apr 26 12:12:29 2011 +0000
@@ -1,27 +1,74 @@
 
 // Commissioning defines
 
-#define SW_I2C                               // define for software I2C - TRAGICALLY SLOW
+//#define SERIAL_TELEMETRY GPSSerial                // Select the one you want Steve
+#define SERIAL_TELEMETRY TelemetrySerial
+
+#define MAX_PID_CYCLE_HZ    200                     // PID cycle rate do not exceed
+#define PID_CYCLE_US        (1000000/MAX_PID_CYCLE_HZ)
+
+#define PWM_UPDATE_HZ       200                     // reduced for Turnigys - I2C runs at PID loop rate always
+                                                    // MUST BE LESS THAN OR EQUAL TO 450HZ
+// LP filter cutoffs for sensors
 
-#define MAGIC 1.0                           // rescales the sensitivity of all PID loop params
+//#define SUPPRESS_ROLL_PITCH_GYRO_FILTERS                           
+#define ROLL_PITCH_FREQ     (0.5*PWM_UPDATE_HZ)     // must be <= ITG3200 LP filter
+#define ATTITUDE_ANGLE_LIMIT QUARTERPI              // set to PI for aerobatics
+#define GYRO_PROP_NOISE     0.1                     // largely prop coupled
+                    
+//#define SUPPRESS_ACC_FILTERS
+#define ACC_FREQ            5                       // could be lower again?
+
+//#define SUPPRESS_YAW_GYRO_FILTERS
+//#define USE_FIXED_YAW_FILTER                      // does not rescale LP cutoff with yaw stick  
+#define MAX_YAW_FREQ            10.0                // <= 10Hz
+#define COMPASS_SANITY_CHECK_RAD_S  TWOPI           // changes greater than this rate ignored
+
+// DCM Attitude Estimation
+
+//#define DCM_YAW_COMP
+//#define USE_ANGLE_DERIVED_RATE                    // Gyros have periodic prop noise - define to use rate derived from angle
 
-#define I2C_MAX_RATE_HZ    400000       
+// The pitch/roll angle should track CLOSELY with the noise "mostly" tuned out.
+// Jitter in the artificial horizon gives part of the story but better to use the UAVXFC logs.
+
+// Assumes normalised gravity vector
+#define TAU_S               5.0                                 //  1-10
+#define Kp_RollPitch        5.0 //(2.0/TAU_S)                   //1.0      // 5.0
+#define Ki_RollPitch        0.005//((1.0/TAU_S)*(1.0/TAU_S))    //0.001    // 0.005
+//#define Ki_RollPitch      (1.0/(TAU_S*TAU_S)) ?
+#define Kp_Yaw 1.2
+#define Ki_Yaw 0.00002
 
-#define PWM_UPDATE_HZ       200             // MUST BE LESS THAN OR EQUAL TO 450HZ
+/*
+                Kp      Ki          KpYaw   KiYaw
+Arducopter      0.0014  0.00002     1.0     0.00002 (200Hz)
+Arducopter      5.0     0.005       1.2     0.00002
+Ihlein          0.2     0.01        0.02    0.01    (50Hz)
+Premerlani      0.0755  0.00943                     (50Hz)
+Bascom          0.02    0.00002     1.2     0.00002
+Matrixpilot     0.04    0.008       0.016   0.0005
+Superstable     0.0014  0.00000015  1.2     0.00005 (200Hz)
 
-#define DISABLE_EXTRAS                       // suppress altitude hold, position hold and inertial compensation
-#define SUPPRESS_SDCARD                     // no logging to check if buffering backup is an issue
+*/
+
+//#define DISABLE_LED_DRIVER                             // disables the PCA driver and also the BUZZER
+
+#define DISABLE_EXTRAS                      // suppress altitude hold, position hold and inertial compensation
+#define SUPPRESS_SDCARD                     //DO NOT RE-ENABLE - MOTOR INTERMITTENTS WILL OCCUR
 
 //BMP occasional returns bad results - changes outside this rate are deemed sensor/buss errors
 #define BARO_SANITY_CHECK_MPS    10.0       // dm/S 20,40,60,80 or 100
 
 #define SIX_DOF                             // effects acc and gyro addresses
 
-#define SOFTWARE_CAM_PWM
-
 #define BATTERY_VOLTS_SCALE   57.85         // 51.8144    // Volts scaling for internal divider
 
-//#define DCM_YAW_COMP
+#define SW_I2C                                      // define for software I2C 400KHz 
+
+//#define INC_ALL_SCHEMES                            // runs all attitude control schemes - use "custom" telemetry
+
+#define I2C_MAX_RATE_HZ     400000
 
 #define USING_MBED
 
@@ -29,7 +76,7 @@
 // =                              UAVXArm Quadrocopter Controller                                =
 // =                           Copyright (c) 2008 by Prof. Greg Egan                             =
 // =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
-// =                     http://code.google.com/p/uavp-mods/ http://uavp.ch                      =
+// =                           http://code.google.com/p/uavp-mods/                               =
 // ===============================================================================================
 
 //    This is part of UAVXArm.
@@ -80,7 +127,9 @@
 #define false   0
 
 #define PI              3.141592654
+#define THIRDPI         (PI/3.0)
 #define HALFPI          (PI*0.5)
+#define ONEANDHALFPI    (PI * 1.5)
 #define QUARTERPI       (PI*0.25)
 #define SIXTHPI         (PI/6.0)
 #define TWOPI           (PI*2.0)
@@ -171,8 +220,9 @@
 
 #define UAVX_TEL_INTERVAL_MS            125L    // mS. emit an interleaved telemetry packet
 #define ARDU_TEL_INTERVAL_MS            200L    // mS. Ardustation
-#define UAVX_CONTROL_TEL_INTERVAL_MS    10L     // mS. flight control only
-#define CUSTOM_TEL_INTERVAL_MS          250L    // mS.
+#define UAVX_CONTROL_TEL_INTERVAL_MS    100L    // mS. flight control only
+#define CUSTOM_TEL_INTERVAL_MS          125L    // mS.
+#define FAST_CUSTOM_TEL_INTERVAL_MS     5L      // mS.
 #define UAVX_MIN_TEL_INTERVAL_MS        1000L    // mS. emit minimum FPV telemetry packet slow rate for example to FrSky
 
 #define GPS_TIMEOUT_MS                  2000L   // mS.
@@ -199,16 +249,15 @@
 // unfortunately there seems to be a leak which cause the roll/pitch
 // to increase without the decay.
 
-#define ATTITUDE_SCALE                  0.5     // scaling of stick units for attitude angle control 
+#define ATTITUDE_SCALE                  0.008     // scaling of stick units for attitude angle control 
 
 // Enable "Dynamic mass" compensation Roll and/or Pitch
 // Normally disabled for pitch only
-//#define DISABLE_DYNAMIC_MASS_COMP_ROLL
-//#define DISABLE_DYNAMIC_MASS_COMP_PITCH
+//#define ENABLE_DYNAMIC_MASS_COMP_ROLL
+//#define ENABLE_DYNAMIC_MASS_COMP_PITCH
 
 // Altitude Hold
 
-
 #define ALT_HOLD_MAX_ROC_MPS            0.5      // Must be changing altitude at less than this for alt. hold to be detected
 
 // the range within which throttle adjustment is proportional to altitude error
@@ -279,15 +328,6 @@
 #define THROTTLE_MIDDLE                 10      // throttle stick dead zone for baro 
 #define THROTTLE_MIN_ALT_HOLD           75      // min throttle stick for altitude lock
 
-// RC
-
-#define RC_INIT_FRAMES                  32      // number of initial RC frames to allow filters to settle
-
-#define RC_MIN_WIDTH_US                 900
-#define RC_MAX_WIDTH_US                 2100
-
-#define RC_NO_CHANGE_TIMEOUT_MS         20000L        // mS.
-
 typedef union {
     int16 i16;
     uint16 u16;
@@ -365,6 +405,7 @@
 #define Min(i,j)                ((i<j) ? i : j )
 #define Decay1(i)               (((i) < 0) ? (i+1) : (((i) > 0) ? (i-1) : 0))
 #define Limit(i,l,u)            (((i) < l) ? l : (((i) > u) ? u : (i)))
+#define Limit1(i,l)            (((i) < -(l)) ? -(l) : (((i) > (l)) ? (l) : (i)))
 #define Sqr(v)                  ( v * v )
 
 // To speed up NMEA sentence processing
@@ -398,7 +439,7 @@
 #define MAX_PARAMETERS      64        // parameters in PXPROM start at zero
 
 #define STATS_ADDR_PX       ( PARAMS_ADDR_PX + (MAX_PARAMETERS *2) )
-#define MAX_STATS           64
+#define MAX_STATS           32 // 64 bytes
 
 // uses second Page of PXPROM
 #define NAV_ADDR_PX         256L
@@ -421,7 +462,7 @@
 // main.c
 
 enum Directions {BF, LR, UD, Alt }; // x forward, y left and z down
-enum Angles { Pitch, Roll, Yaw };   // X, Y, Z
+enum Angles { Roll, Pitch, Yaw };   // X, Y, Z
 
 #define FLAG_BYTES 10
 #define TELEMETRY_FLAG_BYTES 6
@@ -469,7 +510,7 @@
         1,
 WayPointCentred:
         1,
-Unused2: // was UsingGPSAlt:
+UnusedGPSAlt: // was UsingGPSAlt:
         1,
 UsingRTHAutoDescend:
         1,
@@ -563,6 +604,8 @@
 Using9DOF:
         1,
 HaveBatterySensor:
+        1,
+AccMagnitudeOK:
         1;
     };
 } Flags;
@@ -570,14 +613,12 @@
 enum FlightStates { Starting, Landing, Landed, Shutdown, InFlight};
 extern volatile Flags F;
 extern int8 State;
+extern int8 r;
 
 //______________________________________________________________________________________________
 
 // accel.c
 
-#define ACC_FREQ  50     // Hz must be less than 100Hz
-const real32 OneOnTwoPiAccF = ( 1.0 / ( TWOPI * ACC_FREQ ));
-
 enum AccelerometerTypes { LISLAcc, ADXL345Acc, AccUnknown };
 
 extern void ShowAccType(void);
@@ -598,16 +639,12 @@
 #define ADXL345_WR           ADXL345_ID
 #define ADXL345_RD           (ADXL345_ID+1)
 
-extern const float GRAVITY_ADXL345;
-
 extern void ReadADXL345Acc(void);
 extern void InitADXL345Acc(void);
 extern boolean ADXL345AccActive(void);
 
 // LIS3LV02DG 3-Axis Accelerometer 400KHz
 
-extern const float GRAVITY_LISL;
-
 #define LISL_WHOAMI      0x0f
 #define LISL_OFFSET_X    0x16
 #define LISL_OFFSET_Y    0x17
@@ -636,15 +673,20 @@
 #define LISL_READ        0x80
 #define LISL_ID          0x3a
 
-extern void WriteLISL(uint8, uint8);
+#define LISL_WR         LISL_ID
+#define LISL_RD         (LISL_ID+1)
+
+extern boolean WriteLISL(uint8, uint8);
 extern void ReadLISLAcc(void);
+extern void InitLISLAcc(void);
 extern boolean LISLAccActive(void);
 
 // other accelerometers
 
-extern real32 Vel[3], Acc[3], AccNeutral[3];
+extern real32 Vel[3], AccADC[3], AccNoise[3], Acc[3], AccNeutral[3], Accp[3];
 extern int16 NewAccNeutral[3];
 extern uint8 AccelerometerType;
+extern real32 GravityR; // recip gravity scaling
 
 //______________________________________________________________________________________________
 
@@ -668,17 +710,36 @@
 
 // attitude.c
 
-enum AttitudeMethods { PremerlaniDCM,  MadgwickIMU,  MadgwickAHRS};
+enum AttitudeMethods { Integrator, Wolferl,  Complementary, Kalman, PremerlaniDCM,  MadgwickIMU,
+                MadgwickIMU2, MadgwickAHRS, MultiWii,  MaxAttitudeScheme};
 
+extern void AdaptiveYawLPFreq(void);
 extern void GetAttitude(void);
-extern void DoLegacyYawComp(void);
+extern void DoLegacyYawComp(uint8);
+extern void NormaliseAccelerations(void);
 extern void AttitudeTest(void);
 extern void InitAttitude(void);
 
-extern real32 dT, dTR;
+extern real32 dT, dTOn2, dTR, dTmS;
+extern real32 YawFilterLPFreq;
 extern uint32 PrevDCMUpdate;
 extern uint8 AttitudeMethod;
 
+extern real32 EstAngle[3][MaxAttitudeScheme];
+extern real32 EstRate[3][MaxAttitudeScheme];
+
+extern real32 HE;
+
+// SimpleIntegrator
+
+extern void DoIntegrator(void);
+
+// Wolferl
+
+extern real32 Correction[2];
+
+extern void DoWolferl(void);
+
 // DCM Premerlani
 
 extern void DCMNormalise(void);
@@ -687,6 +748,7 @@
 extern void DCMMotionCompensation(void);
 extern void DCMUpdate(void);
 extern void DCMEulerAngles(void);
+extern void DoDCM(void);
 
 extern real32 RollPitchError[3];
 extern real32 AccV[3];
@@ -701,12 +763,22 @@
 
 // IMU & AHRS S.O.H. Madgwick
 
-extern void IMUupdate(real32, real32, real32, real32, real32, real32);
-extern void AHRSupdate(real32, real32, real32, real32, real32, real32, real32, real32, real32);
-extern void EulerAngles(void);
+extern void DoMadgwickIMU(real32, real32, real32, real32, real32, real32);
+extern void DoMadgwickIMU2(real32, real32, real32, real32, real32, real32);
+extern void DoMadgwickAHRS(real32, real32, real32, real32, real32, real32, real32, real32, real32);
+extern void MadgwickEulerAngles(uint8);
 
 extern real32 q0, q1, q2, q3;    // quaternion elements representing the estimated orientation
 
+// Kalman
+extern void DoKalman(void);
+
+// Complementary
+extern void DoCF(void);
+
+// MultiWii
+extern  void DoMultiWii();
+
 //______________________________________________________________________________________________
 
 // autonomous.c
@@ -718,7 +790,6 @@
 extern void SetDesiredAltitude(int16);
 extern void DoFailsafeLanding(void);
 extern void AcquireHoldPosition(void);
-extern void NavGainSchedule(int16);
 extern void DoNavigation(void);
 extern void FakeFlight(void);
 extern void DoPPMFailsafe(void);
@@ -834,17 +905,12 @@
 
 enum CompassTypes { HMC5843, HMC6352, NoCompass };
 
-#define COMPASS_UPDATE_MS               50
-#define COMPASS_UPDATE_S                (real32)(COMPASS_UPDATE_MS * 0.001)
-#define COMPASS_FREQ                    10      // Hz must be less than 10Hz
+//#define SUPPRESS_COMPASS_FILTER
 
-#define COMPASS_MAXDEV                30        // maximum yaw compensation of compass heading 
-#define COMPASS_MIDDLE                10        // yaw stick neutral dead zone
-
-const real32 OneOnTwoPiCompassF = ( 1.0 / ( TWOPI * COMPASS_FREQ ));
-
+extern real32 AdaptiveCompassFreq(void);
 extern void ReadCompass(void);
 extern void GetHeading(void);
+extern real32 MinimumTurn(real32);
 extern void ShowCompassType(void);
 extern void DoCompassTest(void);
 extern void CalibrateCompass(void);
@@ -852,6 +918,12 @@
 
 // HMC5843 Compass
 
+#define HMC5843_DR          6       // 50Hz
+#define HMC5843_UPDATE_S    0.02
+
+//#define HMC5843_DR            5    // 20Hz
+//#define HMC5843_UPDATE_S      0.05
+
 #define HMC5843_ID      0x3C        // 0x1E 9DOF
 #define HMC5843_WR      HMC5843_ID
 #define HMC5843_RD      (HMC5843_ID+1)
@@ -865,6 +937,8 @@
 
 // HMC6352
 
+#define HMC6352_UPDATE_S    0.05    // 20Hz
+
 #define HMC6352_ID       0x42
 #define HMC6352_WR       HMC6352_ID
 #define HMC6352_RD       (HMC6352_ID+1)
@@ -883,7 +957,8 @@
 } MagStruct;
 extern MagStruct Mag[3];
 extern real32 MagDeviation, CompassOffset;
-extern real32 MagHeading, Heading, FakeHeading;
+extern real32 MagHeading, Heading, HeadingP, FakeHeading;
+extern real32 CompassMaxSlew;
 extern real32 HeadingSin, HeadingCos;
 extern uint8 CompassType;
 
@@ -891,10 +966,9 @@
 
 // control.c
 
-extern real32 PTerm, ITerm, DTerm; //zzz
-
 extern void DoAltitudeHold(void);
 extern void UpdateAltitudeSource(void);
+extern real32 AltitudeCF( real32, real32);
 extern void AltitudeHold(void);
 
 extern void LimitYawSum(void);
@@ -905,18 +979,22 @@
 extern void LightsAndSirens(void);
 extern void InitControl(void);
 
-extern real32 Angle[3], Anglep[3], Rate[3], Ratep[3];
-extern real32 Comp[4];
+extern real32 Angle[3], Anglep[3], Rate[3], Ratep[3], YawRateEp;
+extern real32 AccAltComp, AltComp;
 extern real32 DescentComp;
 extern real32 Rl, Pl, Yl, Ylp;
 extern real32 GS;
 
 extern int16 HoldYaw, YawSlewLimit;
 extern int16 CruiseThrottle, MaxCruiseThrottle, DesiredThrottle, IdleThrottle, InitialThrottle, StickThrottle;
-extern int16 DesiredRoll, DesiredPitch, DesiredYaw, DesiredHeading, DesiredCamPitchTrim;
+extern int16 DesiredRoll, DesiredPitch, DesiredYaw, DesiredCamPitchTrim;
+extern real32 DesiredHeading;
 extern real32 ControlRoll, ControlPitch, ControlRollP, ControlPitchP;
 extern real32 CameraRollAngle, CameraPitchAngle;
 extern int16 CurrMaxRollPitch;
+extern int16 Trim[3];
+
+extern real32 GRollKp, GRollKi, GRollKd, GPitchKp, GPitchKi, GPitchKd;
 
 extern int16 AttitudeHoldResetCount;
 extern real32 AltDiffSum, AltD, AltDSum;
@@ -925,6 +1003,7 @@
 extern boolean FirstPass;
 
 extern uint32 AltuSp;
+extern uint32 ControlUpdateTimeuS;
 extern int16 DescentLimiter;
 
 extern int16 FakeDesiredPitch, FakeDesiredRoll, FakeDesiredYaw;
@@ -1037,13 +1116,13 @@
 
 extern void ReadITG3200Gyro(void);
 extern uint8 ReadByteITG3200(uint8);
-extern void WriteByteITG3200(uint8, uint8);
+extern boolean WriteByteITG3200(uint8, uint8);
 extern void InitITG3200Gyro(void);
 extern void ITG3200Test(void);
 extern boolean ITG3200GyroActive(void);
 
 extern const real32 GyroToRadian[];
-extern real32 GyroADC[3], GyroNeutral[3], Gyro[3]; // Radians
+extern real32 GyroADC[3], GyroADCp[3], GyroNoise[3], GyroNeutral[3], Gyrop[3], Gyro[3]; // Radians
 extern uint8 GyroType;
 
 //______________________________________________________________________________________________
@@ -1055,6 +1134,8 @@
 
 extern LocalFileSystem Flash;
 
+extern uint8 I2C0SDAPin, I2C0SCLPin;
+
 // 1 GND
 // 2 4.5-9V
 // 3 VBat
@@ -1104,13 +1185,13 @@
     void stop(void);
     uint8 blockread(uint8 r, char* b, uint8);
     uint8 read(uint8 r);
-    void blockwrite(uint8 a, const char* b, uint8 l);
+    boolean blockwrite(uint8 a, const char* b, uint8 l);
     uint8 write(uint8 d);
 };
 
 extern MyI2C I2C0;                    // 27, 28
-extern DigitalInOut I2C0SCL;
-extern DigitalInOut I2C0SDA;
+extern PortInOut I2C0SCL;
+extern PortInOut I2C0SDA;
 #else
 
 extern I2C I2C0;                    // 27, 28
@@ -1120,6 +1201,8 @@
 #endif // SW_I2C
 
 extern DigitalIn  RCIn;             // 29 CAN
+extern InterruptIn RCInterrupt;
+
 extern DigitalOut PWMCamRoll;      // 30 CAN
 
 //extern Serial TelemetrySerial;
@@ -1134,8 +1217,6 @@
 extern DigitalOut RedLED;
 extern DigitalOut YellowLED;
 
-extern InterruptIn RCInterrupt;
-
 extern char RTCString[], RTCLogfile[];
 extern struct tm* RTCTime;
 
@@ -1210,6 +1291,9 @@
 extern void RazorInISR(void);
 extern void RazorOutISR(void);
 
+extern void TurnBeeperOff(void);
+extern void DoBeeperPulse1mS(int16);
+
 enum { Clock, GeneralCountdown, UpdateTimeout, RCSignalTimeout, BeeperTimeout, ThrottleIdleTimeout,
        FailsafeTimeout, AbortTimeout, RxFailsafeTimeout, DescentUpdate, StickChangeUpdate, NavStateTimeout, LastValidRx,
        LastGPS, StartTime, GPSTimeout, LEDChaserUpdate, LastBattery, TelemetryUpdate, NavActiveTime, BeeperUpdate,
@@ -1230,6 +1314,11 @@
 extern int8 SignalCount;
 extern uint16 RCGlitches;
 
+extern Timer timer;
+extern Timeout CamRollTimeout, CamPitchTimeout;
+extern Ticker CameraTicker;
+extern Timeout RCTimeout;
+
 //______________________________________________________________________________________________
 
 // ir.c
@@ -1244,13 +1333,8 @@
 
 // i2c.c
 
-#ifdef SW_I2C
-#define I2C_ACK  0
-#define I2C_NACK 1
-#else
-#define I2C_ACK  1
-#define I2C_NACK 0
-#endif // SW_I2C
+#define I2C_ACK  true
+#define I2C_NACK false
 
 extern boolean I2C0AddressResponds(uint8);
 #ifdef HAVE_I2C1
@@ -1262,8 +1346,10 @@
 extern boolean ESCWaitClkHi(void);
 extern void ProgramSlaveAddress(uint8);
 extern void ConfigureESCs(void);
+extern void InitI2C(void);
 
 extern uint32 MinI2CRate;
+extern uint16 I2CError[256];
 
 //______________________________________________________________________________________________
 
@@ -1399,6 +1485,16 @@
 
 //______________________________________________________________________________________________
 
+// NXP1768pins.c
+
+extern boolean PinRead(uint8);
+extern void PinWrite(uint8, boolean);
+extern void PinSetOutput(uint8, boolean);
+
+extern const uint8 mbed1768Pins[];
+
+//______________________________________________________________________________________________
+
 // outputs.c
 
 #define OUT_MINIMUM            1.0              // Required for PPM timing loops
@@ -1467,7 +1563,7 @@
     RollKp,                // 01
     RollKi,                // 02
     RollKd,                // 03
-    HorizDampKp,           // 04c
+    Unused04,           // 04c HorizDampKp
     RollIntLimit,          // 05
     PitchKp,               // 06
     PitchKi,               // 07
@@ -1477,16 +1573,16 @@
 
     YawKp,                 // 11
     YawKi,                 // 12
-    YawKd,                 // 13
+    Unused13,                 // 13 YawKd
     YawLimit,              // 14
     YawIntLimit,           // 15
     ConfigBits,            // 16c
-    unused17 ,             // 17 TimeSlots
+    Unused17 ,             // 17 TimeSlots
     LowVoltThres,          // 18c
     CamRollKp,             // 19
     PercentCruiseThr,      // 20c
 
-    VertDampKp,            // 21c
+    VertDamp,            // 21c
     MiddleUD,              // 22c
     PercentIdleThr,        // 23c
     MiddleLR,              // 24c
@@ -1494,7 +1590,7 @@
     CamPitchKp,            // 26
     CompassKp,             // 27
     AltKi,                 // 28c
-    unused29 ,             // 29 NavRadius
+    Unused29 ,             // 29 NavRadius
     NavKi,                 // 30
 
     GSThrottle,            // 31
@@ -1508,20 +1604,20 @@
     PercentNavSens6Ch,     // 39
     CamRollTrim,           // 40
     NavKd,                 // 41
-    VertDampDecay,         // 42
-    HorizDampDecay,        // 43
+    Unused42 ,             // 42  VertDampDecay
+    Unused43,              // 43  HorizDampDecay
     BaroScale,             // 44
     TelemetryType,         // 45
     MaxDescentRateDmpS,    // 46
     DescentDelayS,         // 47
     NavIntLimit,           // 48
     AltIntLimit,           // 49
-    unused50,              // 50 GravComp
-    unused51 ,             // 51 CompSteps
+    Unused50,              // 50 GravComp
+    Unused51 ,             // 51 CompSteps
     ServoSense,            // 52
     CompassOffsetQtr,      // 53
     BatteryCapacity,       // 54
-    unused55,              // 55 GyroYawType
+    Unused55,              // 55 GyroYawType
     AltKd,                 // 56
     Orient,                // 57
     NavYawLimit,           // 58
@@ -1576,6 +1672,13 @@
 
 // rc.c
 
+#define RC_INIT_FRAMES                  32      // number of initial RC frames to allow filters to settle
+
+#define RC_MIN_WIDTH_US                 1000    // temporarily to prevent wraparound 900
+#define RC_MAX_WIDTH_US                 2100
+
+#define RC_NO_CHANGE_TIMEOUT_MS         20000L
+
 extern void DoRxPolarity(void);
 extern void InitRC(void);
 extern void MapRC(void);
@@ -1594,7 +1697,6 @@
 extern int16x8x4Q PPMQ;
 extern boolean RCPositiveEdge;
 extern int16 RC[], RCp[];
-extern int16 Trim[3];
 extern int16 ThrLow, ThrNeutral, ThrHigh;
 
 //__________________________________________________________________________________________
@@ -1635,8 +1737,7 @@
 enum Statistics {
     GPSAltitudeS, BaroRelAltitudeS, ESCI2CFailS, GPSMinSatsS, MinROCS, MaxROCS, GPSVelS,
     AccFailS, CompassFailS, BaroFailS, GPSInvalidS, GPSMaxSatsS, NavValidS,
-    MinHDiluteS, MaxHDiluteS, RCGlitchesS, GPSBaroScaleS, GyroFailS, RCFailsafesS, I2CFailS, MinTempS, MaxTempS, BadS, BadNumS
-}; // NO MORE THAN 32 or 64 bytes
+    MinHDiluteS, MaxHDiluteS, RCGlitchesS, GPSBaroScaleS, GyroFailS, RCFailsafesS, I2CFailS, MinTempS, MaxTempS, BadS, BadNumS }; // NO MORE THAN 32 or 64 bytes
 
 extern int16 Stats[];
 
@@ -1662,6 +1763,7 @@
 extern void SendParameters(uint8);
 extern void SendMinPacket(void);
 extern void SendArduStation(void);
+extern void SendPIDTuning(void);
 extern void SendCustom(void);
 extern void SensorTrace(void);
 extern void CheckTelemetry(void);
@@ -1672,8 +1774,8 @@
 enum PacketTags {UnknownPacketTag = 0, LevPacketTag, NavPacketTag, MicropilotPacketTag, WayPacketTag,
                  AirframePacketTag, NavUpdatePacketTag, BasicPacketTag, RestartPacketTag, TrimblePacketTag,
                  MessagePacketTag, EnvironmentPacketTag, BeaconPacketTag, UAVXFlightPacketTag,
-                 UAVXNavPacketTag, UAVXStatsPacketTag, UAVXControlPacketTag, UAVXParamPacketTag, UAVXMinPacketTag, 
-                 UAVXArmParamPacketTag, UAVXStickPacketTag, FrSkyPacketTag = 99
+                 UAVXNavPacketTag, UAVXStatsPacketTag, UAVXControlPacketTag, UAVXParamPacketTag, UAVXMinPacketTag,
+                 UAVXArmParamPacketTag, UAVXStickPacketTag,  UAVXCustomPacketTag, FrSkyPacketTag = 99
                 };
 
 enum TelemetryTypes { NoTelemetry, GPSTelemetry, UAVXTelemetry, UAVXControlTelemetry, UAVXMinTelemetry,
@@ -1725,7 +1827,7 @@
 extern void DoStartingBeeps(uint8);
 extern real32 SlewLimit(real32, real32, real32);
 extern real32 DecayX(real32, real32);
-extern void LPFilter(real32*, real32*, real32, real32);
+extern real32 LPFilter(real32, real32, real32);
 extern void CheckAlarms(void);
 extern void Timing(uint8, uint32);
 
@@ -1734,6 +1836,8 @@
     uint32 Count;
 } TimingRec;
 
+extern uint32 BeeperOnTime, BeeperOffTime;
+
 enum Timed { GetAttitudeT , UnknownT };
 extern TimingRec Times[];
 
diff -r 1e3318a30ddd -r 90292f8bd179 UAVXRevision.h
--- a/UAVXRevision.h	Fri Feb 25 01:35:24 2011 +0000
+++ b/UAVXRevision.h	Tue Apr 26 12:12:29 2011 +0000
@@ -2,7 +2,7 @@
 // =                              UAVXArm Quadrocopter Controller                                =
 // =                           Copyright (c) 2008 by Prof. Greg Egan                             =
 // =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
-// =                     http://code.google.com/p/uavp-mods/ http://uavp.ch                      =
+// =                           http://code.google.com/p/uavp-mods/                               =
 // ===============================================================================================
 
 //    This is part of UAVXArm.
diff -r 1e3318a30ddd -r 90292f8bd179 UAVXRevisionSVN.h
--- a/UAVXRevisionSVN.h	Fri Feb 25 01:35:24 2011 +0000
+++ b/UAVXRevisionSVN.h	Tue Apr 26 12:12:29 2011 +0000
@@ -1,10 +1,9 @@
-
-// =======================================================================
-// =                     UAVX Quadrocopter Controller                    =
-// =               Copyright (c) 2008-9 by Prof. Greg Egan               =
-// =     Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer       =
-// =                          http://uavp.ch                             =
-// =======================================================================
+// ===============================================================================================
+// =                              UAVXArm Quadrocopter Controller                                =
+// =                           Copyright (c) 2008 by Prof. Greg Egan                             =
+// =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
+// =                           http://code.google.com/p/uavp-mods/                               =
+// ===============================================================================================
 
 //  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
@@ -21,7 +20,7 @@
 //  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
 
 
-#define Version	"1.$WCREV$"
+#define Version    "1.$WCREV$"
 
 
 
diff -r 1e3318a30ddd -r 90292f8bd179 accel.c
--- a/accel.c	Fri Feb 25 01:35:24 2011 +0000
+++ b/accel.c	Tue Apr 26 12:12:29 2011 +0000
@@ -2,7 +2,7 @@
 // =                              UAVXArm Quadrocopter Controller                                =
 // =                           Copyright (c) 2008 by Prof. Greg Egan                             =
 // =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
-// =                     http://code.google.com/p/uavp-mods/ http://uavp.ch                      =
+// =                           http://code.google.com/p/uavp-mods/                               =
 // ===============================================================================================
 
 //    This is part of UAVXArm.
@@ -27,20 +27,21 @@
 void AccelerometerTest(void);
 void InitAccelerometers(void);
 
-real32 Vel[3], Acc[3], AccNeutral[3], Accp[3];
+real32 Vel[3], AccADC[3], AccADCp[3], AccNoise[3], Acc[3], AccNeutral[3], Accp[3];
 int16 NewAccNeutral[3];
 uint8 AccelerometerType;
+real32 GravityR;
 
 void ShowAccType(void) {
     switch ( AccelerometerType ) {
         case LISLAcc:
-            TxString("LIS3L");
+            TxString(" LIS3L");
             break;
         case ADXL345Acc:
-            TxString("ADXL345");
+            TxString(" ADXL345");
             break;
         case AccUnknown:
-            TxString("unknown");
+            TxString(" unknown");
             break;
         default:
             ;
@@ -61,25 +62,33 @@
             Acc[UD] = 1.0;
             break;
     } // switch
+
 } // ReadAccelerometers
 
 void GetNeutralAccelerations(void) {
-    static uint8 i, a;
+    static int16 i;
+    static uint8 a;
     static real32 Temp[3] = {0.0, 0.0, 0.0};
+    const int16 Samples = 100;
 
     if ( F.AccelerationsValid ) {
-        for ( i = 16; i; i--) {
+        for ( i = Samples; i; i--) {
             ReadAccelerometers();
             for ( a = 0; a <(uint8)3; a++ )
-                Temp[a] += Acc[a];
+                Temp[a] += AccADC[a];
         }
 
         for ( a = 0; a <(uint8)3; a++ )
-            Temp[a] = Temp[a] * 0.0625;
+            Temp[a] /= Samples;
 
-        NewAccNeutral[BF] = Limit((int16)(Temp[BF] * 1000.0 ), -99, 99);
-        NewAccNeutral[LR] = Limit( (int16)(Temp[LR] * 1000.0 ), -99, 99);
-        NewAccNeutral[UD] = Limit( (int16)(( Temp[UD] - 1.0 ) * 1000.0) , -99, 99);
+        // removes other accelerations
+        GravityR = 1.0/sqrt(Sqr(Temp[BF])+Sqr(Temp[LR])+Sqr(Temp[UD]));
+        for ( a = 0; a <(uint8)3; a++ )
+            Acc[a] = Temp[a] * GravityR;
+
+        NewAccNeutral[BF] = Limit1((int16)(Acc[BF] * 1000.0 ), 99);
+        NewAccNeutral[LR] = Limit1( (int16)(Acc[LR] * 1000.0 ), 99);
+        NewAccNeutral[UD] = Limit1( (int16)(( Acc[UD] + 1.0 ) * 1000.0) , 99);
 
     } else
         for ( a = 0; a <(uint8)3; a++ )
@@ -89,8 +98,8 @@
 
 void GetAccelerations(void) {
 
+    static uint8 a;
     static real32 AccA;
-    static uint8 a;
 
     if ( F.AccelerationsValid ) {
         ReadAccelerometers();
@@ -98,15 +107,17 @@
         // Neutral[ {LR, BF, UD} ] pass through UAVPSet
         // and come back as AccMiddle[LR] etc.
 
-        Acc[BF] -= K[MiddleBF];
-        Acc[LR] -= K[MiddleLR];
-        Acc[UD] -= K[MiddleUD];
+        Acc[BF] = AccADC[BF] * GravityR - K[MiddleBF];
+        Acc[LR] = AccADC[LR] * GravityR - K[MiddleLR];
+        Acc[UD] = AccADC[UD] * GravityR - K[MiddleUD];
 
-        AccA = dT / ( OneOnTwoPiAccF + dT );
-        for ( a = 0; a < (uint8)3; a++ ) {
-            Acc[a] = Accp[a] + (Acc[a] - Accp[a]) * AccA;
+#ifndef SUPPRESS_ACC_FILTERS
+        AccA = dT / ( 1.0 / ( TWOPI * ACC_FREQ ) + dT );
+        for ( a = 0; a < (uint8)3; a++ )
+            Acc[a] = LPFilter( Acc[a], Accp[a], AccA );
+#endif // !SUPPRESS_ACC_FILTERS
+        for ( a = 0; a < (uint8)3; a++ )
             Accp[a] = Acc[a];
-        }
 
     } else {
         Acc[LR] = Acc[BF] = 0;
@@ -122,23 +133,27 @@
     InitAccelerometers();
 
     if ( F.AccelerationsValid ) {
-        ReadAccelerometers();
+        GetAccelerations();
+
+        TxString("Sensor & Max Delta\r\n");
 
         TxString("\tL->R: \t");
-        TxVal32( Acc[LR] * 1000.0, 3, 'G');
+        TxVal32( AccADC[LR], 0, HT);
+        TxVal32( AccNoise[LR], 0, 0);
         if ( fabs(Acc[LR]) > 0.2 )
             TxString(" fault?");
         TxNextLine();
 
         TxString("\tB->F: \t");
-        TxVal32( Acc[BF] * 1000.0, 3, 'G');
+        TxVal32( AccADC[BF], 0, HT);
+        TxVal32( AccNoise[BF], 0, 0);
         if ( fabs(Acc[BF]) > 0.2 )
             TxString(" fault?");
         TxNextLine();
 
-
         TxString("\tU->D:    \t");
-        TxVal32( Acc[UD] * 1000.0, 3, 'G');
+        TxVal32( AccADC[UD], 0, HT);
+        TxVal32( AccNoise[UD], 0, 0);
         if ( fabs(Acc[UD]) > 1.2 )
             TxString(" fault?");
         TxNextLine();
@@ -150,20 +165,19 @@
 
     F.AccelerationsValid = true; // optimistic
 
-    for ( a = 0; a < (uint8)3; a++ ) {
-        NewAccNeutral[a] = Acc[a] = Accp[a] = Vel[a] = 0.0;
-        Comp[a] = 0;
-    }
+    for ( a = 0; a < (uint8)3; a++ ) 
+        NewAccNeutral[a] = AccADC[a] = AccADCp[a] = AccNoise[a] = Acc[a] = Accp[a] = Vel[a] = 0.0;
+
     Acc[2] = Accp[2] = 1.0;
-
     if ( ADXL345AccActive() ) {
         InitADXL345Acc();
         AccelerometerType = ADXL345Acc;
 
     } else
-        if ( LISLAccActive() )
+        if ( LISLAccActive() ) {
+            InitLISLAcc();
             AccelerometerType = LISLAcc;
-        else
+        } else
             // check for other accs in preferred order
         {
             AccelerometerType = AccUnknown;
@@ -186,31 +200,19 @@
 void InitADXL345Acc(void);
 boolean ADXL345AccActive(void);
 
-const float GRAVITY_ADXL345 = 250.0; // ~4mG/LSB
-
 void ReadADXL345Acc(void) {
 
-    static uint8 a, r;
+    static uint8 a;
     static char b[6];
     static i16u X, Y, Z;
-
-    /*
-    r = 0;
-    while ( r == 0 ) {
-        I2CACC.start();
-        r = I2CACC.write(ADXL345_ID);
-        r = I2CACC.write(0x30);
-        r = I2CACC.read(true) & 0x80;
-        I2CACC.stop();
-    }
-    */
+    static real32 d;
 
     I2CACC.start();
-    r = I2CACC.write(ADXL345_WR);
-    r = I2CACC.write(0x32); // point to acc data
+    if ( I2CACC.write(ADXL345_WR) != I2C_ACK ) goto ADXL345Error; // point to acc data
+    if ( I2CACC.write(0x32) != I2C_ACK ) goto ADXL345Error; // point to acc data
     I2CACC.stop();
 
-    I2CACC.blockread(ADXL345_ID, b, 6);
+    if ( I2CACC.blockread(ADXL345_ID, b, 6) ) goto ADXL345Error;
 
     X.b1 = b[1];
     X.b0 = b[0];
@@ -220,60 +222,77 @@
     Z.b0 = b[4];
 
     if ( F.Using9DOF ) { // SparkFun/QuadroUFO 9DOF breakouts pins forward components up
-        Acc[BF] = -Y.i16;
-        Acc[LR] = -X.i16;
-        Acc[UD] = -Z.i16;
-    } else {// SparkFun 6DOF breakouts pins forward components down 
-        Acc[BF] = -X.i16;
-        Acc[LR] = -Y.i16;
-        Acc[UD] = -Z.i16; 
+        AccADC[BF] = -Y.i16;
+        AccADC[LR] = -X.i16;
+        AccADC[UD] = -Z.i16;
+    } else {// SparkFun 6DOF breakouts pins forward components down
+        AccADC[BF] = -X.i16;
+        AccADC[LR] = -Y.i16;
+        AccADC[UD] = Z.i16;
     }
 
-    // LP filter here?
+    for ( a = 0; a < (int8)3; a++ ) {
+        d = fabs(AccADC[a]-AccADCp[a]);
+        if ( d>AccNoise[a] ) AccNoise[a] = d;
+    }
+
+    return;
 
-    for ( a = 0; a < (int8)3; a++ )
-        Acc[a] /= GRAVITY_ADXL345;
+ADXL345Error:
+    I2CACC.stop();
+
+    I2CError[ADXL345_ID]++;
+    if ( State == InFlight ) {
+        Stats[AccFailS]++;    // data over run - acc out of range
+        // use neutral values!!!!
+        F.AccFailure = true;
+    }
 
 } // ReadADXL345Acc
 
 void InitADXL345Acc() {
-    static uint8 r;
 
     I2CACC.start();
-    I2CACC.write(ADXL345_WR);
-    r = I2CACC.write(0x2D);  // power register
-    r = I2CACC.write(0x08);  // measurement mode
+    if ( I2CACC.write(ADXL345_WR) != I2C_ACK ) goto ADXL345Error;;
+    if ( I2CACC.write(0x2D) != I2C_ACK ) goto ADXL345Error;  // power register
+    if ( I2CACC.write(0x08) != I2C_ACK ) goto ADXL345Error;  // measurement mode
     I2CACC.stop();
 
     Delay1mS(5);
 
     I2CACC.start();
-    r = I2CACC.write(ADXL345_WR);
-    r =  I2CACC.write(0x31);  // format
-    r =  I2CACC.write(0x08);  // full resolution, 2g
+    if ( I2CACC.write(ADXL345_WR) != I2C_ACK ) goto ADXL345Error;
+    if ( I2CACC.write(0x31) != I2C_ACK ) goto ADXL345Error;  // format
+    if ( I2CACC.write(0x08) != I2C_ACK ) goto ADXL345Error;  // full resolution, 2g
     I2CACC.stop();
 
     Delay1mS(5);
 
     I2CACC.start();
-    r =  I2CACC.write(ADXL345_WR);
-    r =  I2CACC.write(0x2C);  // Rate
-    r =  I2CACC.write(0x09);  // 50Hz, 400Hz 0x0C
+    if ( I2CACC.write(ADXL345_WR) != I2C_ACK ) goto ADXL345Error;
+    if ( I2CACC.write(0x2C) != I2C_ACK ) goto ADXL345Error;  // Rate
+    if ( I2CACC.write(0x09) != I2C_ACK ) goto ADXL345Error;  // 50Hz, 400Hz 0x0C
     I2CACC.stop();
 
     Delay1mS(5);
 
+    return;
+
+ADXL345Error:
+    I2CACC.stop();
+
+    I2CError[ADXL345_ID]++;
+
 } // InitADXL345Acc
 
 boolean ADXL345AccActive(void) {
 
-    I2CACC.start();
-    F.AccelerationsValid = I2CACC.write(ADXL345_WR) == I2C_ACK;
-    I2CACC.stop();
-    
-    TrackMinI2CRate(400000);
-     
-    return( true ); //zzz F.AccelerationsValid );
+    F.AccelerationsValid = I2CACCAddressResponds(ADXL345_ID);
+
+    if ( F.AccelerationsValid)
+        TrackMinI2CRate(400000);
+
+    return( F.AccelerationsValid );
 
 } // ADXL345AccActive
 
@@ -281,81 +300,107 @@
 
 // LIS3LV02DG Accelerometer 400KHz
 
-void WriteLISL(uint8, uint8);
+boolean WriteLISL(uint8, uint8);
 void ReadLISLAcc(void);
+void InitLISLAcc(void);
 boolean LISLAccActive(void);
 
-const float GRAVITY_LISL = 1024.0;
+void ReadLISLAcc(void) {
 
-void ReadLISLAcc(void) {
     static uint8 a;
+    static real32 d;
     static char b[6];
     static i16u X, Y, Z;
 
     F.AccelerationsValid = I2CACCAddressResponds( LISL_ID ); // Acc still there?
-    if ( F.AccelerationsValid ) {
+
+    if ( !F.AccelerationsValid ) goto LISLError;
 
-        // Ax.b0 = I2CACC.write(LISL_OUTX_L + LISL_INCR_ADDR + LISL_READ);
-
-        I2CACC.blockread(LISL_READ, b, 6);
+    I2CACC.start();
+    if ( I2CACC.write(LISL_WR) != I2C_ACK ) goto LISLError;
+    if ( I2CACC.write(LISL_OUTX_L | 0x80 ) != I2C_ACK ) goto LISLError; // auto increment address
+    I2CACC.stop();
 
-        X.b1 = b[1];
-        X.b0 = b[0];
-        Y.b1 = b[3];
-        Y.b0 = b[2];
-        Z.b1 = b[5];
-        Z.b0 = b[4];
+    if ( I2CACC.blockread(LISL_RD, b, 6) ) goto LISLError;
 
-        Acc[BF] = Z.i16;
-        Acc[LR] = -X.i16;
-        Acc[UD] = Y.i16;
+    X.b1 = b[1];
+    X.b0 = b[0];
+    Y.b1 = b[3];
+    Y.b0 = b[2];
+    Z.b1 = b[5];
+    Z.b0 = b[4];
 
-        // LP Filter here?
+    // UAVP Breakout Board pins to the rear components up
+    AccADC[BF] = Y.i16;
+    AccADC[LR] = X.i16;
+    AccADC[UD] = -Z.i16;
 
-        for ( a = 0; a < (uint8)3; a++ )
-            Acc[a] /= GRAVITY_LISL;
+    for ( a = 0; a < (int8)3; a++ ) {
+        d = fabs(AccADC[a]-AccADCp[a]);
+        if ( d>AccNoise[a] ) AccNoise[a] = d;
+    }
+
+    return;
 
-    } else {
-        for ( a = 0; a < (uint8)3; a++ )
-            Acc[a] = AccNeutral[a];
-        Acc[UD] += 1.0;
+LISLError:
+    I2CACC.stop();
+
+    I2CError[LISL_ID]++;
 
-        if ( State == InFlight ) {
-            Stats[AccFailS]++;    // data over run - acc out of range
-            // use neutral values!!!!
-            F.AccFailure = true;
-        }
+    if ( State == InFlight ) {
+        Stats[AccFailS]++;    // data over run - acc out of range
+        // use neutral values!!!!
+        F.AccFailure = true;
     }
+
 } // ReadLISLAccelerometers
 
-void WriteLISL(uint8 d, uint8 a) {
+boolean WriteLISL(uint8 d, uint8 a) {
+
     I2CACC.start();
-    I2CACC.write(a);
-    I2CACC.write(d);
+    if ( I2CACC.write(LISL_WR) != I2C_ACK ) goto LISLError;
+    if ( I2CACC.write(a) != I2C_ACK ) goto LISLError;
+    if ( I2CACC.write(d) != I2C_ACK ) goto LISLError;
+    I2CACC.stop();
+
+    return(false);
+
+LISLError:
     I2CACC.stop();
+
+    I2CError[LISL_ID]++;
+
+    return(true);
+
 } // WriteLISL
 
+void InitLISLAcc(void) {
+
+    if ( WriteLISL(0x4a, LISL_CTRLREG_2) ) goto LISLError; // enable 3-wire, BDU=1, +/-2g
+    if ( WriteLISL(0xc7, LISL_CTRLREG_1) ) goto LISLError; // on always, 40Hz sampling rate,  10Hz LP cutoff, enable all axes
+    if ( WriteLISL(0, LISL_CTRLREG_3) ) goto LISLError;
+    if ( WriteLISL(0x40, LISL_FF_CFG) ) goto LISLError;    // latch, no interrupts;
+    if ( WriteLISL(0, LISL_FF_THS_L) ) goto LISLError;
+    if ( WriteLISL(0xFC, LISL_FF_THS_H) ) goto LISLError;  // -0,5g threshold
+    if ( WriteLISL(255, LISL_FF_DUR) ) goto LISLError;
+    if ( WriteLISL(0, LISL_DD_CFG) ) goto LISLError;
+
+    TrackMinI2CRate(400000);
+    F.AccelerationsValid = true;
+
+    return;
+
+LISLError:
+    F.AccelerationsValid = false;
+
+} // InitLISLAcc
+
 boolean LISLAccActive(void) {
-    F.AccelerationsValid = false;
-    /*
-        WriteLISL(0x4a, LISL_CTRLREG_2);           // enable 3-wire, BDU=1, +/-2g
+
+    F.AccelerationsValid = I2CACCAddressResponds( LISL_ID );
 
-        if ( I2CACC.write(LISL_ID) == I2C_ACK ) {
-            WriteLISL(0xc7, LISL_CTRLREG_1);       // on always, 40Hz sampling rate,  10Hz LP cutoff, enable all axes
-            WriteLISL(0, LISL_CTRLREG_3);
-            WriteLISL(0x40, LISL_FF_CFG);          // latch, no interrupts;
-            WriteLISL(0, LISL_FF_THS_L);
-            WriteLISL(0xFC, LISL_FF_THS_H);        // -0,5g threshold
-            WriteLISL(255, LISL_FF_DUR);
-            WriteLISL(0, LISL_DD_CFG);
-            F.AccelerationsValid = true;
-        } else
-            F.AccFailure = true;
-    */
-    
-    TrackMinI2CRate(400000);
-        
-    return ( false );//F.AccelerationsValid );
+    return ( F.AccelerationsValid );
+
 } // LISLAccActive
 
 
diff -r 1e3318a30ddd -r 90292f8bd179 analog.c
--- a/analog.c	Fri Feb 25 01:35:24 2011 +0000
+++ b/analog.c	Tue Apr 26 12:12:29 2011 +0000
@@ -2,7 +2,7 @@
 // =                              UAVXArm Quadrocopter Controller                                =
 // =                           Copyright (c) 2008 by Prof. Greg Egan                             =
 // =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
-// =                     http://code.google.com/p/uavp-mods/ http://uavp.ch                      =
+// =                           http://code.google.com/p/uavp-mods/                               =
 // ===============================================================================================
 
 //    This is part of UAVXArm.
@@ -35,7 +35,7 @@
     static uint32 data;
 
     // Select channel and start conversion
-    LPC_ADC->ADCR &= ~0xFF;
+    LPC_ADC->ADCR &= ~0xff;
     LPC_ADC->ADCR |= 1 << 5; // ADC0[5]
     LPC_ADC->ADCR |= 1 << 24;
 
@@ -51,7 +51,7 @@
 
 void InitDirectAnalog(void) {
 
-// power on, clk divider /4
+    // power on, clk divider /4
     LPC_SC->PCONP |= (1 << 12);
     LPC_SC->PCLKSEL0 &= ~(0x3 << 24);
 
@@ -77,7 +77,6 @@
 
     static real32 r;
 
-DebugPin = 1;
     switch (p) {
         case ADCPitch:
             r = PitchADC.read();
@@ -98,8 +97,6 @@
             r =  BatteryVoltsADC.read();
             break;
     }
-    
-  DebugPin = 0;
 
     return ( r );
 
diff -r 1e3318a30ddd -r 90292f8bd179 attitude.c
--- a/attitude.c	Fri Feb 25 01:35:24 2011 +0000
+++ b/attitude.c	Tue Apr 26 12:12:29 2011 +0000
@@ -2,7 +2,7 @@
 // =                              UAVXArm Quadrocopter Controller                                =
 // =                           Copyright (c) 2008 by Prof. Greg Egan                             =
 // =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
-// =                     http://code.google.com/p/uavp-mods/ http://uavp.ch                      =
+// =                           http://code.google.com/p/uavp-mods/                               =
 // ===============================================================================================
 
 //    This is part of UAVXArm.
@@ -20,53 +20,85 @@
 
 #include "UAVXArm.h"
 
-//#define USE_GYRO_RATE
-
 // Reference frame is positive X forward, Y left, Z down, Roll right, Pitch up, Yaw CW.
+// CAUTION: Because of the coordinate frame the LR Acc sense must be negated for roll compensation.
 
-void AttitudeFailsafeEstimate(void);
-void DoLegacyYawComp(void);
+void AdaptiveYawLPFreq(void);
+void DoLegacyYawComp(uint8);
+void NormaliseAccelerations(void);
 void AttitudeTest(void);
+void InitAttitude(void);
 
-real32 dT, HalfdT, dTR, dTmS;
+real32 AccMagnitude;
+real32 EstAngle[3][MaxAttitudeScheme];
+real32 EstRate[3][MaxAttitudeScheme];
+real32 Correction[2];
+real32 YawFilterLPFreq;
+real32 dT, dTOn2, dTR, dTmS;
 uint32 uSp;
-uint8 AttitudeMethod = PremerlaniDCM; //MadgwickIMU PremerlaniDCM MadgwickAHRS;
 
-void AttitudeFailsafeEstimate(void) {
+uint8 AttitudeMethod = Wolferl; //Integrator, Wolferl MadgwickIMU PremerlaniDCM MadgwickAHRS, MultiWii;
+
+void AdaptiveYawLPFreq(void) { // Filter LP freq is decreased with reduced yaw stick deflection
 
-    static uint8 i;
+    YawFilterLPFreq = ( MAX_YAW_FREQ*abs(DesiredYaw) / RC_NEUTRAL );
+    YawFilterLPFreq = Limit(YawFilterLPFreq, 0.5, MAX_YAW_FREQ);
+
+} // AdaptiveYawFilterA
 
-    for ( i = 0; i < (uint8)3; i++ ) {
-        Rate[i] = Gyro[i];
-        Angle[i] += Rate[i] * dTmS;
-    }
-} // AttitudeFailsafeEstimate
+real32 HE;
+    
+void DoLegacyYawComp(uint8 S) {
 
-void DoLegacyYawComp(void) {
+#define COMPASS_MIDDLE          10                 // yaw stick neutral dead zone
+#define DRIFT_COMP_YAW_RATE     QUARTERPI          // Radians/Sec
 
-    static real32 Temp, HE;
+    static int16 Temp;
+
+    // Yaw Angle here is meant to be interpreted as the Heading Error
+
+    Rate[Yaw] = Gyro[Yaw];
 
-    if ( F.CompassValid ) {
-        // + CCW
-        Temp = DesiredYaw - Trim[Yaw];
-        if ( fabs(Temp) > COMPASS_MIDDLE ) { // acquire new heading
-            DesiredHeading = Heading;
-            HE = 0.0;
+    Temp = DesiredYaw - Trim[Yaw];
+    if ( F.CompassValid )  // CW+
+        if ( abs(Temp) > COMPASS_MIDDLE ) {
+            DesiredHeading = Heading; // acquire new heading
+            Angle[Yaw] = 0.0;
         } else {
-            HE = MakePi(DesiredHeading - Heading);
-            HE = Limit(HE, -SIXTHPI, SIXTHPI); // 30 deg limit
-            HE = HE * K[CompassKp];
-            Rate[Yaw] -= Limit(HE, -COMPASS_MAXDEV, COMPASS_MAXDEV);
+            HE = MinimumTurn(DesiredHeading - Heading);
+            HE = Limit1(HE, SIXTHPI); // 30 deg limit
+            HE = HE*K[CompassKp];
+            Rate[Yaw] = -Limit1(HE, DRIFT_COMP_YAW_RATE);
         }
+    else {
+        DesiredHeading = Heading;
+        Angle[Yaw] = 0.0;
     }
 
-    Angle[Yaw] += Rate[Yaw];
-    Angle[Yaw] = Limit(Angle[Yaw], -K[YawIntLimit], K[YawIntLimit]);
-
-    Angle[Yaw] = DecayX(Angle[Yaw], 0.0002);                 // GKE added to kill gyro drift
+    Angle[Yaw] += Rate[Yaw]*dT;
+ //   Angle[Yaw] = Limit1(Angle[Yaw], K[YawIntLimit]);
 
 } // DoLegacyYawComp
 
+void NormaliseAccelerations(void) {
+
+    const real32 MIN_ACC_MAGNITUDE = 0.7; // below this the accelerometers are deemed unreliable - falling?
+
+    static real32 ReNorm;
+
+    AccMagnitude = sqrt(Sqr(Acc[BF]) + Sqr(Acc[LR]) + Sqr(Acc[UD]));
+    F.AccMagnitudeOK = AccMagnitude > MIN_ACC_MAGNITUDE;
+    if ( F.AccMagnitudeOK ) {
+        ReNorm = 1.0 / AccMagnitude;
+        Acc[BF] *= ReNorm;
+        Acc[LR] *= ReNorm;
+        Acc[UD] *= ReNorm;
+    } else {
+        Acc[LR] = Acc[BF]  = 0.0;
+        Acc[UD] = 1.0;
+    }
+} // NormaliseAccelerations
+
 void GetAttitude(void) {
 
     static uint32 Now;
@@ -79,24 +111,18 @@
         GetAccelerations();
     }
 
-    if ( mSClock() > mS[CompassUpdate] ) {
-        mS[CompassUpdate] = mSClock() + COMPASS_UPDATE_MS;
-        GetHeading();
-#ifndef USE_DCM_YAW
-        DoLegacyYawComp();
-#endif // !USE_DCM_YAW
-    }
-
     Now = uSClock();
-    dT = ( Now - uSp) * 0.000001;
-    HalfdT = 0.5 * dT;
+    dT = ( Now - uSp)*0.000001;
+    dTOn2 = 0.5 * dT;
     dTR = 1.0 / dT;
     uSp = Now;
 
+    GetHeading(); // only updated every 50mS but read continuously anyway
+
     if ( GyroType == IRSensors ) {
 
         for ( i = 0; i < (uint8)2; i++ ) {
-            Rate[i] = ( Angle[i] - Anglep[i] ) * dT;
+            Rate[i] = ( Angle[i] - Anglep[i] )*dT;
             Anglep[i] = Angle[i];
         }
 
@@ -104,23 +130,46 @@
 
     } else {
         DebugPin = true;
-        switch  ( AttitudeMethod ) {
-            case PremerlaniDCM:
-                DCMUpdate();
-                DCMNormalise();
-                DCMDriftCorrection();
-                DCMEulerAngles();
-                break;
-            case MadgwickIMU:
-                IMUupdate(Gyro[Roll], Gyro[Pitch], Gyro[Yaw], Acc[BF], Acc[LR], Acc[UD]);
-                EulerAngles();
-                //   DoLegacyYawComp();
-                break;
-            case MadgwickAHRS: // must have magnetometer
-                AHRSupdate(Gyro[Roll], Gyro[Pitch], Gyro[Yaw], Acc[BF], Acc[LR], Acc[UD], 1,0,0);//Mag[BF].V, Mag[LR].V, Mag[UD].V);
-                EulerAngles();
-                break;
-        } // switch
+
+        NormaliseAccelerations(); // rudimentary check for free fall etc
+
+        // Wolferl
+        DoWolferl();
+
+#ifdef INC_ALL_SCHEMES
+
+        // Complementary
+        DoCF();
+
+        // Kalman
+        DoKalman();
+
+        //Premerlani DCM
+        DoDCM();
+
+        // MultiWii
+        DoMultiWii();
+
+        // Madgwick IMU
+        // DoMadgwickIMU(Gyro[Roll], Gyro[Pitch], Gyro[Yaw], Acc[BF], -Acc[LR], -Acc[UD]);
+
+        //#define INC_IMU2
+
+#ifdef INC_IMU2
+        DoMadgwickIMU2(Gyro[Roll], Gyro[Pitch], Gyro[Yaw], Acc[BF], -Acc[LR], -Acc[UD]);    
+#else
+        Madgwick IMU April 30, 2010 Paper Version
+#endif
+        // Madgwick AHRS BROKEN
+         DoMadgwickAHRS(Gyro[Roll], Gyro[Pitch], Gyro[Yaw], Acc[BF], -Acc[LR], -Acc[UD], Mag[BF].V, Mag[LR].V, -Mag[UD].V);
+
+        // Integrator - REFERENCE/FALLBACK
+        DoIntegrator();
+
+#endif // INC_ALL_SCHEMES
+
+        Angle[Roll] = EstAngle[Roll][AttitudeMethod];
+        Angle[Pitch] = EstAngle[Pitch][AttitudeMethod];
 
         DebugPin = false;
     }
@@ -129,82 +178,105 @@
 
 } // GetAttitude
 
-
-void AttitudeTest(void) {
-
-    TxString("\r\nAttitude Test\r\n");
+//____________________________________________________________________________________________
 
-    GetAttitude();
+// Integrator
 
-    TxString("\r\ndT \t");
-    TxVal32(dT * 1000.0, 3, 0);
-    TxString(" Sec.\r\n\r\n");
-
-    if ( GyroType == IRSensors ) {
+void DoIntegrator(void) {
 
-        TxString("IR Sensors:\r\n");
-        TxString("\tRoll \t");
-        TxVal32(IR[Roll] * 100.0, 2, HT);
-        TxNextLine();
-        TxString("\tPitch\t");
-        TxVal32(IR[Pitch] * 100.0, 2, HT);
-        TxNextLine();
-        TxString("\tZ    \t");
-        TxVal32(IR[Yaw] * 100.0, 2, HT);
-        TxNextLine();
-        TxString("\tMin/Max\t");
-        TxVal32(IRMin * 100.0, 2, HT);
-        TxVal32(IRMax * 100.0, 2, HT);
-        TxString("\tSwing\t");
-        TxVal32(IRSwing * 100.0, 2, HT);
-        TxNextLine();
-
-    } else {
+    static uint8 g;
 
-        TxString("Rates (Raw and Compensated):\r\n");
-        TxString("\tRoll \t");
-        TxVal32(Gyro[Roll] * MILLIANGLE, 3, HT);
-        TxVal32(Rate[Roll] * MILLIANGLE, 3, 0);
-        TxNextLine();
-        TxString("\tPitch\t");
-        TxVal32(Gyro[Pitch] * MILLIANGLE, 3, HT);
-        TxVal32(Rate[Pitch] * MILLIANGLE, 3, 0);
-        TxNextLine();
-        TxString("\tYaw  \t");
-        TxVal32(Gyro[Yaw] * MILLIANGLE, 3, HT);
-        TxVal32(Rate[Yaw] * MILLIANGLE, 3, 0);
-        TxNextLine();
-
-        TxString("Accelerations:\r\n");
-        TxString("\tB->F \t");
-        TxVal32(Acc[BF] * 1000.0, 3, 0);
-        TxNextLine();
-        TxString("\tL->R \t");
-        TxVal32(Acc[LR] * 1000.0, 3, 0);
-        TxNextLine();
-        TxString("\tU->D \t");
-        TxVal32(Acc[UD] * 1000.0, 3, 0);
-        TxNextLine();
+    for ( g = 0; g < (uint8)3; g++ ) {
+        EstRate[g][Integrator] = Gyro[g];
+        EstAngle[g][Integrator] +=  EstRate[g][Integrator]*dT;
+        //  EstAngle[g][Integrator] = DecayX(EstAngle[g][Integrator], 0.0001*dT);
     }
 
-    if ( CompassType != HMC6352 ) {
-        TxString("Magnetic:\r\n");
-        TxString("\tX    \t");
-        TxVal32(Mag[Roll].V, 0, 0);
-        TxNextLine();
-        TxString("\tY    \t");
-        TxVal32(Mag[Pitch].V, 0, 0);
-        TxNextLine();
-        TxString("\tZ    \t");
-        TxVal32(Mag[Yaw].V, 0, 0);
-        TxNextLine();
+} // DoIntegrator
+
+//____________________________________________________________________________________________
+
+// Original simple accelerometer compensation of gyros developed for UAVP by Wolfgang Mahringer
+// and adapted for UAVXArm
+
+void DoWolferl(void) { // NO YAW ESTIMATE
+
+    const real32 WKp = 0.13; // 0.1
+
+    static real32 Grav[2], Dyn[2];
+    static real32 CompStep;
+
+    Rate[Roll] = Gyro[Roll];
+    Rate[Pitch] = Gyro[Pitch];
+
+    if ( F.AccMagnitudeOK ) {
+
+        CompStep = WKp*dT;
+
+        // Roll
+
+        Grav[LR] = -sin(EstAngle[Roll][Wolferl]);   // original used approximation for small angles
+        Dyn[LR] = 0.0; //Rate[Roll];                // lateral acceleration due to rate - do later:).
+
+        Correction[LR] = -Acc[LR] + Grav[LR] + Dyn[LR]; // Acc is reversed
+        Correction[LR] = Limit1(Correction[LR], CompStep);
+
+        EstAngle[Roll][Wolferl] += Rate[Roll]*dT;
+        EstAngle[Roll][Wolferl] += Correction[LR];
+
+        // Pitch
+
+        Grav[BF] = -sin(EstAngle[Pitch][Wolferl]);
+        Dyn[BF] = 0.0; // Rate[Pitch];
+
+        Correction[BF] = Acc[BF] + Grav[BF] + Dyn[BF];
+        Correction[BF] = Limit1(Correction[BF], CompStep);
+
+        EstAngle[Pitch][Wolferl] += Rate[Pitch]*dT;
+        EstAngle[Pitch][Wolferl] += Correction[BF];
+        
+    } else {
+    
+        EstAngle[Roll][Wolferl] += Rate[Roll]*dT;
+        EstAngle[Pitch][Wolferl] += Rate[Pitch]*dT;
+        
     }
 
-    TxString("Heading: \t");
-    TxVal32(Make2Pi(Heading) * MILLIANGLE, 3, 0);
-    TxNextLine();
+} // DoWolferl
+
+//_________________________________________________________________________________
+
+// Complementary Filter originally authored by RoyLB
+// http://www.rcgroups.com/forums/showpost.php?p=12082524&postcount=1286
+
+const real32 TauCF = 1.1;
+
+real32 AngleCF[2] = {0,0};
+real32 F0[2] = {0,0};
+real32 F1[2] = {0,0};
+real32 F2[2] = {0,0};
+
+real32 CF(uint8 a, real32 NewAngle, real32 NewRate) {
 
-} // AttitudeTest
+    if ( F.AccMagnitudeOK ) {
+        F0[a] = (NewAngle - AngleCF[a])*Sqr(TauCF);
+        F2[a] += F0[a]*dT;
+        F1[a] = F2[a] + (NewAngle - AngleCF[a])*2.0*TauCF + NewRate;
+        AngleCF[a] = (F1[a]*dT) + AngleCF[a];
+    } else
+        AngleCF[a] += NewRate*dT;
+
+    return ( AngleCF[a] ); // This is actually the current angle, but is stored for the next iteration
+} // CF
+
+void DoCF(void) { // NO YAW ANGLE ESTIMATE
+
+    EstAngle[Roll][Complementary] = CF(Roll, asin(-Acc[LR]), Gyro[Roll]);
+    EstAngle[Pitch][Complementary] = CF(Pitch, asin(-Acc[BF]), Gyro[Pitch]); // zzz minus???
+    EstRate[Roll][Complementary] = Gyro[Roll];
+    EstRate[Pitch][Complementary] = Gyro[Pitch];
+
+} // DoCF
 
 //____________________________________________________________________________________________
 
@@ -212,19 +284,14 @@
 // Direction Cosine Matrix IMU: Theory, Draft 17 June 2009. This paper draws upon the original
 // work by R. Mahony et al. - Thanks Rob!
 
+// SEEMS TO BE A FAIRLY LARGE PHASE DELAY OF 2 SAMPLE INTERVALS
+
 void DCMNormalise(void);
 void DCMDriftCorrection(void);
 void DCMMotionCompensation(void);
 void DCMUpdate(void);
 void DCMEulerAngles(void);
 
-// requires rescaling for the much faster PID cycle in UAVXArm
-// guess x5 initially
-#define Kp_RollPitch 25.0       // 5.0
-#define Ki_RollPitch 0.025      // 0.005
-#define Kp_Yaw 1.2
-#define Ki_Yaw 0.00002
-
 real32   RollPitchError[3] = {0,0,0};
 real32   OmegaV[3] = {0,0,0}; // corrected gyro data
 real32   OmegaP[3] = {0,0,0}; // proportional correction
@@ -233,6 +300,7 @@
 real32   DCM[3][3] = {{1,0,0 },{0,1,0} ,{0,0,1}};
 real32   U[3][3] = {{0,1,2},{ 3,4,5} ,{6,7,8}};
 real32   TempM[3][3] = {{0,0,0},{0,0,0},{ 0,0,0}};
+real32   AccV[3];
 
 void DCMNormalise(void) {
 
@@ -241,7 +309,7 @@
     static boolean Problem;
     static uint8 r;
 
-    Error = -VDot(&DCM[0][0], &DCM[1][0]) * 0.5;        //eq.19
+    Error = -VDot(&DCM[0][0], &DCM[1][0])*0.5;        //eq.19
 
     VScale(&TempM[0][0], &DCM[1][0], Error);            //eq.19
     VScale(&TempM[1][0], &DCM[0][0], Error);            //eq.19
@@ -249,13 +317,13 @@
     VAdd(&TempM[0][0], &TempM[0][0], &DCM[0][0]);       //eq.19
     VAdd(&TempM[1][0], &TempM[1][0], &DCM[1][0]);       //eq.19
 
-    VCross(&TempM[2][0],&TempM[0][0], &TempM[1][0]);    // c= a * b eq.20
+    VCross(&TempM[2][0],&TempM[0][0], &TempM[1][0]);    // c= a*b eq.20
 
     Problem = false;
     for ( r = 0; r < (uint8)3; r++ ) {
-        Renorm = VDot(&TempM[r][0],&TempM[r][0]);
+        Renorm = VDot(&TempM[r][0], &TempM[r][0]);
         if ( (Renorm <  1.5625) && (Renorm > 0.64) )
-            Renorm = 0.5 * (3.0 - Renorm);               //eq.21
+            Renorm = 0.5*(3.0 - Renorm);               //eq.21
         else
             if ( (Renorm < 100.0) && (Renorm > 0.01) )
                 Renorm = 1.0 / sqrt( Renorm );
@@ -282,34 +350,32 @@
 void DCMMotionCompensation(void) {
     // compensation for rate of change of velocity LR/BF needs GPS velocity but
     // updates probably too slow?
-    Acc[LR ] += 0.0;
-    Acc[BF] += 0.0;
+    AccV[LR ] += 0.0;
+    AccV[BF] += 0.0;
 } // DCMMotionCompensation
 
 void DCMDriftCorrection(void) {
 
-    static real32 ScaledOmegaP[3], ScaledOmegaI[3];
-    static real32 YawError[3];
-    static real32 AccMagnitude, AccWeight;
-    static real32 ErrorCourse;
-
-    AccMagnitude = sqrt( Sqr(Acc[0]) + Sqr(Acc[1]) + Sqr(Acc[2]) );
+    static real32 ScaledOmegaI[3];
 
-    // dynamic weighting of Accelerometer info (reliability filter)
-    // weight for Accelerometer info ( < 0.5G = 0.0, 1G = 1.0 , > 1.5G = 0.0)
-    AccWeight = Limit(1.0 - 2.0 * fabs(1.0 - AccMagnitude), 0.0, 1.0);
+    //DON'T USE  #define USE_DCM_YAW_COMP
+#ifdef USE_DCM_YAW_COMP
+    static real32 ScaledOmegaP[3];
+    static real32 YawError[3];
+    static real32 ErrorCourse;
+#endif // USE_DCM_YAW_COMP
 
-    VCross(&RollPitchError[0], &Acc[0], &DCM[2][0]); //adjust the reference ground
-    VScale(&OmegaP[0], &RollPitchError[0], Kp_RollPitch * AccWeight);
+    VCross(&RollPitchError[0], &AccV[0], &DCM[2][0]); //adjust the reference ground
+    VScale(&OmegaP[0], &RollPitchError[0], Kp_RollPitch);
 
-    VScale(&ScaledOmegaI[0], &RollPitchError[0], Ki_RollPitch * AccWeight);
+    VScale(&ScaledOmegaI[0], &RollPitchError[0], Ki_RollPitch);
     VAdd(&OmegaI[0], &OmegaI[0], &ScaledOmegaI[0]);
 
-#ifdef USE_DCM_YAW
+#ifdef USE_DCM_YAW_COMP
     // Yaw - drift correction based on compass/magnetometer heading
-    HeadingCos = cos( Heading );
-    HeadingSin = sin( Heading );
-    ErrorCourse = ( U[0][0] * HeadingSin ) - ( U[1][0] * HeadingCos );
+    HeadingCos = cos(Heading);
+    HeadingSin = sin(Heading);
+    ErrorCourse = ( U[0][0]*HeadingSin ) - ( U[1][0]*HeadingCos );
     VScale(YawError, &U[2][0], ErrorCourse );
 
     VScale(&ScaledOmegaP[0], &YawError[0], Kp_Yaw );
@@ -317,7 +383,8 @@
 
     VScale(&ScaledOmegaI[0], &YawError[0], Ki_Yaw );
     VAdd(&OmegaI[0], &OmegaI[0], &ScaledOmegaI[0]);
-#endif // USE_DCM_YAW
+#endif // USE_DCM_YAW_COMP
+
 } // DCMDriftCorrection
 
 void DCMUpdate(void) {
@@ -325,25 +392,29 @@
     static uint8 i, j, k;
     static real32 op[3];
 
+    AccV[BF] = Acc[BF];
+    AccV[LR] = -Acc[LR];
+    AccV[UD] = -Acc[UD];
+
     VAdd(&Omega[0], &Gyro[0], &OmegaI[0]);
     VAdd(&OmegaV[0], &Omega[0], &OmegaP[0]);
 
 //   MotionCompensation();
 
     U[0][0] =  0.0;
-    U[0][1] = -dT * OmegaV[2];   //-z
-    U[0][2] =  dT * OmegaV[1];   // y
-    U[1][0] =  dT * OmegaV[2];   // z
+    U[0][1] = -dT*OmegaV[2];   //-z
+    U[0][2] =  dT*OmegaV[1];   // y
+    U[1][0] =  dT*OmegaV[2];   // z
     U[1][1] =  0.0;
-    U[1][2] = -dT * OmegaV[0];   //-x
-    U[2][0] = -dT * OmegaV[1];   //-y
-    U[2][1] =  dT * OmegaV[0];   // x
+    U[1][2] = -dT*OmegaV[0];   //-x
+    U[2][0] = -dT*OmegaV[1];   //-y
+    U[2][1] =  dT*OmegaV[0];   // x
     U[2][2] =  0.0;
 
     for ( i = 0; i < (uint8)3; i++ )
         for ( j = 0; j < (uint8)3; j++ ) {
             for ( k = 0; k < (uint8)3; k++ )
-                op[k] = DCM[i][k] * U[k][j];
+                op[k] = DCM[i][k]*U[k][j];
 
             TempM[i][j] = op[0] + op[1] + op[2];
         }
@@ -361,15 +432,21 @@
     for ( g = 0; g < (uint8)3; g++ )
         Rate[g] = Gyro[g];
 
-    Angle[Pitch] = asin(DCM[2][0]);
-    Angle[Roll] = -atan2(DCM[2][1], DCM[2][2]);
+    EstAngle[Roll][PremerlaniDCM]= atan2(DCM[2][1], DCM[2][2]);
+    EstAngle[Pitch][PremerlaniDCM] = -asin(DCM[2][0]);
+    EstAngle[Yaw][PremerlaniDCM] = atan2(DCM[1][0], DCM[0][0]);
 
-#ifdef USE_DCM_YAW
-    Angle[Yaw] = atan2(DCM[1][0], DCM[0][0]);
-#endif // USE_DCM_YAW
+    // Est. Rates ???
 
 } // DCMEulerAngles
 
+void DoDCM(void) {
+    DCMUpdate();
+    DCMNormalise();
+    DCMDriftCorrection();
+    DCMEulerAngles();
+} // DoDCM
+
 //___________________________________________________________________________________
 
 // IMU.c
@@ -380,80 +457,200 @@
 
 // Quaternion implementation of the 'DCM filter' [Mahony et al.].
 
-// User must define 'HalfdT' as the (sample period / 2), and the filter gains 'MKp' and 'MKi'.
-
 // Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elements representing the estimated
 // orientation.  See my report for an overview of the use of quaternions in this application.
 
 // User must call 'IMUupdate()' every sample period and parse calibrated gyroscope ('gx', 'gy', 'gz')
-// and accelerometer ('ax', 'ay', 'ay') data.  Gyroscope units are radians/second, accelerometer
+// and accelerometer ('ax', 'ay', 'az') data.  Gyroscope units are radians/second, accelerometer
 // units are irrelevant as the vector is normalised.
 
-#include <math.h>
-
-void IMUupdate(real32 gx, real32 gy, real32 gz, real32 ax, real32 ay, real32 az);
-
-#define MKp 2.0          // proportional gain governs rate of convergence to accelerometer/magnetometer
-#define MKi 0.005f        // integral gain governs rate of convergence of gyroscope biases
+const real32 MKp = 2.0;           // proportional gain governs rate of convergence to accelerometer/magnetometer
+const real32 MKi = 1.0;          // integral gain governs rate of convergence of gyroscope biases // 0.005
 
+real32 exInt = 0.0, eyInt = 0.0, ezInt = 0.0;   // scaled integral error
 real32 q0 = 1.0, q1 = 0.0, q2 = 0.0, q3 = 0.0;  // quaternion elements representing the estimated orientation
-real32 exInt = 0.0, eyInt = 0.0, ezInt = 0.0;   // scaled integral error
 
-void IMUupdate(real32 gx, real32 gy, real32 gz, real32 ax, real32 ay, real32 az) {
+void DoMadgwickIMU(real32 gx, real32 gy, real32 gz, real32 ax, real32 ay, real32 az) {
 
-    static real32 rnorm;
+    static uint8 g;
+    static real32 ReNorm;
     static real32 vx, vy, vz;
     static real32 ex, ey, ez;
-    static real32 aMag;
+
+    if ( F.AccMagnitudeOK ) {
+
+        // estimated direction of gravity
+        vx = 2.0*(q1*q3 - q0*q2);
+        vy = 2.0*(q0*q1 + q2*q3);
+        vz = Sqr(q0) - Sqr(q1) - Sqr(q2) + Sqr(q3);
+
+        // error is sum of cross product between reference direction of field and direction measured by sensor
+        ex = (ay*vz - az*vy);
+        ey = (az*vx - ax*vz);
+        ez = (ax*vy - ay*vx);
+
+        // integral error scaled integral gain
+        exInt += ex*MKi*dT;
+        eyInt += ey*MKi*dT;
+        ezInt += ez*MKi*dT;
 
-//swap z and y?
+        // adjusted gyroscope measurements
+        gx += MKp*ex + exInt;
+        gy += MKp*ey + eyInt;
+        gz += MKp*ez + ezInt;
+
+        // integrate quaternion rate and normalise
+        q0 += (-q1*gx - q2*gy - q3*gz)*dTOn2;
+        q1 += (q0*gx + q2*gz - q3*gy)*dTOn2;
+        q2 += (q0*gy - q1*gz + q3*gx)*dTOn2;
+        q3 += (q0*gz + q1*gy - q2*gx)*dTOn2;
+
+        // normalise quaternion
+        ReNorm = 1.0  /sqrt(Sqr(q0) + Sqr(q1) + Sqr(q2) + Sqr(q3));
+        q0 *= ReNorm;
+        q1 *= ReNorm;
+        q2 *= ReNorm;
+        q3 *= ReNorm;
+
+        MadgwickEulerAngles(MadgwickIMU);
 
-    // normalise the measurements
-    aMag = sqrt(Sqr(ax) + Sqr(ay) + Sqr(az)); // zero Acc values into AHRS is FATAL
-    if ( aMag < 0.9 ) {
-        ax = ay = 0.0;
-        az = 1.0;
-    } else {
-        rnorm = 1.0/aMag;
-        ax *= rnorm;
-        ay *= rnorm;
-        az *= rnorm;
-    }
+    } else
+        for ( g = 0; g <(uint8)3; g++) {
+            EstRate[g][MadgwickIMU] = Gyro[g];
+            EstAngle[g][MadgwickIMU] += EstRate[g][MadgwickIMU]*dT;
+        }
+
+}  // DoMadgwickIMU
+
+//_________________________________________________________________________________
+
+// IMU.c
+// S.O.H. Madgwick, 
+// 'An Efficient Orientation Filter for Inertial and Inertial/Magnetic Sensor Arrays',
+// April 30, 2010
+
+#ifdef INC_IMU2
+
+boolean FirstIMU2 = true;
+real32 BetaIMU2 = 0.033;
+// const real32 BetaAHRS = 0.041;
 
-    // estimated direction of gravity
-    vx = 2.0*(q1*q3 - q0*q2);
-    vy = 2.0*(q0*q1 + q2*q3);
-    vz = Sqr(q0) - Sqr(q1) - Sqr(q2) + Sqr(q3);
+//Quaternion orientation of earth frame relative to auxiliary frame.
+real32 AEq_1;
+real32 AEq_2;
+real32 AEq_3;
+real32 AEq_4;
+
+//Estimated orientation quaternion elements with initial conditions.
+real32 SEq_1;
+real32 SEq_2;
+real32 SEq_3;
+real32 SEq_4;
+
+void DoMadgwickIMU2(real32 w_x, real32 w_y, real32 w_z, real32 a_x, real32 a_y, real32 a_z) {
+
+    static uint8 g;
+
+    //Vector norm.
+    static real32 Renorm;
+    //Quaternion rate from gyroscope elements.
+    static real32 SEqDot_omega_1, SEqDot_omega_2, SEqDot_omega_3, SEqDot_omega_4;
+    //Objective function elements.
+    static real32 f_1, f_2, f_3;
+    //Objective function Jacobian elements.
+    static real32 J_11or24, J_12or23, J_13or22, J_14or21, J_32, J_33;
+    //Objective function gradient elements.
+    static real32 SEqHatDot_1, SEqHatDot_2, SEqHatDot_3, SEqHatDot_4;
 
-    // error is sum of cross product between reference direction of field and direction measured by sensor
-    ex = (ay*vz - az*vy);
-    ey = (az*vx - ax*vz);
-    ez = (ax*vy - ay*vx);
+    //Auxiliary variables to avoid reapeated calcualtions.
+    static real32 halfSEq_1, halfSEq_2, halfSEq_3, halfSEq_4;
+    static real32 twoSEq_1, twoSEq_2, twoSEq_3;
+
+    if ( F.AccMagnitudeOK ) {
+
+        halfSEq_1 = 0.5*SEq_1;
+        halfSEq_2 = 0.5*SEq_2;
+        halfSEq_3 = 0.5*SEq_3;
+        halfSEq_4 = 0.5*SEq_4;
+        twoSEq_1 = 2.0*SEq_1;
+        twoSEq_2 = 2.0*SEq_2;
+        twoSEq_3 = 2.0*SEq_3;
+
+        //Compute the quaternion rate measured by gyroscopes.
+        SEqDot_omega_1 = -halfSEq_2*w_x - halfSEq_3*w_y - halfSEq_4*w_z;
+        SEqDot_omega_2 = halfSEq_1*w_x + halfSEq_3*w_z - halfSEq_4*w_y;
+        SEqDot_omega_3 = halfSEq_1*w_y - halfSEq_2*w_z + halfSEq_4*w_x;
+        SEqDot_omega_4 = halfSEq_1*w_z + halfSEq_2*w_y - halfSEq_3*w_x;
 
-    // integral error scaled integral gain
-    exInt += ex*MKi;
-    eyInt += ey*MKi;
-    ezInt += ez*MKi;
+        /*
+        //Normalise the accelerometer measurement.
+        Renorm = 1.0 / sqrt(Sqr(a_x) + Sqr(a_y) + Sqr(a_z));
+        a_x *= Renorm;
+        a_y *= Renorm;
+        a_z *= Renorm;
+        */
 
-    // adjusted gyroscope measurements
-    gx += MKp*ex + exInt;
-    gy += MKp*ey + eyInt;
-    gz += MKp*ez + ezInt;
+        //Compute the objective function and Jacobian.
+        f_1 = twoSEq_2*SEq_4 - twoSEq_1*SEq_3 - a_x;
+        f_2 = twoSEq_1*SEq_2 + twoSEq_3*SEq_4 - a_y;
+        f_3 = 1.0 - twoSEq_2*SEq_2 - twoSEq_3*SEq_3 - a_z;
+        //J_11 negated in matrix multiplication.
+        J_11or24 = twoSEq_3;
+        J_12or23 = 2.0*SEq_4;
+        //J_12 negated in matrix multiplication
+        J_13or22 = twoSEq_1;
+        J_14or21 = twoSEq_2;
+        //Negated in matrix multiplication.
+        J_32 = 2.0*J_14or21;
+        //Negated in matrix multiplication.
+        J_33 = 2.0*J_11or24;
 
-    // integrate quaternion rate and normalise
-    q0 += (-q1*gx - q2*gy - q3*gz)*HalfdT;
-    q1 += (q0*gx + q2*gz - q3*gy)*HalfdT;
-    q2 += (q0*gy - q1*gz + q3*gx)*HalfdT;
-    q3 += (q0*gz + q1*gy - q2*gx)*HalfdT;
+        //Compute the gradient (matrix multiplication).
+        SEqHatDot_1 = J_14or21*f_2 - J_11or24*f_1;
+        SEqHatDot_2 = J_12or23*f_1 + J_13or22*f_2 - J_32*f_3;
+        SEqHatDot_3 = J_12or23*f_2 - J_33*f_3 - J_13or22*f_1;
+        SEqHatDot_4 = J_14or21*f_1 + J_11or24*f_2;
+
+        //Normalise the gradient.
+        Renorm = 1.0 / sqrt(Sqr(SEqHatDot_1) + Sqr(SEqHatDot_2) + Sqr(SEqHatDot_3) + Sqr(SEqHatDot_4));
+        SEqHatDot_1 *= Renorm;
+        SEqHatDot_2 *= Renorm;
+        SEqHatDot_3 *= Renorm;
+        SEqHatDot_4 *= Renorm;
+
+        //Compute then integrate the estimated quaternion rate.
+        SEq_1 += (SEqDot_omega_1 - (BetaIMU2*SEqHatDot_1))*dT;
+        SEq_2 += (SEqDot_omega_2 - (BetaIMU2*SEqHatDot_2))*dT;
+        SEq_3 += (SEqDot_omega_3 - (BetaIMU2*SEqHatDot_3))*dT;
+        SEq_4 += (SEqDot_omega_4 - (BetaIMU2*SEqHatDot_4))*dT;
 
-    // normalise quaternion
-    rnorm = 1.0/sqrt(Sqr(q0) + Sqr(q1) + Sqr(q2) + Sqr(q3));
-    q0 *= rnorm;
-    q1 *= rnorm;
-    q2 *= rnorm;
-    q3 *= rnorm;
+        //Normalise quaternion
+        Renorm = 1.0 / sqrt(Sqr(SEq_1) + Sqr(SEq_2) + Sqr(SEq_3) + Sqr(SEq_4));
+        SEq_1 *= Renorm;
+        SEq_2 *= Renorm;
+        SEq_3 *= Renorm;
+        SEq_4 *= Renorm;
 
-}  // IMUupdate
+        if ( FirstIMU2 ) {
+            //Store orientation of auxiliary frame.
+            AEq_1 = SEq_1;
+            AEq_2 = SEq_2;
+            AEq_3 = SEq_3;
+            AEq_4 = SEq_4;
+            FirstIMU2 = false;
+        }
+
+        MadgwickEulerAngles(MadgwickIMU2);
+
+    } else
+        for ( g = 0; g <(uint8)3; g++) {
+            EstRate[g][MadgwickIMU2] = Gyro[g];
+            EstAngle[g][MadgwickIMU2] += EstRate[g][MadgwickIMU2]*dT;
+        }
+
+} // DoMadgwickIMU2
+
+#endif // INC_IMU2
 
 //_________________________________________________________________________________
 
@@ -466,137 +663,332 @@
 // Quaternion implementation of the 'DCM filter' [Mahoney et al].  Incorporates the magnetic distortion
 // compensation algorithms from my filter [Madgwick] which eliminates the need for a reference
 // direction of flux (bx bz) to be predefined and limits the effect of magnetic distortions to yaw
-// axis only.
+// a only.
 
-// User must define 'HalfdT' as the (sample period / 2), and the filter gains 'MKp' and 'MKi'.
+// User must define 'dTOn2' as the (sample period / 2), and the filter gains 'MKp' and 'MKi'.
 
 // Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elements representing the estimated
 // orientation.  See my report for an overview of the use of quaternions in this application.
 
 // User must call 'AHRSupdate()' every sample period and parse calibrated gyroscope ('gx', 'gy', 'gz'),
-// accelerometer ('ax', 'ay', 'ay') and magnetometer ('mx', 'my', 'mz') data.  Gyroscope units are
+// accelerometer ('ax', 'ay', 'az') and magnetometer ('mx', 'my', 'mz') data.  Gyroscope units are
 // radians/second, accelerometer and magnetometer units are irrelevant as the vector is normalised.
 
-void AHRSupdate(real32 gx, real32 gy, real32 gz, real32 ax, real32 ay, real32 az, real32 mx, real32 my, real32 mz) {
+void DoMadgwickAHRS(real32 gx, real32 gy, real32 gz, real32 ax, real32 ay, real32 az, real32 mx, real32 my, real32 mz) {
 
-    static real32 rnorm;
+    static uint8 g;
+    static real32 ReNorm;
     static real32 hx, hy, hz, bx2, bz2, mx2, my2, mz2;
     static real32 vx, vy, vz, wx, wy, wz;
     static real32 ex, ey, ez;
     static real32 q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
-    static real32 aMag;
+
+    if ( F.AccMagnitudeOK ) {
+
+        // auxiliary variables to reduce number of repeated operations
+        q0q0 = q0*q0;
+        q0q1 = q0*q1;
+        q0q2 = q0*q2;
+        q0q3 = q0*q3;
+        q1q1 = q1*q1;
+        q1q2 = q1*q2;
+        q1q3 = q1*q3;
+        q2q2 = q2*q2;
+        q2q3 = q2*q3;
+        q3q3 = q3*q3;
+
+        ReNorm = 1.0 / sqrt( Sqr( mx ) + Sqr( my ) + Sqr( mz ) );
+        mx *= ReNorm;
+        my *= ReNorm;
+        mz *= ReNorm;
+        mx2 = 2.0*mx;
+        my2 = 2.0*my;
+        mz2 = 2.0*mz;
+
+        // compute reference direction of flux
+        hx = mx2*(0.5 - q2q2 - q3q3) + my2*(q1q2 - q0q3) + mz2*(q1q3 + q0q2);
+        hy = mx2*(q1q2 + q0q3) + my2*( 0.5 - q1q1 - q3q3) + mz2*(q2q3 - q0q1);
+        hz = mx2*(q1q3 - q0q2) + my2*(q2q3 + q0q1) + mz2*( 0.5 - q1q1 - q2q2 );
+        bx2 = 2.0*sqrt( Sqr( hx ) + Sqr( hy ) );
+        bz2 = 2.0*hz;
+
+        // estimated direction of gravity and flux (v and w)
+        vx = 2.0*(q1q3 - q0q2);
+        vy = 2.0*(q0q1 + q2q3);
+        vz = q0q0 - q1q1 - q2q2 + q3q3;
+
+        wx = bx2*(0.5 - q2q2 - q3q3) + bz2*(q1q3 - q0q2);
+        wy = bx2*(q1q2 - q0q3) + bz2*( q0q1 + q2q3 );
+        wz = bx2*(q0q2 + q1q3) + bz2*( 0.5 - q1q1 - q2q2 );
+
+        // error is sum of cross product between reference direction of fields and direction measured by sensors
+        ex = (ay*vz - az*vy) + (my*wz - mz*wy);
+        ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
+        ez = (ax*vy - ay*vx) + (mx*wy - my*wx);
+
+        // integral error scaled integral gain
+        exInt += ex*MKi*dT;
+        eyInt += ey*MKi*dT;
+        ezInt += ez*MKi*dT;
+
+        // adjusted gyroscope measurements
+        gx += MKp*ex + exInt;
+        gy += MKp*ey + eyInt;
+        gz += MKp*ez + ezInt;
+
+        // integrate quaternion rate and normalise
+        q0 += (-q1*gx - q2*gy - q3*gz)*dTOn2;
+        q1 += (q0*gx + q2*gz - q3*gy)*dTOn2;
+        q2 += (q0*gy - q1*gz + q3*gx)*dTOn2;
+        q3 += (q0*gz + q1*gy - q2*gx)*dTOn2;
+
+        // normalise quaternion
+        ReNorm = 1.0 / sqrt(Sqr(q0) + Sqr(q1) + Sqr(q2) + Sqr(q3));
+        q0 *= ReNorm;
+        q1 *= ReNorm;
+        q2 *= ReNorm;
+        q3 *= ReNorm;
+
+        MadgwickEulerAngles(MadgwickAHRS);
+
+    } else
+        for ( g = 0; g <(uint8)3; g++) {
+            EstRate[g][MadgwickAHRS] = Gyro[g];
+            EstAngle[g][MadgwickAHRS] += EstRate[g][MadgwickAHRS]*dT;
+        }
+
+} // DoMadgwickAHRS
+
+void  MadgwickEulerAngles(uint8 S) {
+
+    EstAngle[Roll][S] = atan2(2.0*q2*q3 - 2.0*q0*q1, 2.0*Sqr(q0) + 2.0*Sqr(q3) - 1.0);
+    EstAngle[Pitch][S] = asin(2.0*q1*q2 - 2.0*q0*q2);
+    EstAngle[Yaw][S] = atan2(2.0*q1*q2 - 2.0*q0*q3,  2.0*Sqr(q0) + 2.0*Sqr(q1) - 1.0);
+
+} // MadgwickEulerAngles
+
+//_________________________________________________________________________________
+
+// Kalman Filter originally authored by Tom Pycke
+// http://tom.pycke.be/mav/71/kalman-filtering-of-imu-data
 
-    // auxiliary variables to reduce number of repeated operations
-    q0q0 = q0*q0;
-    q0q1 = q0*q1;
-    q0q2 = q0*q2;
-    q0q3 = q0*q3;
-    q1q1 = q1*q1;
-    q1q2 = q1*q2;
-    q1q3 = q1*q3;
-    q2q2 = q2*q2;
-    q2q3 = q2*q3;
-    q3q3 = q3*q3;
+real32 AngleKF[2] = {0,0};
+real32 BiasKF[2] = {0,0};
+real32 P00[2] = {0,0};
+real32 P01[2] = {0,0};
+real32 P10[2] = {0,0};
+real32 P11[2] = {0,0};
+
+real32 KalmanFilter(uint8 a, real32 NewAngle, real32 NewRate) {
+
+    // Q is a 2x2 matrix that represents the process covariance noise.
+    // In this case, it indicates how much we trust the accelerometer
+    // relative to the gyros.
+    const real32 AngleQ = 0.003;
+    const real32 GyroQ = 0.009;
+
+    // R represents the measurement covariance noise. In this case,
+    // it is a 1x1 matrix that says that we expect AngleR rad jitter
+    // from the accelerometer.
+    const real32 AngleR = GYRO_PROP_NOISE;
+
+    static real32 y, S;
+    static real32 K0, K1;
+
+    AngleKF[a] += (NewRate - BiasKF[a])*dT;
+    P00[a] -=  (( P10[a] + P01[a] ) + AngleQ )*dT;
+    P01[a] -=  P11[a]*dT;
+    P10[a] -=  P11[a]*dT;
+    P11[a] +=  GyroQ*dT;
+
+    y = NewAngle - AngleKF[a];
+    S = 1.0 / ( P00[a] + AngleR );
+    K0 = P00[a]*S;
+    K1 = P10[a]*S;
+
+    AngleKF[a] +=  K0*y;
+    BiasKF[a]  +=  K1*y;
+    P00[a] -= K0*P00[a];
+    P01[a] -= K0*P01[a];
+    P10[a] -= K1*P00[a];
+    P11[a] -= K1*P01[a];
+
+    return ( AngleKF[a] );
+
+}  // KalmanFilter
 
-    aMag = sqrt(Sqr(ax) + Sqr(ay) + Sqr(az)); // zero values into AHRS is FATAL
-    if ( aMag < 0.9 ) {
-        ax = ay = 0.0;
-        az = 1.0;
-    } else {
-        // normalise the measurements
-        rnorm = 1.0/aMag;
-        ax *= rnorm;
-        ay *= rnorm;
-        az *= rnorm;
+void DoKalman(void) { // NO YAW ANGLE ESTIMATE
+    EstAngle[Roll][Kalman] = KalmanFilter(Roll, asin(-Acc[LR]), Gyro[Roll]);
+    EstAngle[Pitch][Kalman] = KalmanFilter(Pitch, asin(Acc[BF]), Gyro[Pitch]);
+    EstRate[Roll][Kalman] = Gyro[Roll];
+    EstRate[Pitch][Kalman] = Gyro[Pitch];
+} // DoKalman
+
+
+//_________________________________________________________________________________
+
+// MultWii
+// Original code by Alex at: http://radio-commande.com/international/triwiicopter-design/
+// simplified IMU based on Kalman Filter
+// inspired from http://starlino.com/imu_guide.html
+// and http://www.starlino.com/imu_kalman_arduino.html
+// with this algorithm, we can get absolute angles for a stable mode integration
+
+real32 AccMW[3] = {0.0, 0.0, 1.0};   // init acc in stable mode
+real32 GyroMW[3] = {0.0, 0.0, 0.0};  // R obtained from last estimated value and gyro movement
+real32 Axz, Ayz;                     // angles between projection of R on XZ/YZ plane and Z axis
+
+void DoMultiWii(void) { // V1.6  NO YAW ANGLE ESTIMATE
+
+    const real32 GyroWt = 50.0;  // gyro weight/smoothing factor
+    const real32 GyroWtR = 1.0 / GyroWt;
+
+    if ( Acc[UD] < 0.0 ) { // check not inverted
+
+        if ( F.AccMagnitudeOK ) {
+            Ayz = atan2( AccMW[LR], AccMW[UD] ) + Gyro[Roll]*dT;
+            Axz = atan2( AccMW[BF], AccMW[UD] ) + Gyro[Pitch]*dT;
+        } else {
+            Ayz += Gyro[Roll]*dT;
+            Axz += Gyro[Pitch]*dT;
+        }
+
+        // reverse calculation of GyroMW from Awz angles,
+        // for formulae deduction see  http://starlino.com/imu_guide.html
+        GyroMW[Roll] = sin( Ayz ) / sqrt( 1.0 + Sqr( cos( Ayz ) )*Sqr( tan( Axz ) ) );
+        GyroMW[Pitch] = sin( Axz ) / sqrt( 1.0 + Sqr( cos( Axz ) )*Sqr( tan( Ayz ) ) );
+        GyroMW[Yaw] = sqrt( fabs( 1.0 - Sqr( GyroMW[Roll] ) - Sqr( GyroMW[Pitch] ) ) );
+
+        //combine accelerometer and gyro readings
+        AccMW[LR] = ( -Acc[LR] + GyroWt*GyroMW[Roll] )*GyroWtR;
+        AccMW[BF] = ( Acc[BF] + GyroWt*GyroMW[Pitch] )*GyroWtR;
+        AccMW[UD] = ( -Acc[UD] + GyroWt*GyroMW[Yaw] )*GyroWtR;
     }
 
-    rnorm = 1.0/sqrt(Sqr(mx) + Sqr(my) + Sqr(mz));
-    mx *= rnorm;
-    my *= rnorm;
-    mz *= rnorm;
-    mx2 = 2.0 * mx;
-    my2 = 2.0 * my;
-    mz2 = 2.0 * mz;
+    EstAngle[Roll][MultiWii] =  Ayz;
+    EstAngle[Pitch][MultiWii] =  Axz;
 
-    // compute reference direction of flux
-    hx = mx2*(0.5 - q2q2 - q3q3) + my2*(q1q2 - q0q3) + mz2*(q1q3 + q0q2);
-    hy = mx2*(q1q2 + q0q3) + my2*(0.5 - q1q1 - q3q3) + mz2*(q2q3 - q0q1);
-    hz = mx2*(q1q3 - q0q2) + my2*(q2q3 + q0q1) + mz2*(0.5 - q1q1 - q2q2);
-    bx2 = 2.0*sqrt(Sqr(hx) + Sqr(hy));
-    bz2 = 2.0*hz;
+//   EstRate[Roll][MultiWii] = GyroMW[Roll];
+//   EstRate[Pitch][MultiWii] = GyroMW[Pitch];
+
+    EstRate[Roll][MultiWii] = Gyro[Roll];
+    EstRate[Pitch][MultiWii] = Gyro[Pitch];
+
+} // DoMultiWii
 
-    // estimated direction of gravity and flux (v and w)
-    vx = 2.0*(q1q3 - q0q2);
-    vy = 2.0*(q0q1 + q2q3);
-    vz = q0q0 - q1q1 - q2q2 + q3q3;
+//_________________________________________________________________________________
 
-    wx = bx2*(0.5 - q2q2 - q3q3) + bz2*(q1q3 - q0q2);
-    wy = bx2*(q1q2 - q0q3) + bz2*(q0q1 + q2q3);
-    wz = bx2*(q0q2 + q1q3) + bz2*(0.5 - q1q1 - q2q2);
+void AttitudeTest(void) {
+
+    TxString("\r\nAttitude Test\r\n");
 
-    // error is sum of cross product between reference direction of fields and direction measured by sensors
-    ex = (ay*vz - az*vy) + (my*wz - mz*wy);
-    ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
-    ez = (ax*vy - ay*vx) + (mx*wy - my*wx);
+    GetAttitude();
 
-    // integral error scaled integral gain
-    exInt += ex*MKi;
-    eyInt += ey*MKi;
-    ezInt += ez*MKi;
+    TxString("\r\ndT \t");
+    TxVal32(dT*1000.0, 3, 0);
+    TxString(" Sec.\r\n\r\n");
+
+    if ( GyroType == IRSensors ) {
 
-    // adjusted gyroscope measurements
-    gx += MKp*ex + exInt;
-    gy += MKp*ey + eyInt;
-    gz += MKp*ez + ezInt;
+        TxString("IR Sensors:\r\n");
+        TxString("\tRoll \t");
+        TxVal32(IR[Roll]*100.0, 2, HT);
+        TxNextLine();
+        TxString("\tPitch\t");
+        TxVal32(IR[Pitch]*100.0, 2, HT);
+        TxNextLine();
+        TxString("\tZ    \t");
+        TxVal32(IR[Yaw]*100.0, 2, HT);
+        TxNextLine();
+        TxString("\tMin/Max\t");
+        TxVal32(IRMin*100.0, 2, HT);
+        TxVal32(IRMax*100.0, 2, HT);
+        TxString("\tSwing\t");
+        TxVal32(IRSwing*100.0, 2, HT);
+        TxNextLine();
 
-    // integrate quaternion rate and normalise
-    q0 += (-q1*gx - q2*gy - q3*gz)*HalfdT;
-    q1 += (q0*gx + q2*gz - q3*gy)*HalfdT;
-    q2 += (q0*gy - q1*gz + q3*gx)*HalfdT;
-    q3 += (q0*gz + q1*gy - q2*gx)*HalfdT;
-
-    // normalise quaternion
-    rnorm = 1.0/sqrt(Sqr(q0) + Sqr(q1) + Sqr(q2) + Sqr(q3));
-    q0 *= rnorm;
-    q1 *= rnorm;
-    q2 *= rnorm;
-    q3 *= rnorm;
+    } else {
 
-} // AHRSupdate
-
-void  EulerAngles(void) {
-
-    static uint8 g;
+        TxString("Gyro, Compensated, Max Delta(Deg./Sec.):\r\n");
+        TxString("\tRoll \t");
+        TxVal32(Gyro[Roll]*MILLIANGLE, 3, HT);
+        TxVal32(Rate[Roll]*MILLIANGLE, 3, HT);
+        TxVal32(GyroNoise[Roll]*MILLIANGLE,3, 0);
+        TxNextLine();
+        TxString("\tPitch\t");
+        TxVal32(Gyro[Pitch]*MILLIANGLE, 3, HT);
+        TxVal32(Rate[Pitch]*MILLIANGLE, 3, HT);
+        TxVal32(GyroNoise[Pitch]*MILLIANGLE,3, 0);
+        TxNextLine();
+        TxString("\tYaw  \t");
+        TxVal32(Gyro[Yaw]*MILLIANGLE, 3, HT);
+        TxVal32(Rate[Yaw]*MILLIANGLE, 3, HT);
+        TxVal32(GyroNoise[Yaw]*MILLIANGLE, 3, 0);
+        TxNextLine();
 
-    Angle[Roll] = atan2(2.0*q2*q3 - 2.0*q0*q1 , 2.0*Sqr(q0) + 2.0*Sqr(q3) - 1.0);
-    Angle[Pitch] = asin(2.0*q1*q2 - 2.0*q0*q2);
-    Angle[Yaw] = -atan2(2.0*q1*q2 - 2.0*q0*q3 ,  2.0*Sqr(q0) + 2.0*Sqr(q1) - 1.0);
-
-    for ( g = 0; g < (uint8)3; g++ ) {
-#ifdef USE_GYRO_RATE
-        Rate[g] = Gyro[g];
-#else
-        Rate[g] = ( Angle[g] - Anglep[g] ) * dTR;
-        Anglep[g] = Angle[g];
-#endif // USE_GYRO_RATE
+        TxString("Accelerations , peak change(G):\r\n");
+        TxString("\tB->F \t");
+        TxVal32(Acc[BF]*1000.0, 3, HT);
+        TxVal32( AccNoise[BF]*1000.0, 3, 0);
+        TxNextLine();
+        TxString("\tL->R \t");
+        TxVal32(Acc[LR]*1000.0, 3, HT);
+        TxVal32( AccNoise[LR]*1000.0, 3, 0);
+        TxNextLine();
+        TxString("\tU->D \t");
+        TxVal32(Acc[UD]*1000.0, 3, HT);
+        TxVal32( AccNoise[UD]*1000.0, 3, 0);
+        TxNextLine();
     }
 
-} // EulerAngles
+    if ( CompassType != HMC6352 ) {
+        TxString("Magnetic:\r\n");
+        TxString("\tX    \t");
+        TxVal32(Mag[Roll].V, 0, 0);
+        TxNextLine();
+        TxString("\tY    \t");
+        TxVal32(Mag[Pitch].V, 0, 0);
+        TxNextLine();
+        TxString("\tZ    \t");
+        TxVal32(Mag[Yaw].V, 0, 0);
+        TxNextLine();
+    }
 
-/*
-heading = atan2(2*qy*qw-2*qx*qz , 1 - 2*qy2 - 2*qz2)
-attitude = asin(2*qx*qy + 2*qz*qw)
-bank = atan2(2*qx*qw-2*qy*qz , 1 - 2*qx2 - 2*qz2)
+    TxString("Heading: \t");
+    TxVal32(Make2Pi(Heading)*MILLIANGLE, 3, 0);
+    TxNextLine();
+
+} // AttitudeTest
+
+void InitAttitude(void) {
+
+    static uint8 a, s;
+
+#ifdef INC_IMU2
 
-except when qx*qy + qz*qw = 0.5 (north pole)
-which gives:
-heading = 2 * atan2(x,w)
-bank = 0
-and when qx*qy + qz*qw = -0.5 (south pole)
-which gives:
-heading = -2 * atan2(x,w)
-bank = 0
-*/
+    FirstIMU2 = true;
+    BetaIMU2 = sqrt(0.75) * GyroNoiseRadian[GyroType];
+
+    //Quaternion orientation of earth frame relative to auxiliary frame.
+    AEq_1 = 1.0;
+    AEq_2 = 0.0;
+    AEq_3 = 0.0;
+    AEq_4 = 0.0;
 
+    //Estimated orientation quaternion elements with initial conditions.
+    SEq_1 = 1.0;
+    SEq_2 = 0.0;
+    SEq_3 = 0.0;
+    SEq_4 = 0.0;
 
+#endif // INC_IMU2
 
+    for ( a = 0; a < (uint8)2; a++ )
+        AngleCF[a] = AngleKF[a] = BiasKF[a] = F0[a] = F1[a] = F2[a] = 0.0;
+
+    for ( a = 0; a < (uint8)3; a++ )
+        for ( s = 0; s < MaxAttitudeScheme; s++ )
+            EstAngle[a][s] = EstRate[a][s] = 0.0;
+
+} // InitAttitude
+
diff -r 1e3318a30ddd -r 90292f8bd179 autonomous.c
--- a/autonomous.c	Fri Feb 25 01:35:24 2011 +0000
+++ b/autonomous.c	Tue Apr 26 12:12:29 2011 +0000
@@ -2,7 +2,7 @@
 // =                              UAVXArm Quadrocopter Controller                                =
 // =                           Copyright (c) 2008 by Prof. Greg Egan                             =
 // =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
-// =                     http://code.google.com/p/uavp-mods/ http://uavp.ch                      =
+// =                           http://code.google.com/p/uavp-mods/                               =
 // ===============================================================================================
 
 //    This is part of UAVXArm.
@@ -26,7 +26,6 @@
 void SetDesiredAltitude(int16);
 void DoFailsafeLanding(void);
 void AcquireHoldPosition(void);
-void NavGainSchedule(int16);
 void DoNavigation(void);
 void DoPPMFailsafe(void);
 void UAVXNavCommand(void);
@@ -217,7 +216,7 @@
         // Just use simple rudder only for now.
         if ( !F.WayPointAchieved ) {
             NavCorr[Yaw] = -(RelHeading * NAV_YAW_LIMIT) / HALFPI;
-            NavCorr[Yaw] = Limit(NavCorr[Yaw], -NAV_YAW_LIMIT, NAV_YAW_LIMIT); // gently!
+            NavCorr[Yaw] = Limit1(NavCorr[Yaw], NAV_YAW_LIMIT); // gently!
         }
 
 #else // MULTICOPTER
@@ -233,24 +232,24 @@
         // Roll & Pitch
 
         for ( a = 0; a < (uint8)2 ; a++ ) {
-            NavP = Limit(NavE[a], -NAV_MAX_ROLL_PITCH, NAV_MAX_ROLL_PITCH);
+            NavP = Limit1(NavE[a], NAV_MAX_ROLL_PITCH);
 
             NavIntE[a] += NavE[a];
-            NavIntE[a] = Limit(NavIntE[a], -K[NavIntLimit], K[NavIntLimit]);
+            NavIntE[a] = Limit1(NavIntE[a], K[NavIntLimit]);
             NavI = NavIntE[a] * K[NavKi] * GPSdT;
             NavIntE[a] = Decay1(NavIntE[a]);
 
             Diff = (NavEp[a] - NavE[a]);
-            Diff = Limit(Diff, -256, 256);
+            Diff = Limit1(Diff, 256);
             NavD = Diff * K[NavKd] * GPSdTR;
-            NavD = Limit(NavD, -NAV_DIFF_LIMIT, NAV_DIFF_LIMIT);
+            NavD = Limit1(NavD, NAV_DIFF_LIMIT);
 
             NavEp[a] = NavE[a];
 
             NavCorr[a] = (NavP + NavI + NavD ) * NavSensitivity;
 
             NavCorr[a] = SlewLimit(NavCorrp[a], NavCorr[a], NAV_CORR_SLEW_LIMIT);
-            NavCorr[a] = Limit(NavCorr[a], -NAV_MAX_ROLL_PITCH, NAV_MAX_ROLL_PITCH);
+            NavCorr[a] = Limit1(NavCorr[a], NAV_MAX_ROLL_PITCH);
 
             NavCorrp[a] = NavCorr[a];
         }
@@ -259,7 +258,7 @@
         if ( F.AllowTurnToWP && !F.WayPointAchieved ) {
             RelHeading = MakePi(WayHeading - Heading); // make +/- MilliPi
             NavCorr[Yaw] = -(RelHeading * NavYCorrLimit) / HALFPI;
-            NavCorr[Yaw] = Limit(NavCorr[Yaw], -NavYCorrLimit, NavYCorrLimit); // gently!
+            NavCorr[Yaw] = Limit1(NavCorr[Yaw], NavYCorrLimit); // gently!
         } else
             NavCorr[Yaw] = 0;
 
@@ -362,12 +361,9 @@
                         if ( F.AcquireNewPosition && !( F.Ch5Active & F.UsingPositionHoldLock ) ) {
                             F.AllowTurnToWP = SaveAllowTurnToWP;
                             AcquireHoldPosition();
-#ifdef NAV_ACQUIRE_BEEPER
-                            if ( !F.BeeperInUse ) {
-                                mS[BeeperTimeout] = mSClock() + 500L;
-                                Beeper_ON;
-                            }
-#endif // NAV_ACQUIRE_BEEPER
+                            #ifdef NAV_ACQUIRE_BEEPER
+                            DoBeeperPulse1mS(500);
+                            #endif // NAV_ACQUIRE_BEEPER
                         }
                     } else
                         F.AcquireNewPosition = true;
diff -r 1e3318a30ddd -r 90292f8bd179 baro.c
--- a/baro.c	Fri Feb 25 01:35:24 2011 +0000
+++ b/baro.c	Tue Apr 26 12:12:29 2011 +0000
@@ -2,7 +2,7 @@
 // =                              UAVXArm Quadrocopter Controller                                =
 // =                           Copyright (c) 2008 by Prof. Greg Egan                             =
 // =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
-// =                     http://code.google.com/p/uavp-mods/ http://uavp.ch                      =
+// =                           http://code.google.com/p/uavp-mods/                               =
 // ===============================================================================================
 
 //    This is part of UAVXArm.
@@ -75,8 +75,8 @@
 } // ShowBaro
 
 void BaroTest(void) {
+
     TxString("\r\nAltitude test\r\n");
-
     TxString("Initialising\r\n");
 
     InitBarometer();
@@ -87,7 +87,9 @@
     TxString("\r\nType:\t");
     ShowBaroType();
 
-    TxString("Init Retries:\t");
+    TxString("BaroScale:\t");
+    TxVal32(P[BaroScale],0,0);
+    TxString("\r\nInit Retries:\t");
     TxVal32((int32)BaroRetries - 2, 0, ' '); // always minimum of 2
     if ( BaroRetries >= BARO_INIT_RETRIES )
         TxString(" FAILED Init.\r\n");
@@ -95,7 +97,9 @@
         TxNextLine();
 
     if ( BaroType == BaroMPX4115 ) {
-        TxString("Range   :\t");
+        TxString("\r\nAddress :\t0x");
+        TxValH(MCP4725_ID_Actual);
+        TxString("\r\nRange   :\t");
         TxVal32((int32) BaroDescentAvailable * 10.0, 1, ' ');
         TxString("-> ");
         TxVal32((int32) BaroClimbAvailable * 10.0, 1, 'M');
@@ -106,15 +110,19 @@
         TxNextLine();
     }
 
-    if ( !F.BaroAltitudeValid ) goto BAerror;
+    if ( F.BaroAltitudeValid ) {
+
+        while ( !F.NewBaroValue )
+            GetBaroAltitude();
+
+        F.NewBaroValue = false;
 
-    while ( !F.NewBaroValue )
-        GetBaroAltitude();
-    F.NewBaroValue = false;
+        TxString("Alt.:     \t");
+        TxVal32(BaroRelAltitude * 10.0, 1, ' ');
+        TxString("M\r\n");
 
-    TxString("Alt.:     \t");
-    TxVal32(BaroRelAltitude * 10.0, 1, ' ');
-    TxString("M\r\n");
+    } else
+        TxString("Barometer FAILED\r\n");
 
     TxString("\r\nR.Finder: \t");
     if ( F.RangefinderAltitudeValid ) {
@@ -128,9 +136,6 @@
     TxVal32((int32)AmbientTemperature.i16, 1, ' ');
     TxString("C\r\n");
 
-    return;
-BAerror:
-    TxString("FAIL\r\n");
 } // BaroTest
 
 void GetBaroAltitude(void) {
@@ -167,7 +172,7 @@
             Stats[BaroFailS]++;
         }
 
-        Temp = Limit( Temp , -BARO_SANITY_CHECK_MPS, BARO_SANITY_CHECK_MPS );
+        Temp = Limit1(Temp, BARO_SANITY_CHECK_MPS);
         ROC = ROC * 0.9 + Temp * 0.1;
         BaroRelAltitudeP = BaroRelAltitude;
 
@@ -191,18 +196,17 @@
     BaroRelAltitude = BaroRelAltitudeP = CompBaroPressure = OriginBaroPressure = 0;
     BaroType = BaroUnknown;
 
-    Comp[Alt] = AltDiffSum = AltDSum = 0;
+    AltComp = AltDiffSum = AltDSum = 0;
     F.BaroAltitudeValid= true; // optimistic
- 
+
     if ( IsFreescaleBaroActive() )
         InitFreescaleBarometer();
     else
         if ( IsBoschBaroActive() )
             InitBoschBarometer();
-        else {
+        else
             F.BaroAltitudeValid = F.HoldingAlt = false;
-            Stats[BaroFailS]++;
-        }
+
 } // InitBarometer
 
 // -----------------------------------------------------------
@@ -222,32 +226,43 @@
 
 void SetFreescaleMCP4725(int16 d) {
     static i16u dd;
-    static uint8 r;
 
     dd.u16 = d << 4;                            // left align
 
     I2CBARO.start();
-    r = I2CBARO.write(MCP4725_ID_Actual) != I2C_ACK;
-    r = I2CBARO.write(MCP4725_CMD) != I2C_ACK;
-    r = I2CBARO.write(dd.b1) != I2C_ACK;
-    r = I2CBARO.write(dd.b0) != I2C_ACK;
+    if ( I2CBARO.write(MCP4725_ID_Actual) != I2C_ACK ) goto MCP4725Error;
+    if ( I2CBARO.write(MCP4725_CMD) != I2C_ACK ) goto MCP4725Error;
+    if ( I2CBARO.write(dd.b1) != I2C_ACK ) goto MCP4725Error;
+    if ( I2CBARO.write(dd.b0) != I2C_ACK ) goto MCP4725Error;
     I2CBARO.stop();
 
+    return;
+
+MCP4725Error:
+    I2CBARO.stop();
+    I2CError[MCP4725_ID_Actual]++;
+
 } // SetFreescaleMCP4725
 
 boolean IdentifyMCP4725(void) {
 
     static boolean r;
-    
+
     r = true;
-    if ( I2CBAROAddressResponds( MCP4725_ID_0xC8 ) )
-        MCP4725_ID_Actual = MCP4725_ID_0xC8;
-    else 
-        if ( I2CBAROAddressResponds( MCP4725_ID_0xCC ) )
-            MCP4725_ID_Actual = MCP4725_ID_0xCC;
+    MCP4725_ID_Actual = MCP4725_ID_0xCC;
+    if ( I2CBAROAddressResponds( MCP4725_ID_0xCC ) )
+        MCP4725_ID_Actual = MCP4725_ID_0xCC;
+    else
+        if ( I2CBAROAddressResponds( MCP4725_ID_0xC8 ) )
+            MCP4725_ID_Actual = MCP4725_ID_0xC8;
         else
             r = false;
-   return(r);    
+
+    return(r);
+
+//   MCP4725_ID_Actual = FORCE_BARO_ID;
+//   return(true);
+
 } // IdentifyMCP4725
 
 void SetFreescaleOffset(void) {
@@ -280,13 +295,14 @@
     Delay1mS(100);
     ReadFreescaleBaro();
 
-    while ( (BaroVal.u16 < (uint16)(((uint24)ADS7823_MAX*4L*3L)/4L) ) && (BaroOffsetDAC > 2) ) {
-        BaroOffsetDAC -= 2;
+    while ( (BaroVal.u16 < (uint16)(((uint24)ADS7823_MAX*4L*3L)/4L) ) && (BaroOffsetDAC > 1) ) {
+        BaroOffsetDAC -= 1;
         SetFreescaleMCP4725(BaroOffsetDAC);
-        Delay1mS(10);
+        Delay1mS(10); // 10
         ReadFreescaleBaro();
         TxVal32(BaroOffsetDAC,0,HT);
-        TxVal32(BaroVal.u16,0,' ');
+        TxVal32(BaroVal.u16,0,HT);
+        TxVal32((int32)FreescaleToDM(BaroVal.u16), 1, 0);
         TxNextLine();
         LEDYellow_TOG;
     }
@@ -303,11 +319,13 @@
     mS[BaroUpdate] = mSClock() + ADS7823_TIME_MS;
 
     I2CBARO.start();  // start conversion
+    if ( I2CBARO.write(ADS7823_WR) != I2C_ACK ) goto ADS7823Error;
+    if ( I2CBARO.write(ADS7823_CMD) != I2C_ACK ) goto ADS7823Error;
+    I2CBARO.stop();
 
-    if ( I2CBARO.write(ADS7823_WR) != I2C_ACK ) goto FSError;
-    if ( I2CBARO.write(ADS7823_CMD) != I2C_ACK ) goto FSError;
+    if ( I2CBARO.blockread(ADS7823_RD, B, 8) ) goto ADS7823Error;
 
-    I2CBARO.blockread(ADS7823_RD, B, 8);  // read block of 4 baro samples
+    // read block of 4 baro samples
 
     B0.b0 = B[1];
     B0.b1 = B[0];
@@ -324,15 +342,17 @@
 
     return;
 
-FSError:
+ADS7823Error:
     I2CBARO.stop();
 
-    F.BaroAltitudeValid = F.HoldingAlt = false;
+    I2CError[ADS7823_ID]++;
+
+    // F.BaroAltitudeValid = F.HoldingAlt = false;
     if ( State == InFlight ) {
         Stats[BaroFailS]++;
         F.BaroFailure = true;
     }
-    return;
+
 } // ReadFreescaleBaro
 
 real32 FreescaleToDM(int24 p) { // decreasing pressure is increase in altitude negate and rescale to metre altitude
@@ -369,15 +389,15 @@
 
 void InitFreescaleBarometer(void) {
     static int16 BaroOriginAltitude, MinAltitude;
-    real32 Error;
     static int24 BaroPressureP;
 
     AltitudeUpdateRate = 1000L/ADS7823_TIME_MS;
 
     BaroTemperature = 0;
-    Error = ( (int16)P[BaroScale] * 20 ) / 16;  // 0.2M
+    if ( P[BaroScale] <= 0 )
+        P[BaroScale] = 56; // failsafe setting
+
     BaroPressure =  0;
-
     BaroRetries = 0;
     do {
         BaroPressureP = BaroPressure;
@@ -388,7 +408,7 @@
         ReadFreescaleBaro();
         BaroPressure = (int24)BaroVal.u16;
     } while ( ( ++BaroRetries < BARO_INIT_RETRIES )
-              && ( abs((int16)(BaroPressure - BaroPressureP)) > Error ) );
+              && ( fabs( FreescaleToDM(BaroPressure - BaroPressureP) ) > 5 ) );
 
     F.BaroAltitudeValid = BaroRetries < BARO_INIT_RETRIES;
 
@@ -444,54 +464,57 @@
     }
 
     I2CBARO.start();
-    if ( I2CBARO.write(BOSCH_ID) != I2C_ACK ) goto SBerror;
-
+    if ( I2CBARO.write(BOSCH_ID) != I2C_ACK ) goto BoschError;
     // access control register, start measurement
-    if ( I2CBARO.write(BOSCH_CTL) != I2C_ACK ) goto SBerror;
-
+    if ( I2CBARO.write(BOSCH_CTL) != I2C_ACK ) goto BoschError;
     // select 32kHz input, measure temperature
-    if ( I2CBARO.write(TempOrPress) != I2C_ACK ) goto SBerror;
+    if ( I2CBARO.write(TempOrPress) != I2C_ACK ) goto BoschError;
     I2CBARO.stop();
 
     F.BaroAltitudeValid = true;
     return;
 
-SBerror:
+BoschError:
     I2CBARO.stop();
-    F.BaroAltitudeValid = F.HoldingAlt = false;
-    return;
+
+    I2CError[BOSCH_ID]++;
+    F.BaroAltitudeValid = false;
+
 } // StartBoschBaroADC
 
 void ReadBoschBaro(void) {
+
     // Possible I2C protocol error - split read of ADC
     I2CBARO.start();
-    if ( I2CBARO.write(BOSCH_WR) != I2C_ACK ) goto RVerror;
-    if ( I2CBARO.write(BOSCH_ADC_MSB) != I2C_ACK ) goto RVerror;
+    if ( I2CBARO.write(BOSCH_WR) != I2C_ACK ) goto BoschError;
+    if ( I2CBARO.write(BOSCH_ADC_MSB) != I2C_ACK ) goto BoschError;
     I2CBARO.start();    // restart
-    if ( I2CBARO.write(BOSCH_RD) != I2C_ACK ) goto RVerror;
+    if ( I2CBARO.write(BOSCH_RD) != I2C_ACK ) goto BoschError;
     BaroVal.b1 = I2CBARO.read(I2C_NACK);
     I2CBARO.stop();
 
     I2CBARO.start();
-    if ( I2CBARO.write(BOSCH_WR) != I2C_ACK ) goto RVerror;
-    if ( I2CBARO.write(BOSCH_ADC_LSB) != I2C_ACK ) goto RVerror;
+    if ( I2CBARO.write(BOSCH_WR) != I2C_ACK ) goto BoschError;
+    if ( I2CBARO.write(BOSCH_ADC_LSB) != I2C_ACK ) goto BoschError;
     I2CBARO.start();    // restart
-    if ( I2CBARO.write(BOSCH_RD) != I2C_ACK ) goto RVerror;
+    if ( I2CBARO.write(BOSCH_RD) != I2C_ACK ) goto BoschError;
     BaroVal.b0 = I2CBARO.read(I2C_NACK);
     I2CBARO.stop();
 
     F.BaroAltitudeValid = true;
     return;
 
-RVerror:
+BoschError:
     I2CBARO.stop();
 
+    I2CError[BOSCH_ID]++;
+
     F.BaroAltitudeValid = F.HoldingAlt = false;
     if ( State == InFlight ) {
         Stats[BaroFailS]++;
         F.BaroFailure = true;
     }
-    return;
+
 } // ReadBoschBaro
 
 #define BOSCH_BMP085_TEMP_COEFF        62L
@@ -535,10 +558,8 @@
 
                 F.NewBaroValue = F.BaroAltitudeValid;
             }
-        else {
+        else
             AcquiringPressure = true;
-            Stats[BaroFailS]++;
-        }
 
         StartBoschBaroADC(AcquiringPressure);
     }
@@ -565,6 +586,7 @@
     return(true);
 
 BoschInactive:
+
     return(false);
 
 } // IsBoschBaroActive
diff -r 1e3318a30ddd -r 90292f8bd179 compass.c
--- a/compass.c	Fri Feb 25 01:35:24 2011 +0000
+++ b/compass.c	Tue Apr 26 12:12:29 2011 +0000
@@ -2,7 +2,7 @@
 // =                              UAVXArm Quadrocopter Controller                                =
 // =                           Copyright (c) 2008 by Prof. Greg Egan                             =
 // =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
-// =                     http://code.google.com/p/uavp-mods/ http://uavp.ch                      =
+// =                           http://code.google.com/p/uavp-mods/                               =
 // ===============================================================================================
 
 //    This is part of UAVXArm.
@@ -23,9 +23,9 @@
 // Local magnetic declination not included
 // http://www.ngdc.noaa.gov/geomagmodels/Declination.jsp
 
-
 void ReadCompass(void);
 void GetHeading(void);
+real32 MinimumTurn(real32);
 void CalibrateCompass(void);
 void ShowCompassType(void);
 void DoCompassTest(void);
@@ -33,10 +33,13 @@
 
 MagStruct Mag[3] = {{ 0,0 },{ 0,0 },{ 0,0 }};
 real32 MagDeviation, CompassOffset;
-real32 MagHeading, Heading, Headingp, FakeHeading;
+real32 MagHeading, Heading, HeadingP, FakeHeading;
 real32 HeadingSin, HeadingCos;
+real32 CompassMaxSlew;
 uint8 CompassType;
 
+enum MagCoords { MX, MY, MZ };
+
 void ReadCompass(void) {
     switch ( CompassType ) {
         case HMC5843:
@@ -94,16 +97,25 @@
 
 void GetHeading(void) {
 
-    const real32 CompassA = COMPASS_UPDATE_S / ( OneOnTwoPiCompassF + COMPASS_UPDATE_S );
+    static real32 HeadingChange, CompassA;
 
     ReadCompass();
 
-    Heading = Make2Pi( MagHeading - MagDeviation - CompassOffset );
-    if ( fabs(Heading - Headingp ) > PI )
-        Headingp = Heading;
+    Heading = Make2Pi( MagHeading + MagDeviation - CompassOffset );
+    HeadingChange = fabs( Heading - HeadingP );
+    if ( HeadingChange > ONEANDHALFPI ) // wrap 0 -> TwoPI
+        HeadingP = Heading;
+    else
+        if ( HeadingChange > CompassMaxSlew ) { // Sanity check - discard reading
+            Heading =SlewLimit(HeadingP, Heading, CompassMaxSlew ); 
+            Stats[CompassFailS]++;
+        }
 
-    Heading = Headingp + (Heading - Headingp) * CompassA;
-    Headingp = Heading;
+#ifndef SUPPRESS_COMPASS_FILTER
+    CompassA = dT / ( 1.0 / ( TWOPI * YawFilterLPFreq ) + dT );
+    Heading = Make2Pi( LPFilter(Heading, HeadingP, CompassA) );
+#endif // !SUPPRESS_COMPASS_FILTER
+    HeadingP = Heading;
 
 #ifdef SIMULATE
 #if ( defined AILERON | defined ELEVON )
@@ -121,6 +133,24 @@
 #endif // SIMULATE 
 } // GetHeading
 
+//boolean DirectionSelected = false;
+//real32 DirectionSense = 1.0;
+
+real32 MinimumTurn(real32 A ) {
+
+    static real32 AbsA;
+
+    AbsA = fabs(A);
+    if ( AbsA > PI )
+        A = ( AbsA - TWOPI ) * Sign(A);
+
+    //DirectionSelected = fabs(A) > THIRDPI; // avoid dithering around reciprocal heading
+    //DirectionSense = Sign(A);
+
+    return ( A );
+
+} // MinimumTurn
+
 void InitCompass(void) {
     if ( IsHMC5843Active() )
         CompassType = HMC5843;
@@ -145,7 +175,7 @@
 
     ReadCompass();
     mS[CompassUpdate] = mSClock();
-    Heading = Headingp = Make2Pi( MagHeading - MagDeviation - CompassOffset );
+    Heading = HeadingP = Make2Pi( MagHeading - MagDeviation - CompassOffset );
 
 } // InitCompass
 
@@ -163,16 +193,15 @@
 void ReadHMC5843(void) {
     static char b[6];
     static i16u X, Y, Z;
-    static uint8 r;
-    static real32 mx, my;
+    static real32 FX,FY;
     static real32 CRoll, SRoll, CPitch, SPitch;
 
     I2CCOMPASS.start();
-    r = I2CCOMPASS.write(HMC5843_WR);
-    r = I2CCOMPASS.write(0x03); // point to data
+    if ( I2CCOMPASS.write(HMC5843_WR) != I2C_ACK ) goto HMC5843Error;
+    if ( I2CCOMPASS.write(0x03) != I2C_ACK ) goto HMC5843Error; // point to data
     I2CCOMPASS.stop();
 
-    I2CCOMPASS.blockread(HMC5843_RD, b, 6);
+    if ( I2CCOMPASS.blockread(HMC5843_RD, b, 6) ) goto HMC5843Error;
 
     X.b1 = b[0];
     X.b0 = b[1];
@@ -192,55 +221,180 @@
     }
     DebugPin = true;
     CRoll = cos(Angle[Roll]);
-    SRoll = sin(Angle[Roll]);
+    SRoll = sin(Angle[Roll]);       // Acc[LR] - optimisation not worthwhile
     CPitch = cos(Angle[Pitch]);
-    SPitch = sin(Angle[Pitch]);
+    SPitch = sin(Angle[Pitch]);     // Acc[BF]
 
-    mx = (Mag[BF].V-Mag[BF].Offset) * CPitch + (Mag[LR].V-Mag[LR].Offset) * SRoll * SPitch + (Mag[UD].V-Mag[UD].Offset) * CRoll * SPitch;
-    my =  (Mag[LR].V-Mag[LR].Offset) * CRoll - (Mag[UD].V-Mag[UD].Offset) * SRoll;
+    FX = (Mag[BF].V-Mag[BF].Offset) * CPitch + (Mag[LR].V-Mag[LR].Offset) * SRoll * SPitch + (Mag[UD].V-Mag[UD].Offset) * CRoll * SPitch;
+    FY = (Mag[LR].V-Mag[LR].Offset) * CRoll - (Mag[UD].V-Mag[UD].Offset) * SRoll;
 
     // Magnetic Heading
-    MagHeading = MakePi(atan2( -my, mx ));
+    MagHeading = MakePi(atan2( -FY, FX ));
+
     DebugPin = false;
     F.CompassValid = true;
+
     return;
 
+HMC5843Error:
+    I2CCOMPASS.stop();
+
+    I2CError[HMC5843_ID]++;
+    if ( State == InFlight ) Stats[CompassFailS]++;
+
+    F.CompassMissRead = true;
+    F.CompassValid = false;
+
 } // ReadHMC5843
 
+real32  magFieldEarth[3],  magFieldBody[3], dcmMatrix[9];
+boolean firstPassMagOffset = true;
+
 void CalibrateHMC5843(void) {
 
-} // DoHMC5843Test
+    /*
+    void magOffsetCalc()
+    {
+      int16   i, j ;
+      static real32 tempMatrix[3] ;
+      static real32 offsetSum[3] ;
+
+      // Compute magnetic field of the earth
+
+      magFieldEarth[0] = vectorDotProduct(3, &dcmMatrix[0], &magFieldBody[0]);
+      magFieldEarth[1] = vectorDotProduct(3, &dcmMatrix[3], &magFieldBody[0]);
+      magFieldEarth[2] = vectorDotProduct(3, &dcmMatrix[6], &magFieldBody[0]);
+
+      // First pass thru?
+
+      if ( firstPassMagOffset )
+      {
+        setPastValues();                         // Yes, set initial values for previous values
+        firstPassMagOffset = false;              // Clear first pass flag
+      }
+
+      // Compute the offsets in the magnetometer:
+      vectorAdd(3, offsetSum , magFieldBody, magFieldBodyPrevious) ;
+
+      matrixMultiply(1, 3, 3, tempMatrix, magFieldEarthPrevious, dcmMatrix);
+      vectorSubtract(3, offsetSum, offsetSum, tempMatrix);
+      matrixMultiply(1, 3, 3, tempMatrix, magFieldEarth, dcmMatrixPrevious);
+      vectorSubtract(3, offsetSum, offsetSum, tempMatrix) ;
+
+      for ( i = 0 ; i < 3 ; i++ )
+        if ( abs(offsetSum[i] ) < 3 )
+          offsetSum[i] = 0 ;
+
+      vectorAdd (3, magOffset, magOffset, offsetSum);
+      setPastValues();
+    }
+
+    void setPastValues()
+    {
+      static uint8 i;
+
+      for ( i = 0; i < 3; i++)
+      {
+        magFieldEarthPrevious[i] = magFieldEarth[i];
+        magFieldBodyPrevious[i]  = magFieldBody[i];
+      }
+
+      for ( i = 0; i < 9; i++)
+        dcmMatrixPrevious[i] = dcmMatrix[i];
+
+    }
+    */
+} // CalibrateHMC5843
+
+void GetHMC5843Parameters(void) {
+
+    static char CP[16];
+    static uint8 i;
+
+    I2CCOMPASS.start();
+    if ( I2CCOMPASS.write(HMC5843_WR) != I2C_ACK) goto HMC5843Error;
+    if ( I2CCOMPASS.write(0x00) != I2C_ACK) goto HMC5843Error; // point to data
+    I2CCOMPASS.stop();
+
+    if ( I2CCOMPASS.blockread(HMC5843_RD, CP, 13) ) goto HMC5843Error;
+
+    for ( i = 0; i < (uint8)13; i++ ) {
+        TxVal32(i,0,0);
+        TxString(":\t0x");
+        TxValH(CP[i]);
+        TxChar(HT);
+        TxBin8(CP[i]);
+        TxNextLine();
+    }
+
+    return;
+
+HMC5843Error:
+    I2CCOMPASS.stop();
+
+    I2CError[HMC5843_ID]++;
+
+} // GetHMC5843Parameters
 
 void DoHMC5843Test(void) {
+
     TxString("\r\nCompass test (HMC5843)\r\n\r\n");
 
     ReadHMC5843();
 
-    TxString("Mag:\t");
+    GetHMC5843Parameters();
+
+    TxString("\r\nMag:\t");
+    TxVal32(Mag[BF].V, 0, HT);
     TxVal32(Mag[LR].V, 0, HT);
-    TxVal32(Mag[BF].V, 0, HT);
     TxVal32(Mag[UD].V, 0, HT);
     TxNextLine();
     TxNextLine();
 
-    TxVal32(MagHeading * RADDEG * 10.0, 1, 0);
-    TxString(" deg (Magnetic)\r\n");
+    TxVal32(MagHeading * RADDEG * 10.0, 1, ' ');
+    TxString("deg (Magnetic)\r\n");
+
+    Heading = HeadingP = Make2Pi( MagHeading + MagDeviation - CompassOffset );
+    TxVal32(Heading * RADDEG * 10.0, 1, ' ');
+    TxString("deg (True)\r\n");
+} // DoHMC5843Test
+
+boolean WriteByteHMC5843(uint8 a, uint8 d) {
 
-    Heading = Headingp = Make2Pi( MagHeading - MagDeviation - CompassOffset );
-    TxVal32(Heading * RADDEG * 10.0, 1, 0);
-    TxString(" deg (True)\r\n");
-} // DoHMC5843Test
+    I2CCOMPASS.start();
+    if ( I2CCOMPASS.write(HMC5843_WR) != I2C_ACK ) goto HMC5843Error;
+    if ( I2CCOMPASS.write(a) != I2C_ACK ) goto HMC5843Error;
+    if ( I2CCOMPASS.write(d) != I2C_ACK ) goto HMC5843Error;
+    I2CCOMPASS.stop();
+
+    return( false );
+
+HMC5843Error:
+    I2CCOMPASS.stop();
+
+    I2CError[HMC5843_ID]++;
+
+    return(true);
+
+} // WriteByteHMC5843
 
 void InitHMC5843(void) {
-    static uint8 r;
+
+    CompassMaxSlew = (COMPASS_SANITY_CHECK_RAD_S/HMC5843_UPDATE_S);
+
+    if ( WriteByteHMC5843(0x00, HMC5843_DR  << 2) ) goto HMC5843Error; // rate, normal measurement mode
+    if ( WriteByteHMC5843(0x02, 0x00) ) goto HMC5843Error; // mode continuous
 
-    I2CCOMPASS.start();
-    r = I2CCOMPASS.write(HMC5843_WR);
-    r = I2CCOMPASS.write(0x02);
-    r = I2CCOMPASS.write(0x00);   // Set continuous mode (default to 10Hz)
+    Delay1mS(50);
+
+    return;
+
+HMC5843Error:
     I2CCOMPASS.stop();
 
-    Delay1mS(50);
+    I2CError[HMC5843_ID]++;
+
+    F.CompassValid = false;
 
 } // InitHMC5843Magnetometer
 
@@ -271,23 +425,38 @@
     static i16u v;
 
     I2CCOMPASS.start();
-    F.CompassMissRead = I2CCOMPASS.write(HMC6352_RD) != I2C_ACK;
+    if ( I2CCOMPASS.write(HMC6352_RD) != I2C_ACK ) goto HMC6352Error;
     v.b1 = I2CCOMPASS.read(I2C_ACK);
     v.b0 = I2CCOMPASS.read(I2C_NACK);
     I2CCOMPASS.stop();
 
     MagHeading = Make2Pi( ((real32)v.i16 * PI) / 1800.0 - CompassOffset ); // Radians
+
+    return;
+
+HMC6352Error:
+    I2CCOMPASS.stop();
+
+    if ( State == InFlight ) Stats[CompassFailS]++;
+
+    F.CompassMissRead = true;
+
 } // ReadHMC6352
 
 uint8 WriteByteHMC6352(uint8 d) {
+
     I2CCOMPASS.start();
-    if ( I2CCOMPASS.write(HMC6352_WR) != I2C_ACK ) goto WError;
-    if ( I2CCOMPASS.write(d) != I2C_ACK ) goto WError;
+    if ( I2CCOMPASS.write(HMC6352_WR) != I2C_ACK ) goto HMC6352Error;
+    if ( I2CCOMPASS.write(d) != I2C_ACK ) goto HMC6352Error;
     I2CCOMPASS.stop();
 
     return( I2C_ACK );
-WError:
+
+HMC6352Error:
     I2CCOMPASS.stop();
+
+    I2CError[HMC6352_ID]++;
+
     return ( I2C_NACK );
 } // WriteByteHMC6352
 
@@ -296,13 +465,13 @@
 #define TEST_COMP_OPMODE 0x70    // standby mode to reliably read EEPROM
 
 void GetHMC6352Parameters(void) {
-    uint8 r;
+    int16 Temp;
 
     I2CCOMPASS.start();
-    if ( I2CCOMPASS.write(HMC6352_WR) != I2C_ACK ) goto CTerror;
-    if ( I2CCOMPASS.write('G')  != I2C_ACK ) goto CTerror;
-    if ( I2CCOMPASS.write(0x74) != I2C_ACK ) goto CTerror;
-    if ( I2CCOMPASS.write(TEST_COMP_OPMODE) != I2C_ACK ) goto CTerror;
+    if ( I2CCOMPASS.write(HMC6352_WR) != I2C_ACK ) goto HMC6352Error;
+    if ( I2CCOMPASS.write('G')  != I2C_ACK ) goto HMC6352Error;
+    if ( I2CCOMPASS.write(0x74) != I2C_ACK ) goto HMC6352Error;
+    if ( I2CCOMPASS.write(TEST_COMP_OPMODE) != I2C_ACK ) goto HMC6352Error;
     I2CCOMPASS.stop();
 
     Delay1mS(20);
@@ -310,50 +479,21 @@
     for (r = 0; r <= (uint8)8; r++) { // must have this timing - not block read!
 
         I2CCOMPASS.start();
-        if ( I2CCOMPASS.write(HMC6352_WR) != I2C_ACK ) goto CTerror;
-        if ( I2CCOMPASS.write('r')  != I2C_ACK ) goto CTerror;
-        if ( I2CCOMPASS.write(r)  != I2C_ACK ) goto CTerror;
+        if ( I2CCOMPASS.write(HMC6352_WR) != I2C_ACK ) goto HMC6352Error;
+        if ( I2CCOMPASS.write('r')  != I2C_ACK ) goto HMC6352Error;
+        if ( I2CCOMPASS.write(r)  != I2C_ACK ) goto HMC6352Error;
         I2CCOMPASS.stop();
 
         Delay1mS(10);
 
         I2CCOMPASS.start();
-        if ( I2CCOMPASS.write(HMC6352_RD) != I2C_ACK ) goto CTerror;
+        if ( I2CCOMPASS.write(HMC6352_RD) != I2C_ACK ) goto HMC6352Error;
         CP[r] = I2CCOMPASS.read(I2C_NACK);
         I2CCOMPASS.stop();
 
         Delay1mS(10);
     }
 
-    return;
-
-CTerror:
-    I2CCOMPASS.stop();
-    TxString("FAIL\r\n");
-
-} // GetHMC6352Parameters
-
-void DoHMC6352Test(void) {
-    static real32 Temp;
-
-    TxString("\r\nCompass test (HMC6352)\r\n");
-
-    I2CCOMPASS.start();
-    if ( I2CCOMPASS.write(HMC6352_WR) != I2C_ACK ) goto CTerror;
-    if ( I2CCOMPASS.write('G')  != I2C_ACK ) goto CTerror;
-    if ( I2CCOMPASS.write(0x74) != I2C_ACK ) goto CTerror;
-    if ( I2CCOMPASS.write(TEST_COMP_OPMODE) != I2C_ACK ) goto CTerror;
-    I2CCOMPASS.stop();
-
-    Delay1mS(1);
-
-    //  I2CCOMPASS.start(); // Do Set/Reset now
-    if ( WriteByteHMC6352('O')  != I2C_ACK ) goto CTerror;
-
-    Delay1mS(7);
-
-    GetHMC6352Parameters();
-
     TxString("\r\nRegisters\r\n");
     TxString("\t0:\tI2C");
     TxString("\t 0x");
@@ -419,24 +559,59 @@
     }
     TxNextLine();
 
+    return;
+
+HMC6352Error:
+    I2CCOMPASS.stop();
+
+    I2CError[HMC6352_ID]++;
+
+    TxString("FAIL\r\n");
+
+} // GetHMC6352Parameters
+
+void DoHMC6352Test(void) {
+
+    TxString("\r\nCompass test (HMC6352)\r\n");
+
+    I2CCOMPASS.start();
+    if ( I2CCOMPASS.write(HMC6352_WR) != I2C_ACK ) goto HMC6352Error;
+    if ( I2CCOMPASS.write('G')  != I2C_ACK ) goto HMC6352Error;
+    if ( I2CCOMPASS.write(0x74) != I2C_ACK ) goto HMC6352Error;
+    if ( I2CCOMPASS.write(TEST_COMP_OPMODE) != I2C_ACK ) goto HMC6352Error;
+    I2CCOMPASS.stop();
+
+    Delay1mS(1);
+
+    //  I2CCOMPASS.start(); // Do Set/Reset now
+    if ( WriteByteHMC6352('O')  != I2C_ACK ) goto HMC6352Error;
+
+    Delay1mS(7);
+
+    GetHMC6352Parameters();
+
     InitCompass();
-    if ( !F.CompassValid ) goto CTerror;
+    if ( !F.CompassValid ) goto HMC6352Error;
 
     Delay1mS(50);
 
     ReadHMC6352();
-    if ( F.CompassMissRead ) goto CTerror;
+    if ( F.CompassMissRead ) goto HMC6352Error;
 
     TxNextLine();
     TxVal32(MagHeading * RADDEG * 10.0, 1, 0);
     TxString(" deg (Magnetic)\r\n");
-    Heading = Headingp = Make2Pi( MagHeading - MagDeviation - CompassOffset );
+    Heading = HeadingP = Make2Pi( MagHeading - MagDeviation - CompassOffset );
     TxVal32(Heading * RADDEG * 10.0, 1, 0);
     TxString(" deg (True)\r\n");
 
     return;
-CTerror:
+
+HMC6352Error:
     I2CCOMPASS.stop();
+
+    I2CError[HMC6352_ID]++;
+
     TxString("FAIL\r\n");
 } // DoHMC6352Test
 
@@ -445,18 +620,18 @@
     while ( PollRxChar() != 'x' ); // UAVPSet uses 'x' for CONTINUE button
 
     // Do Set/Reset now
-    if ( WriteByteHMC6352('O') != I2C_ACK ) goto CCerror;
+    if ( WriteByteHMC6352('O') != I2C_ACK ) goto HMC6352Error;
 
     Delay1mS(7);
 
     // set Compass device to Calibration mode
-    if ( WriteByteHMC6352('C') != I2C_ACK ) goto CCerror;
+    if ( WriteByteHMC6352('C') != I2C_ACK ) goto HMC6352Error;
 
     TxString("\r\nRotate horizontally 720 deg in ~30 sec. - Press CONTINUE button (x) to Finish\r\n");
     while ( PollRxChar() != 'x' );
 
     // set Compass device to End-Calibration mode
-    if ( WriteByteHMC6352('E') != I2C_ACK ) goto CCerror;
+    if ( WriteByteHMC6352('E') != I2C_ACK ) goto HMC6352Error;
 
     TxString("\r\nCalibration complete\r\n");
 
@@ -466,7 +641,11 @@
 
     return;
 
-CCerror:
+HMC6352Error:
+    I2CCOMPASS.stop();
+
+    I2CError[HMC6352_ID]++;
+
     TxString("Calibration FAILED\r\n");
 } // CalibrateHMC6352
 
@@ -479,35 +658,41 @@
 #define COMP_OPMODE 0x72
 #endif // SUPPRESS_COMPASS_SR
 
+    CompassMaxSlew =  (COMPASS_SANITY_CHECK_RAD_S/HMC6352_UPDATE_S);
+
     // Set device to Compass mode
     I2CCOMPASS.start();
-    if ( I2CCOMPASS.write(HMC6352_WR) != I2C_ACK ) goto CTerror;
-    if ( I2CCOMPASS.write('G')  != I2C_ACK ) goto CTerror;
-    if ( I2CCOMPASS.write(0x74) != I2C_ACK ) goto CTerror;
-    if ( I2CCOMPASS.write(COMP_OPMODE) != I2C_ACK ) goto CTerror;
+    if ( I2CCOMPASS.write(HMC6352_WR) != I2C_ACK ) goto HMC6352Error;
+    if ( I2CCOMPASS.write('G')  != I2C_ACK ) goto HMC6352Error;
+    if ( I2CCOMPASS.write(0x74) != I2C_ACK ) goto HMC6352Error;
+    if ( I2CCOMPASS.write(COMP_OPMODE) != I2C_ACK ) goto HMC6352Error;
     I2CCOMPASS.stop();
 
-    Delay1mS(1);
+    Delay1mS(10);
 
     // save operation mode in Flash
-    if ( WriteByteHMC6352('L') != I2C_ACK ) goto CTerror;
+    if ( WriteByteHMC6352('L') != I2C_ACK ) goto HMC6352Error;
 
-    Delay1mS(1);
+    Delay1mS(10);
 
     // Do Bridge Offset Set/Reset now
-    if ( WriteByteHMC6352('O') != I2C_ACK ) goto CTerror;
+    if ( WriteByteHMC6352('O') != I2C_ACK ) goto HMC6352Error;
 
     Delay1mS(50);
 
     F.CompassValid = true;
 
     return;
-CTerror:
+
+HMC6352Error:
+    I2CCOMPASS.stop();
+
+    I2CError[HMC6352_ID]++;
+
     F.CompassValid = false;
-    Stats[CompassFailS]++;
+
     F.CompassFailure = true;
 
-    I2CCOMPASS.stop();
 } // InitHMC6352
 
 boolean HMC6352Active(void) {
diff -r 1e3318a30ddd -r 90292f8bd179 control.c
--- a/control.c	Fri Feb 25 01:35:24 2011 +0000
+++ b/control.c	Tue Apr 26 12:12:29 2011 +0000
@@ -2,7 +2,7 @@
 // =                              UAVXArm Quadrocopter Controller                                =
 // =                           Copyright (c) 2008 by Prof. Greg Egan                             =
 // =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
-// =                     http://code.google.com/p/uavp-mods/ http://uavp.ch                      =
+// =                           http://code.google.com/p/uavp-mods/                               =
 // ===============================================================================================
 
 //    This is part of UAVXArm.
@@ -20,12 +20,10 @@
 
 #include "UAVXArm.h"
 
-real32 PTerm, ITerm, DTerm;
-
 void DoAltitudeHold(void);
 void UpdateAltitudeSource(void);
+real32 AltitudeCF( real32, real32);
 void AltitudeHold(void);
-void InertialDamping(void);
 void DoOrientationTransform(void);
 void DoControl(void);
 
@@ -33,20 +31,20 @@
 void LightsAndSirens(void);
 void InitControl(void);
 
-real32 Angle[3], Anglep[3], Rate[3], Ratep[3]; // Milliradians
-real32 Comp[4];
+real32 Angle[3], Anglep[3], Rate[3], Ratep[3], YawRateIntE;
+real32 AccAltComp, AltComp;
 real32 DescentComp;
 
-real32 AngleE[3], AngleIntE[3];
-
 real32 GS;
 real32 Rl, Pl, Yl, Ylp;
 int16 HoldYaw;
 int16 CruiseThrottle, MaxCruiseThrottle, DesiredThrottle, IdleThrottle, InitialThrottle, StickThrottle;
-int16 DesiredRoll, DesiredPitch, DesiredYaw, DesiredHeading, DesiredCamPitchTrim;
-real32 ControlRoll, ControlPitch, ControlRollP, ControlPitchP;
+int16 DesiredRoll, DesiredPitch, DesiredYaw, DesiredCamPitchTrim;
+real32 DesiredHeading;
+real32 ControlRoll, ControlPitch;
 real32 CameraRollAngle, CameraPitchAngle;
 int16 CurrMaxRollPitch;
+int16 Trim[3];
 
 int16 AttitudeHoldResetCount;
 real32 AltDiffSum, AltD, AltDSum;
@@ -55,12 +53,47 @@
 
 uint32 AltuSp;
 int16 DescentLimiter;
+uint32 ControlUpdateTimeuS;
 
 real32 GRollKp, GRollKi, GRollKd, GPitchKp, GPitchKi, GPitchKd;
 
-boolean    FirstPass;
+boolean FirstPass;
 int8 BeepTick = 0;
 
+
+
+real32 AltCF = 0.0;
+real32 AltF[3] = { 0.0, 0.0, 0.0};
+
+real32 AltitudeCF( real32 AltE, real32 dT ) {
+
+    // Complementary Filter originally authored by RoyLB originally for attitude estimation
+    // http://www.rcgroups.com/forums/showpost.php?p=12082524&postcount=1286
+    
+    // Using acceleration as an alias for vertical displacement to avoid integration "issues"
+    
+    static real32 TauCF;
+     
+    TauCF = K[VertDamp];
+
+    if ( F.AccelerationsValid && F.NearLevel ) {
+
+        AltF[0] = (AltE - AltCF) * Sqr(TauCF);
+        AltF[2] += AltF[0] * dT;
+        
+        AltF[1] =  AltF[2] + (AltE - AltCF) * 2.0 * TauCF + ( 0.5 *  Acc[UD] * Sqr(dT) ); // ??? some scaling on Acc perhaps
+        AltCF += AltF[1] * dT;
+
+    } else
+        AltCF = AltE;
+        
+    AccAltComp = AltCF - AltE;
+
+    return( AltCF );
+
+} // AltitudeCF
+
+
 void DoAltitudeHold(void) { // Syncronised to baro intervals independant of active altitude source
 
     static int16 NewAltComp;
@@ -75,36 +108,32 @@
     AltdTR = 1.0 / AltdT;
     AltuSp = Now;
 
-    AltE = DesiredAltitude - Altitude;
-    LimAltE = Limit(AltE, -ALT_BAND_M, ALT_BAND_M);
+    AltE = AltitudeCF( DesiredAltitude - Altitude, AltdT );
+    LimAltE = Limit1(AltE, ALT_BAND_M);
 
     AltP = LimAltE * K[AltKp];
-    AltP = Limit(AltP, -ALT_MAX_THR_COMP, ALT_MAX_THR_COMP);
+    AltP = Limit1(AltP, ALT_MAX_THR_COMP);
 
     AltDiffSum += LimAltE;
-    AltDiffSum = Limit(AltDiffSum, -ALT_INT_WINDUP_LIMIT, ALT_INT_WINDUP_LIMIT);
+    AltDiffSum = Limit1(AltDiffSum, ALT_INT_WINDUP_LIMIT);
     AltI = AltDiffSum * K[AltKi] * AltdT;
-    AltI = Limit(AltDiffSum, -K[AltIntLimit], K[AltIntLimit]);
+    AltI = Limit1(AltDiffSum, K[AltIntLimit]);
 
     ROC = ( Altitude - AltitudeP ) * AltdTR; // may neeed filtering - noisy
     AltitudeP = Altitude;
 
     AltD = ROC * K[AltKd];
-    AltD = Limit(AltD, -ALT_MAX_THR_COMP, ALT_MAX_THR_COMP);
+    AltD = Limit1(AltD, ALT_MAX_THR_COMP);
 
     if ( ROC < ( -K[MaxDescentRateDmpS] * 10.0 ) ) {
         DescentLimiter += 1;
         DescentLimiter = Limit(DescentLimiter, 0, ALT_MAX_THR_COMP * 2.0);
-
     } else
         DescentLimiter = DecayX(DescentLimiter, 1);
 
     NewAltComp = AltP + AltI + AltD + AltDSum + DescentLimiter;
-
-    NewAltComp = Limit(NewAltComp, -ALT_MAX_THR_COMP, ALT_MAX_THR_COMP);
-
-    Comp[Alt] = SlewLimit(Comp[Alt], NewAltComp, 1.0);
-
+    NewAltComp = Limit1(NewAltComp, ALT_MAX_THR_COMP);
+    AltComp = SlewLimit(AltComp, NewAltComp, 1.0);
 
     if ( ROC > Stats[MaxROCS] )
         Stats[MaxROCS] = ROC;
@@ -142,11 +171,11 @@
                 if ( F.ThrottleMoving ) {
                     F.HoldingAlt = false;
                     DesiredAltitude = Altitude;
-                    Comp[Alt] = Decay1(Comp[Alt]);
+                    AltComp = Decay1(AltComp);
                 } else {
                     F.HoldingAlt = true;
                     if ( fabs(ROC) < ALT_HOLD_MAX_ROC_MPS  ) {
-                        NewCruiseThrottle = DesiredThrottle + Comp[Alt];
+                        NewCruiseThrottle = DesiredThrottle + AltComp;
                         CruiseThrottle = HardFilter(CruiseThrottle, NewCruiseThrottle);
                         CruiseThrottle = Limit( CruiseThrottle , IdleThrottle, MaxCruiseThrottle );
                     }
@@ -154,54 +183,12 @@
                 }
         }
     } else {
-        Comp[Alt] = Decay1(Comp[Alt]);
+        AltComp = Decay1(AltComp);
         ROC = 0.0;
         F.HoldingAlt = false;
     }
 } // AltitudeHold
 
-void InertialDamping(void) { // Uses accelerometer to damp disturbances while holding altitude
-    static uint8 i;
-
-    if ( F.AccelerationsValid  && F.NearLevel ) {
-        // Up - Down
-
-        Vel[UD] += Acc[UD] * dT;
-        Comp[UD] = Vel[UD] * K[VertDampKp];
-        Comp[UD] = Limit(Comp[UD], DAMP_VERT_LIMIT_LOW, DAMP_VERT_LIMIT_HIGH);
-
-        Vel[UD] = DecayX(Vel[UD], K[VertDampDecay]);
-
-        // Lateral compensation only when holding altitude
-        if ( F.HoldingAlt && F.AttitudeHold ) {
-            if ( F.WayPointCentred ) {
-                // Left - Right
-                Vel[LR] += Acc[LR] * dT;
-                Comp[LR] = Vel[LR] * K[HorizDampKp];
-                Comp[LR] = Limit(Comp[LR], -DAMP_HORIZ_LIMIT, DAMP_HORIZ_LIMIT);
-                Vel[LR] = DecayX(Vel[LR], K[HorizDampDecay]);
-
-                // Back - Front
-                Vel[BF] += Acc[BF] * dT;
-                Comp[BF] = Vel[BF] * K[HorizDampKp];
-                Comp[BF] = Limit(Comp[BF], -DAMP_HORIZ_LIMIT, DAMP_HORIZ_LIMIT);
-                Vel[BF] = DecayX(Vel[BF], K[HorizDampDecay]);
-            } else {
-                Vel[LR] = Vel[BF] = 0;
-                Comp[LR] = Decay1(Comp[LR]);
-                Comp[BF] = Decay1(Comp[BF]);
-            }
-        } else {
-            Vel[LR] = Vel[BF] = 0;
-
-            Comp[LR] = Decay1(Comp[LR]);
-            Comp[BF] = Decay1(Comp[BF]);
-        }
-    } else
-        for ( i = 0; i < (uint8)3; i++ )
-            Comp[i] = Vel[i] = 0.0;
-
-} // InertialDamping
 
 void DoOrientationTransform(void) {
     static real32 OSO, OCO;
@@ -222,13 +209,13 @@
 
     // PC+RS
     ControlPitch = (int16)( DesiredPitch * OCO + DesiredRoll * OSO );
-    
+
     CameraRollAngle = Angle[Pitch] * OSO + Angle[Roll] * OCO;
     CameraPitchAngle = Angle[Pitch] * OCO - Angle[Roll] * OSO;
 
 } // DoOrientationTransform
 
-void GainSchedule(boolean UseAngle) {
+void GainSchedule(void) {
 
     /*
     // rudimentary gain scheduling (linear)
@@ -258,21 +245,64 @@
 
     GS = 1.0; // Temp
 
-    GRollKp = K[RollKp];
-    GRollKi = K[RollKi];
-    GRollKd = K[RollKd];
 
-    GPitchKp = K[PitchKp];
-    GPitchKi = K[PitchKi];
-    GPitchKd = K[PitchKd];
 
 } // GainSchedule
 
+const real32 RelayKcu = ( 4.0 * 10 )/ ( PI * 0.8235 ); // stimulus 10 => Kcu 15.5
+const real32 RelayPd  = 2.0 * 1.285;
+const real32 RelayStim = 3.0;
+
+real32 RelayA = 0.0;
+real32 RelayTau = 0.0;
+uint32 RelayIteration = 0;
+real32 RelayP, RelayW;
+
+void DoRelayTuning(void) {
+
+    static real32 Temp;
+
+    Temp = fabs(Angle[Roll]);
+    if ( Temp > RelayA ) RelayA = Temp;
+
+    if ( ( RelayP < 0.0 ) && ( Angle[Roll] >= 0.0 ) ) {
+
+        RelayTau = RelayIteration * dT;
+
+        RelayP = - (PI * RelayA) / (4.0 * RelayStim);
+        RelayW = (2.0 * PI) / RelayTau;
+
+#ifndef PID_RAW
+        SendPIDTuning();
+#endif // PID_RAW
+
+        RelayA = 0.0;
+        RelayIteration = 0;
+    }
+
+    RelayIteration++;
+    RelayP = Angle[Roll];
+
+} // DoRelayTuning
+
+void Relay(void) {
+
+    if ( Angle[Roll] < 0.0 )
+        Rl = -RelayStim;
+    else
+        Rl = +RelayStim;
+
+    DesiredRoll = Rl;
+
+} // Relay
+
 void DoControl(void) {
 
+    static real32 RateE;
+
+
     GetAttitude();
     AltitudeHold();
-    InertialDamping();
 
 #ifdef SIMULATE
 
@@ -290,78 +320,70 @@
 
     DoOrientationTransform();
 
-    GainSchedule(F.UsingAngleControl);
+    GainSchedule();
 
 #ifdef DISABLE_EXTRAS
     // for commissioning
-    Comp[BF] = Comp[LR] = Comp[UD] = Comp[Alt] = 0;
+    AccAltComp = AltComp = 0.0;
     NavCorr[Roll] = NavCorr[Pitch] = NavCorr[Yaw] = 0;
-    F.UsingAngleControl = false;
 #endif // DISABLE_EXTRAS
 
-    if ( F.UsingAngleControl ) {
-        // Roll
-
-        AngleE[Roll] = ( ControlRoll * ATTITUDE_SCALE ) - Angle[Roll];
-        AngleIntE[Roll] += AngleE[Roll] * dT;
-        AngleIntE[Roll] = Limit(AngleIntE[Roll], -K[RollIntLimit], K[RollIntLimit]);
-        Rl  = -(AngleE[Roll] * GRollKp + AngleIntE[Roll] * GRollKi + Rate[Roll] * GRollKd * dTR);
-        Rl -=  NavCorr[Roll] - Comp[LR];
+    // Roll
 
-        // Pitch
+#ifdef USE_ANGLE_DERIVED_RATE  // Gyro rate noisy so compute from angle
+    RateE = ( Angle[Roll] - Anglep[Roll] ) * dTR;
+#else
+    RateE = Rate[Roll];
+#endif // USE_ANGLE_DERIVED_RATE
+    Rl  = RateE * GRollKp + Angle[Roll] * GRollKi + ( RateE - Ratep[Roll] ) * GRollKd * dTR;
+    Rl += ControlRoll + NavCorr[Roll];
 
-        AngleE[Pitch] = ( ControlPitch * ATTITUDE_SCALE ) - Angle[Pitch];
-        AngleIntE[Pitch] += AngleE[Pitch] * dT;
-        AngleIntE[Pitch] = Limit(AngleIntE[Pitch], -K[PitchIntLimit], K[PitchIntLimit]);
-        Pl  = -(AngleE[Pitch] * GPitchKp + AngleIntE[Pitch] * GPitchKi + Rate[Pitch] * GPitchKd * dTR);
-        Pl -= NavCorr[Pitch] - Comp[BF];
-
-    } else {
-        // Roll
+#ifdef USE_ANGLE_DERIVED_RATE
+    Ratep[Roll] = RateE;
+    Anglep[Roll] = Angle[Roll];
+#else
+    Ratep[Roll] = Rate[Roll];
+#endif // USE_ANGLE_DERIVED_RATE
 
-        AngleE[Roll] = Limit(Angle[Roll],  -K[RollIntLimit], K[RollIntLimit]);
-        Rl  = Rate[Roll] * GRollKp + AngleE[Roll] * GRollKi + (Rate[Roll]-Ratep[Roll]) * GRollKd * dTR;
-        Rl -=  NavCorr[Roll] - Comp[LR];
-          
-        Rl *= GS;
-
-        Rl -= ControlRoll;
-
-        ControlRollP = ControlRoll;
-        Ratep[Roll] = Rate[Roll];
+    // Pitch
 
-        // Pitch
+#ifdef USE_ANGLE_DERIVED_RATE // Gyro rate noisy so compute from angle
+    RateE = ( Angle[Pitch] - Anglep[Pitch] ) * dTR;
+#else
+    RateE = Rate[Pitch];
+#endif // USE_ANGLE_DERIVED_RATE
+    Pl  = RateE * GPitchKp + Angle[Pitch] * GPitchKi + ( RateE - Ratep[Pitch] ) * GPitchKd * dTR;
+    Pl += ControlPitch + NavCorr[Pitch];
 
-        AngleE[Pitch] = Limit(Angle[Pitch],  -K[PitchIntLimit], K[PitchIntLimit]);
-        Pl  = Rate[Pitch] * GPitchKp + AngleE[Pitch] * GPitchKi + (Rate[Pitch]-Ratep[Pitch]) * GPitchKd * dTR;
-        Pl -= NavCorr[Pitch] - Comp[BF];
-        
-        Pl *= GS;
-
-        Pl -= ControlPitch;
-
-        ControlPitchP = ControlPitch;
-        Ratep[Pitch] = Rate[Pitch];
-    }
+#ifdef USE_ANGLE_DERIVED_RATE
+    Ratep[Pitch] = RateE;
+    Anglep[Pitch] = Angle[Pitch];
+#else
+    Ratep[Pitch] = Rate[Pitch];
+#endif // USE_ANGLE_DERIVED_RATE    
 
     // Yaw
 
-    Rate[Yaw] -= NavCorr[Yaw];
-    if ( abs(DesiredYaw) > 5 )
-        Rate[Yaw] -= DesiredYaw;
+#define MAX_YAW_RATE  (HALFPI / RC_NEUTRAL)  // Radians/Sec e.g. HalfPI is 90deg/sec
+
+    DoLegacyYawComp(AttitudeMethod); // returns Angle as heading error along with compensated rate
 
-    Yl  = Rate[Yaw] * K[YawKp] + Angle[Yaw] * K[YawKi] + (Rate[Yaw]-Ratep[Yaw]) * K[YawKd] * dTR;
-    Ratep[Yaw] = Rate[Yaw];
+    RateE =  Rate[Yaw] + ( DesiredYaw + NavCorr[Yaw] ) * MAX_YAW_RATE;
+
+    YawRateIntE += RateE;
+    YawRateIntE = Limit1(YawRateIntE, YawIntLimit);
+
+    Yl =  RateE * K[YawKp] + YawRateIntE * K[YawKi];
 
 #ifdef TRICOPTER
     Yl = SlewLimit(Ylp, Yl, 2.0);
     Ylp = Yl;
-    Yl = Limit(Yl, -K[YawLimit] * 4.0, K[YawLimit] * 4.0);
+    Yl = Limit1(Yl, K[YawLimit] * 4.0);
 #else
-    Yl = Limit(Yl, -K[YawLimit], K[YawLimit]);
+    Yl = Limit1(Yl, K[YawLimit]); // currently 25 default
 #endif // TRICOPTER
 
-#endif // SIMULATE        
+#endif // SIMULATE 
 
 } // DoControl
 
@@ -435,8 +457,14 @@
     AltuSp = DescentLimiter = 0;
 
     for ( i = 0; i < (uint8)3; i++ )
-        AngleE[i] = AngleIntE[i] = Angle[i] = Anglep[i] = Rate[i] = Trim[i] = Vel[i] = Comp[i] = 0.0;
+        Angle[i] = Anglep[i] = Rate[i] = Vel[i] =  0.0;
+       
+    AccAltComp = AltComp = 0.0;
 
-    Comp[Alt] = AltSum = Ylp = ControlRollP = ControlPitchP = AltitudeP = 0.0;
+    YawRateIntE = 0.0;
+
+    AltSum = Ylp =  AltitudeP = 0.0;
+    ControlUpdateTimeuS = 0;
 
 } // InitControl
+
diff -r 1e3318a30ddd -r 90292f8bd179 gps.c
--- a/gps.c	Fri Feb 25 01:35:24 2011 +0000
+++ b/gps.c	Tue Apr 26 12:12:29 2011 +0000
@@ -2,7 +2,7 @@
 // =                              UAVXArm Quadrocopter Controller                                =
 // =                           Copyright (c) 2008 by Prof. Greg Egan                             =
 // =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
-// =                     http://code.google.com/p/uavp-mods/ http://uavp.ch                      =
+// =                           http://code.google.com/p/uavp-mods/                               =
 // ===============================================================================================
 
 //    This is part of UAVXArm.
diff -r 1e3318a30ddd -r 90292f8bd179 gyro.c
--- a/gyro.c	Fri Feb 25 01:35:24 2011 +0000
+++ b/gyro.c	Tue Apr 26 12:12:29 2011 +0000
@@ -2,7 +2,7 @@
 // =                              UAVXArm Quadrocopter Controller                                =
 // =                           Copyright (c) 2008 by Prof. Greg Egan                             =
 // =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
-// =                     http://code.google.com/p/uavp-mods/ http://uavp.ch                      =
+// =                           http://code.google.com/p/uavp-mods/                               =
 // ===============================================================================================
 
 //    This is part of UAVXArm.
@@ -20,12 +20,12 @@
 
 #include "UAVXArm.h"
 const real32 GyroToRadian[UnknownGyro] = {
-    0.023841,       // MLX90609
-    0.044680,       // ADXRS150
-    0.007149,       // IDG300
-    0.011796,       // LY530
-    0.017872,       // ADXRS300
-    0.000607,       // ITG3200
+    8.635062,       // MLX90609
+    4.607669,       // ADXRS150
+    28.797933,      // IDG300
+    17.453293,      // LY530
+    11.519173,      // ADXRS300
+    0.000438704 * 2.0 * 1.31,    // ITG3200 16bit 2's complement
     1.0             // Infrared Sensors
     // add others as required
 };
@@ -38,15 +38,41 @@
 void GyroTest(void);
 void ShowGyroType(void);
 
-real32 GyroADC[3], GyroNeutral[3], Gyro[3]; // Radians
+real32 GyroADC[3], GyroNoise[3], GyroNeutral[3], Gyro[3], Gyrop[3]; // Radians
 uint8 GyroType;
 
 void GetGyroRates(void) {
     static uint8 g;
+    static real32 d, GyroA;
 
     ReadGyros();
+
+    for ( g = 0; g < (uint8)3; g++ ) {
+        d = fabs(Gyro[g]-Gyrop[g]);
+        if ( d > GyroNoise[g] ) GyroNoise[g] = d;
+    }
+
+#ifndef SUPPRESS_ROLL_PITCH_GYRO_FILTERS
+    // dT is almost unchanged so this could be optimised
+    GyroA = dT / ( 1.0 / ( TWOPI * ROLL_PITCH_FREQ ) + dT );
+    Gyro[Roll] = LPFilter( Gyro[Roll] - GyroNeutral[Roll], Gyrop[Roll], GyroA );
+    Gyro[Pitch] = LPFilter( Gyro[Pitch] - GyroNeutral[Pitch], Gyrop[Pitch], GyroA );
+#endif // !SUPPRESS_ROLL_PITCH_GYRO_FILTERS 
+
+#ifndef SUPPRESS_YAW_GYRO_FILTERS
+
+#ifdef USE_FIXED_YAW_FILTER
+    GyroA = dT / ( 1.0 / ( TWOPI * MAX_YAW_FREQ ) + dT );
+#else
+    GyroA = dT / ( 1.0 / ( TWOPI * YawFilterLPFreq ) + dT );
+#endif // USE_FIXED_YAW_FILTER
+
+    Gyro[Yaw] = LPFilter( Gyro[Yaw] - GyroNeutral[Yaw], Gyrop[Yaw], GyroA );
+#endif // !SUPPRESS_GYRO_FILTERS
+
     for ( g = 0; g < (uint8)3; g++ )
-        Gyro[g] = ( GyroADC[g] - GyroNeutral[g] ) ;
+        Gyrop[g] = Gyro[g];
+
 } // GetGyroRates
 
 void ReadGyros(void) {
@@ -64,24 +90,26 @@
 } // ReadGyros
 
 void ErectGyros(void) {
-    static int16 i, g;
+    static uint8 s, i, g;
 
     LEDRed_ON;
 
-    for ( g = 0; g <(int8)3; g++ )
+    for ( g = 0; g <(uint8)3; g++ )
         GyroNeutral[g] = 0.0;
 
     for ( i = 0; i < 32 ; i++ ) {
         Delay1mS(10);
 
         ReadGyros();
-        for ( g = 0; g <(int8)3; g++ )
-            GyroNeutral[g] += GyroADC[g];
+        for ( g = 0; g <(uint8)3; g++ )
+            GyroNeutral[g] += Gyro[g];
     }
 
-    for ( g = 0; g <(int8)3; g++ ) {
+    for ( g = 0; g <(uint8)3; g++ ) {
         GyroNeutral[g] *= 0.03125;
-        Gyro[g] = 0.0;
+        Gyro[g] = Gyrop[g] = 0.0;
+        for ( s = 0; s < MaxAttitudeScheme; s++ )
+            EstAngle[g][s] = EstRate[g][s] = 0.0;
     }
 
     LEDRed_OFF;
@@ -94,18 +122,21 @@
 
     ReadGyros();
 
+    TxString("\r\n\tRate and Max Delta(Deg./Sec.)\r\n");
+
     TxString("\r\n\tRoll:   \t");
-    TxVal32(GyroADC[Roll] * 1000.0, 3, 0);
+    TxVal32(Gyro[Roll] * MILLIANGLE, 3, HT);
+    TxVal32(GyroNoise[Roll] * MILLIANGLE, 3, 0);
     TxNextLine();
     TxString("\tPitch:  \t");
-    TxVal32(GyroADC[Pitch] * 1000.0, 3, 0);
+    TxVal32(Gyro[Pitch] * MILLIANGLE, 3, HT);
+    TxVal32(GyroNoise[Pitch] * MILLIANGLE, 3, 0);
     TxNextLine();
     TxString("\tYaw:    \t");
-    TxVal32(GyroADC[Yaw] * 1000.0, 3, 0);
+    TxVal32(Gyro[Yaw] * MILLIANGLE, 3, HT);
+    TxVal32(GyroNoise[Yaw] * MILLIANGLE, 3, 0);
     TxNextLine();
 
-    TxString("Expect ~0.0 ( ~0.5 for for analog gyros)\r\n");
-
     switch ( GyroType ) {
         case ITG3200Gyro:
             ITG3200Test();
@@ -120,6 +151,7 @@
 } // GyroTest
 
 void InitGyros(void) {
+
     if ( ITG3200GyroActive() )
         GyroType = ITG3200Gyro;
     else
@@ -182,12 +214,13 @@
 void ReadAnalogGyros(void) {
     static uint8 g;
 
-    GyroADC[Roll] = RollADC.read();
-    GyroADC[Pitch] = PitchADC.read();
+    GyroADC[Roll] = -RollADC.read();
+    GyroADC[Pitch] = -PitchADC.read();
     GyroADC[Yaw] = YawADC.read();
 
     for ( g = 0; g < (uint8)3; g++ )
-        GyroADC[g] *= GyroToRadian[GyroType];
+        Gyro[g] = GyroADC[g] * GyroToRadian[GyroType];
+
 } // ReadAnalogGyros
 
 void InitAnalogGyros(void) {
@@ -201,7 +234,7 @@
 
 void ReadITG3200(void);
 uint8 ReadByteITG3200(uint8);
-void WriteByteITG3200(uint8, uint8);
+boolean WriteByteITG3200(uint8, uint8);
 void InitITG3200(void);
 void ITG3200Test(void);
 boolean ITG3200Active(void);
@@ -210,41 +243,45 @@
 
 void ReadITG3200Gyro(void) {
     static char G[6];
-    static uint8 g, r;
+    static uint8 g;
     static i16u GX, GY, GZ;
 
     I2CGYRO.start();
-    r = ( I2CGYRO.write(ITG3200_WR) != I2C_ACK );
-    r = ( I2CGYRO.write(ITG3200_GX_H) != I2C_ACK );
+    if ( I2CGYRO.write(ITG3200_WR) != I2C_ACK ) goto ITG3200Error;
+    if ( I2CGYRO.write(ITG3200_GX_H) != I2C_ACK ) goto ITG3200Error;
     I2CGYRO.stop();
 
-    if ( I2CGYRO.blockread(ITG3200_ID, G, 6) == 0 ) {
+    if ( I2CGYRO.blockread(ITG3200_ID, G, 6) ) goto ITG3200Error;
 
-        GX.b0 = G[1];
-        GX.b1 = G[0];
-        GY.b0 = G[3];
-        GY.b1 = G[2];
-        GZ.b0 = G[5];
-        GZ.b1 = G[4];
+    GX.b0 = G[1];
+    GX.b1 = G[0];
+    GY.b0 = G[3];
+    GY.b1 = G[2];
+    GZ.b0 = G[5];
+    GZ.b1 = G[4];
 
-        if ( F.Using9DOF ) { // SparkFun/QuadroUFO breakout pins forward components up
-            GyroADC[Roll] = -(real32)GY.i16;
-            GyroADC[Pitch] = -(real32)GX.i16;
-            GyroADC[Yaw] = -(real32)GZ.i16;
-        } else { // SparkFun 6DOF breakout pins forward components down
-            GyroADC[Roll] = -(real32)GX.i16;
-            GyroADC[Pitch] = -(real32)GY.i16;
-            GyroADC[Yaw] = (real32)GZ.i16;
-        }
+    if ( F.Using9DOF ) { // SparkFun/QuadroUFO breakout pins forward components up
+        GyroADC[Roll] = -(real32)GY.i16;
+        GyroADC[Pitch] = -(real32)GX.i16;
+        GyroADC[Yaw] = -(real32)GZ.i16;
+    } else { // SparkFun 6DOF breakout pins forward components down
+        GyroADC[Roll] = -(real32)GX.i16;
+        GyroADC[Pitch] = -(real32)GY.i16;
+        GyroADC[Yaw] = (real32)GZ.i16;
+    }
 
-        for ( g = 0; g < (uint8)3; g++ )
-            GyroADC[g] *= GyroToRadian[ITG3200Gyro];
+    for ( g = 0; g < (uint8)3; g++ )
+        Gyro[g] = GyroADC[g] * GyroToRadian[ITG3200Gyro];
+
+    return;
 
-    } else {
-        // GYRO FAILURE - FATAL
-        Stats[GyroFailS]++;
-      // not in flight keep trying   F.GyroFailure = true;
-    }
+ITG3200Error:
+    I2CGYRO.stop();
+
+    I2CError[ITG3200_ID]++;
+
+    Stats[GyroFailS]++; // not in flight keep trying
+    F.GyroFailure = true;
 
 } // ReadITG3200Gyro
 
@@ -252,53 +289,76 @@
     static uint8 d;
 
     I2CGYRO.start();
-    if ( I2CGYRO.write(ITG3200_WR) != I2C_ACK ) goto SGerror;
-    if ( I2CGYRO.write(a) != I2C_ACK ) goto SGerror;
-
+    if ( I2CGYRO.write(ITG3200_WR) != I2C_ACK ) goto ITG3200Error;
+    if ( I2CGYRO.write(a) != I2C_ACK ) goto ITG3200Error;
     I2CGYRO.start();
-    if ( I2CGYRO.write(ITG3200_RD) != I2C_ACK ) goto SGerror;
+    if ( I2CGYRO.write(ITG3200_RD) != I2C_ACK ) goto ITG3200Error;
     d = I2CGYRO.read(I2C_NACK);
     I2CGYRO.stop();
 
     return ( d );
 
-SGerror:
+ITG3200Error:
     I2CGYRO.stop();
+
+    I2CError[ITG3200_ID]++;
+    // GYRO FAILURE - FATAL
+    Stats[GyroFailS]++;
+
+    //F.GyroFailure = true;
+
+    return ( 0 );
+
+} // ReadByteITG3200
+
+boolean WriteByteITG3200(uint8 a, uint8 d) {
+
+    I2CGYRO.start();    // restart
+    if ( I2CGYRO.write(ITG3200_WR) != I2C_ACK ) goto ITG3200Error;
+    if ( I2CGYRO.write(a) != I2C_ACK ) goto ITG3200Error;
+    if ( I2CGYRO.write(d) != I2C_ACK ) goto ITG3200Error;
+    I2CGYRO.stop();
+
+    return(false);
+
+ITG3200Error:
+    I2CGYRO.stop();
+
+    I2CError[ITG3200_ID]++;
     // GYRO FAILURE - FATAL
     Stats[GyroFailS]++;
     F.GyroFailure = true;
-    return (I2C_NACK);
-} // ReadByteITG3200
 
-void WriteByteITG3200(uint8 a, uint8 d) {
-    I2CGYRO.start();    // restart
-    if ( I2CGYRO.write(ITG3200_WR) != I2C_ACK ) goto SGerror;
-    if ( I2CGYRO.write(a) != I2C_ACK ) goto SGerror;
-    if ( I2CGYRO.write(d) != I2C_ACK ) goto SGerror;
-    I2CGYRO.stop();
-    return;
+    return(true);
 
-SGerror:
-    I2CGYRO.stop();
-    // GYRO FAILURE - FATAL
-    Stats[GyroFailS]++;
-    F.GyroFailure = true;
-    return;
 } // WriteByteITG3200
 
 void InitITG3200Gyro(void) {
-    F.GyroFailure = false; // reset optimistically!
+
+#define FS_SEL 3
 
-    WriteByteITG3200(ITG3200_PWR_M, 0x80);    // Reset to defaults
-    WriteByteITG3200(ITG3200_SMPL, 0x00);     // continuous update
-    WriteByteITG3200(ITG3200_DLPF, 0x19);     // 188Hz, 2000deg/S
-    WriteByteITG3200(ITG3200_INT_C, 0x00);    // no interrupts
-    WriteByteITG3200(ITG3200_PWR_M, 0x01);    // X Gyro as Clock Ref.
+//#define DLPF_CFG 1 // 188HZ
+#define DLPF_CFG 2 // 98HZ
+//#define DLPF_CFG 3 // 42HZ
+
+    if ( WriteByteITG3200(ITG3200_PWR_M, 0x80) ) goto ITG3200Error; // Reset to defaults
+    if ( WriteByteITG3200(ITG3200_SMPL, 0x00) ) goto ITG3200Error; // continuous update
+    if ( WriteByteITG3200(ITG3200_DLPF, (FS_SEL << 3) | DLPF_CFG ) ) goto ITG3200Error; // 188Hz, 2000deg/S
+    if ( WriteByteITG3200(ITG3200_INT_C, 0x00) ) goto ITG3200Error; // no interrupts
+    if ( WriteByteITG3200(ITG3200_PWR_M, 0x01) ) goto ITG3200Error; // X Gyro as Clock Ref.
 
     Delay1mS(50);
 
+    F.GyroFailure = false;
+
     ReadITG3200Gyro();
 
+    return;
+
+ITG3200Error:
+
+    F.GyroFailure = true;
+
 } // InitITG3200Gyro
 
 void ITG3200Test(void) {
diff -r 1e3318a30ddd -r 90292f8bd179 harness.c
--- a/harness.c	Fri Feb 25 01:35:24 2011 +0000
+++ b/harness.c	Tue Apr 26 12:12:29 2011 +0000
@@ -2,7 +2,7 @@
 // =                              UAVXArm Quadrocopter Controller                                =
 // =                           Copyright (c) 2008 by Prof. Greg Egan                             =
 // =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
-// =                     http://code.google.com/p/uavp-mods/ http://uavp.ch                      =
+// =                           http://code.google.com/p/uavp-mods/                               =
 // ===============================================================================================
 
 //    This is part of UAVXArm.
@@ -25,6 +25,23 @@
 
 LocalFileSystem Flash("local");
 
+const uint8 mbed1768Pins[32] = { // Maping of mbed pins to LPC 1768 "port" pins
+    255,255,255,255,255,9,8,7,6,0,
+    1,18,17,15,16,23,24,25,26,62,
+    63,69,68,67,66,65,64,11,10,5,
+    4,255
+};
+
+/*
+const uint32 mbed1768Ports[8] = { 
+    LPC_GPIO0_BASE,
+    LPC_GPIO1_BASE,
+    LPC_GPIO2_BASE,
+    LPC_GPIO3_BASE,
+    LPC_GPIO4_BASE
+};
+*/
+
 // connections to ARM
 // 1 GND
 // 2 4.5-9V
@@ -61,12 +78,23 @@
 DigitalOut DebugPin(p25);                  // 25
 
 #ifdef SW_I2C
+
 MyI2C I2C0;
-DigitalInOut I2C0SCL(p27);
-DigitalInOut I2C0SDA(p28);
+
+#define I2C0SDASet (1<<mbed1768Pins[28&0x1f]) // 10
+#define I2C0SDAPort (mbed1768Pins[28&0x0f]>>5)
+PortInOut I2C0SDA(Port0, I2C0SDASet );
+
+#define I2C0SCLSet (1<<mbed1768Pins[27&0x1f]) // 11
+#define I2C0SCLPort (mbed1768Pins[27&0x0f]>>5)
+PortInOut I2C0SCL(Port0, I2C0SCLSet );
+
 #else
+
 I2C I2C0(p28, p27);                     // 27, 28
+
 #endif // SW_I2C
+
 DigitalIn RCIn(p29);                    // 29 CAN
 DigitalOut PWMCamRoll(p30);             // 30 CAN
 
diff -r 1e3318a30ddd -r 90292f8bd179 i2c.c
--- a/i2c.c	Fri Feb 25 01:35:24 2011 +0000
+++ b/i2c.c	Tue Apr 26 12:12:29 2011 +0000
@@ -2,7 +2,7 @@
 // =                              UAVXArm Quadrocopter Controller                                =
 // =                           Copyright (c) 2008 by Prof. Greg Egan                             =
 // =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
-// =                     http://code.google.com/p/uavp-mods/ http://uavp.ch                      =
+// =                           http://code.google.com/p/uavp-mods/                               =
 // ===============================================================================================
 
 //    This is part of UAVXArm.
@@ -31,6 +31,8 @@
 void ProgramSlaveAddress(uint8);
 void ConfigureESCs(void);
 
+uint16 I2CError[256];
+uint8 I2CSpeed = 10; // 100KHz
 uint32 MinI2CRate = I2C_MAX_RATE_HZ;
 
 //______________________________________________________________________________________________
@@ -39,51 +41,154 @@
 
 #ifdef SW_I2C
 
-#define I2CDelay5uS wait_us(5)
-#define I2CDelay2uS // wait_us(2)
-#define HighLowDelay // wait_us(1)
-#define FloatDelay // ???
+void SDelay(uint16 d) { // 1.25 + 0.0475 * n uS ~0.05uS per click
+
+    volatile int16 v;
+
+    for (v = 0; v < d ; v++ ) {};
+
+}  // SDelay
+
+/*
+#define SCLLowStartT SDelay(I2CSpeed)
+#define DataLowPadT SDelay(I2CSpeed)
+#define SCLLowPadT SDelay(I2CSpeed)
+#define SCLHighT SDelay(I2CSpeed)
+*/
+
+#if ( I2C_MAX_RATE_HZ == 400000 )
 
-#define I2CSDALow {I2C0SDA.write(0);HighLowDelay;I2C0SDA.output();}
-#define I2CSDAFloat {I2C0SDA.input();}
-#define I2CSCLLow {I2C0SCL.write(0);HighLowDelay;I2C0SCL.output();}
-#define I2CSCLFloat {I2C0SCL.input();FloatDelay;}
+#define SCLLowStartT SDelay(10)
+#define DataLowPadT SDelay(16) // 10
+#define SCLLowPadT SDelay(6)
+#define SCLHighT SDelay(10)
+
+#else
+
+#define SCLLowStartT  SDelay(82)
+#define DataLowPadT SDelay(82)
+#define SCLLowPadT SDelay(82)
+#define SCLHighT SDelay(85)
+
+#endif //I2C400KHZ
+
+
+#define I2CSDALow {I2C0SDA.write(0);I2C0SDA.output();SCLLowPadT;}
+#define I2CSDAFloat {I2C0SDA.input();SCLLowPadT;}
+#define I2CSCLLow {I2C0SCL.write(0);I2C0SCL.output();}
+#define I2CSCLFloat {I2C0SCL.input();SCLHighT;}
 
 void MyI2C::frequency(uint32 f) {
 // delay depending on rate
+
+    if ( f == 400000 )
+        I2CSpeed = 10;
+    else
+        I2CSpeed = 80;
 } // frequency
 
+void MyI2C::start(void) {
+
+    I2CSDAFloat;
+    r = waitclock();
+    I2CSDALow;
+    SCLLowStartT;
+    I2CSCLLow;
+} // start
+
+void MyI2C::stop(void) {
+
+    I2CSDALow;
+    r = waitclock();
+    I2CSDAFloat;
+    SCLLowStartT;
+
+} // stop
+
 boolean MyI2C::waitclock(void) {
     static uint32 s;
 
     I2CSCLFloat;        // set SCL to input, output a high
     s = 0;
-    while ( !I2C0SCL.read() )
-        if ( ++s > 60000 ) {
+    while ( I2C0SCL.read() == 0 )
+        if ( ++s > 16000 ) { // ~1mS
+            I2CError[0]++;
+            // possible add SCL cycles here to attempt to force device reset
             Stats[I2CFailS]++;
             return (false);
         }
     return( true );
 } // waitclock
 
-void MyI2C::start(void) {
-    static boolean r;
+uint8 MyI2C::read(uint8 ack) {
+    static uint8 s, d;
 
     I2CSDAFloat;
-    r = waitclock();
-    I2CSDALow;
-    I2CDelay5uS;
-    I2CSCLLow;
-} // start
+    d = 0;
+    s = 8;
+    do {
+        if ( waitclock() ) {
+            d <<= 1;
+            if ( I2C0SDA.read() ) {
+                d |= 1;
+                I2CSCLLow;
+                DataLowPadT;
+            } else {
+                I2CSCLLow;
+                SDelay(10);//DataLowPadT;
+            }
+        } else
+            return( 0 );
+    } while ( --s );
+
+    if ( ack == I2C_NACK )
+        I2C0SDA.write(0xffff); // Port write with mask selecting SDA - messy
+    else
+        I2C0SDA.write(0);
+    I2C0SDA.output();
+
+    SCLLowPadT;
+
+    if ( waitclock() ) {
+        I2CSCLLow;
+        return( d );
+    } else
+        return( 0 );
+
+} // read
 
-void MyI2C::stop(void) {
-    static boolean r;
+uint8 MyI2C::write(uint8 d) {
+    static uint8 s, r;
+
+    for ( s = 0; s < 8; s++) {
+        if ( d & 0x80 ) {
+            I2CSDAFloat;
+        } else {
+            I2CSDALow;
+        }
 
-    I2CSDALow;
-    r = waitclock();
+        if ( waitclock() ) {
+            I2CSCLLow;
+            d <<= 1;
+        } else
+            return(I2C_NACK);
+    }
+
     I2CSDAFloat;
-    I2CDelay5uS;
-} // stop
+    if ( waitclock() ) {
+        if ( I2C0SDA.read() )
+            r = I2C_NACK;
+        else
+            r = I2C_ACK;
+        I2CSDALow;// kill runt pulses
+        I2CSCLLow;
+        return ( r );
+    } else {
+//   I2CSCLLow;
+        return(I2C_NACK);
+    }
+
+} // write
 
 uint8 MyI2C::blockread(uint8 a, char* S, uint8 l) {
     static uint8 b;
@@ -99,79 +204,31 @@
     return( err );
 } // blockread
 
-uint8 MyI2C::read(uint8 ack) {
-    static uint8 s, d;
-
-    I2CSDAFloat;
-    d = 0;
-    s = 8;
-    do {
-        if ( waitclock() ) {
-            d <<= 1;
-            if ( I2C0SDA.read() ) d |= 1;
-            I2CSCLLow;
-            I2CDelay2uS;
-        } else
-            return( 0 );
-    } while ( --s );
-
-    I2C0SDA.write(ack);
-    HighLowDelay;
-    I2C0SDA.output();
-    HighLowDelay;
-
-    if ( waitclock() ) {
-        I2CSCLLow;
-        return( d );
-    } else
-        return( 0 );
-} // read
-
-void MyI2C::blockwrite(uint8 a, const char* S, uint8 l) {
+boolean MyI2C::blockwrite(uint8 a, const char* S, uint8 l) {
     static uint8 b;
-    static boolean r;
 
     I2C0.start();
-    r = I2C0.write(a) == I2C_ACK;  // use this?
+    if ( I2C0.write(a) != I2C_ACK ) goto BlockWriteError; // use this?
     for ( b = 0; b < l; b++ )
-        r |= I2C0.write(S[b]);
+        if ( I2C0.write(S[b]) != I2C_ACK ) goto BlockWriteError;
     I2C0.stop();
 
+    return(false);
+
+BlockWriteError:
+    I2C0.stop();
+
+    return(true);
+
 } // blockwrite
 
-uint8 MyI2C::write(uint8 d) {
-    static uint8 s, r;
-
-    for ( s = 0; s < 8; s++)
-    {
-        if ( d & 0x80 ) {
-            I2CSDAFloat;
-        } else {
-            I2CSDALow;
-        }
-
-        if ( waitclock() ) {
-            I2CSCLLow;
-            d <<= 1;
-        } else
-            return(I2C_NACK);
-    }
-
-    I2CSDAFloat;
-    if ( waitclock() ) {
-        r = I2C0SDA.read();
-        I2CSCLLow;
-        return( r );
-    } else
-        return(I2C_NACK);
-
-} // write
-
 #endif // SW_I2C
 
 //______________________________________________________________________________________________
 
 void TrackMinI2CRate(uint32 r) {
+
+// CURRENTLY USING DEFINE TO SPECIFY RATE - UAVXArm.h
     if ( r < MinI2CRate )
         MinI2CRate = r;
 } // TrackMinI2CRate
@@ -243,26 +300,57 @@
 
     d = 0;
     TxString("Buss 0\r\n");
+
+#ifdef SW_I2C
+    TxString("I2C Ver.:\tSoftware\r\n");
+#else
+    TxString("I2C Ver.:\tHardware (CAUTION)\r\n");
+#endif // SW_I2C
+
+#if ( I2C_MAX_RATE_HZ == 400000 )
+    TxString("Rate:\t400KHz\r\n");
+#else
+    TxString("Rate:\t100KHz\r\n");
+#endif
+    TxString("SCL Hangs:\t");
+    TxVal32(I2CError[0], 0, 0);
+    TxNextLine();
     for ( s = 0x10 ; s <= 0xf6 ; s += 2 ) {
         if (  I2C0AddressResponds(s) ) {
             d++;
-            DebugPin = 1;
             TxString("\t0x");
             TxValH(s);
+            TxChar(HT);
+            TxVal32(I2CError[s], 0, HT);
             ShowI2CDeviceName( s );
             TxNextLine();
-            DebugPin = 0;
         }
         Delay1mS(2);
     }
 
 #ifdef HAVE_I2C1
     TxString("Buss 1\r\n");
+
+#ifdef SW_I2C
+    TxString("I2C Ver.:\tSoftware\r\n");
+#else
+    TxString("I2C Ver.:\tHardware (CAUTION)\r\n");
+#endif // SW_I2C
+
+#if ( I2C_MAX_RATE_HZ == 400000 )
+    TxString("Rate:\t400KHz\r\n");
+#else
+    TxString("Rate:\t100KHz\r\n");
+#endif
+    TxString("SCL Hangs:\t");
+    TxNextLine();
     for ( s = 0x10 ; s <= 0xf6 ; s += 2 ) {
-        if (  I2C0AddressResponds(s) ) {
+        if ( I2C0AddressResponds(s) ) {
             d++;
             TxString("\t0x");
             TxValH(s);
+            TxChar(HT);
+            TxVal32(I2CError[s], 0, HT);
             ShowI2CDeviceName( s );
             TxNextLine();
         }
@@ -328,5 +416,14 @@
         TxString("\r\nYGEI2C not selected as ESC?\r\n");
 } // ConfigureESCs
 
+void InitI2C(void) {
+
+    uint8 i;
+
+    for ( i = 0; i < 255; i++ )
+        I2CError[i] = 0;
+
+} // InitI2C
 
 
+
diff -r 1e3318a30ddd -r 90292f8bd179 ir.c
--- a/ir.c	Fri Feb 25 01:35:24 2011 +0000
+++ b/ir.c	Tue Apr 26 12:12:29 2011 +0000
@@ -2,7 +2,7 @@
 // =                              UAVXArm Quadrocopter Controller                                =
 // =                           Copyright (c) 2008 by Prof. Greg Egan                             =
 // =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
-// =                     http://code.google.com/p/uavp-mods/ http://uavp.ch                      =
+// =                           http://code.google.com/p/uavp-mods/                               =
 // ===============================================================================================
 
 //    This is part of UAVXArm.
diff -r 1e3318a30ddd -r 90292f8bd179 irq.c
--- a/irq.c	Fri Feb 25 01:35:24 2011 +0000
+++ b/irq.c	Tue Apr 26 12:12:29 2011 +0000
@@ -2,7 +2,7 @@
 // =                              UAVXArm Quadrocopter Controller                                =
 // =                           Copyright (c) 2008 by Prof. Greg Egan                             =
 // =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
-// =                     http://code.google.com/p/uavp-mods/ http://uavp.ch                      =
+// =                           http://code.google.com/p/uavp-mods/                               =
 // ===============================================================================================
 
 //    This is part of UAVXArm.
@@ -22,7 +22,6 @@
 
 // Interrupt Routines
 
-
 const int16 MIN_PPM_SYNC_PAUSE = 2400;      // uS
 
 // no less than 1500
@@ -62,22 +61,6 @@
 uint16    RCGlitches;
 int16     PWMCycles = 0;
 
-void enableTxIrq0(void) {
-    LPC_UART0->IER |= 0x0002;
-} // enableTxIrq0
-
-void disableTxIrq0(void) {
-    LPC_UART0->IER &= ~0x0002;
-} // disableTxIrq0
-
-void enableTxIrq1(void) {
-    LPC_UART1->IER |= 0x0002;
-} // enableTxIrq1
-
-void disableTxIrq1(void) {
-    LPC_UART1->IER &= ~0x0002;
-}  // disableTxIrq1
-
 void InitTimersAndInterrupts(void) {
     static int8 i;
 
diff -r 1e3318a30ddd -r 90292f8bd179 leds.c
--- a/leds.c	Fri Feb 25 01:35:24 2011 +0000
+++ b/leds.c	Tue Apr 26 12:12:29 2011 +0000
@@ -2,7 +2,7 @@
 // =                              UAVXArm Quadrocopter Controller                                =
 // =                           Copyright (c) 2008 by Prof. Greg Egan                             =
 // =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
-// =                     http://code.google.com/p/uavp-mods/ http://uavp.ch                      =
+// =                           http://code.google.com/p/uavp-mods/                               =
 // ===============================================================================================
 
 //    This is part of UAVXArm.
@@ -80,12 +80,19 @@
     }
 
     I2CLED.start();
-    I2CLED.write(PCA9551_ID);
-    I2CLED.write(0x15);
-    I2CLED.write(L03);
-    I2CLED.write(L47);
+    if ( I2CLED.write(PCA9551_ID) != I2C_ACK ) goto PCA9551Error;
+    if ( I2CLED.write(0x15) != I2C_ACK ) goto PCA9551Error;
+    if ( I2CLED.write(L03) != I2C_ACK ) goto PCA9551Error;
+    if ( I2CLED.write(L47) != I2C_ACK ) goto PCA9551Error;
     I2CLED.stop();
 
+    return;
+
+PCA9551Error:
+    I2CLED.stop();
+
+    I2CError[PCA9551_ID]++;
+
 } // WritePCA9551
 
 void SendLEDs(void) { // 39.3 uS @ 40MHz
@@ -204,16 +211,17 @@
 // LED Driver
 
 void PCA9551Test(void) {
-    static char b[8], i, r;
+    static char b[8];
+    static uint8 i;
 
     TxString("\r\nPCA9551Test\r\n");
 
     I2CLED.start();
-    I2CGYRO.write(PCA9551_ID);
-    I2CGYRO.write(0x11);
+    if ( I2CGYRO.write(PCA9551_ID) != I2C_ACK ) goto PCA9551Error;
+    if ( I2CGYRO.write(0x11) != I2C_ACK ) goto PCA9551Error;;
     I2CLED.stop();
 
-    if ( I2CLED.blockread(PCA9551_ID, b, 7) == 0 ) {
+    if ( I2CLED.blockread(PCA9551_ID, b, 7) ) goto PCA9551Error;
 
     TxString("0:\t0b");
     TxBin8(b[6]);
@@ -224,33 +232,47 @@
         TxBin8(b[i]);
         TxNextLine();
     }
-    }
-    else
+
+    TxNextLine();
+
+    return;
+
+PCA9551Error:
+    I2CLED.stop();
+
+    I2CError[PCA9551_ID]++;
+
     TxString("FAILED\r\n");
-    
-    TxNextLine();
+
 } // PCA9551Test
 
 boolean IsPCA9551Active(void) {
 
     const char b[7] = {0x11,0x25,0x80,0x25,0x80,0x00,0x00}; // Period 1Sec., PWM 50%, ON
-    boolean r;
 
     F.UsingLEDDriver = I2CGYROAddressResponds( PCA9551_ID );
 
     if ( F.UsingLEDDriver ) {
-        I2CLED.blockwrite(PCA9551_ID, b, 7);
+        if ( I2CLED.blockwrite(PCA9551_ID, b, 7) ) goto PCA9551Error;
         TrackMinI2CRate(400000);
     }
 
+#ifdef DISABLE_LED_DRIVER
+    F.UsingLEDDriver = false;
+#endif // DISABLE_LED_DRIVER
+
     return ( F.UsingLEDDriver );
-    
+
+PCA9551Error:
+
+    F.UsingLEDDriver = false;
+
+    return ( F.UsingLEDDriver );
+
 } //IsPCA9551Active
 
 void InitLEDs(void) {
 
-    boolean r;
-
     r = IsPCA9551Active();
 
     LEDShadow = SavedLEDs = LEDPattern = 0;
diff -r 1e3318a30ddd -r 90292f8bd179 math.c
--- a/math.c	Fri Feb 25 01:35:24 2011 +0000
+++ b/math.c	Tue Apr 26 12:12:29 2011 +0000
@@ -2,7 +2,7 @@
 // =                              UAVXArm Quadrocopter Controller                                =
 // =                           Copyright (c) 2008 by Prof. Greg Egan                             =
 // =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
-// =                     http://code.google.com/p/uavp-mods/ http://uavp.ch                      =
+// =                           http://code.google.com/p/uavp-mods/                               =
 // ===============================================================================================
 
 //    This is part of UAVXArm.
diff -r 1e3318a30ddd -r 90292f8bd179 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue Apr 26 12:12:29 2011 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/63bcd7ba4912
diff -r 1e3318a30ddd -r 90292f8bd179 menu.c
--- a/menu.c	Fri Feb 25 01:35:24 2011 +0000
+++ b/menu.c	Tue Apr 26 12:12:29 2011 +0000
@@ -2,7 +2,7 @@
 // =                              UAVXArm Quadrocopter Controller                                =
 // =                           Copyright (c) 2008 by Prof. Greg Egan                             =
 // =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
-// =                     http://code.google.com/p/uavp-mods/ http://uavp.ch                      =
+// =                           http://code.google.com/p/uavp-mods/                               =
 // ===============================================================================================
 
 //    This is part of UAVXArm.
@@ -316,7 +316,10 @@
 
     if ( InitialThrottle >= RC_THRES_START )
         TxString("\tThrottle may be open - CLOSE!\r\n");
-
+        
+    if ( !F.UsingLEDDriver )
+        TxString("\tPCA Line Driver OFFLINE - no BUZZER warnings!\r\n");
+        
     ShowPrompt();
 } // ShowSetup
 
@@ -333,6 +336,7 @@
             switch ( ch ) {
                 case 'D':
                     UseDefaultParameters();
+                    InitParameters();
                     ShowPrompt();
                     break;
                 case 'L'  :    // List parameters
@@ -380,12 +384,13 @@
                     ShowPrompt();
                     break;
                 case 'N' :    // neutral values
+                    GetNeutralAccelerations();
                     TxString("\r\nNeutral    R:");
-                    TxValS(NewAccNeutral[BF]);
+                    TxValS(NewAccNeutral[LR]);
 
                     TxString("    P:");
-                    TxValS(NewAccNeutral[LR]);
-
+                    TxValS(NewAccNeutral[BF]);
+                    
                     TxString("   V:");
                     TxValS(NewAccNeutral[UD]);
                     ShowPrompt();
@@ -431,7 +436,7 @@
                     ShowPrompt();
                     break;
                 case 'G':    // GPS
-                   GyroTest();// GPSTest();
+                    GPSTest();
                     ShowPrompt();
                     break;
                 case 'H':    // barometer
diff -r 1e3318a30ddd -r 90292f8bd179 nonvolatile.c
--- a/nonvolatile.c	Fri Feb 25 01:35:24 2011 +0000
+++ b/nonvolatile.c	Tue Apr 26 12:12:29 2011 +0000
@@ -2,7 +2,7 @@
 // =                              UAVXArm Quadrocopter Controller                                =
 // =                           Copyright (c) 2008 by Prof. Greg Egan                             =
 // =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
-// =                     http://code.google.com/p/uavp-mods/ http://uavp.ch                      =
+// =                           http://code.google.com/p/uavp-mods/                               =
 // ===============================================================================================
 
 //    This is part of UAVXArm.
@@ -45,16 +45,9 @@
 int8 PX[PX_LENGTH], PXNew[PX_LENGTH];
 
 void CheckSDCardValid(void) {
-    /*
-        FILE *testfile;
 
-        testfile = fopen("/SDCard/OK", "w");
-        F.SDCardValid = testfile != NULL;
-        if ( F.SDCardValid ) fclose(testfile);
-    */
     F.SDCardValid =  SDCard.initialise_card() != 0;
 
-
 } // CheckSDCardValid
 
 void WritePXImagefile(void) {
@@ -93,7 +86,6 @@
 
 boolean ReadPXImagefile(void) {
     static uint16 a;
-    static int8 r;
     static int32 v, CheckSum;
     static boolean OK;
 
@@ -102,7 +94,6 @@
     else
         pxfile = fopen("/local/Params.txt", "r");
 
-
     OK = false;
     if ( pxfile != NULL ) {
         CheckSum = 0;
@@ -129,11 +120,14 @@
 
 } // ReadPXImagefile
 
+void CreateLogfile(void) {
 
-void CreateLogfile(void) {
-    static uint8 i;
+#ifndef SUPPRESS_SDCARD
+
+    static int16 i;
 
     UpdateRTC();
+
     if ( F.SDCardValid )
         strftime(RTCLogfile, 32, "/SDCard/L%H-%M.log", RTCTime );
     else
@@ -143,7 +137,7 @@
 
     LogfileIsOpen = logfile != NULL;
     if ( LogfileIsOpen ) {
-        i=0;
+        i = 0;
         while ( RTCString[i] != 0 )
             TxLogChar(RTCString[i++]);
         TxLogChar(CR);
@@ -151,6 +145,8 @@
     }
     LogChars = 0;
 
+#endif // !SUPPRESS_SDCARD
+
 } // CreateLogfile
 
 void CloseLogfile(void) {
@@ -160,8 +156,11 @@
 void TxLogChar(uint8 ch) {
 
 #ifndef SUPPRESS_SDCARD
-    if ( LogfileIsOpen )
-        fprintf(logfile, "%c",  ch);
+    if ( LogfileIsOpen ) {
+        LogfileIsOpen =  fprintf(logfile, "%c",  ch) > 0;
+        if ( !LogfileIsOpen )
+            CloseLogfile();
+    }
 #endif // !SUPPRESS_SDCARD
 } // TxLogChar
 
diff -r 1e3318a30ddd -r 90292f8bd179 outputs.c
--- a/outputs.c	Fri Feb 25 01:35:24 2011 +0000
+++ b/outputs.c	Tue Apr 26 12:12:29 2011 +0000
@@ -2,7 +2,7 @@
 // =                              UAVXArm Quadrocopter Controller                                =
 // =                           Copyright (c) 2008 by Prof. Greg Egan                             =
 // =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
-// =                     http://code.google.com/p/uavp-mods/ http://uavp.ch                      =
+// =                           http://code.google.com/p/uavp-mods/                               =
 // ===============================================================================================
 
 //    This is part of UAVXArm.
@@ -49,7 +49,9 @@
 } // TC
 
 void DoMulticopterMix(real32 CurrThrottle) {
+#ifndef MULTICOPTER
     static real32 Temp;
+#endif
 
 #ifdef Y6COPTER
     PWM[FrontTC] = PWM[LeftTC] = PWM[RightTC] = CurrThrottle;
@@ -81,10 +83,9 @@
     }
 #else
 #ifdef Y6COPTER
-
     Temp = Pl * 0.5;
-    PWM[FrontTC] -= Pl;             // front motor
-    PWM[LeftTC] += (Temp - Rl);     // right rear
+    PWM[FrontTC] -= Pl;          // front motor
+    PWM[LeftTC] += (Temp - Rl);  // right rear
     PWM[RightTC] += (Temp + Rl); // left rear
 
     PWM[FrontBC] = PWM[FrontTC];
@@ -104,7 +105,6 @@
     PWM[FrontBC] += Temp;
     PWM[LeftBC]  += Temp;
     PWM[RightBC] += Temp;
-
 #else
     PWM[LeftC]  += -Rl + Yl;
     PWM[RightC] +=  Rl + Yl;
@@ -159,8 +159,11 @@
 #endif // MULTICOPTER
 
 void MixAndLimitMotors(void) {
-    static real32 Temp, TempElevon, TempElevator;
+#ifndef MULTICOPTER
+    static TempElevon, TempElevator;
     static uint8 m;
+#endif
+    static real32 Temp;
 
     if ( DesiredThrottle < IdleThrottle )
         CurrThrottle = 0;
@@ -169,7 +172,7 @@
 
 #ifdef MULTICOPTER
     if ( State == InFlight )
-        CurrThrottle += (-Comp[UD] + Comp[Alt]); // vertical compensation not optional
+        CurrThrottle += AltComp; // vertical compensation not optional
 
     Temp = OUT_MAXIMUM * 0.9; // 10% headroom for control
     CurrThrottle = Limit(CurrThrottle, 0, Temp );
@@ -247,7 +250,6 @@
     static uint8 r;
 
 #ifdef MULTICOPTER
-
     if ( P[ESCType] ==  ESCHolger )
         for ( m = 0 ; m < NoOfI2CESCOutputs ; m++ ) {
             I2CESC.start();
@@ -306,7 +308,6 @@
 } // StopMotors
 
 void InitMotors(void) {
-    static uint8 m;
 
     Out0.period_us(PWM_PERIOD_US);
 
@@ -315,9 +316,11 @@
 #ifndef Y6COPTER
 #ifdef TRICOPTER
     PWM[BackC] = OUT_NEUTRAL;
-#endif // !TRICOPTER    
+#endif // !TRICOPTER
     PWM[CamRollC] = OUT_NEUTRAL;
     PWM[CamPitchC] = OUT_NEUTRAL;
+    CamRollPulseWidth = 1000 + (int16)( PWM[CamRollC] * PWMScale );
+    CamPitchPulseWidth = 1000 + (int16)( PWM[CamPitchC] * PWMScale );    
 #endif // Y6COPTER
 
 } // InitMotors
diff -r 1e3318a30ddd -r 90292f8bd179 outputs_conventional.h
--- a/outputs_conventional.h	Fri Feb 25 01:35:24 2011 +0000
+++ b/outputs_conventional.h	Tue Apr 26 12:12:29 2011 +0000
@@ -2,7 +2,7 @@
 // =                              UAVXArm Quadrocopter Controller                                =
 // =                           Copyright (c) 2008 by Prof. Greg Egan                             =
 // =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
-// =                     http://code.google.com/p/uavp-mods/ http://uavp.ch                      =
+// =                           http://code.google.com/p/uavp-mods/                               =
 // ===============================================================================================
 
 //    This is part of UAVXArm.
diff -r 1e3318a30ddd -r 90292f8bd179 outputs_copter.h
--- a/outputs_copter.h	Fri Feb 25 01:35:24 2011 +0000
+++ b/outputs_copter.h	Tue Apr 26 12:12:29 2011 +0000
@@ -2,7 +2,7 @@
 // =                              UAVXArm Quadrocopter Controller                                =
 // =                           Copyright (c) 2008 by Prof. Greg Egan                             =
 // =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
-// =                     http://code.google.com/p/uavp-mods/ http://uavp.ch                      =
+// =                           http://code.google.com/p/uavp-mods/                               =
 // ===============================================================================================
 
 //    This is part of UAVXArm.
@@ -80,7 +80,6 @@
 #endif //  MULTICOPTER
 
 #else
-
     PWM[FrontC] = Limit(PWM[FrontC], ESCMin, ESCMax);
     PWM[LeftC] = Limit(PWM[LeftC], ESCMin, ESCMax);
     PWM[RightC] = Limit(PWM[RightC], ESCMin, ESCMax);
diff -r 1e3318a30ddd -r 90292f8bd179 outputs_y6.h
--- a/outputs_y6.h	Fri Feb 25 01:35:24 2011 +0000
+++ b/outputs_y6.h	Tue Apr 26 12:12:29 2011 +0000
@@ -2,7 +2,7 @@
 // =                              UAVXArm Quadrocopter Controller                                =
 // =                           Copyright (c) 2008 by Prof. Greg Egan                             =
 // =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
-// =                     http://code.google.com/p/uavp-mods/ http://uavp.ch                      =
+// =                           http://code.google.com/p/uavp-mods/                               =
 // ===============================================================================================
 
 //    This is part of UAVXArm.
diff -r 1e3318a30ddd -r 90292f8bd179 params.c
--- a/params.c	Fri Feb 25 01:35:24 2011 +0000
+++ b/params.c	Tue Apr 26 12:12:29 2011 +0000
@@ -2,7 +2,7 @@
 // =                              UAVXArm Quadrocopter Controller                                =
 // =                           Copyright (c) 2008 by Prof. Greg Egan                             =
 // =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
-// =                     http://code.google.com/p/uavp-mods/ http://uavp.ch                      =
+// =                           http://code.google.com/p/uavp-mods/                               =
 // ===============================================================================================
 
 //    This is part of UAVXArm.
@@ -56,35 +56,19 @@
 void Legacy(void) {
     static uint8 p;
     
-
-
-    for ( p = 0; p <MAX_PARAMETERS; p++ ) // brute force
+    for ( p = 0; p < MAX_PARAMETERS; p++ ) // brute force
         K[p] = (float)P[p];
-
-    // Rate Control
-    K[RollKp] *= 2.6 * MAGIC;
-    K[RollKi] *= 20.7 * MAGIC;
-    K[RollKd]  = -K[RollKd] * 0.021 * MAGIC;
-    K[RollIntLimit] *= DEGRAD;
+        
+    GRollKp = K[RollKp];
+    GRollKi = K[RollKi];
+    GRollKd = K[RollKd];
 
-    K[PitchKp] *= 2.6 * MAGIC;
-    K[PitchKi] *= 20.7 * MAGIC;
-    K[PitchKd]  = -K[PitchKd] * 0.021 * MAGIC;
-    K[PitchIntLimit] *= DEGRAD;
-
-    K[YawKp] *= 2.6 * MAGIC;
-    K[YawKi] *= 41.4 * MAGIC;
-    K[YawKd]  = -K[YawKd] * 0.0004 * MAGIC;
+    GPitchKp = K[PitchKp];
+    GPitchKi = K[PitchKi];
+    GPitchKd = K[PitchKd];
 
-    // Angle Control
-
-    // not yet
-
-    // Inertial Damping
-    K[VertDampKp] *= 0.1; // one click/MPS
-    K[HorizDampKp] *= 0.1;
-    K[VertDampDecay] *= 0.01;
-    K[HorizDampDecay] *= 0.01;
+    K[RollIntLimit] *= (DEGRAD * 10.0);
+    K[PitchIntLimit] *= (DEGRAD * 10.0);
 
     // Altitude Hold
     K[AltKp] *= 0.625;
@@ -97,7 +81,11 @@
     
     K[Balance] = ( 128.0 + (float)P[Balance])/128.0;
     
-    K[CompassKp] = P[CompassKp] / 4096.0;
+    K[CompassKp] = P[CompassKp] * 0.01;
+    
+    K[YawKp] *= 2.6;
+    K[YawKi] *= 4.14; // was 41.4
+    
     K[YawIntLimit] = P[YawIntLimit] * 256.0 /1000.0;
 
     // Camera
@@ -267,7 +255,7 @@
             if ( F.AllowNavAltitudeHold )
                 DoBeep100mS(4, 4);
             ParametersChanged |= true;
-            Beeper_OFF;
+       //zzz     Beeper_OFF;
             LEDBlue_OFF;
         }
     }
diff -r 1e3318a30ddd -r 90292f8bd179 rc.c
--- a/rc.c	Fri Feb 25 01:35:24 2011 +0000
+++ b/rc.c	Tue Apr 26 12:12:29 2011 +0000
@@ -2,7 +2,7 @@
 // =                              UAVXArm Quadrocopter Controller                                =
 // =                           Copyright (c) 2008 by Prof. Greg Egan                             =
 // =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
-// =                     http://code.google.com/p/uavp-mods/ http://uavp.ch                      =
+// =                           http://code.google.com/p/uavp-mods/                               =
 // ===============================================================================================
 
 //    This is part of UAVXArm.
@@ -92,7 +92,6 @@
 int16x8x4Q PPMQ;
 int16 PPMQSum[CONTROLS];
 int16 RC[CONTROLS], RCp[CONTROLS];
-int16 Trim[3];
 int16 ThrLow, ThrNeutral, ThrHigh;
 
 boolean RCPositiveEdge;
@@ -115,7 +114,7 @@
 }  // DoRxPolarity
 
 void InitRC(void) {
-    static int8 c, i, q;
+    static int8 c, q;
 
     DoRxPolarity();
 
@@ -133,9 +132,7 @@
     PPMQ.Head = 0;
 
     DesiredRoll = DesiredPitch = DesiredYaw = DesiredThrottle = StickThrottle = 0;
-
-    for ( i = 0; i < 3; i++ )
-        Trim[i] = 0;
+    Trim[Roll] = Trim[Pitch] = Trim[Yaw] = 0;
 
     PPM_Index = PrevEdge = RCGlitches = 0;
 } // InitRC
@@ -288,11 +285,11 @@
     DesiredPitch = SRS16((RC[PitchRC] - RC_NEUTRAL) * RollPitchScale, 7);
 #endif // ATTITUDE_NO_LIMITS
     DesiredYaw = RC[YawRC] - RC_NEUTRAL;
+    
+    AdaptiveYawLPFreq();
 
-    HoldRoll = DesiredRoll - Trim[Roll];
-    HoldRoll = abs(HoldRoll);
-    HoldPitch = DesiredPitch - Trim[Pitch];
-    HoldPitch = abs(HoldPitch);
+    HoldRoll = abs(DesiredRoll - Trim[Roll]);
+    HoldPitch = abs(DesiredPitch - Trim[Pitch]);
     CurrMaxRollPitch = Max(HoldRoll, HoldPitch);
 
     if ( CurrMaxRollPitch > ATTITUDE_HOLD_LIMIT )
@@ -318,11 +315,14 @@
 
 } // UpdateControls
 
-void CaptureTrims(void) {    // only used in detecting movement from neutral in hold GPS position
+void CaptureTrims(void)
+{     // only used in detecting movement from neutral in hold GPS position
     // Trims are invalidated if Nav sensitivity is changed - Answer do not use trims ?
-    Trim[Roll] = Limit(DesiredRoll, -NAV_MAX_TRIM, NAV_MAX_TRIM);
-    Trim[Pitch] = Limit(DesiredPitch, -NAV_MAX_TRIM, NAV_MAX_TRIM);
-    Trim[Yaw] = Limit(DesiredYaw, -NAV_MAX_TRIM, NAV_MAX_TRIM);
+    #ifndef TESTING
+    Trim[Roll] = Limit1(DesiredRoll, NAV_MAX_TRIM);
+    Trim[Yaw] = Limit1(DesiredPitch, NAV_MAX_TRIM);
+    Trim[Yaw] = Limit1(DesiredYaw, NAV_MAX_TRIM);
+    #endif // TESTING
 
     HoldYaw = 0;
 } // CaptureTrims
diff -r 1e3318a30ddd -r 90292f8bd179 sb_globals.cpp
--- a/sb_globals.cpp	Fri Feb 25 01:35:24 2011 +0000
+++ b/sb_globals.cpp	Tue Apr 26 12:12:29 2011 +0000
@@ -1,42 +1,42 @@
-/*
-    Copyright (c) 2010 Andy Kirkham
- 
-    Permission is hereby granted, free of charge, to any person obtaining a copy
-    of this software and associated documentation files (the "Software"), to deal
-    in the Software without restriction, including without limitation the rights
-    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-    copies of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
- 
-    The above copyright notice and this permission notice shall be included in
-    all copies or substantial portions of the Software.
- 
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-    THE SOFTWARE.
-*/
-
-/* Module globals, defined here rather than as private
-   variables in the class so the C interrupt routine can
-   access these variables rather than trying to discover 
-   where they are at runtime. */
-char *_tx_buffer[4];
-int   _tx_buffer_size[4];
-int   _tx_buffer_in[4];
-int   _tx_buffer_out[4];
-bool  _tx_buffer_full[4];
-bool  _tx_buffer_overflow[4];
-bool  _tx_buffer_used_malloc[4];
-
-char *_rx_buffer[4];        
-int   _rx_buffer_size[4];
-int   _rx_buffer_in[4];
-int   _rx_buffer_out[4];
-bool  _rx_buffer_full[4];
-bool  _rx_buffer_overflow[4];
-bool  _rx_buffer_used_malloc[4];
-
+/*
+    Copyright (c) 2010 Andy Kirkham
+ 
+    Permission is hereby granted, free of charge, to any person obtaining a copy
+    of this software and associated documentation files (the "Software"), to deal
+    in the Software without restriction, including without limitation the rights
+    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+    copies of the Software, and to permit persons to whom the Software is
+    furnished to do so, subject to the following conditions:
+ 
+    The above copyright notice and this permission notice shall be included in
+    all copies or substantial portions of the Software.
+ 
+    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+    THE SOFTWARE.
+*/
+
+/* Module globals, defined here rather than as private
+   variables in the class so the C interrupt routine can
+   access these variables rather than trying to discover 
+   where they are at runtime. */
+char *_tx_buffer[4];
+int   _tx_buffer_size[4];
+int   _tx_buffer_in[4];
+int   _tx_buffer_out[4];
+bool  _tx_buffer_full[4];
+bool  _tx_buffer_overflow[4];
+bool  _tx_buffer_used_malloc[4];
+
+char *_rx_buffer[4];        
+int   _rx_buffer_size[4];
+int   _rx_buffer_in[4];
+int   _rx_buffer_out[4];
+bool  _rx_buffer_full[4];
+bool  _rx_buffer_overflow[4];
+bool  _rx_buffer_used_malloc[4];
+
diff -r 1e3318a30ddd -r 90292f8bd179 serial.c
--- a/serial.c	Fri Feb 25 01:35:24 2011 +0000
+++ b/serial.c	Tue Apr 26 12:12:29 2011 +0000
@@ -33,7 +33,7 @@
 void TxBin8(uint8);
 void TxNextLine(uint8);
 void TxNibble( uint8);
-void TxValH( uint8);
+void TxValH(uint8);
 void TxValH16( uint16);
 uint8 RxChar(void);
 uint8 PollRxChar(void);
@@ -57,8 +57,8 @@
 
     TxCheckSum ^= ch;
 
-    while ( !TelemetrySerial.writeable() ) {};
-    TelemetrySerial.putc(ch);
+    while ( !SERIAL_TELEMETRY.writeable() ) {};
+    SERIAL_TELEMETRY.putc(ch);
 
     if ( EchoToLogFile )
         TxLogChar(ch);
@@ -124,8 +124,8 @@
 uint8 PollRxChar(void) {
     uint8    ch;
 
-    if (  TelemetrySerial.readable() ) {    // a character is waiting in the buffer
-        ch = TelemetrySerial.getc();        // get the character
+    if (  SERIAL_TELEMETRY.readable() ) {    // a character is waiting in the buffer
+        ch = SERIAL_TELEMETRY.getc();        // get the character
         TxChar(ch);                         // echo it for UAVPSet
         return(ch);                         // and return it
     }
@@ -136,9 +136,9 @@
 uint8 RxChar(void) {
     uint8    ch;
 
-    while ( !TelemetrySerial.readable() ) {};
+    while ( !SERIAL_TELEMETRY.readable() ) {};
 
-    ch = TelemetrySerial.getc();    // get the character
+    ch = SERIAL_TELEMETRY.getc();    // get the character
 
     return(ch);
 } // RxChar
diff -r 1e3318a30ddd -r 90292f8bd179 stats.c
--- a/stats.c	Fri Feb 25 01:35:24 2011 +0000
+++ b/stats.c	Tue Apr 26 12:12:29 2011 +0000
@@ -2,7 +2,7 @@
 // =                              UAVXArm Quadrocopter Controller                                =
 // =                           Copyright (c) 2008 by Prof. Greg Egan                             =
 // =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
-// =                     http://code.google.com/p/uavp-mods/ http://uavp.ch                      =
+// =                           http://code.google.com/p/uavp-mods/                               =
 // ===============================================================================================
 
 //    This is part of UAVXArm.
diff -r 1e3318a30ddd -r 90292f8bd179 telemetry.c
--- a/telemetry.c	Fri Feb 25 01:35:24 2011 +0000
+++ b/telemetry.c	Tue Apr 26 12:12:29 2011 +0000
@@ -2,7 +2,7 @@
 // =                              UAVXArm Quadrocopter Controller                                =
 // =                           Copyright (c) 2008 by Prof. Greg Egan                             =
 // =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
-// =                     http://code.google.com/p/uavp-mods/ http://uavp.ch                      =
+// =                           http://code.google.com/p/uavp-mods/                               =
 // ===============================================================================================
 
 //    This is part of UAVXArm.
@@ -38,6 +38,7 @@
 void SendParameters(uint8);
 void SendMinPacket(void);
 void SendArduStation(void);
+void SendPIDTuning(void);
 void SendCustom(void);
 void SensorTrace(void);
 
@@ -161,7 +162,7 @@
                 break;
             case CustomTelemetry:
                 mS[TelemetryUpdate] = mSClock() + CUSTOM_TEL_INTERVAL_MS;
-                SendCustom();
+                SendPIDTuning();//SendCustom();
                 break;
             case GPSTelemetry:
                 break;
@@ -170,15 +171,9 @@
 } // CheckTelemetry
 
 void SendPacketHeader(void) {
-    static int8 b;
 
     EchoToLogFile = true;
 
-#ifdef TELEMETRY_PREAMBLE
-    for (b=10;b;b--)
-        TxChar(0x55);
-#endif // TELEMETRY_PREAMBLE
-
     TxChar(0xff); // synchronisation to "jolt" USART
     TxChar(SOH);
     TxCheckSum = 0;
@@ -212,6 +207,7 @@
     TxESCi16(Acc[LR] * 1000.0);
     TxESCi16(Acc[BF] * 1000.0);
     TxESCi16(Acc[UD] * 1000.0);
+
 } // ShowAttitude
 
 void SendFlightPacket(void) {
@@ -236,10 +232,10 @@
 
     ShowAttitude();
 
-    TxESCi8((int8)Comp[LR]);
-    TxESCi8((int8)Comp[BF]);
-    TxESCi8((int8)Comp[UD]);
-    TxESCi8((int8)Comp[Alt]);
+    TxESCi8((int8)Correction[LR]);
+    TxESCi8((int8)Correction[BF]);
+    TxESCi8((int8)AccAltComp);
+    TxESCi8((int8)AltComp);
 
     for ( b = 0; b < 8; b++ )
         TxESCi16((int16)PWM[b]);
@@ -255,7 +251,7 @@
     SendPacketHeader();
 
     TxESCu8(UAVXControlPacketTag);
-    TxESCu8(46);
+    TxESCu8(48);
 
     TxESCi16(DesiredThrottle);
 
@@ -335,40 +331,19 @@
 } // SendStickPacket
 
 void SendStatsPacket(void) {
+
+    static uint8 i;
+
     SendPacketHeader();
 
     TxESCu8(UAVXStatsPacketTag);
-    TxESCu8(44);
-
-    TxESCi16(Stats[I2CFailS]);
-    TxESCi16(Stats[GPSInvalidS]);
-    TxESCi16(Stats[AccFailS]);
-    TxESCi16(Stats[GyroFailS]);
-    TxESCi16(Stats[CompassFailS]);
-    TxESCi16(Stats[BaroFailS]);
-    TxESCi16(Stats[ESCI2CFailS]);
-
-    TxESCi16(Stats[RCFailsafesS]);
+    TxESCu8(MAX_STATS * 2 + 2);
 
-    TxESCi16(Stats[GPSAltitudeS]);
-    TxESCi16(Stats[GPSVelS]);
-    TxESCi16(Stats[GPSMinSatsS]);
-    TxESCi16(Stats[GPSMaxSatsS]);
-    TxESCi16(Stats[MinHDiluteS]);
-    TxESCi16(Stats[MaxHDiluteS]);
-
-    TxESCi16(Stats[BaroRelAltitudeS]);
-    TxESCi16(0);//Stats[MinBaroROCS]);
-    TxESCi16(0);//Stats[MaxBaroROCS]);
-
-    TxESCi16(Stats[MinTempS]);
-    TxESCi16(Stats[MaxTempS]);
-
-    TxESCi16(Stats[BadS]);
+    for ( i = 0; i < MAX_STATS ; i++)
+        TxESCi16(Stats[i]);
 
     TxESCu8(UAVXAirframe | 0x80);
     TxESCu8(Orientation);
-    TxESCi16(Stats[BadNumS]);
 
     SendPacketTrailer();
 
@@ -415,17 +390,13 @@
 void SendParamPacket(uint8 s, uint8 p) {
 
     SendPacketHeader();
-    static union { real32 r32;
-        int32 i32;
-    } Temp;
-
-    //  Temp.r32 = K[p];
 
     TxESCu8(UAVXArmParamPacketTag);
     TxESCu8(6);
     TxESCu8(s);
     TxESCu8(p);
     TxESCi32(K[p] * 1000.0 );
+
     SendPacketTrailer();
 
 } // SendParamPacket
@@ -436,6 +407,7 @@
     for ( p = 0; p < MAX_PARAMETERS; p++ )
         SendParamPacket(s, p);
     SendParamPacket(0, MAX_PARAMETERS);
+
 } // SendParameters
 
 void SendCycle(void) {
@@ -535,45 +507,58 @@
 
 } // SendArduStation
 
+void SendPIDTuning(void) { // user defined telemetry human readable OK for small amounts of data < 1mS
+
+    // Fixed to roll axis
+
+    SendPacketHeader();
+
+    TxESCu8(UAVXCustomPacketTag);
+    TxESCu8(1 + 10);
+    TxESCu8(5); // how many 16bit elements
+/*
+    TxESCi16(DesiredRoll);
+    TxESCi16(PWM[RightC]);
+
+    TxESCi16(Gyro[Roll] * 1000.0);
+    TxESCi16(Acc[Roll] * 1000.0);
+    TxESCi16(Angle[Roll] * 1000.0 );
+    */
+    
+    TxESCi16(DesiredYaw);
+    TxESCi16(PWM[RightC]);
+
+    TxESCi16(Gyro[Yaw] * 1000.0);
+    TxESCi16(HE * 1000.0);
+    TxESCi16(Angle[Yaw] * 1000.0 );
+
+    SendPacketTrailer();
+
+} // SendPIDTuning
+
+
 void SendCustom(void) { // user defined telemetry human readable OK for small amounts of data < 1mS
 
-    EchoToLogFile = true;
+    static uint8 s;
 
-    // insert values here using TxVal32(n, dp, separator)
-    // dp is the scaling to decimal places, separator
-    // separator may be a single 'char', HT for tab, or 0 (no space)
-    // ->
+    // always a vector of int16;
+    SendPacketHeader();
 
-    TxVal32(mSClock(), 3, HT);
-
-    if ( F.HoldingAlt ) // are we holding
-        TxChar('H');
-    else
-        TxChar('N');
-    TxChar(HT);
+    TxESCu8(UAVXCustomPacketTag);
+    TxESCu8(1 + 8 + MaxAttitudeScheme * 2);
+    TxESCu8(4 + MaxAttitudeScheme ); // how many 16bit elements
+    TxESCi16(AttitudeMethod);
 
-    if (F.UsingRangefinderAlt ) // are we using the rangefinder
-        TxChar('R');
-    else
-        TxChar('B');
-    TxChar(HT);
+    TxESCi16(0); // spare
 
-    TxVal32(SRS32(Comp[Alt],1), 1, HT);        // ~% throttle compensation
+    TxESCi16(Gyro[Roll] * 1000.0);
+    TxESCi16(Acc[LR] * 1000.0);
 
-    TxVal32(GPSRelAltitude, 1, HT);
-    TxVal32(BaroRelAltitude, 1, HT);
-    TxVal32(RangefinderAltitude, 2, HT);
+    for ( s = 0; s < MaxAttitudeScheme; s++ )
+        TxESCi16( EstAngle[Roll][s] * 1000.0 );
 
-    TxVal32(BaroPressure, 0, HT);            // eff. sensor reading
-    TxVal32(BaroTemperature, 0, HT);         // eff. sensor reading redundant for MPX4115
-    TxVal32(CompBaroPressure, 0, HT);          // moving sum of last 8 readings
+    SendPacketTrailer();
 
-    // <-
-
-    TxChar(CR);
-    TxChar(LF);
-
-    EchoToLogFile = false;
 } // SendCustom
 
 void SensorTrace(void) {
@@ -622,13 +607,13 @@
         TxValH16(Acc[DU]);
         TxChar(';');
 
-        TxValH16(Comp[LR]);
+        TxValH16(Correction[LR]);
         TxChar(';');
-        TxValH16(Comp[FB]);
+        TxValH16(Correction[FB]);
         TxChar(';');
-        TxValH16(Comp[DU]);
+        TxValH16(AccAltComp);
         TxChar(';');
-        TxValH16(Comp[Alt]);
+        TxValH16(AltComp);
         TxChar(';');
         TxNextLine();
     }
diff -r 1e3318a30ddd -r 90292f8bd179 temperature.c
--- a/temperature.c	Fri Feb 25 01:35:24 2011 +0000
+++ b/temperature.c	Tue Apr 26 12:12:29 2011 +0000
@@ -2,7 +2,7 @@
 // =                              UAVXArm Quadrocopter Controller                                =
 // =                           Copyright (c) 2008 by Prof. Greg Egan                             =
 // =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
-// =                     http://code.google.com/p/uavp-mods/ http://uavp.ch                      =
+// =                           http://code.google.com/p/uavp-mods/                               =
 // ===============================================================================================
 
 //    This is part of UAVXArm.
@@ -27,7 +27,7 @@
 void GetTemperature(void) {
 
     I2CTEMP.start();
-    if ( I2CTEMP.write(TMP100_RD) != I2C_ACK ) goto Terror;
+    if ( I2CTEMP.write(TMP100_RD) != I2C_ACK ) goto TMP100Error;
     AmbientTemperature.b1 = I2CTEMP.read(I2C_ACK);
     AmbientTemperature.b0 = I2CTEMP.read(I2C_NACK);
     I2CTEMP.stop();
@@ -41,8 +41,10 @@
             Stats[MinTempS] = AmbientTemperature.i16;
     return;
 
-Terror:
+TMP100Error:
     I2CTEMP.stop();
+    
+    I2CError[TMP100_ID]++;
     AmbientTemperature.i16 = 0;
 
     return;
diff -r 1e3318a30ddd -r 90292f8bd179 uavx_aileron.h
--- a/uavx_aileron.h	Fri Feb 25 01:35:24 2011 +0000
+++ b/uavx_aileron.h	Tue Apr 26 12:12:29 2011 +0000
@@ -2,7 +2,7 @@
 // =                              UAVXArm Quadrocopter Controller                                =
 // =                           Copyright (c) 2008 by Prof. Greg Egan                             =
 // =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
-// =                     http://code.google.com/p/uavp-mods/ http://uavp.ch                      =
+// =                           http://code.google.com/p/uavp-mods/                               =
 // ===============================================================================================
 
 //    This is part of UAVXArm.
diff -r 1e3318a30ddd -r 90292f8bd179 uavx_elevon.h
--- a/uavx_elevon.h	Fri Feb 25 01:35:24 2011 +0000
+++ b/uavx_elevon.h	Tue Apr 26 12:12:29 2011 +0000
@@ -2,7 +2,7 @@
 // =                              UAVXArm Quadrocopter Controller                                =
 // =                           Copyright (c) 2008 by Prof. Greg Egan                             =
 // =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
-// =                     http://code.google.com/p/uavp-mods/ http://uavp.ch                      =
+// =                           http://code.google.com/p/uavp-mods/                               =
 // ===============================================================================================
 
 //    This is part of UAVXArm.
diff -r 1e3318a30ddd -r 90292f8bd179 uavx_helicopter.h
--- a/uavx_helicopter.h	Fri Feb 25 01:35:24 2011 +0000
+++ b/uavx_helicopter.h	Tue Apr 26 12:12:29 2011 +0000
@@ -2,7 +2,7 @@
 // =                              UAVXArm Quadrocopter Controller                                =
 // =                           Copyright (c) 2008 by Prof. Greg Egan                             =
 // =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
-// =                     http://code.google.com/p/uavp-mods/ http://uavp.ch                      =
+// =                           http://code.google.com/p/uavp-mods/                               =
 // ===============================================================================================
 
 //    This is part of UAVXArm.
diff -r 1e3318a30ddd -r 90292f8bd179 uavx_multicopter.h
--- a/uavx_multicopter.h	Fri Feb 25 01:35:24 2011 +0000
+++ b/uavx_multicopter.h	Tue Apr 26 12:12:29 2011 +0000
@@ -2,7 +2,7 @@
 // =                              UAVXArm Quadrocopter Controller                                =
 // =                           Copyright (c) 2008 by Prof. Greg Egan                             =
 // =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
-// =                     http://code.google.com/p/uavp-mods/ http://uavp.ch                      =
+// =                           http://code.google.com/p/uavp-mods/                               =
 // ===============================================================================================
 
 //    This is part of UAVXArm.
@@ -19,19 +19,19 @@
 //    If not, see http://www.gnu.org/licenses/
 
    const int8 DefaultParams[MAX_PARAMETERS][2] = {
-    {20,0},            // RollKp,             01
-    {12,0},             // RollKi,            02
-    {50, 0},            // RollKd,            03
-    {0,true},            // HorizDampKp,        04c //-1
+    {15,0},            // RollKp,             01
+    {80,0},             // RollKi,            02
+    {0, 0},            // RollKd,            03
+    {0,true},            // was HorizDampKp,        04c //-1
     {5,0},                 // RollIntLimit,    05
-    {20,0},             // PitchKp,            06
-    {12,0},             // PitchKi,            07
-    {50,0},                 // PitchKd,            08
-    {8,0},                 // AltKp,            09
+    {15,0},             // PitchKp,            06
+    {80,0},             // PitchKi,            07
+    {0,0},                 // PitchKd,            08
+    {12,0},                 // AltKp,            09
     {5,0},                 // PitchIntLimit,    10
     
-    {30,0},             // YawKp,             11
-    {25,0},             // YawKi,            12
+    {20,0},             // YawKp,             11
+    {10,0},             // was YawKi,            12
     {0,0},                 // YawKd,            13
     {25,0},                 // YawLimit,        14
     {2,0},                 // YawIntLimit,        15
@@ -41,7 +41,7 @@
     {20,true},             // CamRollKp,        19c
     {45,true},             // PercentCruiseThr,20c 
     
-    {0,true},             // VertDampKp,        21c //-1
+    {5,true},             // VertDamp,        21c //-1
     {0,true},             // MiddleDU,        22c
     {20,true},             // PercentIdleThr,    23c
     {0,true},             // MiddleLR,        24c
@@ -64,8 +64,8 @@
     {1,true},            // CamRollTrim,        40c
 
     {16,0},            // NavKd            41
-    {1,true},            // VertDampDecay    42c
-    {1,true},            // HorizDampDecay    43c
+    {1,true},            // was VertDampDecay    42c
+    {1,true},            // was HorizDampDecay    43c
     {56,true},            // BaroScale        44c
     {UAVXTelemetry,true}, // TelemetryType    45c
     {8,0},                // MaxDescentRateDmpS     46
diff -r 1e3318a30ddd -r 90292f8bd179 utils.c
--- a/utils.c	Fri Feb 25 01:35:24 2011 +0000
+++ b/utils.c	Tue Apr 26 12:12:29 2011 +0000
@@ -2,7 +2,7 @@
 // =                              UAVXArm Quadrocopter Controller                                =
 // =                           Copyright (c) 2008 by Prof. Greg Egan                             =
 // =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
-// =                     http://code.google.com/p/uavp-mods/ http://uavp.ch                      =
+// =                           http://code.google.com/p/uavp-mods/                               =
 // ===============================================================================================
 
 //    This is part of UAVXArm.
@@ -25,14 +25,17 @@
 void Delay100mS(int16);
 void DoBeep100mS(uint8, uint8);
 void DoStartingBeeps(uint8);
+void DoBeeperPulse1mS(uint16);
 void CheckAlarms(void);
 real32 SlewLimit(real32, real32, real32);
 real32 DecayX(real32, real32);
-void LPFilter(real32*, real32*, real32, real32);
+real32 LPFilter(real32, real32, real32);
 void Timing(uint8, uint32);
 
 TimingRec Times[(UnknownT+1)];
 
+uint32 BeeperOnTime, BeeperOffTime;
+
 void InitMisc(void) {
     int8 i;
 
@@ -88,18 +91,29 @@
         DoBeep100mS(2, 8);
 
     DoBeep100mS(8,0);
+
 } // DoStartingBeeps
 
+void DoBeeperPulse1mS(int16 d) {
+
+    if ( !F.BeeperInUse ) {
+        mS[BeeperTimeout] = mSClock() + 500L;
+        Beeper_ON;
+    }
+
+//   BeeperOnTime = d;
+//   BeeperOffTime = 0x7ffffff;
+
+} // DoBeeperPulse1mS
+
 void CheckAlarms(void) {
 
-    static uint16 BeeperOnTime, BeeperOffTime;
-
     F.BeeperInUse = F.LowBatt || F.LostModel  || (State == Shutdown);
 
     if ( F.BeeperInUse ) {
         if ( F.LowBatt ) {
-            BeeperOffTime = 750;
-            BeeperOnTime = 250;
+            BeeperOffTime = 500;
+            BeeperOnTime = 500;
         } else
             if ( State == Shutdown ) {
                 BeeperOffTime = 4750;
@@ -143,13 +157,9 @@
     return (i);
 } // DecayX
 
-void LPFilter(real32* i, real32* ip, real32 FilterF, real32 dT) {
-    static real32 FilterA;
+real32 LPFilter(real32 i, real32 ip, real32 A) {
 
-    FilterA = dT / ( FilterF + dT );
-
-    *i = *ip + (*i - *ip) * FilterA;
-    *ip = *i;
+    return ( ip + (i - ip) * A );
 
 } // LPFilter