Prof Greg Egan
/
UAVXArm-GKE
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Revision 2:90292f8bd179, committed 2011-04-26
- 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
--- /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
+
+
--- 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
--- 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();
--- 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[];
--- 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.
--- 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$"
--- 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
--- 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 );
--- 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
+
--- 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;
--- 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
--- 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) {
--- 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
+
--- 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.
--- 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) {
--- 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
--- 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
+
--- 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.
--- 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;
--- 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;
--- 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.
--- /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
--- 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
--- 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
--- 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
--- 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.
--- 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);
--- 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.
--- 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;
}
}
--- 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
--- 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]; +
--- 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
--- 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.
--- 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();
}
--- 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;
--- 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.
--- 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.
--- 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.
--- 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
--- 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