Prof Greg Egan
UAVX Multicopter Flight Controller.
Revision 0:62a1c91a859a, committed 2011-02-18
- Comitter:
- gke
- Date:
- Fri Feb 18 22:28:05 2011 +0000
- Child:
- 1:1e3318a30ddd
- Commit message:
- First release
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/FATFileSystem.lib Fri Feb 18 22:28:05 2011 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_unsupported/code/fatfilesystem/ \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Methods/sb_aux.cpp Fri Feb 18 22:28:05 2011 +0000
@@ -0,0 +1,127 @@
+/*
+ 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.
+*/
+
+#include "../SerialBuffered.h"
+#include "../sb_globals.h"
+
+void SerialBuffered::format(int bits, int parity, int stopbits) {
+ SET_REGISTER(SERIALBUFFERED_LCR, (bits | parity | stopbits) & 0x7F);
+}
+
+void SerialBuffered::baud(int baud) {
+ uint16_t rate = calculate_baud(baud, uart_number);
+ SET_REGISTER(SERIALBUFFERED_LCR, GET_REGISTER(SERIALBUFFERED_LCR) | 0x80);
+ SET_REGISTER(SERIALBUFFERED_DML, (rate >> 8) & 0xFF);
+ SET_REGISTER(SERIALBUFFERED_DLL, (rate &0xFF));
+ SET_REGISTER(SERIALBUFFERED_LCR, GET_REGISTER(SERIALBUFFERED_LCR) & 0x7F);
+}
+
+uint16_t SerialBuffered::calculate_baud(int baud, int uart) {
+ static int multipliers[4] = { 4, 1, 2, 8 };
+ int clock = 0;
+
+ switch (uart) {
+ case 0: clock = (LPC_SC->PCLKSEL0 >> 6) & 0x3; break;
+ case 1: clock = (LPC_SC->PCLKSEL0 >> 8) & 0x3; break;
+ case 2: clock = (LPC_SC->PCLKSEL1 >> 16) & 0x3; break;
+ case 3: clock = (LPC_SC->PCLKSEL1 >> 18) & 0x3; break;
+ }
+
+ return (uint16_t)(((SystemCoreClock / 16 / baud) * multipliers[clock]) & 0xFFFF);
+}
+
+void SerialBuffered::set_tx_buffer_size(int buffer_size, char *buffer) {
+ SET_REGISTER(SERIALBUFFERED_IER, GET_REGISTER(SERIALBUFFERED_IER) & (1UL << 2));
+ SET_REGISTER(SERIALBUFFERED_FCR, (1UL << 2) | 1);
+ _tx_buffer_size[uart_number] = buffer_size;
+ if (_tx_buffer_used_malloc[uart_number] && _tx_buffer[uart_number]) {
+ free(_tx_buffer[uart_number]);
+ _tx_buffer_used_malloc[uart_number] = false;
+ }
+ if (buffer == (char *)NULL) {
+ _tx_buffer[uart_number] = (char *)malloc(buffer_size);
+ _tx_buffer_used_malloc[uart_number] = true;
+ }
+ else {
+ _tx_buffer[uart_number] = buffer;
+ _tx_buffer_used_malloc[uart_number] = false;
+ }
+ _tx_buffer_in[uart_number] = _tx_buffer_out[uart_number] = 0;
+ _tx_buffer_full[uart_number] = false;
+ SET_REGISTER(SERIALBUFFERED_IER, GET_REGISTER(SERIALBUFFERED_IER) | 0x2);
+}
+
+void SerialBuffered::set_tx_buffer_size(int buffer_size) {
+ set_tx_buffer_size(buffer_size, (char *)NULL);
+}
+
+/**
+ */
+void SerialBuffered::set_rx_buffer_size(int buffer_size, char *buffer) {
+ SET_REGISTER(SERIALBUFFERED_IER, GET_REGISTER(SERIALBUFFERED_IER) & (1UL << 1));
+ SET_REGISTER(SERIALBUFFERED_FCR, (1UL << 1) | 1);
+ _rx_buffer_size[uart_number] = buffer_size;
+ if (_rx_buffer_used_malloc[uart_number] && _rx_buffer[uart_number]) {
+ free(_rx_buffer[uart_number]);
+ _rx_buffer_used_malloc[uart_number] = false;
+ }
+ if (buffer == (char *)NULL) {
+ _rx_buffer[uart_number] = (char *)malloc(buffer_size);
+ _rx_buffer_used_malloc[uart_number] = true;
+ }
+ else {
+ _rx_buffer[uart_number] = buffer;
+ _rx_buffer_used_malloc[uart_number] = false;
+ }
+ _rx_buffer_in[uart_number] = _rx_buffer_out[uart_number] = 0;
+ _rx_buffer_full[uart_number] = false;
+ SET_REGISTER(SERIALBUFFERED_IER, GET_REGISTER(SERIALBUFFERED_IER) | 0x1);
+}
+
+void SerialBuffered::set_rx_buffer_size(int buffer_size) {
+ set_rx_buffer_size(buffer_size, (char *)NULL);
+}
+
+
+void SerialBuffered::reset_uart_tx(int uart_number) {
+ SET_REGISTER(SERIALBUFFERED_IER, GET_REGISTER(SERIALBUFFERED_IER) & (1UL << 1));
+ SET_REGISTER(SERIALBUFFERED_FCR, (1UL << 2) | 1);
+ if (_tx_buffer_used_malloc[uart_number] && _tx_buffer[uart_number]) {
+ free(_tx_buffer[uart_number]);
+ _tx_buffer[uart_number] = (char *)NULL;
+ _tx_buffer_used_malloc[uart_number] = false;
+ }
+ _tx_buffer_in[uart_number] = _tx_buffer_out[uart_number] = 0;
+ _tx_buffer_full[uart_number] = false;
+}
+
+void SerialBuffered::reset_uart_rx(int uart_number) {
+ SET_REGISTER(SERIALBUFFERED_IER, GET_REGISTER(SERIALBUFFERED_IER) & (1UL << 0));
+ SET_REGISTER(SERIALBUFFERED_FCR, (1UL << 1) | 1);
+ if (_rx_buffer_used_malloc[uart_number] && _rx_buffer[uart_number]) {
+ free(_rx_buffer[uart_number]);
+ _rx_buffer[uart_number] = (char *)NULL;
+ _rx_buffer_used_malloc[uart_number] = false;
+ }
+ _rx_buffer_in[uart_number] = _rx_buffer_out[uart_number] = 0;
+ _rx_buffer_full[uart_number] = false;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Methods/sb_getc.cpp Fri Feb 18 22:28:05 2011 +0000
@@ -0,0 +1,46 @@
+/*
+ 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.
+*/
+
+#include "../SerialBuffered.h"
+#include "../sb_globals.h"
+
+int SerialBuffered::getc(bool blocking) {
+ char c;
+
+ if (blocking) while (_rx_buffer_out[uart_number] == _rx_buffer_in[uart_number] && !_rx_buffer_full[uart_number]) ; /* Blocks! */
+ else if (_rx_buffer_in[uart_number] == _rx_buffer_out[uart_number] && !_rx_buffer_full[uart_number]) return -1;
+
+ c = _rx_buffer[uart_number][_rx_buffer_out[uart_number]++];
+ if (_rx_buffer_out[uart_number] >= _rx_buffer_size[uart_number]) {
+ _rx_buffer_out[uart_number] = 0;
+ }
+ if (_rx_buffer_full[uart_number]) _rx_buffer_full[uart_number] = false;
+ return (int)c;
+}
+
+char SerialBuffered::getc(void) {
+ return (char)getc(true);
+}
+
+int SerialBuffered::readable(void) {
+ return (_rx_buffer_full[uart_number] || _rx_buffer_overflow[uart_number] || _rx_buffer_in[uart_number] != _rx_buffer_out[uart_number]) ? 1 : 0;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Methods/sb_putc.cpp Fri Feb 18 22:28:05 2011 +0000
@@ -0,0 +1,56 @@
+/*
+ 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.
+*/
+
+#include "../SerialBuffered.h"
+#include "../sb_globals.h"
+
+int SerialBuffered::putc(int c, bool blocking) {
+ if ((GET_REGISTER(SERIALBUFFERED_LSR) & 0x20) && (_tx_buffer_in[uart_number] == _tx_buffer_out[uart_number] && !_tx_buffer_full[uart_number])) {
+ SET_REGISTER(SERIALBUFFERED_THR, (uint8_t)c);
+ }
+ else {
+ if (_tx_buffer_full[uart_number]) {
+ if (blocking) while (_tx_buffer_full[uart_number]) ; /* Blocks!!! */
+ else {
+ _tx_buffer_overflow[uart_number] = true;
+ return -1;
+ }
+ }
+ _tx_buffer[uart_number][_tx_buffer_in[uart_number]++] = (char)c;
+ if (_tx_buffer_in[uart_number] >= _tx_buffer_size[uart_number]) {
+ _tx_buffer_in[uart_number] = 0;
+ }
+ if (_tx_buffer_in[uart_number] == _tx_buffer_out[uart_number] && !_tx_buffer_full[uart_number]) _tx_buffer_full[uart_number] = true;
+ SET_REGISTER(SERIALBUFFERED_IER, 0x3);
+ }
+ return 0;
+}
+
+/** putc
+ */
+int SerialBuffered::putc(int c) {
+ return putc(c, true);
+}
+
+int SerialBuffered::writeable(void) {
+ return (!_tx_buffer_full[uart_number] && !_tx_buffer_overflow[uart_number]) ? 1 : 0;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Methods/sb_uart0.cpp Fri Feb 18 22:28:05 2011 +0000
@@ -0,0 +1,63 @@
+/*
+ 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.
+*/
+
+#include "../SerialBuffered.h"
+#include "../sb_globals.h"
+
+extern "C" void SerialBuffered_ISR(unsigned long, int);
+
+extern "C" static void SerialBuffered_ISR_0(void) __irq { SerialBuffered_ISR(LPC_UART0_BASE, 0); }
+
+void SerialBuffered::set_uart0(PinName tx, PinName rx) {
+ unsigned ier = 0;
+
+ if (tx == NC && rx == NC) return; // Eh, why would one?
+
+ LPC_SC->PCONP |= (1UL << 3);
+ LPC_SC->PCLKSEL0 &= ~(3UL << 6);
+ LPC_SC->PCLKSEL0 |= (1UL << 6);
+
+ if (tx == USBTX) {
+ set_tx_buffer_size(SERIALBUFFERED_BUFFER_SIZE);
+ LPC_PINCON->PINSEL0 &= ~(1UL << 4);
+ LPC_PINCON->PINSEL0 |= (1UL << 4);
+ ier |= 1;
+ }
+ else { reset_uart_tx(0); }
+
+ if (rx == USBRX) {
+ set_rx_buffer_size(SERIALBUFFERED_BUFFER_SIZE);
+ LPC_PINCON->PINSEL0 &= ~(1UL << 6);
+ LPC_PINCON->PINSEL0 |= (1UL << 6);
+ ier |= 2;
+ }
+ else { reset_uart_rx(0); }
+
+ if (ier) {
+ baud(9600);
+ format(SerialBuffered::WordLength8, SerialBuffered::NoParity, SerialBuffered::StopBit1);
+ LPC_UART0->FCR = 0x7;
+ NVIC_SetVector(UART0_IRQn, (uint32_t)SerialBuffered_ISR_0);
+ NVIC_EnableIRQ(UART0_IRQn);
+ LPC_UART0->IER = ier;
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Methods/sb_uart1.cpp Fri Feb 18 22:28:05 2011 +0000
@@ -0,0 +1,75 @@
+/*
+ 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.
+*/
+
+#include "../SerialBuffered.h"
+#include "../sb_globals.h"
+
+extern "C" void SerialBuffered_ISR(unsigned long, int);
+
+extern "C" static void SerialBuffered_ISR_1(void) __irq { SerialBuffered_ISR(LPC_UART1_BASE, 1); }
+
+void SerialBuffered::set_uart1(PinName tx, PinName rx) {
+ unsigned ier = 0;
+
+ if (tx == NC && rx == NC) return; // Eh, why would one?
+
+ LPC_SC->PCONP |= (1UL << 4);
+ LPC_SC->PCLKSEL0 &= ~(3UL << 8);
+ LPC_SC->PCLKSEL0 |= (1UL << 8);
+
+ if (rx == p14) {
+ set_rx_buffer_size(SERIALBUFFERED_BUFFER_SIZE);
+ LPC_PINCON->PINSEL1 &= ~(3UL << 0);
+ LPC_PINCON->PINSEL1 |= (1UL << 0);
+ ier |= 1;
+ }
+ else if (rx == p25) {
+ set_rx_buffer_size(SERIALBUFFERED_BUFFER_SIZE);
+ LPC_PINCON->PINSEL4 &= ~(3UL << 2);
+ LPC_PINCON->PINSEL4 |= (2UL << 2);
+ ier |= 1;
+ }
+ else { reset_uart_rx(1); }
+
+ if (tx == p13) {
+ set_tx_buffer_size(SERIALBUFFERED_BUFFER_SIZE);
+ LPC_PINCON->PINSEL0 &= ~(3UL << 30);
+ LPC_PINCON->PINSEL0 |= (1UL << 30);
+ ier |= 2;
+ }
+ else if (tx == p26) {
+ set_tx_buffer_size(SERIALBUFFERED_BUFFER_SIZE);
+ LPC_PINCON->PINSEL4 &= ~(3UL << 0);
+ LPC_PINCON->PINSEL4 |= (2UL << 0);
+ ier |= 2;
+ }
+ else { reset_uart_tx(1); }
+
+ if (ier) {
+ baud(9600);
+ format(SerialBuffered::WordLength8, SerialBuffered::NoParity, SerialBuffered::StopBit1);
+ LPC_UART1->FCR = 0x7;
+ NVIC_SetVector(UART1_IRQn, (uint32_t)SerialBuffered_ISR_1);
+ NVIC_EnableIRQ(UART1_IRQn);
+ LPC_UART1->IER = ier;
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Methods/sb_uart2.cpp Fri Feb 18 22:28:05 2011 +0000
@@ -0,0 +1,63 @@
+/*
+ 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.
+*/
+
+#include "../SerialBuffered.h"
+#include "../sb_globals.h"
+
+extern "C" void SerialBuffered_ISR(unsigned long, int);
+
+extern "C" static void SerialBuffered_ISR_2(void) __irq { SerialBuffered_ISR(LPC_UART2_BASE, 2); }
+
+void SerialBuffered::set_uart2(PinName tx, PinName rx) {
+ volatile char c __attribute__((unused));
+ unsigned ier = 0;
+
+ if (tx == NC && rx == NC) return; // Eh, why would one?
+
+ LPC_SC->PCONP |= (1UL << 24);
+ LPC_SC->PCLKSEL1 &= ~(3UL << 16);
+ LPC_SC->PCLKSEL1 |= (1UL << 16);
+ if (rx == p27) {
+ set_rx_buffer_size(SERIALBUFFERED_BUFFER_SIZE);
+ LPC_PINCON->PINSEL0 &= ~(3UL << 22);
+ LPC_PINCON->PINSEL0 |= (1UL << 22);
+ ier |= 1;
+ }
+ else { reset_uart_rx(2); }
+ if (tx == p28) {
+ set_tx_buffer_size(SERIALBUFFERED_BUFFER_SIZE);
+ LPC_PINCON->PINSEL0 &= ~(3UL << 20);
+ LPC_PINCON->PINSEL0 |= (1UL << 20);
+ ier |= 2;
+ }
+ else { reset_uart_tx(2); }
+
+ if (ier) {
+ baud(9600);
+ format(SerialBuffered::WordLength8, SerialBuffered::NoParity, SerialBuffered::StopBit1);
+ LPC_UART2->FCR = 0x7;
+ NVIC_SetVector(UART2_IRQn, (uint32_t)SerialBuffered_ISR_2);
+ NVIC_EnableIRQ(UART2_IRQn);
+ LPC_UART2->IER = ier;
+ }
+}
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Methods/sb_uart3.cpp Fri Feb 18 22:28:05 2011 +0000
@@ -0,0 +1,63 @@
+/*
+ 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.
+*/
+
+#include "../SerialBuffered.h"
+#include "../sb_globals.h"
+
+extern "C" void SerialBuffered_ISR(unsigned long, int);
+
+extern "C" static void SerialBuffered_ISR_3(void) __irq { SerialBuffered_ISR(LPC_UART3_BASE, 3); }
+
+void SerialBuffered::set_uart3(PinName tx, PinName rx) {
+ volatile char c __attribute__((unused));
+ unsigned ier = 0;
+
+ if (tx == NC && rx == NC) return; // Eh, why would one?
+
+ LPC_SC->PCONP |= (1UL << 25);
+ LPC_SC->PCLKSEL1 &= ~(3UL << 18);
+ LPC_SC->PCLKSEL1 |= (1UL << 18);
+ if (rx == p10) {
+ set_rx_buffer_size(SERIALBUFFERED_BUFFER_SIZE);
+ LPC_PINCON->PINSEL0 &= ~(3UL << 2);
+ LPC_PINCON->PINSEL0 |= (2UL << 2);
+ ier |= 1;
+ }
+ else { reset_uart_rx(3); }
+
+ if (tx == p9) {
+ set_tx_buffer_size(SERIALBUFFERED_BUFFER_SIZE);
+ LPC_PINCON->PINSEL0 &= ~(3UL << 0);
+ LPC_PINCON->PINSEL0 |= (2UL << 0);
+ ier |= 2;
+ }
+ else { reset_uart_tx(3); }
+
+ if (ier) {
+ baud(9600);
+ format(SerialBuffered::WordLength8, SerialBuffered::NoParity, SerialBuffered::StopBit1);
+ LPC_UART3->FCR = 0x7;
+ NVIC_SetVector(UART3_IRQn, (uint32_t)SerialBuffered_ISR_3);
+ NVIC_EnableIRQ(UART3_IRQn);
+ LPC_UART3->IER = ier;
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/SDFileSystem/SDFileSystem.cpp Fri Feb 18 22:28:05 2011 +0000
@@ -0,0 +1,457 @@
+/* mbed SDFileSystem Library, for providing file access to SD cards
+ * Copyright (c) 2008-2010, sford
+ *
+ * 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.
+ */
+
+/* Introduction
+ * ------------
+ * SD and MMC cards support a number of interfaces, but common to them all
+ * is one based on SPI. This is the one I'm implmenting because it means
+ * it is much more portable even though not so performant, and we already
+ * have the mbed SPI Interface!
+ *
+ * The main reference I'm using is Chapter 7, "SPI Mode" of:
+ * http://www.sdcard.org/developers/tech/sdcard/pls/Simplified_Physical_Layer_Spec.pdf
+ *
+ * SPI Startup
+ * -----------
+ * The SD card powers up in SD mode. The SPI interface mode is selected by
+ * asserting CS low and sending the reset command (CMD0). The card will
+ * respond with a (R1) response.
+ *
+ * CMD8 is optionally sent to determine the voltage range supported, and
+ * indirectly determine whether it is a version 1.x SD/non-SD card or
+ * version 2.x. I'll just ignore this for now.
+ *
+ * ACMD41 is repeatedly issued to initialise the card, until "in idle"
+ * (bit 0) of the R1 response goes to '0', indicating it is initialised.
+ *
+ * You should also indicate whether the host supports High Capicity cards,
+ * and check whether the card is high capacity - i'll also ignore this
+ *
+ * SPI Protocol
+ * ------------
+ * The SD SPI protocol is based on transactions made up of 8-bit words, with
+ * the host starting every bus transaction by asserting the CS signal low. The
+ * card always responds to commands, data blocks and errors.
+ *
+ * The protocol supports a CRC, but by default it is off (except for the
+ * first reset CMD0, where the CRC can just be pre-calculated, and CMD8)
+ * I'll leave the CRC off I think!
+ *
+ * Standard capacity cards have variable data block sizes, whereas High
+ * Capacity cards fix the size of data block to 512 bytes. I'll therefore
+ * just always use the Standard Capacity cards with a block size of 512 bytes.
+ * This is set with CMD16.
+ *
+ * You can read and write single blocks (CMD17, CMD25) or multiple blocks
+ * (CMD18, CMD25). For simplicity, I'll just use single block accesses. When
+ * the card gets a read command, it responds with a response token, and then
+ * a data token or an error.
+ *
+ * SPI Command Format
+ * ------------------
+ * Commands are 6-bytes long, containing the command, 32-bit argument, and CRC.
+ *
+ * +---------------+------------+------------+-----------+----------+--------------+
+ * | 01 | cmd[5:0] | arg[31:24] | arg[23:16] | arg[15:8] | arg[7:0] | crc[6:0] | 1 |
+ * +---------------+------------+------------+-----------+----------+--------------+
+ *
+ * As I'm not using CRC, I can fix that byte to what is needed for CMD0 (0x95)
+ *
+ * All Application Specific commands shall be preceded with APP_CMD (CMD55).
+ *
+ * SPI Response Format
+ * -------------------
+ * The main response format (R1) is a status byte (normally zero). Key flags:
+ * idle - 1 if the card is in an idle state/initialising
+ * cmd - 1 if an illegal command code was detected
+ *
+ * +-------------------------------------------------+
+ * R1 | 0 | arg | addr | seq | crc | cmd | erase | idle |
+ * +-------------------------------------------------+
+ *
+ * R1b is the same, except it is followed by a busy signal (zeros) until
+ * the first non-zero byte when it is ready again.
+ *
+ * Data Response Token
+ * -------------------
+ * Every data block written to the card is acknowledged by a byte
+ * response token
+ *
+ * +----------------------+
+ * | xxx | 0 | status | 1 |
+ * +----------------------+
+ * 010 - OK!
+ * 101 - CRC Error
+ * 110 - Write Error
+ *
+ * Single Block Read and Write
+ * ---------------------------
+ *
+ * Block transfers have a byte header, followed by the data, followed
+ * by a 16-bit CRC. In our case, the data will always be 512 bytes.
+ *
+ * +------+---------+---------+- - - -+---------+-----------+----------+
+ * | 0xFE | data[0] | data[1] | | data[n] | crc[15:8] | crc[7:0] |
+ * +------+---------+---------+- - - -+---------+-----------+----------+
+ */
+
+#include "SDFileSystem.h"
+
+#define SD_COMMAND_TIMEOUT 5000
+
+SDFileSystem::SDFileSystem(PinName mosi, PinName miso, PinName sclk, PinName cs, const char* name) :
+ FATFileSystem(name), _spi(mosi, miso, sclk), _cs(cs) {
+ _cs = 1;
+}
+
+#define R1_IDLE_STATE (1 << 0)
+#define R1_ERASE_RESET (1 << 1)
+#define R1_ILLEGAL_COMMAND (1 << 2)
+#define R1_COM_CRC_ERROR (1 << 3)
+#define R1_ERASE_SEQUENCE_ERROR (1 << 4)
+#define R1_ADDRESS_ERROR (1 << 5)
+#define R1_PARAMETER_ERROR (1 << 6)
+
+// Types
+// - v1.x Standard Capacity
+// - v2.x Standard Capacity
+// - v2.x High Capacity
+// - Not recognised as an SD Card
+
+#define SDCARD_FAIL 0
+#define SDCARD_V1 1
+#define SDCARD_V2 2
+#define SDCARD_V2HC 3
+
+int SDFileSystem::initialise_card() {
+ // Set to 100kHz for initialisation, and clock card with cs = 1
+ _spi.frequency(100000);
+ _cs = 1;
+ for(int i=0; i<16; i++) {
+ _spi.write(0xFF);
+ }
+
+ // send CMD0, should return with all zeros except IDLE STATE set (bit 0)
+ if(_cmd(0, 0) != R1_IDLE_STATE) {
+ fprintf(stderr, "No disk, or could not put SD card in to SPI idle state\n");
+ return SDCARD_FAIL;
+ }
+
+ // send CMD8 to determine whther it is ver 2.x
+ int r = _cmd8();
+ if(r == R1_IDLE_STATE) {
+ return initialise_card_v2();
+ } else if(r == (R1_IDLE_STATE | R1_ILLEGAL_COMMAND)) {
+ return initialise_card_v1();
+ } else {
+ fprintf(stderr, "Not in idle state after sending CMD8 (not an SD card?)\n");
+ return SDCARD_FAIL;
+ }
+}
+
+int SDFileSystem::initialise_card_v1() {
+ for(int i=0; i<SD_COMMAND_TIMEOUT; i++) {
+ _cmd(55, 0);
+ if(_cmd(41, 0) == 0) {
+ return SDCARD_V1;
+ }
+ }
+
+ fprintf(stderr, "Timeout waiting for v1.x card\n");
+ return SDCARD_FAIL;
+}
+
+int SDFileSystem::initialise_card_v2() {
+
+ for(int i=0; i<SD_COMMAND_TIMEOUT; i++) {
+ _cmd(55, 0);
+ if(_cmd(41, 0) == 0) {
+ _cmd58();
+ return SDCARD_V2;
+ }
+ }
+
+ fprintf(stderr, "Timeout waiting for v2.x card\n");
+ return SDCARD_FAIL;
+}
+
+int SDFileSystem::disk_initialize() {
+
+ int i = initialise_card();
+// printf("init card = %d\n", i);
+// printf("OK\n");
+
+ _sectors = _sd_sectors();
+
+ // Set block length to 512 (CMD16)
+ if(_cmd(16, 512) != 0) {
+ fprintf(stderr, "Set 512-byte block timed out\n");
+ return 1;
+ }
+
+ _spi.frequency(1000000); // Set to 1MHz for data transfer
+ return 0;
+}
+
+int SDFileSystem::disk_write(const char *buffer, int block_number) {
+ // set write address for single block (CMD24)
+ if(_cmd(24, block_number * 512) != 0) {
+ return 1;
+ }
+
+ // send the data block
+ _write(buffer, 512);
+ return 0;
+}
+
+int SDFileSystem::disk_read(char *buffer, int block_number) {
+ // set read address for single block (CMD17)
+ if(_cmd(17, block_number * 512) != 0) {
+ return 1;
+ }
+
+ // receive the data
+ _read(buffer, 512);
+ return 0;
+}
+
+int SDFileSystem::disk_status() { return 0; }
+int SDFileSystem::disk_sync() { return 0; }
+int SDFileSystem::disk_sectors() { return _sectors; }
+
+// PRIVATE FUNCTIONS
+
+int SDFileSystem::_cmd(int cmd, int arg) {
+ _cs = 0;
+
+ // send a command
+ _spi.write(0x40 | cmd);
+ _spi.write(arg >> 24);
+ _spi.write(arg >> 16);
+ _spi.write(arg >> 8);
+ _spi.write(arg >> 0);
+ _spi.write(0x95);
+
+ // wait for the repsonse (response[7] == 0)
+ for(int i=0; i<SD_COMMAND_TIMEOUT; i++) {
+ int response = _spi.write(0xFF);
+ if(!(response & 0x80)) {
+ _cs = 1;
+ _spi.write(0xFF);
+ return response;
+ }
+ }
+ _cs = 1;
+ _spi.write(0xFF);
+ return -1; // timeout
+}
+int SDFileSystem::_cmdx(int cmd, int arg) {
+ _cs = 0;
+
+ // send a command
+ _spi.write(0x40 | cmd);
+ _spi.write(arg >> 24);
+ _spi.write(arg >> 16);
+ _spi.write(arg >> 8);
+ _spi.write(arg >> 0);
+ _spi.write(0x95);
+
+ // wait for the repsonse (response[7] == 0)
+ for(int i=0; i<SD_COMMAND_TIMEOUT; i++) {
+ int response = _spi.write(0xFF);
+ if(!(response & 0x80)) {
+ return response;
+ }
+ }
+ _cs = 1;
+ _spi.write(0xFF);
+ return -1; // timeout
+}
+
+
+int SDFileSystem::_cmd58() {
+ _cs = 0;
+ int arg = 0;
+
+ // send a command
+ _spi.write(0x40 | 58);
+ _spi.write(arg >> 24);
+ _spi.write(arg >> 16);
+ _spi.write(arg >> 8);
+ _spi.write(arg >> 0);
+ _spi.write(0x95);
+
+ // wait for the repsonse (response[7] == 0)
+ for(int i=0; i<SD_COMMAND_TIMEOUT; i++) {
+ int response = _spi.write(0xFF);
+ if(!(response & 0x80)) {
+ int ocr = _spi.write(0xFF) << 24;
+ ocr |= _spi.write(0xFF) << 16;
+ ocr |= _spi.write(0xFF) << 8;
+ ocr |= _spi.write(0xFF) << 0;
+// printf("OCR = 0x%08X\n", ocr);
+ _cs = 1;
+ _spi.write(0xFF);
+ return response;
+ }
+ }
+ _cs = 1;
+ _spi.write(0xFF);
+ return -1; // timeout
+}
+
+int SDFileSystem::_cmd8() {
+ _cs = 0;
+
+ // send a command
+ _spi.write(0x40 | 8); // CMD8
+ _spi.write(0x00); // reserved
+ _spi.write(0x00); // reserved
+ _spi.write(0x01); // 3.3v
+ _spi.write(0xAA); // check pattern
+ _spi.write(0x87); // crc
+
+ // wait for the repsonse (response[7] == 0)
+ for(int i=0; i<SD_COMMAND_TIMEOUT * 1000; i++) {
+ char response[5];
+ response[0] = _spi.write(0xFF);
+ if(!(response[0] & 0x80)) {
+ for(int j=1; j<5; j++) {
+ response[i] = _spi.write(0xFF);
+ }
+ _cs = 1;
+ _spi.write(0xFF);
+ return response[0];
+ }
+ }
+ _cs = 1;
+ _spi.write(0xFF);
+ return -1; // timeout
+}
+
+int SDFileSystem::_read(char *buffer, int length) {
+ _cs = 0;
+
+ // read until start byte (0xFF)
+ while(_spi.write(0xFF) != 0xFE);
+
+ // read data
+ for(int i=0; i<length; i++) {
+ buffer[i] = _spi.write(0xFF);
+ }
+ _spi.write(0xFF); // checksum
+ _spi.write(0xFF);
+
+ _cs = 1;
+ _spi.write(0xFF);
+ return 0;
+}
+
+int SDFileSystem::_write(const char *buffer, int length) {
+ _cs = 0;
+
+ // indicate start of block
+ _spi.write(0xFE);
+
+ // write the data
+ for(int i=0; i<length; i++) {
+ _spi.write(buffer[i]);
+ }
+
+ // write the checksum
+ _spi.write(0xFF);
+ _spi.write(0xFF);
+
+ // check the repsonse token
+ if((_spi.write(0xFF) & 0x1F) != 0x05) {
+ _cs = 1;
+ _spi.write(0xFF);
+ return 1;
+ }
+
+ // wait for write to finish
+ while(_spi.write(0xFF) == 0);
+
+ _cs = 1;
+ _spi.write(0xFF);
+ return 0;
+}
+
+static int ext_bits(char *data, int msb, int lsb) {
+ int bits = 0;
+ int size = 1 + msb - lsb;
+ for(int i=0; i<size; i++) {
+ int position = lsb + i;
+ int byte = 15 - (position >> 3);
+ int bit = position & 0x7;
+ int value = (data[byte] >> bit) & 1;
+ bits |= value << i;
+ }
+ return bits;
+}
+
+int SDFileSystem::_sd_sectors() {
+
+ // CMD9, Response R2 (R1 byte + 16-byte block read)
+ if(_cmdx(9, 0) != 0) {
+ fprintf(stderr, "Didn't get a response from the disk\n");
+ return 0;
+ }
+
+ char csd[16];
+ if(_read(csd, 16) != 0) {
+ fprintf(stderr, "Couldn't read csd response from disk\n");
+ return 0;
+ }
+
+ // csd_structure : csd[127:126]
+ // c_size : csd[73:62]
+ // c_size_mult : csd[49:47]
+ // read_bl_len : csd[83:80] - the *maximum* read block length
+
+ int csd_structure = ext_bits(csd, 127, 126);
+ int c_size = ext_bits(csd, 73, 62);
+ int c_size_mult = ext_bits(csd, 49, 47);
+ int read_bl_len = ext_bits(csd, 83, 80);
+
+// printf("CSD_STRUCT = %d\n", csd_structure);
+
+ if(csd_structure != 0) {
+ fprintf(stderr, "This disk tastes funny! I only know about type 0 CSD structures\n");
+ return 0;
+ }
+
+ // memory capacity = BLOCKNR * BLOCK_LEN
+ // where
+ // BLOCKNR = (C_SIZE+1) * MULT
+ // MULT = 2^(C_SIZE_MULT+2) (C_SIZE_MULT < 8)
+ // BLOCK_LEN = 2^READ_BL_LEN, (READ_BL_LEN < 12)
+
+ int block_len = 1 << read_bl_len;
+ int mult = 1 << (c_size_mult + 2);
+ int blocknr = (c_size + 1) * mult;
+ int capacity = blocknr * block_len;
+
+ int blocks = capacity / 512;
+
+ return blocks;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/SDFileSystem/SDFileSystem.h Fri Feb 18 22:28:05 2011 +0000
@@ -0,0 +1,83 @@
+/* mbed SDFileSystem Library, for providing file access to SD cards
+ * Copyright (c) 2008-2010, sford
+ *
+ * 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 MBED_SDFILESYSTEM_H
+#define MBED_SDFILESYSTEM_H
+
+#include "mbed.h"
+#include "FATFileSystem.h"
+
+/** Access the filesystem on an SD Card using SPI
+ *
+ * @code
+ * #include "mbed.h"
+ * #include "SDFileSystem.h"
+ *
+ * SDFileSystem sd(p5, p6, p7, p12, "sd"); // mosi, miso, sclk, cs
+ *
+ * int main() {
+ * FILE *fp = fopen("/sd/myfile.txt", "w");
+ * fprintf(fp, "Hello World!\n");
+ * fclose(fp);
+ * }
+ */
+class SDFileSystem : public FATFileSystem {
+public:
+
+ /** Create the File System for accessing an SD Card using SPI
+ *
+ * @param mosi SPI mosi pin connected to SD Card
+ * @param miso SPI miso pin conencted to SD Card
+ * @param sclk SPI sclk pin connected to SD Card
+ * @param cs DigitalOut pin used as SD Card chip select
+ * @param name The name used to access the virtual filesystem
+ */
+ SDFileSystem(PinName mosi, PinName miso, PinName sclk, PinName cs, const char* name);
+ virtual int disk_initialize();
+ virtual int disk_write(const char *buffer, int block_number);
+ virtual int disk_read(char *buffer, int block_number);
+ virtual int disk_status();
+ virtual int disk_sync();
+ virtual int disk_sectors();
+
+ int initialise_card(); // unprotect to use to check for disk presence - gke
+
+protected:
+
+ int _cmd(int cmd, int arg);
+ int _cmdx(int cmd, int arg);
+ int _cmd8();
+ int _cmd58();
+ // int initialise_card();
+ int initialise_card_v1();
+ int initialise_card_v2();
+
+ int _read(char *buffer, int length);
+ int _write(const char *buffer, int length);
+ int _sd_sectors();
+ int _sectors;
+
+ SPI _spi;
+ DigitalOut _cs;
+};
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/SerialBuffered.cpp Fri Feb 18 22:28:05 2011 +0000
@@ -0,0 +1,123 @@
+/*
+ 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.
+*/
+
+
+#include "mbed.h"
+
+#ifndef SERIALBUFFERED_C
+#define SERIALBUFFERED_C
+#endif
+
+#include "SerialBuffered.h"
+#include "sb_globals.h"
+
+enum { None, One, Two };
+enum { WordLength5, WordLength6, WordLength7, WordLength8 };
+enum { NoParity, OddParity, EvenParity, Forced1, Forced0 };
+enum { StopBit1, StopBit2 };
+
+SerialBuffered::SerialBuffered(PinName tx, PinName rx) : Serial(tx, rx) {
+
+ // Depending upon the pins used, set-up the Uart.
+ if (tx == p9 || rx == p10) {
+ uart_number = 3;
+ uart_base = LPC_UART3_BASE;
+ set_uart3(tx, rx);
+ }
+ else if (tx == p13 || tx == p26 || rx == p14 || rx == p25) {
+ uart_number = 1;
+ uart_base = LPC_UART1_BASE;
+ set_uart1(tx, rx);
+ }
+ else if (tx == p28 || rx == p27) {
+ uart_number = 2;
+ uart_base = LPC_UART2_BASE;
+ set_uart2(tx, rx);
+ }
+ else if (tx == USBTX || rx == USBRX) {
+ uart_number = 0;
+ uart_base = LPC_UART0_BASE;
+ set_uart0(tx, rx);
+ }
+ else {
+ uart_number = -1;
+ return;
+ }
+}
+
+SerialBuffered::~SerialBuffered() {
+ if (_tx_buffer_used_malloc[uart_number] && _tx_buffer[uart_number]) free(_tx_buffer[uart_number]);
+ if (_rx_buffer_used_malloc[uart_number] && _rx_buffer[uart_number]) free(_rx_buffer[uart_number]);
+}
+
+/** SerialBuffered_ISR
+ *
+ * The main Uart interrupt handler.
+ */
+extern "C" void SerialBuffered_ISR(unsigned long uart_base, int uart_number) {
+ char c __attribute__((unused));
+ volatile uint32_t iir, lsr;
+
+ iir = GET_REGISTER(SERIALBUFFERED_IIR);
+
+ if (iir & 1) {
+ return; /* Eh, wtf? */
+ }
+
+ iir = (iir >> 1) & 0x3;
+
+ if (iir == 2) {
+ while(GET_REGISTER(SERIALBUFFERED_LSR) & 0x1) {
+ if (_rx_buffer_in[uart_number] == _rx_buffer_out[uart_number] && _rx_buffer_full[uart_number]) {
+ c = GET_REGISTER(SERIALBUFFERED_RBR); /* Oh dear, we need a bigger buffer!, send to dev/null */
+ _rx_buffer_overflow[uart_number] = true;
+ }
+ else {
+ if (_rx_buffer[uart_number]) { /* Ensure buffer pointer is not null before use. */
+ c = GET_REGISTER(SERIALBUFFERED_RBR);
+ _rx_buffer[uart_number][_rx_buffer_in[uart_number]++] = c;
+ if (_rx_buffer_in[uart_number] >= _rx_buffer_size[uart_number]) _rx_buffer_in[uart_number] = 0;
+ if (_rx_buffer_in[uart_number] == _rx_buffer_out[uart_number]) _rx_buffer_full[uart_number] = true;
+ }
+ }
+ }
+ }
+
+ if (iir == 1) {
+ if (_tx_buffer[uart_number]) { /* Ensure buffer pointer is not null before use. */
+ if (_tx_buffer_in[uart_number] != _tx_buffer_out[uart_number] || _tx_buffer_full[uart_number]) {
+ SET_REGISTER(SERIALBUFFERED_THR, (uint32_t)(_tx_buffer[uart_number][_tx_buffer_out[uart_number]++]));
+ if (_tx_buffer_out[uart_number] >= _tx_buffer_size[uart_number]) _tx_buffer_out[uart_number] = 0;
+ _tx_buffer_full[uart_number] = false;
+ }
+ else {
+ SET_REGISTER(SERIALBUFFERED_IER, 1);
+ }
+ }
+ }
+
+ if (iir == 3) {
+ /* We need to provide an error handling system. For now, just dismiss the IRQ. */
+ lsr = GET_REGISTER(SERIALBUFFERED_LSR);
+ }
+}
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/SerialBuffered.h Fri Feb 18 22:28:05 2011 +0000
@@ -0,0 +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
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/UAVXArm.c Fri Feb 18 22:28:05 2011 +0000
@@ -0,0 +1,192 @@
+// ===============================================================================================
+// = 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 =
+// ===============================================================================================
+
+// 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/
+
+#include "UAVXArm.h"
+
+volatile Flags F;
+
+uint32 T1;
+
+boolean OK;
+uint8 ch;
+
+int main(void) {
+
+ InitMisc();
+ InitHarness();
+
+ InitRC();
+ InitTimersAndInterrupts();
+ GreenLED = 1;
+ InitLEDs();
+
+ InitParameters();
+ ReadStatsPX();
+ InitMotors();
+ InitBattery();
+
+ LEDYellow_ON;
+ InitAccelerometers();
+ InitGyros();
+ InitIRSensors();
+ InitCompass();
+ InitRangefinder();
+
+ InitGPS();
+ InitNavigation();
+
+ InitTemperature();
+ InitBarometer();
+
+ ShowSetup(true);
+
+ I2C0.frequency(MinI2CRate);
+
+ FirstPass = true;
+
+ while ( true ) {
+ StopMotors();
+
+ LightsAndSirens(); // Check for Rx signal, disarmed on power up, throttle closed, gyros ONLINE
+
+ State = Starting;
+ F.FirstArmed = false;
+
+ while ( Armed.read() ) { // no command processing while the Quadrocopter is armed
+
+ UpdateGPS();
+ if ( F.RCNewValues )
+ UpdateControls();
+
+ if ( ( F.Signal ) && ( FailState == MonitoringRx ) ) {
+ switch ( State ) {
+ case Starting: // this state executed once only after arming
+
+ LEDYellow_OFF;
+
+ CreateLogfile();
+
+ if ( !F.FirstArmed ) {
+ mS[StartTime] = mSClock();
+ F.FirstArmed = true;
+ }
+
+ InitControl();
+ CaptureTrims();
+ InitGPS();
+ InitNavigation();
+
+ DesiredThrottle = 0;
+ ErectGyros(); // DO NOT MOVE AIRCRAFT!
+ ZeroStats();
+ DoStartingBeeps(3);
+
+ SendParamPacket(0);
+ SendParamPacket(1);
+
+ mS[ArmedTimeout] = mSClock() + ARMED_TIMEOUT_MS;
+ mS[RxFailsafeTimeout] = mSClock() + RC_NO_CHANGE_TIMEOUT_MS;
+ F.ForceFailsafe = F.LostModel = false;
+
+ State = Landed;
+ break;
+ case Landed:
+ DesiredThrottle = 0;
+ if ( mSClock() > mS[ArmedTimeout] )
+ DoShutdown();
+ else
+ if ( StickThrottle < IdleThrottle ) {
+ SetGPSOrigin();
+ GetHeading();
+ if ( F.NewCommands )
+ F.LostModel = F.ForceFailsafe;
+ } else {
+#ifdef SIMULATE
+ FakeBaroRelAltitude = 0;
+#endif // SIMULATE
+ LEDPattern = 0;
+ mS[NavActiveTime] = mSClock() + NAV_ACTIVE_DELAY_MS;
+ Stats[RCGlitchesS] = RCGlitches; // start of flight
+ SaveLEDs();
+ if ( ParameterSanityCheck() )
+ State = InFlight;
+ else
+ ALL_LEDS_ON;
+ }
+ break;
+ case Landing:
+ if ( StickThrottle > IdleThrottle ) {
+ DesiredThrottle = 0;
+ State = InFlight;
+ } else
+ if ( mSClock() < mS[ThrottleIdleTimeout] )
+ DesiredThrottle = IdleThrottle;
+ else {
+ DesiredThrottle = 0; // to catch cycles between Rx updates
+ F.MotorsArmed = false;
+ Stats[RCGlitchesS] = RCGlitches - Stats[RCGlitchesS];
+ WriteStatsPX();
+ WritePXImagefile();
+ mS[ArmedTimeout] = mSClock() + ARMED_TIMEOUT_MS;
+ State = Landed;
+ }
+ break;
+ case Shutdown:
+ // wait until arming switch is cycled
+ F.LostModel = true;
+ DesiredRoll = DesiredPitch = DesiredYaw = DesiredThrottle = 0;
+ StopMotors();
+ break;
+ case InFlight:
+ F.MotorsArmed = true;
+ DoNavigation();
+ LEDChaser();
+
+ DesiredThrottle = SlewLimit(DesiredThrottle, StickThrottle, 1);
+
+ if ( StickThrottle < IdleThrottle ) {
+ mS[ThrottleIdleTimeout] = mSClock() + THROTTLE_LOW_DELAY_MS;
+ RestoreLEDs();
+ State = Landing;
+ }
+ break;
+ } // Switch State
+ mS[FailsafeTimeout] = mSClock() + FAILSAFE_TIMEOUT_MS;
+ FailState = MonitoringRx;
+ } else
+ DoPPMFailsafe();
+
+ DoControl();
+
+ MixAndLimitMotors();
+ MixAndLimitCam();
+ OutSignals();
+// zzz??? need to do some minor scheduling here with housekeeping
+ GetTemperature();
+ GetBattery();
+ CheckAlarms();
+ CheckTelemetry();
+
+ SensorTrace();
+
+ } // flight while armed
+ }
+} // main
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/UAVXArm.h Fri Feb 18 22:28:05 2011 +0000
@@ -0,0 +1,1683 @@
+
+// Commissioning defines
+
+#define I2C_MAX_RATE_HZ 400000
+
+#define PWM_UPDATE_HZ 200 // MUST BE LESS THAN OR EAUAL TO 450HZ
+
+#define MCP4725_ID 0xc8 // or 0xcc
+
+#define SUPPRESS_SDCARD // no logging to check if buffering backup is an issue
+
+//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 DISABLE_EXTRAS
+
+#define USING_MBED
+
+// ===============================================================================================
+// = 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 =
+// ===============================================================================================
+
+// 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/
+
+#include "mbed.h"
+#include "SDFileSystem.h"
+#include "SerialBuffered.h" // used in preference to MODSERIAL
+
+// Additional Types
+typedef uint8_t uint8 ;
+typedef int8_t int8;
+typedef uint16_t uint16;
+typedef int16_t int16;
+typedef int32_t int24;
+typedef uint32_t uint24;
+typedef int32_t int32;
+typedef uint32_t uint32;
+typedef uint8_t boolean;
+typedef float real32;
+
+extern Timer timer;
+
+//________________________________________________________________________________________
+
+
+// Useful Constants
+#define NUL 0
+#define SOH 1
+#define EOT 4
+#define ACK 6
+#define HT 9
+#define LF 10
+#define CR 13
+#define NAK 21
+#define ESC 27
+#define true 1
+#define false 0
+
+#define PI 3.141592654
+#define HALFPI (PI*0.5)
+#define QUARTERPI (PI*0.25)
+#define SIXTHPI (PI/6.0)
+#define TWOPI (PI*2.0)
+#define RADDEG (180.0/PI)
+#define MILLIANGLE (180000.0/PI)
+#define DEGRAD (PI/180.0)
+
+#define MILLIRAD 18
+#define CENTIRAD 2
+
+#define MAXINT32 0x7fffffff
+#define MAXINT16 0x7fff
+
+//#define BATCHMODE
+
+#ifndef BATCHMODE
+//#define RX6CH
+//#define EXPERIMENTAL
+//#define TESTING
+//#define RX6CH // 6ch Receivers
+//#define SIMULATE
+//#define VTCOPTER
+//#define Y6COPTER
+#define QUADROCOPTER
+//#define TRICOPTER
+//#define HELICOPTER
+//#define AILERON
+// #define ELEVON
+#endif // !BATCHMODE
+
+//________________________________________________________________________________________________
+
+#define USE_PPM_FAILSAFE
+
+// Airframe
+
+#if ( defined TRICOPTER | defined QUADROCOPTER | defined VTCOPTER | defined Y6COPTER )
+#define MULTICOPTER
+#endif
+
+#if ( defined HELICOPTER | defined AILERON | defined ELEVON )
+#if ( defined AILERON | defined ELEVON )
+#define NAV_WING
+#endif
+#endif
+
+#ifdef QUADROCOPTER
+#define AF_TYPE QuadAF
+#endif
+#ifdef TRICOPTER
+#define AF_TYPE TriAF
+#endif
+#ifdef VTCOPTER
+#define AF_TYPE VTAF
+#endif
+#ifdef Y6COPTER
+#define AF_TYPE Y6AF
+#endif
+#ifdef HELICOPTER
+#define AF_TYPE HeliAF
+#endif
+#ifdef ELEVON
+#define AF_TYPE ElevAF
+#endif
+#ifdef AILERON
+#define AF_TYPE AilAF
+#endif
+
+#define GPS_INC_GROUNDSPEED // GPS groundspeed is not used for flight but may be of interest
+
+// Timeouts and Update Intervals
+
+#define FAILSAFE_TIMEOUT_MS 1000L // mS. hold last "good" settings and then restore flight or abort
+#define ABORT_TIMEOUT_GPS_MS 5000L // mS. go to descend on position hold if GPS valid.
+#define ABORT_TIMEOUT_NO_GPS_MS 0L // mS. go to descend on position hold if GPS valid.
+#define ABORT_UPDATE_MS 1000L // mS. retry period for RC Signal and restore Pilot in Control
+#define ARMED_TIMEOUT_MS 150000L // mS. automatic disarming if armed for this long and landed
+
+#define ALT_DESCENT_UPDATE_MS 1000L // mS time between throttle reduction clicks in failsafe descent without baro
+
+#define RC_STICK_MOVEMENT 5L // minimum to be recognised as a stick input change without triggering failsafe
+
+#define THROTTLE_LOW_DELAY_MS 1000L // mS. that motor runs at idle after the throttle is closed
+#define THROTTLE_UPDATE_MS 3000L // mS. constant throttle time for altitude hold
+
+#define NAV_ACTIVE_DELAY_MS 10000L // mS. after throttle exceeds idle that Nav becomes active
+#define NAV_RTH_LAND_TIMEOUT_MS 10000L // mS. Shutdown throttle if descent lasts too long
+
+#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_MIN_TEL_INTERVAL_MS 1000L // mS. emit minimum FPV telemetry packet slow rate for example to FrSky
+
+#define GPS_TIMEOUT_MS 2000L // mS.
+
+#define ALT_UPDATE_HZ 20L // Hz based on 50mS update time for Baro
+
+#ifdef MULTICOPTER
+//#define PWM_UPDATE_HZ 450L // PWM motor update rate must be <450 and >100
+#else
+#define PWM_UPDATE_HZ 40L // standard RC servo update rate
+#endif // MULTICOPTER
+
+// Accelerometers
+
+#define DAMP_HORIZ_LIMIT 3L // equivalent stick units - no larger than 5
+#define DAMP_VERT_LIMIT_LOW -5L // maximum throttle reduction
+#define DAMP_VERT_LIMIT_HIGH 20L // maximum throttle increase
+
+// Gyros
+
+#define ATTITUDE_FF_DIFF (24.0/16.0) // 0 - 32 max feedforward speeds up roll/pitch recovery on fast stick change
+
+#define ATTITUDE_ENABLE_DECAY // enables decay to zero angle when roll/pitch is not in fact zero!
+// 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
+
+// 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
+
+// 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
+#define ALT_BAND_M 5.0 // Metres
+
+#define LAND_M 3.0 // Metres deemed to have landed when below this height
+
+#define ALT_MAX_THR_COMP 40L // Stick units was 32
+
+#define ALT_INT_WINDUP_LIMIT 16L
+
+#define ALT_RF_ENABLE_M 5.0 // altitude below which the rangefiner is selected as the altitude source
+#define ALT_RF_DISABLE_M 6.0 // altitude above which the rangefiner is deselected as the altitude source
+
+// Navigation
+
+#define NAV_ACQUIRE_BEEPER
+
+//#define ATTITUDE_NO_LIMITS // full stick range is available otherwise it is scaled to Nav sensitivity
+
+#define NAV_RTH_LOCKOUT ((10.0*PI)/180.0) // first number is degrees
+
+#define NAV_MAX_ROLL_PITCH 25L // Rx stick units
+#define NAV_CONTROL_HEADROOM 10L // at least this much stick control headroom above Nav control
+#define NAV_DIFF_LIMIT 24L // Approx double NAV_INT_LIMIT
+#define NAV_INT_WINDUP_LIMIT 64L // ???
+
+#define NAV_ENFORCE_ALTITUDE_CEILING // limit all autonomous altitudes
+#define NAV_CEILING 120L // 400 feet
+#define NAV_MAX_NEUTRAL_RADIUS 3L // Metres also minimum closing radius
+#define NAV_MAX_RADIUS 99L // Metres
+
+#ifdef NAV_WING
+#define NAV_PROXIMITY_RADIUS 20.0 // Metres if there are no WPs
+#define NAV_PROXIMITY_ALTITUDE 5.0 // Metres
+#else
+#define NAV_PROXIMITY_RADIUS 5.0 // Metres if there are no WPs
+#define NAV_PROXIMITY_ALTITUDE 3.0 // Metres
+#endif // NAV_WING
+
+// reads $GPGGA sentence - all others discarded
+
+#define GPS_MIN_SATELLITES 6 // preferably > 5 for 3D fix
+#define GPS_MIN_FIX 1 // must be 1 or 2
+#define GPS_ORIGIN_SENTENCES 30L // Number of sentences needed to obtain reasonable Origin
+#define GPS_MIN_HDILUTE 130L // HDilute * 100
+
+#define NAV_SENS_THRESHOLD 40L // Navigation disabled if Ch7 is less than this
+#define NAV_SENS_ALTHOLD_THRESHOLD 20L // Altitude hold disabled if Ch7 is less than this
+#define NAV_SENS_6CH 80L // Low GPS gain for 6ch Rx
+
+#define NAV_YAW_LIMIT 10L // yaw slew rate for RTH
+#define NAV_MAX_TRIM 20L // max trim offset for altitude hold
+#define NAV_CORR_SLEW_LIMIT 1L // *5L maximum change in roll or pitch correction per GPS update
+
+#define ATTITUDE_HOLD_LIMIT 8L // dead zone for roll/pitch stick for position hold
+#define ATTITUDE_HOLD_RESET_INTERVAL 25L // number of impulse cycles before GPS position is re-acquired
+
+//#define NAV_PPM_FAILSAFE_RTH // PPM signal failure causes RTH with Signal sampled periodically
+
+// Throttle
+
+#define THROTTLE_MAX_CURRENT 40L // Amps total current at full throttle for estimated mAH
+#define CURRENT_SENSOR_MAX 50L // Amps range of current sensor - used for estimated consumption - no actual sensor yet.
+#define THROTTLE_CURRENT_SCALE ((THROTTLE_MAX_CURRENT * 1024L)/(200L * CURRENT_SENSOR_MAX ))
+
+#define THROTTLE_SLEW_LIMIT 0 // limits the rate at which the throttle can change (=0 no slew limit, 5 OK)
+#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;
+ struct {
+ uint8 b0;
+ uint8 b1;
+ };
+ struct {
+ int8 pad;
+ int8 i1;
+ };
+} i16u;
+
+typedef union {
+ int24 i24;
+ uint24 u24;
+ struct {
+ uint8 b0;
+ uint8 b1;
+ uint8 b2;
+ };
+ struct {
+ uint8 pad;
+ int16 i2_1;
+ };
+} i24u;
+
+typedef union {
+ int32 i32;
+ uint32 u32;
+ struct {
+ uint8 b0;
+ uint8 b1;
+ uint8 b2;
+ uint8 b3;
+ };
+ struct {
+ uint16 w0;
+ uint16 w1;
+ };
+ struct {
+ int16 pad;
+ int16 iw1;
+ };
+
+ struct {
+ uint8 padding;
+ int24 i3_1;
+ };
+} i32u;
+
+#define TX_BUFF_MASK 511
+#define RX_BUFF_MASK 511
+
+typedef struct { // PPM
+ uint8 Head;
+ int16 B[4][8];
+} int16x8x4Q;
+
+typedef struct { // Baro
+ uint8 Head, Tail;
+ int24 B[8];
+} int24x8Q;
+
+typedef struct { // GPS
+ uint8 Head, Tail;
+ int32 Lat[4], Lon[4];
+} int32x4Q;
+
+// Macros
+
+#define Sign(i) (((i)<0) ? -1 : 1)
+
+#define Max(i,j) ((i<j) ? j : i)
+#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 Sqr(v) ( v * v )
+
+// To speed up NMEA sentence processing
+// must have a positive argument
+#define ConvertDDegToMPi(d) (((int32)d * 3574L)>>11)
+#define ConvertMPiToDDeg(d) (((int32)d * 2048L)/3574L)
+
+#define ToPercent(n, m) (((n)*100L)/m)
+
+// Simple filters using weighted averaging
+#define VerySoftFilter(O,N) (SRS16((O)+(N)*3, 2))
+#define SoftFilter(O,N) (SRS16((O)+(N), 1))
+#define MediumFilter(O,N) (SRS16((O)*3+(N), 2))
+#define HardFilter(O,N) (SRS16((O)*7+(N), 3))
+
+// Unsigned
+#define VerySoftFilterU(O,N) (((O)+(N)*3+2)>>2)
+#define SoftFilterU(O,N) (((O)+(N)+1)>>1)
+#define MediumFilterU(O,N) (((O)*3+(N)+2)>>2)
+#define HardFilterU(O,N) (((O)*7+(N)+4)>>3)
+
+#define NoFilter(O,N) (N)
+
+#define DisableInterrupts (INTCONbits.GIEH=0)
+#define EnableInterrupts (INTCONbits.GIEH=1)
+#define InterruptsEnabled (INTCONbits.GIEH)
+
+// PARAMS
+
+#define PARAMS_ADDR_PX 0 // code assumes zero!
+#define MAX_PARAMETERS 64 // parameters in PXPROM start at zero
+
+#define STATS_ADDR_PX ( PARAMS_ADDR_PX + (MAX_PARAMETERS *2) )
+#define MAX_STATS 64
+
+// uses second Page of PXPROM
+#define NAV_ADDR_PX 256L
+// 0 - 8 not used
+
+#define NAV_NO_WP (NAV_ADDR_PX + 9)
+#define NAV_PROX_ALT (NAV_ADDR_PX + 10)
+#define NAV_PROX_RADIUS (NAV_ADDR_PX + 11)
+#define NAV_ORIGIN_ALT (NAV_ADDR_PX + 12)
+#define NAV_ORIGIN_LAT (NAV_ADDR_PX + 14)
+#define NAV_ORIGIN_LON (NAV_ADDR_PX + 18)
+#define NAV_RTH_ALT_HOLD (NAV_ADDR_PX + 22) // P[NavRTHAlt]
+#define NAV_WP_START (NAV_ADDR_PX + 24)
+
+#define WAYPOINT_REC_SIZE (4 + 4 + 2 + 1) // Lat + Lon + Alt + Loiter
+#define NAV_MAX_WAYPOINTS ((256 - 24 - 1)/WAYPOINT_REC_SIZE)
+
+//______________________________________________________________________________________________
+
+// main.c
+
+enum Directions {BF, LR, UD, Alt }; // x forward, y left and z down
+enum Angles { Pitch, Roll, Yaw }; // X, Y, Z
+
+#define FLAG_BYTES 10
+#define TELEMETRY_FLAG_BYTES 6
+typedef union {
+ uint8 AllFlags[FLAG_BYTES];
+ struct { // Order of these flags subject to change
+ uint8
+AltHoldEnabled:
+ 1,
+AllowTurnToWP:
+ 1, // stick programmed
+GyroFailure:
+ 1,
+LostModel:
+ 1,
+NearLevel:
+ 1,
+LowBatt:
+ 1,
+GPSValid:
+ 1,
+NavValid:
+ 1,
+
+BaroFailure:
+ 1,
+AccFailure:
+ 1,
+CompassFailure:
+ 1,
+GPSFailure:
+ 1,
+AttitudeHold:
+ 1,
+ThrottleMoving:
+ 1,
+HoldingAlt:
+ 1,
+Navigate:
+ 1,
+
+ReturnHome:
+ 1,
+WayPointAchieved:
+ 1,
+WayPointCentred:
+ 1,
+Unused2: // was UsingGPSAlt:
+ 1,
+UsingRTHAutoDescend:
+ 1,
+BaroAltitudeValid:
+ 1,
+RangefinderAltitudeValid:
+ 1,
+UsingRangefinderAlt:
+ 1,
+
+ // internal flags not really useful externally
+
+AllowNavAltitudeHold:
+ 1, // stick programmed
+UsingPositionHoldLock:
+ 1,
+Ch5Active:
+ 1,
+Simulation:
+ 1,
+AcquireNewPosition:
+ 1,
+MotorsArmed:
+ 1,
+NavigationActive:
+ 1,
+ForceFailsafe:
+ 1,
+
+Signal:
+ 1,
+RCFrameOK:
+ 1,
+ParametersValid:
+ 1,
+RCNewValues:
+ 1,
+NewCommands:
+ 1,
+AccelerationsValid:
+ 1,
+CompassValid:
+ 1,
+CompassMissRead:
+ 1,
+
+UsingPolarCoordinates:
+ 1,
+UsingAngleControl:
+ 1,
+GPSPacketReceived:
+ 1,
+NavComputed:
+ 1,
+AltitudeValid:
+ 1,
+UsingSerialPPM:
+ 1,
+UsingTxMode2:
+ 1,
+UsingAltOrientation:
+ 1,
+
+ // outside telemetry flags
+
+UsingTelemetry:
+ 1,
+TxToBuffer:
+ 1,
+NewBaroValue:
+ 1,
+BeeperInUse:
+ 1,
+RFInInches:
+ 1,
+FirstArmed:
+
+ 1,
+HaveGPRMC:
+ 1,
+UsingPolar:
+ 1,
+RTCValid:
+ 1,
+SDCardValid:
+ 1,
+PXImageStale:
+ 1,
+UsingLEDDriver:
+ 1,
+Using9DOF:
+ 1,
+HaveBatterySensor:
+ 1;
+ };
+} Flags;
+
+enum FlightStates { Starting, Landing, Landed, Shutdown, InFlight};
+extern volatile Flags F;
+extern int8 State;
+
+//______________________________________________________________________________________________
+
+// 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);
+extern void ReadAccelerometers(void);
+extern void GetNeutralAccelerations(void);
+extern void GetAccelerations(void);
+extern void AccelerometerTest(void);
+extern void InitAccelerometers(void);
+
+// ADXL345 3-Axis Accelerometer
+
+#ifdef SIX_DOF
+#define ADXL345_ID 0xA6
+#else
+#define ADXL345_ID 0x53
+#endif // 6DOF
+
+#define ADXL345_W ADXL345_ID
+
+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
+#define LISL_OFFSET_Z 0x18
+#define LISL_GAIN_X 0x19
+#define LISL_GAIN_Y 0x1A
+#define LISL_GAIN_Z 0x1B
+#define LISL_CTRLREG_1 0x20
+#define LISL_CTRLREG_2 0x21
+#define LISL_CTRLREG_3 0x22
+#define LISL_STATUS 0x27
+#define LISL_OUTX_L 0x28
+#define LISL_OUTX_H 0x29
+#define LISL_OUTY_L 0x2A
+#define LISL_OUTY_H 0x2B
+#define LISL_OUTZ_L 0x2C
+#define LISL_OUTZ_H 0x2D
+#define LISL_FF_CFG 0x30
+#define LISL_FF_SRC 0x31
+#define LISL_FF_ACK 0x32
+#define LISL_FF_THS_L 0x34
+#define LISL_FF_THS_H 0x35
+#define LISL_FF_DUR 0x36
+#define LISL_DD_CFG 0x38
+#define LISL_INCR_ADDR 0x40
+#define LISL_READ 0x80
+#define LISL_ID 0x3a
+
+extern void WriteLISL(uint8, uint8);
+extern void ReadLISLAcc(void);
+extern boolean LISLAccActive(void);
+
+// other accelerometers
+
+extern real32 Vel[3], Acc[3], AccNeutral[3];
+extern int16 NewAccNeutral[3];
+extern uint8 AccelerometerType;
+
+//______________________________________________________________________________________________
+
+// analog.c
+
+extern void GetBattery(void);
+extern void BatteryTest(void);
+extern void InitBattery(void);
+
+extern void GetRangefinderAltitude(void);
+extern void InitRangefinder(void);
+
+extern real32 BatteryVolts, BatteryCurrentADCEstimated, BatteryChargeUsedAH;
+extern real32 BatteryCharge, BatteryCurrent;
+extern real32 BatteryVoltsScale;
+
+extern real32 RangefinderAltitude;
+
+//______________________________________________________________________________________________
+
+// attitude.c
+
+enum AttitudeMethods { PremerlaniDCM, MadgwickIMU, MadgwickAHRS};
+
+extern void GetAttitude(void);
+extern void DoLegacyYawComp(void);
+extern void AttitudeTest(void);
+extern void InitAttitude(void);
+
+extern real32 dT, dTR;
+extern uint32 PrevDCMUpdate;
+extern uint8 AttitudeMethod;
+
+// DCM Premerlani
+
+extern void DCMNormalise(void);
+extern void DCMDriftCorrection(void);
+extern void AccAdjust(void);
+extern void DCMMotionCompensation(void);
+extern void DCMUpdate(void);
+extern void DCMEulerAngles(void);
+
+extern real32 RollPitchError[3];
+extern real32 AccV[3];
+extern real32 GyroV[3];
+extern real32 OmegaV[3];
+extern real32 OmegaP[3];
+extern real32 OmegaI[3];
+extern real32 Omega[3];
+extern real32 DCM[3][3];
+extern real32 U[3][3];
+extern real32 TempM[3][3];
+
+// 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 real32 q0, q1, q2, q3; // quaternion elements representing the estimated orientation
+
+//______________________________________________________________________________________________
+
+// autonomous.c
+
+extern void DoShutdown(void);
+extern void FailsafeHoldPosition(void);
+extern void DoPolarOrientation(void);
+extern void Navigate(int32, int32);
+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);
+extern void WriteWayPointPX(uint8, int32, int32, int16, uint8);
+extern void UAVXNavCommand(void);
+extern void GetWayPointPX(int8);
+extern void InitNavigation(void);
+
+typedef struct {
+ int32 E, N;
+ int16 A;
+ uint8 L;
+} WayPoint;
+
+enum NavStates { HoldingStation, ReturningHome, AtHome, Descending, Touchdown, Navigating, Loitering};
+enum FailStates { MonitoringRx, Aborting, Terminating, Terminated };
+
+extern real32 NavCorr[3], NavCorrp[3];
+extern real32 NavE[3], NavEp[3], NavIntE[3];
+extern int16 NavYCorrLimit;
+
+extern int8 FailState;
+extern WayPoint WP;
+extern int8 CurrWP;
+extern int8 NoOfWayPoints;
+extern int16 WPAltitude;
+extern int32 WPLatitude, WPLongitude;
+
+extern real32 WayHeading;
+extern real32 NavPolarRadius, NavNeutralRadius, NavProximityRadius, NavProximityAltitude;
+
+extern int24 NavRTHTimeoutmS;
+extern int8 NavState;
+extern int16 NavSensitivity, RollPitchMax;
+extern int16 AltSum;
+
+extern int16 EffNavSensitivity;
+extern int16 EastP, EastDiffSum, EastI, EastCorr, NorthP, NorthDiffSum, NorthI, NorthCorr;
+extern int24 EastD, EastDiffP, NorthD, NorthDiffP;
+
+//______________________________________________________________________________________________
+
+// baro.c
+
+#define BARO_INIT_RETRIES 10 // max number of initialisation retries
+
+enum BaroTypes { BaroBMP085, BaroSMD500, BaroMPX4115, BaroUnknown };
+
+#define ADS7823_TIME_MS 50 // 20Hz
+#define ADS7823_MAX 4095 // 12 bits
+#define ADS7823_ID 0x90 // ADS7823 ADC
+#define ADS7823_WR 0x90 // ADS7823 ADC
+#define ADS7823_RD 0x91 // ADS7823 ADC
+#define ADS7823_CMD 0x00
+
+#define MCP4725_MAX 4095 // 12 bits
+//#define MCP4725_ID 0xC8
+#define MCP4725_WR MCP4725_ID
+#define MCP4725_RD (MCP4725_ID+1)
+#define MCP4725_CMD 0x40 // write to DAC registor in next 2 bytes
+#define MCP4725_EPROM 0x60 // write to DAC registor and eprom
+
+extern void SetFreescaleMCP4725(int16);
+extern void SetFreescaleOffset(void);
+extern void ReadFreescaleBaro(void);
+extern real32 FreescaleToDM(int24);
+extern void GetFreescaleBaroAltitude(void);
+extern boolean IsFreescaleBaroActive(void);
+extern void InitFreescaleBarometer(void);
+
+#define BOSCH_ID_BMP085 0x55
+#define BOSCH_ID 0xee
+#define BOSCH_TEMP_BMP085 0x2e
+#define BOSCH_TEMP_SMD500 0x6e
+#define BOSCH_PRESS 0xf4
+#define BOSCH_CTL 0xf4 // OSRS=3 for BMP085 25.5mS, SMD500 34mS
+#define BOSCH_ADC_MSB 0xf6
+#define BOSCH_ADC_LSB 0xf7
+#define BOSCH_ADC_XLSB 0xf8 // BMP085
+#define BOSCH_TYPE 0xd0
+
+extern void StartBoschBaroADC(boolean);
+extern void ReadBoschBaro(void);
+extern int24 CompensatedBoschPressure(uint16, uint16);
+extern void GetBoschBaroAltitude(void);
+extern boolean IsBoschBaroActive(void);
+extern void InitBoschBarometer(void);
+
+extern void GetBaroAltitude(void);
+extern void InitBarometer(void);
+
+extern void ShowBaroType(void);
+extern void BaroTest(void);
+
+extern int32 OriginBaroPressure, CompBaroPressure;
+extern uint16 BaroPressure, BaroTemperature;
+extern boolean AcquiringPressure;
+extern real32 BaroRelAltitude, BaroRelAltitudeP;
+extern i16u BaroVal;
+extern int8 BaroType;
+extern int16 AltitudeUpdateRate;
+extern int8 BaroRetries;
+
+extern real32 FakeBaroRelAltitude;
+
+//______________________________________________________________________________________________
+
+// compass.c
+
+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 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 void ReadCompass(void);
+extern void GetHeading(void);
+extern void ShowCompassType(void);
+extern void DoCompassTest(void);
+extern void CalibrateCompass(void);
+extern void InitCompass(void);
+
+// HMC5843 Compass
+
+#define HMC5843_ID 0x3C // 0x1E 9DOF
+
+extern void ReadHMC5843(void);
+extern void GetHMC5843Parameters(void);
+extern void DoHMC5843Test(void);
+extern void CalibrateHMC5843(void);
+extern void InitHMC5843(void);
+extern boolean IsHMC5843Active(void);
+
+// HMC6352
+
+#define HMC6352_ID 0x42
+
+extern void ReadHMC6352(void);
+extern uint8 WriteByteHMC6352(uint8);
+extern void GetHMC6352Parameters(void);
+extern void DoHMC6352Test(void);
+extern void CalibrateHMC6352(void);
+extern void InitHMC6352(void);
+extern boolean HMC6352Active(void);
+
+typedef struct { real32 V; real32 Offset; } MagStruct;
+extern MagStruct Mag[3];
+extern real32 MagDeviation, CompassOffset;
+extern real32 MagHeading, Heading, FakeHeading;
+extern real32 HeadingSin, HeadingCos;
+extern uint8 CompassType;
+
+//______________________________________________________________________________________________
+
+// control.c
+
+extern void DoAltitudeHold(void);
+extern void UpdateAltitudeSource(void);
+extern void AltitudeHold(void);
+
+extern void LimitYawSum(void);
+extern void InertialDamping(void);
+extern void DoOrientationTransform(void);
+extern void DoControl(void);
+
+extern void LightsAndSirens(void);
+extern void InitControl(void);
+
+extern real32 Angle[3], Anglep[3], Rate[3], Ratep[3];
+extern real32 Comp[4];
+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 real32 ControlRoll, ControlPitch, ControlRollP, ControlPitchP;
+extern int16 CurrMaxRollPitch;
+
+extern int16 AttitudeHoldResetCount;
+extern real32 AltDiffSum, AltD, AltDSum;
+extern real32 DesiredAltitude, Altitude, AltitudeP;
+extern real32 ROC;
+extern boolean FirstPass;
+
+extern uint32 AltuSp;
+extern int16 DescentLimiter;
+
+extern int16 FakeDesiredPitch, FakeDesiredRoll, FakeDesiredYaw;
+
+//______________________________________________________________________________________________
+
+// gps.c
+
+extern void UpdateField(void);
+extern int32 ConvertGPSToM(int32);
+extern int32 ConvertMToGPS(int32);
+extern int24 ConvertInt(uint8, uint8);
+extern real32 ConvertReal(uint8, uint8);
+extern int32 ConvertLatLonM(uint8, uint8);
+extern void ConvertUTime(uint8, uint8);
+extern void ConvertUDate(uint8, uint8);
+extern void ParseGPGGASentence(void);
+extern void ParseGPRMCSentence(void);
+extern void SetGPSOrigin(void);
+extern void ParseGPSSentence(void);
+extern void RxGPSPacket(uint8);
+extern void SetGPSOrigin(void);
+extern void DoGPS(void);
+extern void GPSTest(void);
+extern void UpdateGPS(void);
+extern void InitGPS(void);
+
+#define MAXTAGINDEX 4
+#define GPSRXBUFFLENGTH 80
+typedef struct {
+ uint8 s[GPSRXBUFFLENGTH];
+ uint8 length;
+} NMEAStruct;
+
+#define MAX_NMEA_SENTENCES 2
+#define NMEA_TAG_INDEX 4
+
+enum GPSPackeType { GPGGAPacketTag, GPRMCPacketTag, GPSUnknownPacketTag };
+extern NMEAStruct NMEA;
+extern const uint8 NMEATags[MAX_NMEA_SENTENCES][5];
+
+extern uint8 GPSPacketTag;
+extern tm GPSTime;
+extern int32 GPSStartTime, GPSSeconds;
+extern int32 GPSLatitude, GPSLongitude;
+extern int32 OriginLatitude, OriginLongitude;
+extern real32 GPSAltitude, GPSRelAltitude, GPSOriginAltitude;
+extern int32 DesiredLatitude, DesiredLongitude;
+extern int32 LatitudeP, LongitudeP, HoldLatitude, HoldLongitude;
+extern real32 GPSLongitudeCorrection;
+extern real32 GPSHeading, GPSMagHeading, GPSMagDeviation, GPSVel, GPSVelp, GPSROC;
+extern int8 GPSNoOfSats;
+extern int8 GPSFix;
+extern int16 GPSHDilute;
+
+extern real32 GPSdT, GPSdTR;
+extern uint32 GPSuSp;
+
+extern int32 FakeGPSLongitude, FakeGPSLatitude;
+
+extern uint8 ll, tt, ss, RxCh;
+extern uint8 GPSCheckSumChar, GPSTxCheckSum;
+
+//______________________________________________________________________________________________
+
+// gyro.c
+
+enum GyroTypes { MLX90609Gyro, ADXRS150Gyro, IDG300Gyro, LY530Gyro, ADXRS300Gyro, ITG3200Gyro,
+ IRSensors, UnknownGyro
+ };
+
+extern void ReadGyros(void);
+extern void GetGyroRates(void);
+extern void CheckGyroFault(uint8, uint8, uint8);
+extern void ErectGyros(void);
+extern void GyroTest(void);
+extern void InitGyros(void);
+extern void ShowGyroType(void);
+
+// Generic Analog Gyrso
+extern void ReadAnalogGyros(void);
+extern void InitAnalogGyros(void);
+extern void CheckAnalogGyroFault(uint8, uint8, uint8);
+extern void AnalogGyroTest(void);
+
+// ITG3200 3 Axis Gyro
+
+#define ITG3200_WHO 0x00
+#define ITG3200_SMPL 0x15
+#define ITG3200_DLPF 0x16
+#define ITG3200_INT_C 0x17
+#define ITG3200_TMP_H 0x18
+#define ITG3200_TMP_L 0x1C
+#define ITG3200_GX_H 0x1D
+#define ITG3200_GX_L 0x1E
+#define ITG3200_GY_H 0x1F
+#define ITG3200_GY_L 0x20
+#define ITG3200_GZ_H 0x21
+#define ITG3200_GZ_L 0x22
+#define ITG3200_PWR_M 0x3E
+
+#ifdef SIX_DOF
+#define ITG3200_ID 0xD0
+#else
+#define ITG3200_ID 0xD2
+#endif // 6DOF
+
+#define ITG3200_R (ITG3200_ID+1)
+#define ITG3200_W ITG3200_ID
+
+extern void ReadITG3200Gyro(void);
+extern uint8 ReadByteITG3200(uint8);
+extern void 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 uint8 GyroType;
+
+//______________________________________________________________________________________________
+
+// harness.c
+
+
+extern void UpdateRTC(void);
+extern void InitHarness(void);
+
+extern LocalFileSystem Flash;
+
+// 1 GND
+// 2 4.5-9V
+// 3 VBat
+// 4 NReset
+
+//extern SPI SPI0; // 5 SPI MOSI, 6 SPI MOSO, 7 SPI CLK
+//extern DigitalOut SPICS; // 8
+extern SDFileSystem SDCard;
+
+//extern I2C I2C1; // 9, 10
+extern SerialBuffered TelemetrySerial;
+extern DigitalIn Armed; // 11 SPI MOSI
+extern DigitalOut PWMCamPitch; // 12 SPI MOSO // 12 SPI MOSO
+extern Serial GPSSerial; // 13 Tx1 / SPI CLK, 14 Rx1
+
+extern AnalogIn PitchADC; // 15 AN0
+extern AnalogIn RollADC; // 16 AN1
+extern AnalogIn YawADC; // 17 AN2
+
+extern AnalogIn RangefinderADC; // 18 AN3
+extern AnalogIn BatteryCurrentADC; // 19 AN4
+extern AnalogIn BatteryVoltsADC; // 20 AN5
+
+extern PwmOut Out0; // 21
+extern PwmOut Out1; // 22
+extern PwmOut Out2; // 23
+extern PwmOut Out3; // 24
+
+//extern PwmOut Out4; // 25
+//extern PwmOut Out5; // 26
+extern DigitalOut DebugPin; // 25
+
+extern I2C I2C0; // 27, 28
+
+extern DigitalIn RCIn; // 29 CAN
+extern DigitalOut PWMCamRoll; // 30 CAN
+
+//extern Serial TelemetrySerial;
+// 31 USB +, 32 USB -
+// 34 -37 Ethernet
+// 38 IF +
+// 39 IF -
+// 40 3.3V Out
+
+extern DigitalOut BlueLED;
+extern DigitalOut GreenLED;
+extern DigitalOut RedLED;
+extern DigitalOut YellowLED;
+
+extern InterruptIn RCInterrupt;
+
+extern char RTCString[], RTCLogfile[];
+extern struct tm* RTCTime;
+
+#define I2CTEMP I2C0
+#define I2CBARO I2C0
+#define I2CGYRO I2C0
+#define I2CACC I2C0
+#define I2CCOMPASS I2C0
+#define I2CESC I2C0
+#define I2CLED I2C0
+
+#define SPIACC SPI0
+
+#define mSClock timer.read_ms
+#define uSClock timer.read_us
+
+//______________________________________________________________________________________________
+
+// irq.c
+
+#define CONTROLS 7
+#define MAX_CONTROLS 12 // maximum Rx channels
+
+#define RxFilter MediumFilterU
+//#define RxFilter SoftFilterU
+//#define RxFilter NoFilter
+
+#define RC_GOOD_BUCKET_MAX 20
+#define RC_GOOD_RATIO 4
+
+#define RC_MINIMUM 0
+
+#define RC_MAXIMUM 238
+
+#define RC_NEUTRAL ((RC_MAXIMUM-RC_MINIMUM+1)/2)
+
+#define RC_MAX_ROLL_PITCH (170)
+
+#define RC_THRES_STOP ((6L*RC_MAXIMUM)/100)
+#define RC_THRES_START ((10L*RC_MAXIMUM)/100)
+
+#define RC_FRAME_TIMEOUT_MS 25
+#define RC_SIGNAL_TIMEOUT_MS (5L*RC_FRAME_TIMEOUT_MS)
+#define RC_THR_MAX RC_MAXIMUM
+
+#define MAX_ROLL_PITCH RC_NEUTRAL // Rx stick units - rely on Tx Rate/Exp
+
+#ifdef RX6CH
+#define RC_CONTROLS 5
+#else
+#define RC_CONTROLS CONTROLS
+#endif //RX6CH
+
+extern void InitTimersAndInterrupts(void);
+
+extern void enableTxIrq0(void);
+extern void disableTxIrq0(void);
+extern void enableTxIrq1(void);
+extern void disableTxIrq1(void);
+
+extern void RCISR(void);
+extern void RCNullISR(void);
+extern void RCTimeoutISR(void);
+extern void GPSInISR(void);
+extern void GPSOutISR(void);
+extern void TelemetryInISR(void);
+extern void TelemetryOutISR(void);
+extern void RazorInISR(void);
+extern void RazorOutISR(void);
+
+enum { Clock, GeneralCountdown, UpdateTimeout, RCSignalTimeout, BeeperTimeout, ThrottleIdleTimeout,
+ FailsafeTimeout, AbortTimeout, RxFailsafeTimeout, DescentUpdate, StickChangeUpdate, NavStateTimeout, LastValidRx,
+ LastGPS, StartTime, GPSTimeout, LEDChaserUpdate, LastBattery, TelemetryUpdate, NavActiveTime, BeeperUpdate,
+ ArmedTimeout, ThrottleUpdate, RazorUpdate, VerticalDampingUpdate, CamUpdate, BaroUpdate, CompassUpdate
+ };
+
+enum WaitPacketStates { WaitSentinel, WaitTag, WaitBody, WaitCheckSum};
+
+extern uint32 mS[];
+extern int16 PPM[];
+extern int8 PPM_Index;
+extern uint32 PrevEdge, CurrEdge;
+extern uint8 Intersection, PrevPattern, CurrPattern;
+extern uint32 Width;
+extern uint8 RxState;
+extern boolean WaitingForSync;
+
+extern int8 SignalCount;
+extern uint16 RCGlitches;
+
+//______________________________________________________________________________________________
+
+// ir.c
+
+extern void GetIRAttitude(void);
+extern void TrackIRMaxMin(uint8);
+extern void InitIRSensors(void);
+
+extern real32 IR[3], IRMax, IRMin, IRSwing;
+
+//______________________________________________________________________________________________
+
+// i2c.c
+
+#define I2C_ACK 1
+#define I2C_NACK 0
+
+extern void TrackMinI2CRate(uint32);
+extern void ShowI2CDeviceName(uint8);
+extern uint8 ScanI2CBus(void);
+extern boolean ESCWaitClkHi(void);
+extern void ProgramSlaveAddress(uint8);
+extern void ConfigureESCs(void);
+
+extern uint32 MinI2CRate;
+
+//______________________________________________________________________________________________
+
+// leds.c
+
+#define PCA9551_ID 0xc0
+
+#define DRV0M 0x0001
+#define DRV1M 0x0002
+#define DRV2M 0x0004
+#define DRV3M 0x0008
+
+#define AUX0M 0x0010
+#define AUX1M 0x0020
+#define AUX2M 0x0040
+#define AUX3M 0x0080
+
+#define BeeperM DRV0M
+
+#define YellowM 0x0100
+#define RedM 0x0200
+#define GreenM 0x0400
+#define BlueM 0x0800
+
+#define ALL_LEDS_ON LEDsOn(BlueM|RedM|GreenM|YellowM)
+#define ALL_LEDS_OFF LEDsOff(BlueM|RedM|GreenM|YellowM)
+
+#define AUX_LEDS_ON LEDsOn(AUX0M|AUX1M|AUX2M|AUX3M)
+#define AUX_LEDS_OFF LEDsOff(AUX0M|AUX1M|AUX2M|AUX3M)
+
+#define AUX_DRVS_ON LEDsOn(DRV0M|DRV1M|DRV2M|DRV3M)
+#define AUX_DRVS_OFF LEDsOff(DRV0M|DRV1M|DRV2M|DRV3M)
+
+#define ALL_LEDS_ARE_OFF ( (LEDShadow&(BlueM|RedM|GreenM|YellowM))== (uint8)0 )
+
+#define BEEPER_IS_ON ( (LEDShadow & BeeperM) != (uint8)0 )
+#define BEEPER_IS_OFF ( (LEDShadow & BeeperM) == (uint8)0 )
+
+#define LEDRed_ON LEDsOn(RedM)
+#define LEDBlue_ON LEDsOn(BlueM)
+#define LEDGreen_ON LEDsOn(GreenM)
+#define LEDYellow_ON LEDsOn(YellowM)
+#define LEDAUX1_ON LEDsOn(AUX1M)
+#define LEDAUX2_ON LEDsOn(AUX2M)
+#define LEDAUX3_ON LEDsOn(AUX3M)
+#define LEDRed_OFF LEDsOff(RedM)
+#define LEDBlue_OFF LEDsOff(BlueM)
+#define LEDGreen_OFF LEDsOff(GreenM)
+#define LEDYellow_OFF LEDsOff(YellowM)
+#define LEDYellow_TOG if( (LEDShadow&YellowM) == (uint8)0 ) LEDsOn(YellowM); else LEDsOff(YellowM)
+#define LEDRed_TOG if( (LEDShadow&RedM) == (uint8)0 ) LEDsOn(RedM); else LEDsOff(RedM)
+#define LEDBlue_TOG if( (LEDShadow&BlueM) == (uint8)0 ) LEDsOn(BlueM); else LEDsOff(BlueM)
+#define LEDGreen_TOG if( (LEDShadow&GreenM) == (uint8)0 ) LEDsOn(GreenM); else LEDsOff(GreenM)
+#define Beeper_OFF LEDsOff(BeeperM)
+#define Beeper_ON LEDsOn(BeeperM)
+#define Beeper_TOG if( (LEDShadow&BeeperM) == (uint8)0 ) LEDsOn(BeeperM); else LEDsOff(BeeperM)
+
+extern void SendLEDs(void);
+extern void SaveLEDs(void);
+extern void RestoreLEDs(void);
+extern void LEDsOn(uint16);
+extern void LEDsOff(uint16);
+extern void LEDChaser(void);
+extern void DoLEDs(void);
+extern void PowerOutput(int8);
+extern void LEDsAndBuzzer(void);
+
+extern void PCA9551Test(void);
+extern void WritePCA9551(uint8);
+extern boolean IsPCA9551Active(void);
+
+extern void InitLEDs(void);
+
+extern uint16 LEDShadow, PrevLEDShadow, SavedLEDs, LEDPattern;
+extern uint8 PrevPCA9551LEDShadow;
+
+//______________________________________________________________________________________________
+
+// math.c
+
+extern int16 SRS16(int16, uint8);
+extern int32 SRS32(int32, uint8);
+extern real32 Make2Pi(real32);
+extern real32 MakePi(real32);
+extern int16 Table16(int16, const int16 *);
+
+extern real32 VDot(real32 v1[3], real32 v2[3]);
+extern void VCross(real32 VOut[3], real32 v1[3], real32 v2[3]);
+extern void VScale(real32 VOut[3], real32 v[3], real32 s);
+extern void VAdd(real32 VOut[3],real32 v1[3], real32 v2[3]);
+extern void VSub(real32 VOut[3],real32 v1[3], real32 v2[3]);
+
+//______________________________________________________________________________________________
+
+// menu.c
+
+extern void ShowPrompt(void);
+extern void ShowRxSetup(void);
+extern void ShowSetup(boolean);
+extern uint8 MakeUpper(uint8);
+extern void ProcessCommand(void);
+
+extern const uint8 SerHello[];
+extern const uint8 SerSetup[];
+extern const uint8 SerPrompt[];
+
+extern const uint8 RxChMnem[];
+
+//______________________________________________________________________________________________
+
+// nonvolatiles.c
+
+extern void CheckSDCardValid(void);
+
+extern void CreateLogfile(void);
+extern void CloseLogfile(void);
+extern void TxLogChar(uint8);
+
+extern void WritePXImagefile(void);
+extern boolean ReadPXImagefile(void);
+extern int8 ReadPX(uint16);
+extern int16 Read16PX(uint16);
+extern int32 Read32PX(uint16);
+extern void WritePX(uint16, int8);
+extern void Write16PX(uint16, int16);
+extern void Write32PX(uint16, int32);
+
+extern FILE *pxfile;
+extern FILE *newpxfile;
+
+extern const int PX_LENGTH;
+extern int8 PX[], PXNew[];
+
+//______________________________________________________________________________________________
+
+// outputs.c
+
+#define OUT_MINIMUM 1.0 // Required for PPM timing loops
+#define OUT_MAXIMUM 200.0 // to reduce Rx capture and servo pulse output interaction
+#define OUT_NEUTRAL 105.0 // 1.503mS @ 105 16MHz
+#define OUT_HOLGER_MAXIMUM 225.0
+#define OUT_YGEI2C_MAXIMUM 240.0
+#define OUT_X3D_MAXIMUM 200.0
+
+extern uint8 SaturInt(int16);
+extern void DoMulticopterMix(real32 CurrThrottle);
+extern void CheckDemand(real32 CurrThrottle);
+extern void MixAndLimitMotors(void);
+extern void MixAndLimitCam(void);
+extern void OutSignals(void);
+extern void InitI2CESCs(void);
+extern void StopMotors(void);
+extern void ExercisePWM(void);
+extern void InitMotors(void);
+
+enum PWMCamTags { CamRollC = 6, CamPitchC = 7 };
+enum PWMQuadTags {FrontC=0, BackC, RightC, LeftC }; // order is important for X3D & Holger ESCs
+enum PWMConvTags {ThrottleC=0, AileronC, ElevatorC, RudderC, RTHC };
+enum PWMWingTags3 {RightElevonC=1, LeftElevonC=2};
+enum PWMVTTags {FrontLeftC=0, FrontRightC};
+enum PWMY6Tags {FrontTC=0, LeftTC, RightTC, FrontBC, LeftBC, RightBC };
+enum PWMHexTags {FrontHC=0, FrontLeftHC, FrontRightHC, BackLeftHC, BackRightHC,BackHC };
+
+//#define NoOfPWMOutputs 4
+#define NoOfI2CESCOutputs 4
+
+extern const real32 PWMScale;
+
+extern real32 PWM[8];
+extern real32 PWMSense[8];
+extern int16 ESCI2CFail[6];
+extern int16 CurrThrottle;
+extern int16 CamRollPulseWidth, CamPitchPulseWidth;
+extern int16 ESCMin, ESCMax;
+
+//______________________________________________________________________________________________
+
+// params.c
+
+extern void ReadParameters(void);
+extern void UseDefaultParameters(void);
+extern void UpdateParamSetChoice(void);
+extern boolean ParameterSanityCheck(void);
+extern void InitParameters(void);
+
+enum TxRxTypes {
+ FutabaCh3, FutabaCh2, FutabaDM8, JRPPM, JRDM9, JRDXS12,
+ DX7AR7000, DX7AR6200, FutabaCh3_6_7, DX7AR6000, GraupnerMX16s, DX6iAR6200, FutabaCh3_R617FS,
+ DX7aAR7000, ExternalDecoder, FrSkyDJT_D8R, UnknownTxRx, CustomTxRx
+};
+enum RCControls {ThrottleRC, RollRC, PitchRC, YawRC, RTHRC, CamPitchRC, NavGainRC};
+enum ESCTypes { ESCPPM, ESCHolger, ESCX3D, ESCYGEI2C };
+enum AFs { QuadAF, TriAF, VTAF, Y6AF, HeliAF, ElevAF, AilAF };
+
+enum Params { // MAX 64
+ RollKp, // 01
+ RollKi, // 02
+ RollKd, // 03
+ HorizDampKp, // 04c
+ RollIntLimit, // 05
+ PitchKp, // 06
+ PitchKi, // 07
+ PitchKd, // 08
+ AltKp, // 09c
+ PitchIntLimit, // 10
+
+ YawKp, // 11
+ YawKi, // 12
+ YawKd, // 13
+ YawLimit, // 14
+ YawIntLimit, // 15
+ ConfigBits, // 16c
+ unused17 , // 17 TimeSlots
+ LowVoltThres, // 18c
+ CamRollKp, // 19
+ PercentCruiseThr, // 20c
+
+ VertDampKp, // 21c
+ MiddleUD, // 22c
+ PercentIdleThr, // 23c
+ MiddleLR, // 24c
+ MiddleBF, // 25c
+ CamPitchKp, // 26
+ CompassKp, // 27
+ AltKi, // 28c
+ unused29 , // 29 NavRadius
+ NavKi, // 30
+
+ GSThrottle, // 31
+ Acro, // 32
+ NavRTHAlt, // 33
+ NavMagVar, // 34c
+ GyroRollPitchType, // 35
+ ESCType, // 36
+ TxRxType, // 37
+ NeutralRadius, // 38
+ PercentNavSens6Ch, // 39
+ CamRollTrim, // 40
+ NavKd, // 41
+ VertDampDecay, // 42
+ HorizDampDecay, // 43
+ BaroScale, // 44
+ TelemetryType, // 45
+ MaxDescentRateDmpS, // 46
+ DescentDelayS, // 47
+ NavIntLimit, // 48
+ AltIntLimit, // 49
+ unused50, // 50 GravComp
+ unused51 , // 51 CompSteps
+ ServoSense, // 52
+ CompassOffsetQtr, // 53
+ BatteryCapacity, // 54
+ unused55, // 55 GyroYawType
+ AltKd, // 56
+ Orient, // 57
+ NavYawLimit, // 58
+ Balance // 59
+
+ // 56 - 64 unused currently
+};
+
+//#define FlyXMode 0
+//#define FlyAltOrientationMask 0x01
+
+#define UsePositionHoldLock 0
+#define UsePositionHoldLockMask 0x01
+
+#define UseRTHDescend 1
+#define UseRTHDescendMask 0x02
+
+#define TxMode2 2
+#define TxMode2Mask 0x04
+
+#define RxSerialPPM 3
+#define RxSerialPPMMask 0x08
+
+#define RFInches 4
+#define RFInchesMask 0x10
+
+// bit 4 is pulse polarity for 3.15
+
+#define UseUseAngleControl 5
+#define UseAngleControlMask 0x20
+
+#define UsePolar 6
+#define UsePolarMask 0x40
+
+// bit 7 unusable in UAVPSet
+
+extern const int8 DefaultParams[MAX_PARAMETERS][2];
+
+extern const uint8 ESCLimits [];
+
+extern real32 OSin[], OCos[];
+extern uint8 Orientation, PolarOrientation;
+
+extern uint8 ParamSet;
+extern boolean ParametersChanged, SaveAllowTurnToWP;
+extern int8 P[];
+extern real32 K[MAX_PARAMETERS]; // Arm rescaled legacy parameters as appropriate
+
+extern uint8 UAVXAirframe;
+
+//__________________________________________________________________________________________
+
+// rc.c
+
+extern void DoRxPolarity(void);
+extern void InitRC(void);
+extern void MapRC(void);
+extern void CheckSticksHaveChanged(void);
+extern void UpdateControls(void);
+extern void CaptureTrims(void);
+extern void CheckThrottleMoved(void);
+extern void ReceiverTest(void);
+
+extern const boolean PPMPosPolarity[];
+extern const uint8 Map[CustomTxRx+1][CONTROLS];
+extern int8 RMap[];
+
+#define PPMQMASK 3
+extern int16 PPMQSum[];
+extern int16x8x4Q PPMQ;
+extern boolean RCPositiveEdge;
+extern int16 RC[], RCp[];
+extern int16 Trim[3];
+extern int16 ThrLow, ThrNeutral, ThrHigh;
+
+//__________________________________________________________________________________________
+
+// serial.c
+
+extern void TxString(const uint8*);
+extern void TxChar(uint8);
+extern void TxValU(uint8);
+extern void TxValS(int8);
+extern void TxBin8(uint8);
+extern void TxNextLine(void);
+extern void TxNibble(uint8);
+extern void TxValH( uint8);
+extern void TxValH16(uint16);
+extern uint8 RxChar(void);
+extern uint8 PollRxChar(void);
+extern uint8 RxNumU(void);
+extern int8 RxNumS(void);
+extern void TxVal32(int32, int8, uint8);
+extern void TxChar(uint8);
+extern void TxESCu8(uint8);
+extern void Sendi16(int16);
+extern void TxESCi8(int8);
+extern void TxESCi16(int16);
+extern void TxESCi24(int24);
+extern void TxESCi32(int32);
+
+//______________________________________________________________________________________________
+
+// stats.c
+
+extern void ZeroStats(void);
+extern void ReadStatsPX(void);
+extern void WriteStatsPX(void);
+extern void ShowStats(void);
+
+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
+
+extern int16 Stats[];
+
+//______________________________________________________________________________________________
+
+// telemetry.c
+
+extern void RxTelemetryPacket(uint8);
+extern void InitTelemetryPacket(void);
+extern void BuildTelemetryPacket(uint8);
+
+extern void SendPacketHeader(void);
+extern void SendPacketTrailer(void);
+
+extern void SendTelemetry(void);
+extern void SendUAVX(void);
+extern void SendUAVXControl(void);
+extern void SendFlightPacket(void);
+extern void SendNavPacket(void);
+extern void SendControlPacket(void);
+extern void SendStatsPacket(void);
+extern void SendParamPacket(uint8);
+extern void SendMinPacket(void);
+extern void SendArduStation(void);
+extern void SendCustom(void);
+extern void SensorTrace(void);
+extern void CheckTelemetry(void);
+
+enum TelemetryStates { WaitRxSentinel, WaitRxESC, WaitRxBody };
+
+
+enum PacketTags {UnknownPacketTag = 0, LevPacketTag, NavPacketTag, MicropilotPacketTag, WayPacketTag,
+ AirframePacketTag, NavUpdatePacketTag, BasicPacketTag, RestartPacketTag, TrimblePacketTag,
+ MessagePacketTag, EnvironmentPacketTag, BeaconPacketTag, UAVXFlightPacketTag,
+ UAVXNavPacketTag, UAVXStatsPacketTag, UAVXControlPacketTag, UAVXParamsPacketTag, UAVXMinPacketTag, FrSkyPacketTag
+ };
+
+enum TelemetryTypes { NoTelemetry, GPSTelemetry, UAVXTelemetry, UAVXControlTelemetry, UAVXMinTelemetry,
+ ArduStationTelemetry, CustomTelemetry
+ };
+
+extern uint8 UAVXCurrPacketTag;
+extern uint8 RxPacketLength, RxPacketByteCount;
+extern uint8 RxCheckSum;
+extern uint8 RxPacketTag, ReceivedPacketTag;
+extern uint8 PacketRxState;
+extern boolean CheckSumError, TelemetryPacketReceived;
+
+extern int16 RxLengthErrors, RxTypeErrors, RxCheckSumErrors;
+
+extern uint8 TxCheckSum;
+
+extern FILE *logfile;
+extern boolean EchoToLogFile, LogfileIsOpen;
+extern uint32 LogChars;
+
+//______________________________________________________________________________________________
+
+// temperature.c
+
+#define TMP100_MAX_ADC 4095 // 12 bits
+#define TMP100_ID 0x96
+#define TMP100_WR 0x96 // Write
+#define TMP100_RD 0x97 // Read
+#define TMP100_TMP 0x00 // Temperature
+#define TMP100_CMD 0x01
+#define TMP100_LOW 0x02 // Alarm low limit
+#define TMP100_HI 0x03 // Alarm high limit
+#define TMP100_CFG 0 // 0.5 deg resolution continuous
+
+extern void GetTemperature(void);
+extern void InitTemperature(void);
+
+extern i16u AmbientTemperature;
+
+//______________________________________________________________________________________________
+
+// utils.c
+
+extern void InitMisc(void);
+extern void Delay1mS(int16);
+extern void Delay100mS(int16);
+extern void DoBeep100mS(uint8, uint8);
+extern void DoStartingBeeps(uint8);
+extern real32 SlewLimit(real32, real32, real32);
+extern real32 DecayX(real32, real32);
+extern void LPFilter(real32*, real32*, real32, real32);
+extern void CheckAlarms(void);
+extern void Timing(uint8, uint32);
+
+typedef struct {
+ uint32 T;
+ uint32 Count;
+} TimingRec;
+
+enum Timed { GetAttitudeT , UnknownT };
+extern TimingRec Times[];
+
+#define TimeS uint32 TStart;TStart=timer.read_us();
+#define TimeF(w, tf) Time(w, tf)
+
+//______________________________________________________________________________________________
+
+
+// Sanity checks
+
+#if (( defined TRICOPTER + defined QUADROCOPTER + defined VTCOPTER + defined Y6COPTER + defined HELICOPTER + defined AILERON + defined ELEVON ) != 1)
+#error None or more than one aircraft configuration defined !
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/UAVXRevision.h Fri Feb 18 22:28:05 2011 +0000 @@ -0,0 +1,24 @@ +// =============================================================================================== +// = 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 = +// =============================================================================================== + +// 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/ + +#define Version "ARM" + + +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/accel.c Fri Feb 18 22:28:05 2011 +0000
@@ -0,0 +1,362 @@
+// ===============================================================================================
+// = 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 =
+// ===============================================================================================
+
+// 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/
+
+#include "UAVXArm.h"
+
+void ShowAccType(void);
+void ReadAccelerometers(void);
+void GetAccelerations(void);
+void GetNeutralAccelerations(void);
+void AccelerometerTest(void);
+void InitAccelerometers(void);
+
+real32 Vel[3], Acc[3], AccNeutral[3], Accp[3];
+int16 NewAccNeutral[3];
+uint8 AccelerometerType;
+
+void ShowAccType(void) {
+ switch ( AccelerometerType ) {
+ case LISLAcc:
+ TxString("LIS3L");
+ break;
+ case ADXL345Acc:
+ TxString("ADXL345");
+ break;
+ case AccUnknown:
+ TxString("unknown");
+ break;
+ default:
+ ;
+ } // switch
+} // ShowAccType
+
+void ReadAccelerometers(void) {
+ switch ( AccelerometerType ) {
+ case LISLAcc:
+ ReadLISLAcc();
+ break;
+ case ADXL345Acc:
+ ReadADXL345Acc();
+ break;
+ // other accelerometers
+ default:
+ Acc[BF] = Acc[LR] = 0.0;
+ Acc[UD] = 1.0;
+ break;
+ } // switch
+} // ReadAccelerometers
+
+void GetNeutralAccelerations(void) {
+ static uint8 i, a;
+ static real32 Temp[3] = {0.0, 0.0, 0.0};
+
+ if ( F.AccelerationsValid ) {
+ for ( i = 16; i; i--) {
+ ReadAccelerometers();
+ for ( a = 0; a <(uint8)3; a++ )
+ Temp[a] += Acc[a];
+ }
+
+ for ( a = 0; a <(uint8)3; a++ )
+ Temp[a] = Temp[a] * 0.0625;
+
+ 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);
+
+ } else
+ for ( a = 0; a <(uint8)3; a++ )
+ AccNeutral[a] = 0.0;
+
+} // GetNeutralAccelerations
+
+void GetAccelerations(void) {
+
+ static real32 AccA;
+ static uint8 a;
+
+ if ( F.AccelerationsValid ) {
+ ReadAccelerometers();
+
+ // 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];
+
+ AccA = dT / ( OneOnTwoPiAccF + dT );
+ for ( a = 0; a < (uint8)3; a++ ) {
+ Acc[a] = Accp[a] + (Acc[a] - Accp[a]) * AccA;
+ Accp[a] = Acc[a];
+ }
+
+ } else {
+ Acc[LR] = Acc[BF] = 0;
+ Acc[UD] = 1.0;
+ }
+} // GetAccelerations
+
+void AccelerometerTest(void) {
+ TxString("\r\nAccelerometer test -");
+ ShowAccType();
+ TxString("\r\nRead once - no averaging\r\n");
+
+ InitAccelerometers();
+
+ if ( F.AccelerationsValid ) {
+ ReadAccelerometers();
+
+ TxString("\tL->R: \t");
+ TxVal32( Acc[LR] * 1000.0, 3, 'G');
+ if ( fabs(Acc[LR]) > 0.2 )
+ TxString(" fault?");
+ TxNextLine();
+
+ TxString("\tB->F: \t");
+ TxVal32( Acc[BF] * 1000.0, 3, 'G');
+ if ( fabs(Acc[BF]) > 0.2 )
+ TxString(" fault?");
+ TxNextLine();
+
+
+ TxString("\tU->D: \t");
+ TxVal32( Acc[UD] * 1000.0, 3, 'G');
+ if ( fabs(Acc[UD]) > 1.2 )
+ TxString(" fault?");
+ TxNextLine();
+ }
+} // AccelerometerTest
+
+void InitAccelerometers(void) {
+ static uint8 a;
+
+ F.AccelerationsValid = true; // optimistic
+
+ for ( a = 0; a < (uint8)3; a++ ) {
+ NewAccNeutral[a] = Acc[a] = Accp[a] = Vel[a] = 0.0;
+ Comp[a] = 0;
+ }
+ Acc[2] = Accp[2] = 1.0;
+
+ if ( ADXL345AccActive() ) {
+ InitADXL345Acc();
+ AccelerometerType = ADXL345Acc;
+
+ } else
+ if ( LISLAccActive() )
+ AccelerometerType = LISLAcc;
+ else
+ // check for other accs in preferred order
+ {
+ AccelerometerType = AccUnknown;
+ F.AccelerationsValid = false;
+ }
+
+ if ( F.AccelerationsValid ) {
+ LEDYellow_ON;
+ GetNeutralAccelerations();
+ LEDYellow_OFF;
+ } else
+ F.AccFailure = true;
+} // InitAccelerometers
+
+//________________________________________________________________________________________
+
+// ADXL345 3 Axis Accelerometer
+
+void ReadADXL345Acc(void);
+void InitADXL345Acc(void);
+boolean ADXL345AccActive(void);
+
+const float GRAVITY_ADXL345 = 250.0; // ~4mG/LSB
+
+void ReadADXL345Acc(void) {
+
+ static uint8 a, r;
+ 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();
+ }
+ */
+
+ I2CACC.start();
+ r = I2CACC.write(ADXL345_ID);
+ r = I2CACC.write(0x32); // point to acc data
+ I2CACC.stop();
+
+ I2CACC.read(ADXL345_ID, b, 6);
+
+ 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 ( 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;
+ }
+
+ // LP filter here?
+
+ for ( a = 0; a < (int8)3; a++ )
+ Acc[a] /= GRAVITY_ADXL345;
+
+} // ReadADXL345Acc
+
+void InitADXL345Acc() {
+ static uint8 r;
+
+ I2CACC.start();
+ I2CACC.write(ADXL345_W);
+ r = I2CACC.write(0x2D); // power register
+ r = I2CACC.write(0x08); // measurement mode
+ I2CACC.stop();
+
+ Delay1mS(5);
+
+ I2CACC.start();
+ r = I2CACC.write(ADXL345_W);
+ r = I2CACC.write(0x31); // format
+ r = I2CACC.write(0x08); // full resolution, 2g
+ I2CACC.stop();
+
+ Delay1mS(5);
+
+ I2CACC.start();
+ r = I2CACC.write(ADXL345_W);
+ r = I2CACC.write(0x2C); // Rate
+ r = I2CACC.write(0x09); // 50Hz, 400Hz 0x0C
+ I2CACC.stop();
+
+ Delay1mS(5);
+
+} // InitADXL345Acc
+
+boolean ADXL345AccActive(void) {
+
+ I2CACC.start();
+ F.AccelerationsValid = I2CACC.write(ADXL345_ID) == I2C_ACK;
+ I2CACC.stop();
+
+ TrackMinI2CRate(400000);
+
+ return( true ); //zzz F.AccelerationsValid );
+
+} // ADXL345AccActive
+
+//________________________________________________________________________________________
+
+// LIS3LV02DG Accelerometer 400KHz
+
+void WriteLISL(uint8, uint8);
+void ReadLISLAcc(void);
+boolean LISLAccActive(void);
+
+const float GRAVITY_LISL = 1024.0;
+
+void ReadLISLAcc(void) {
+ static uint8 a;
+ static char b[6];
+ static i16u X, Y, Z;
+
+ F.AccelerationsValid = I2CACC.write(LISL_ID) == I2C_ACK; // Acc still there?
+ if ( F.AccelerationsValid ) {
+
+ // Ax.b0 = I2CACC.write(LISL_OUTX_L + LISL_INCR_ADDR + LISL_READ);
+
+ I2CACC.read(LISL_ID, b, 6);
+
+ X.b1 = b[1];
+ X.b0 = b[0];
+ Y.b1 = b[3];
+ Y.b0 = b[2];
+ Z.b1 = b[5];
+ Z.b0 = b[4];
+
+ Acc[BF] = Z.i16;
+ Acc[LR] = -X.i16;
+ Acc[UD] = Y.i16;
+
+ // LP Filter here?
+
+ for ( a = 0; a < (uint8)3; a++ )
+ Acc[a] /= GRAVITY_LISL;
+
+ } else {
+ for ( a = 0; a < (uint8)3; a++ )
+ Acc[a] = AccNeutral[a];
+ Acc[UD] += 1.0;
+
+ 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) {
+ I2CACC.start();
+ I2CACC.write(a);
+ I2CACC.write(d);
+ I2CACC.stop();
+} // WriteLISL
+
+boolean LISLAccActive(void) {
+ F.AccelerationsValid = false;
+ /*
+ WriteLISL(0x4a, LISL_CTRLREG_2); // enable 3-wire, BDU=1, +/-2g
+
+ 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 );
+} // LISLAccActive
+
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/analog.c Fri Feb 18 22:28:05 2011 +0000
@@ -0,0 +1,117 @@
+// ===============================================================================================
+// = 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 =
+// ===============================================================================================
+
+// 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//
+
+#include "UAVXArm.h"
+
+void GetBattery(void);
+void BatteryTest(void);
+void InitBattery(void);
+
+real32 BatteryVolts, BatteryCurrentADCEstimated, BatteryChargeUsedAH;
+real32 BatteryCharge, BatteryCurrent;
+real32 BatteryVoltsScale;
+
+void GetBattery(void) {
+ // AttoPilot Voltage and Current Sense Breakout SparkFun Part: SEN-09028
+
+ const real32 BatteryCurrentScale = 90.15296; // Amps FS
+
+ if ( F.HaveBatterySensor ) {
+ BatteryCurrent = BatteryCurrentADC.read() * BatteryCurrentScale;
+ BatteryChargeUsedAH += BatteryCurrent * (real32)(mSClock() - mS[LastBattery]) * 2.777777e-7;
+ mS[LastBattery] = mSClock();
+ } else
+ BatteryCurrent = BatteryChargeUsedAH = 0;
+
+ BatteryVolts = BatteryVoltsADC.read() * BatteryVoltsScale;
+ F.LowBatt = BatteryVolts < K[LowVoltThres];
+
+} // GetBattery
+
+void BatteryTest(void) {
+
+ TxString("\r\nBattery test\r\n");
+
+ GetBattery();
+
+ // Battery
+ TxString("Volts :\t");
+ TxVal32(BatteryVolts * 10.0, 1, 'V');
+ TxString(" Limit > ");
+ TxVal32( K[LowVoltThres] * 10.0, 1, 'V');
+
+ if ( F.HaveBatterySensor ) {
+ TxString("\r\nCurrent:\t");
+ TxVal32(BatteryCurrent * 10.0, 1, 'A');
+ TxString(" ( ");
+ TxVal32(BatteryChargeUsedAH * 1000.0, 0, 0 );
+ TxString(" mAH )\r\n");
+ } else
+ TxString("\r\nCurrent:\tnot available - no battery sensor\r\n");
+
+} // BatteryTest
+
+void InitBattery() {
+
+ F.HaveBatterySensor = true;
+ GetBattery();
+ F.HaveBatterySensor = BatteryCurrent < 2.0;
+ if ( F.HaveBatterySensor )
+ BatteryVoltsScale = 51.8144; // Volts FS
+ else
+ BatteryVoltsScale = BATTERY_VOLTS_SCALE;
+
+} // InitBattery
+
+//_____________________________________________________________________
+
+void GetRangefinderAltitude(void);
+void InitRangefinder(void);
+
+real32 RangefinderAltitude;
+
+const real32 RangefinderScale = 10.24; // Metres FS
+
+void GetRangefinderAltitude(void) {
+
+ if ( F.RangefinderAltitudeValid ) {
+ RangefinderAltitude = RangefinderADC.read() * RangefinderScale;
+ if ( F.RFInInches )
+ RangefinderAltitude *= 2.54;
+
+ if (( RangefinderAltitude < ALT_RF_ENABLE_M ) && !F.UsingRangefinderAlt)
+ F.UsingRangefinderAlt = true;
+ else
+ if (( RangefinderAltitude > ALT_RF_DISABLE_M ) && F.UsingRangefinderAlt)
+ F.UsingRangefinderAlt = false;
+ } else {
+ RangefinderAltitude = 0.0;
+ F.UsingRangefinderAlt = false;
+ }
+} // GetRangefinderAltitude
+
+void InitRangefinder(void) {
+
+ F.RangefinderAltitudeValid = true;
+ GetRangefinderAltitude();
+ F.RangefinderAltitudeValid = RangefinderAltitude < 1.0; // => supply not RF
+ GetRangefinderAltitude();
+
+} // InitRangefinder
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/attitude.c Fri Feb 18 22:28:05 2011 +0000
@@ -0,0 +1,602 @@
+// ===============================================================================================
+// = 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 =
+// ===============================================================================================
+
+// 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/
+
+#include "UAVXArm.h"
+
+//#define USE_GYRO_RATE
+
+// Reference frame is positive X forward, Y left, Z down, Roll right, Pitch up, Yaw CW.
+
+void AttitudeFailsafeEstimate(void);
+void DoLegacyYawComp(void);
+void AttitudeTest(void);
+
+real32 dT, HalfdT, dTR, dTmS;
+uint32 uSp;
+uint8 AttitudeMethod = PremerlaniDCM; //MadgwickIMU PremerlaniDCM MadgwickAHRS;
+
+void AttitudeFailsafeEstimate(void) {
+
+ static uint8 i;
+
+ for ( i = 0; i < (uint8)3; i++ ) {
+ Rate[i] = Gyro[i];
+ Angle[i] += Rate[i] * dTmS;
+ }
+} // AttitudeFailsafeEstimate
+
+void DoLegacyYawComp(void) {
+
+ static real32 Temp, HE;
+
+ if ( F.CompassValid ) {
+ // + CCW
+ Temp = DesiredYaw - Trim[Yaw];
+ if ( fabs(Temp) > COMPASS_MIDDLE ) { // acquire new heading
+ DesiredHeading = Heading;
+ HE = 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);
+ }
+ }
+
+ 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
+
+} // DoLegacyYawComp
+
+void GetAttitude(void) {
+
+ static uint32 Now;
+ static uint8 i;
+
+ if ( GyroType == IRSensors )
+ GetIRAttitude();
+ else {
+ GetGyroRates();
+ 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;
+ dTR = 1.0 / dT;
+ uSp = Now;
+
+ if ( GyroType == IRSensors ) {
+
+ for ( i = 0; i < (uint8)2; i++ ) {
+ Rate[i] = ( Angle[i] - Anglep[i] ) * dT;
+ Anglep[i] = Angle[i];
+ }
+
+ Rate[Yaw] = 0.0; // need Yaw gyro!
+
+ } 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
+
+ DebugPin = false;
+ }
+
+ F.NearLevel = Max(fabs(Angle[Roll]), fabs(Angle[Pitch])) < NAV_RTH_LOCKOUT;
+
+} // GetAttitude
+
+
+void AttitudeTest(void) {
+
+ TxString("\r\nAttitude Test\r\n");
+
+ GetAttitude();
+
+ TxString("\r\ndT \t");
+ TxVal32(dT * 1000.0, 3, 0);
+ TxString(" Sec.\r\n\r\n");
+
+ if ( GyroType == IRSensors ) {
+
+ 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 {
+
+ 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();
+ }
+
+ 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();
+ }
+
+ TxString("Compass: \t");
+ TxVal32(Make2Pi(MagHeading) * MILLIANGLE, 3, 0);
+ TxNextLine();
+
+} // AttitudeTest
+
+//____________________________________________________________________________________________
+
+// The DCM formulation used here is due to W. Premerlani and P. Bizard in a paper entitled:
+// Direction Cosine Matrix IMU: Theory, Draft 17 June 2009. This paper draws upon the original
+// work by R. Mahony et al. - Thanks Rob!
+
+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
+real32 OmegaI[3] = {0,0,0}; // integral correction
+real32 Omega[3] = {0,0,0};
+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}};
+
+void DCMNormalise(void) {
+
+ static real32 Error = 0;
+ static real32 Renorm = 0.0;
+ static boolean Problem;
+ static uint8 r;
+
+ 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
+
+ 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
+
+ Problem = false;
+ for ( r = 0; r < (uint8)3; r++ ) {
+ Renorm = VDot(&TempM[r][0],&TempM[r][0]);
+ if ( (Renorm < 1.5625) && (Renorm > 0.64) )
+ Renorm = 0.5 * (3.0 - Renorm); //eq.21
+ else
+ if ( (Renorm < 100.0) && (Renorm > 0.01) )
+ Renorm = 1.0 / sqrt( Renorm );
+ else
+ Problem = true;
+
+ VScale(&DCM[r][0], &TempM[r][0], Renorm);
+ }
+
+ if ( Problem ) { // Divergent - force to initial conditions and hope!
+ DCM[0][0] = 1.0;
+ DCM[0][1] = 0.0;
+ DCM[0][2] = 0.0;
+ DCM[1][0] = 0.0;
+ DCM[1][1] = 1.0;
+ DCM[1][2] = 0.0;
+ DCM[2][0] = 0.0;
+ DCM[2][1] = 0.0;
+ DCM[2][2] = 1.0;
+ }
+
+} // DCMNormalise
+
+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;
+} // 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]) );
+
+ // 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);
+
+ VCross(&RollPitchError[0], &Acc[0], &DCM[2][0]); //adjust the reference ground
+ VScale(&OmegaP[0], &RollPitchError[0], Kp_RollPitch * AccWeight);
+
+ VScale(&ScaledOmegaI[0], &RollPitchError[0], Ki_RollPitch * AccWeight);
+ VAdd(&OmegaI[0], &OmegaI[0], &ScaledOmegaI[0]);
+
+#ifdef USE_DCM_YAW
+ // Yaw - drift correction based on compass/magnetometer heading
+ 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 );
+ VAdd(&OmegaP[0], &OmegaP[0], &ScaledOmegaP[0]);
+
+ VScale(&ScaledOmegaI[0], &YawError[0], Ki_Yaw );
+ VAdd(&OmegaI[0], &OmegaI[0], &ScaledOmegaI[0]);
+#endif // USE_DCM_YAW
+} // DCMDriftCorrection
+
+void DCMUpdate(void) {
+
+ static uint8 i, j, k;
+ static real32 op[3];
+
+ 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[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[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];
+
+ TempM[i][j] = op[0] + op[1] + op[2];
+ }
+
+ for ( i = 0; i < (uint8)3; i++ )
+ for (j = 0; j < (uint8)3; j++ )
+ DCM[i][j] += TempM[i][j];
+
+} // DCMUpdate
+
+void DCMEulerAngles(void) {
+
+ static uint8 g;
+
+ 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]);
+
+#ifdef USE_DCM_YAW
+ Angle[Yaw] = atan2(DCM[1][0], DCM[0][0]);
+#endif // USE_DCM_YAW
+
+} // DCMEulerAngles
+
+//___________________________________________________________________________________
+
+// IMU.c
+// S.O.H. Madgwick
+// 25th September 2010
+
+// Description:
+
+// 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
+// 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
+
+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) {
+
+ static real32 rnorm;
+ static real32 vx, vy, vz;
+ static real32 ex, ey, ez;
+ static real32 aMag;
+
+//swap z and y?
+
+ // 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;
+ }
+
+ // 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;
+ eyInt += ey*MKi;
+ ezInt += ez*MKi;
+
+ // 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)*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;
+
+} // IMUupdate
+
+//_________________________________________________________________________________
+
+// AHRS.c
+// S.O.H. Madgwick
+// 25th August 2010
+
+// Description:
+
+// 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.
+
+// 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 'AHRSupdate()' every sample period and parse calibrated gyroscope ('gx', 'gy', 'gz'),
+// accelerometer ('ax', 'ay', 'ay') 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) {
+
+ static real32 rnorm;
+ 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;
+
+ // 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;
+
+ 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;
+ }
+
+ 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;
+
+ // 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;
+ eyInt += ey*MKi;
+ ezInt += ez*MKi;
+
+ // 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)*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;
+
+} // AHRSupdate
+
+void EulerAngles(void) {
+
+ static uint8 g;
+
+ 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
+ }
+
+} // EulerAngles
+
+/*
+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)
+
+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
+*/
+
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/autonomous.c Fri Feb 18 22:28:05 2011 +0000
@@ -0,0 +1,586 @@
+// ===============================================================================================
+// = 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 =
+// ===============================================================================================
+
+// 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/
+
+#include "UAVXArm.h"
+
+void DoShutdown(void);
+void DoPolarOrientation(void);
+void Navigate(int32, int32);
+void SetDesiredAltitude(int16);
+void DoFailsafeLanding(void);
+void AcquireHoldPosition(void);
+void NavGainSchedule(int16);
+void DoNavigation(void);
+void DoPPMFailsafe(void);
+void UAVXNavCommand(void);
+void GetWayPointPX(int8);
+void InitNavigation(void);
+
+real32 NavCorr[3], NavCorrp[3];
+real32 NavE[3], NavEp[3], NavIntE[3];
+int16 NavYCorrLimit;
+
+#ifdef SIMULATE
+int16 FakeDesiredRoll, FakeDesiredPitch, FakeDesiredYaw, FakeHeading;
+#endif // SIMULATE
+
+typedef union {
+ uint8 b[256];
+ struct {
+ uint8 u0;
+ int16 u1;
+ int8 u3;
+ int8 u4;
+ uint16 u5;
+ uint16 u6;
+ uint8 NoOfWPs;
+ uint8 ProximityAltitude;
+ uint8 ProximityRadius;
+ int16 OriginAltitude;
+ int32 OriginLatitude;
+ int32 OriginLongitude;
+ int16 RTHAltitude;
+ struct {
+ int32 Latitude;
+ int32 Longitude;
+ int16 Altitude;
+ int8 Loiter;
+ } WP[23];
+ };
+} UAVXNav;
+
+uint8 BufferPX[256];
+
+int8 CurrWP;
+int8 NoOfWayPoints;
+int16 WPAltitude;
+int32 WPLatitude, WPLongitude;
+int24 WPLoiter;
+
+real32 WayHeading;
+real32 NavPolarRadius, NavNeutralRadius, NavProximityRadius, NavProximityAltitude;
+int24 NavRTHTimeoutmS;
+
+int8 NavState;
+int16 NavSensitivity, RollPitchMax;
+int16 AltSum;
+
+void DoShutdown(void)
+{
+ State = Shutdown;
+ DesiredThrottle = 0;
+ StopMotors();
+} // DoShutdown
+
+void SetDesiredAltitude(int16 NewDesiredAltitude) { // Metres
+ if ( F.AllowNavAltitudeHold && F.AltHoldEnabled ) {
+ AltSum = 0;
+ DesiredAltitude = NewDesiredAltitude * 10L; // Decimetres
+ }
+} // SetDesiredAltitude
+
+void DoFailsafeLanding(void) {
+ // InTheAir micro switch RC0 Pin 11 to ground when landed
+
+ const boolean InTheAir = true;
+
+ DesiredAltitude = -20.0;
+if ( F.BaroAltitudeValid )
+ {
+ if ( Altitude > LAND_M )
+ mS[NavStateTimeout] = mSClock() + NAV_RTH_LAND_TIMEOUT_MS;
+
+ if ( !InTheAir || (( mSClock() > mS[NavStateTimeout] )
+ && ( F.ForceFailsafe || ( NavState == Touchdown ) || (FailState == Terminated))) )
+ DoShutdown();
+ else
+ DesiredThrottle = CruiseThrottle;
+ }
+ else
+ {
+ if ( mSClock() > mS[DescentUpdate] )
+ {
+ mS[DescentUpdate] = mSClock() + ALT_DESCENT_UPDATE_MS;
+ DesiredThrottle = CruiseThrottle - DescentComp;
+ if ( DesiredThrottle < IdleThrottle )
+ StopMotors();
+ else
+ if ( DescentComp < CruiseThrottle )
+ DescentComp++;
+ }
+ }
+} // DoFailsafeLanding
+
+void FailsafeHoldPosition(void) {
+ DesiredRoll = DesiredPitch = DesiredYaw = 0;
+ if ( F.GPSValid && F.CompassValid )
+ Navigate(HoldLatitude, HoldLongitude);
+} // FailsafeHoldPosition
+
+void AcquireHoldPosition(void) {
+ static int8 i;
+
+ for ( i = 0; i < (uint8)3; i++ )
+ NavCorr[i] = NavCorrp[i] = 0;
+
+ F.NavComputed = false;
+
+ HoldLatitude = GPSLatitude;
+ HoldLongitude = GPSLongitude;
+ F.WayPointAchieved = F.WayPointCentred = F.AcquireNewPosition = false;
+
+ NavState = HoldingStation;
+} // AcquireHoldPosition
+
+void DoPolarOrientation(void) {
+ static real32 EastDiff, NorthDiff, Radius;
+ static real32 DesiredRelativeHeading;
+ static int16 P;
+
+ F.UsingPolar = F.UsingPolarCoordinates && F.NavValid && ( NavState == HoldingStation );
+
+ if ( F.UsingPolar ) { // needs rethink - probably arm using RTH switch
+ EastDiff = (OriginLongitude - GPSLongitude) * GPSLongitudeCorrection;
+ NorthDiff = OriginLatitude - GPSLatitude;
+
+ Radius = Max(abs(EastDiff), abs(NorthDiff));
+ if ( Radius > NavPolarRadius ) {
+ DesiredRelativeHeading = Make2Pi(atan2((real32)EastDiff, (real32)NorthDiff) - PI - Heading );
+
+ P = ( (int24)DesiredRelativeHeading * 24L + HALFPI )/ PI + Orientation;
+
+ while ( P > 24 ) P -=24;
+ while ( P < 0 ) P +=24;
+
+ } else
+ P = 0;
+ } else
+ P = 0;
+
+ PolarOrientation = P;;
+
+} // DoPolarOrientation
+
+void Navigate(int32 NavLatitude, int32 NavLongitude ) {
+ // F.GPSValid must be true immediately prior to entry
+ // This routine does not point the quadrocopter at the destination
+ // waypoint. It simply rolls/pitches towards the destination.
+ // GPS coordinates MUST be int32 to allow sufficient range - real32 is insufficient.
+
+ static real32 RelHeadingSin, RelHeadingCos;
+ static real32 Radius;
+ static real32 AltE;
+ static real32 EastDiff, NorthDiff;
+ static real32 RelHeading;
+ static uint8 a;
+ static real32 Diff, NavP, NavI, NavD;
+
+ DoPolarOrientation();
+
+ DesiredLatitude = NavLatitude;
+ DesiredLongitude = NavLongitude;
+
+ EastDiff = (real32)(DesiredLongitude - GPSLongitude) * GPSLongitudeCorrection;
+ NorthDiff = (real32)(DesiredLatitude - GPSLatitude);
+
+ Radius = sqrt( Sqr(EastDiff) + Sqr(NorthDiff) );
+
+ F.WayPointCentred = Radius < NavNeutralRadius;
+ AltE = DesiredAltitude - Altitude;
+ F.WayPointAchieved = ( Radius < NavProximityRadius ) && ( abs(AltE) < NavProximityAltitude );
+
+ WayHeading = Make2Pi(atan2((real32)EastDiff, (real32)NorthDiff));
+ RelHeading = MakePi(WayHeading - Heading); // make +/- MilliPi
+
+ if ( ( NavSensitivity > NAV_SENS_THRESHOLD ) && !F.WayPointCentred ) {
+#ifdef NAV_WING
+
+ // no Nav for conventional aircraft - yet!
+ NavCorr[Pitch] = -5; // always moving forward
+ // 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!
+ }
+
+#else // MULTICOPTER
+
+ // revert to original simpler version from UAVP->UAVX transition
+
+ RelHeadingSin = sin(RelHeading);
+ RelHeadingCos = cos(RelHeading);
+
+ NavE[Roll] = Radius * RelHeadingSin;
+ NavE[Pitch] = -Radius * RelHeadingCos;
+
+ // Roll & Pitch
+
+ for ( a = 0; a < (uint8)2 ; a++ ) {
+ NavP = Limit(NavE[a], -NAV_MAX_ROLL_PITCH, NAV_MAX_ROLL_PITCH);
+
+ NavIntE[a] += NavE[a];
+ NavIntE[a] = Limit(NavIntE[a], -K[NavIntLimit], K[NavIntLimit]);
+ NavI = NavIntE[a] * K[NavKi] * GPSdT;
+ NavIntE[a] = Decay1(NavIntE[a]);
+
+ Diff = (NavEp[a] - NavE[a]);
+ Diff = Limit(Diff, -256, 256);
+ NavD = Diff * K[NavKd] * GPSdTR;
+ NavD = Limit(NavD, -NAV_DIFF_LIMIT, 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);
+
+ NavCorrp[a] = NavCorr[a];
+ }
+
+ // Yaw
+ if ( F.AllowTurnToWP && !F.WayPointAchieved ) {
+ RelHeading = MakePi(WayHeading - Heading); // make +/- MilliPi
+ NavCorr[Yaw] = -(RelHeading * NavYCorrLimit) / HALFPI;
+ NavCorr[Yaw] = Limit(NavCorr[Yaw], -NavYCorrLimit, NavYCorrLimit); // gently!
+ } else
+ NavCorr[Yaw] = 0;
+
+#endif // NAV_WING
+ } else {
+ // Neutral Zone - no GPS influence
+ NavCorr[Pitch] = DecayX(NavCorr[Pitch], 2);
+ NavCorr[Roll] = DecayX(NavCorr[Roll], 2);
+ NavCorr[Yaw] = 0;
+ NavIntE[Roll] = NavIntE[Pitch] = NavEp[Roll] = NavEp[Pitch] = 0;
+ }
+
+ F.NavComputed = true;
+
+} // Navigate
+
+void DoNavigation(void) {
+
+ F.NavigationActive = F.GPSValid && F.CompassValid && ( mSClock() > mS[NavActiveTime]);
+
+ if ( F.NavigationActive ) {
+
+ F.LostModel = F.ForceFailsafe = false;
+
+ if ( !F.NavComputed )
+ switch ( NavState ) { // most case last - switches in C18 are IF chains not branch tables!
+ case Touchdown:
+ Navigate(OriginLatitude, OriginLongitude);
+ DoFailsafeLanding();
+ break;
+ case Descending:
+ Navigate( OriginLatitude, OriginLongitude );
+ if ( F.ReturnHome || F.Navigate )
+#ifdef NAV_WING
+ {
+ // needs more thought - runway direction?
+ DoFailsafeLanding();
+ }
+#else
+ if ( Altitude < LAND_M ) {
+ mS[NavStateTimeout] = mSClock() + NAV_RTH_LAND_TIMEOUT_MS;
+ NavState = Touchdown;
+ } else
+ DoFailsafeLanding();
+#endif // NAV_WING
+ else
+ AcquireHoldPosition();
+ break;
+ case AtHome:
+ Navigate(OriginLatitude, OriginLongitude);
+ SetDesiredAltitude((int16)P[NavRTHAlt]);
+ if ( F.ReturnHome || F.Navigate )
+ if ( F.WayPointAchieved ) { // check still @ Home
+ if ( F.UsingRTHAutoDescend && ( mSClock() > mS[NavStateTimeout] ) )
+ NavState = Descending;
+ } else
+ NavState = ReturningHome;
+ else
+ AcquireHoldPosition();
+ break;
+ case ReturningHome:
+ Navigate(OriginLatitude, OriginLongitude);
+ SetDesiredAltitude((int16)P[NavRTHAlt]);
+ if ( F.ReturnHome || F.Navigate ) {
+ if ( F.WayPointAchieved ) {
+ mS[NavStateTimeout] = mSClock() + NavRTHTimeoutmS;
+ NavState = AtHome;
+ }
+ } else
+ AcquireHoldPosition();
+ break;
+ case Loitering:
+ Navigate(WPLatitude, WPLongitude);
+ SetDesiredAltitude(WPAltitude);
+ if ( F.Navigate ) {
+ if ( F.WayPointAchieved && (mSClock() > mS[NavStateTimeout]) )
+ if ( CurrWP == NoOfWayPoints ) {
+ CurrWP = 1;
+ NavState = ReturningHome;
+ } else {
+ GetWayPointPX(++CurrWP);
+ NavState = Navigating;
+ }
+ } else
+ AcquireHoldPosition();
+ break;
+ case Navigating:
+ Navigate(WPLatitude, WPLongitude);
+ SetDesiredAltitude(WPAltitude);
+ if ( F.Navigate ) {
+ if ( F.WayPointAchieved ) {
+ mS[NavStateTimeout] = mSClock() + WPLoiter;
+ NavState = Loitering;
+ }
+ } else
+ AcquireHoldPosition();
+ break;
+ case HoldingStation:
+ if ( F.AttitudeHold ) {
+ 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
+ }
+ } else
+ F.AcquireNewPosition = true;
+
+ Navigate(HoldLatitude, HoldLongitude);
+
+ if ( F.NavValid && F.NearLevel ) // Origin must be valid for ANY navigation!
+ if ( F.Navigate ) {
+ GetWayPointPX(CurrWP); // resume from previous WP
+ SetDesiredAltitude(WPAltitude);
+ NavState = Navigating;
+ } else
+ if ( F.ReturnHome )
+ NavState = ReturningHome;
+ break;
+ } // switch NavState
+ } else
+ if ( F.ForceFailsafe && F.NewCommands )
+ {
+ F.AltHoldEnabled = F.AllowNavAltitudeHold = true;
+ F.LostModel = true;
+ DoFailsafeLanding();
+ }
+ else // kill nav correction immediately
+ NavCorr[Pitch] = NavCorr[Roll] = NavCorr[Yaw] = 0; // zzz
+
+ F.NewCommands = false; // Navigate modifies Desired Roll, Pitch and Yaw values.
+
+} // DoNavigation
+
+void CheckFailsafeAbort(void) {
+ if ( mSClock() > mS[AbortTimeout] ) {
+ if ( F.Signal ) {
+ LEDGreen_ON;
+ mS[NavStateTimeout] = 0;
+ mS[FailsafeTimeout] = mSClock() + FAILSAFE_TIMEOUT_MS; // may be redundant?
+ NavState = HoldingStation;
+ FailState = MonitoringRx;
+ }
+ } else
+ mS[AbortTimeout] += ABORT_UPDATE_MS;
+} // CheckFailsafeAbort
+
+void DoPPMFailsafe(void) { // only relevant to PPM Rx or Quad NOT synchronising with Rx
+
+ if ( State == InFlight )
+ switch ( FailState ) { // FailStates { MonitoringRx, Aborting, Terminating, Terminated }
+ case Terminated: // Basic assumption is that aircraft is being flown over a safe area!
+ FailsafeHoldPosition();
+ DoFailsafeLanding();
+ break;
+ case Terminating:
+ FailsafeHoldPosition();
+ if ( Altitude < LAND_M ) {
+ mS[NavStateTimeout] = mSClock() + NAV_RTH_LAND_TIMEOUT_MS;
+ NavState = Touchdown;
+ FailState = Terminated;
+ }
+ DoFailsafeLanding();
+ break;
+ case Aborting:
+ FailsafeHoldPosition();
+ F.AltHoldEnabled = true;
+ SetDesiredAltitude((int16)P[NavRTHAlt]);
+ if ( mSClock() > mS[NavStateTimeout] ) {
+ F.LostModel = true;
+ LEDGreen_OFF;
+ LEDRed_ON;
+
+ mS[NavStateTimeout] = mSClock() + NAV_RTH_LAND_TIMEOUT_MS;
+ NavState = Descending;
+ FailState = Terminating;
+ } else
+ CheckFailsafeAbort();
+ break;
+ case MonitoringRx:
+ if ( mSClock() > mS[FailsafeTimeout] ) {
+ // use last "good" throttle
+ Stats[RCFailsafesS]++;
+ if ( F.GPSValid && F.CompassValid )
+ mS[NavStateTimeout] = mSClock() + ABORT_TIMEOUT_GPS_MS;
+ else
+ mS[NavStateTimeout] = mSClock() + ABORT_TIMEOUT_NO_GPS_MS;
+ mS[AbortTimeout] = mSClock() + ABORT_UPDATE_MS;
+ FailState = Aborting;
+ }
+ break;
+ } // Switch FailState
+ else
+ DesiredRoll = DesiredPitch = DesiredYaw = DesiredThrottle = 0;
+
+} // DoPPMFailsafe
+
+void UAVXNavCommand(void) {
+
+ static int16 b;
+ static uint8 c, d, csum;
+
+ c = RxChar();
+ LEDBlue_ON;
+
+ switch ( c ) {
+ case '0': // hello
+ TxChar(ACK);
+ break;
+ case '1': // write
+ csum = 0;
+ for ( b = 0; b < 256; b++) { // cannot write fast enough so buffer
+ d = RxChar();
+ csum ^= d;
+ BufferPX[b] = d;
+ }
+ if ( csum == (uint8)0 ) {
+ for ( b = 0; b < 256; b++)
+ WritePX(NAV_ADDR_PX + b, BufferPX[b]);
+ TxChar(ACK);
+ } else
+ TxChar(NAK);
+
+ InitNavigation();
+
+ break;
+ case '2':
+ csum = 0;
+ for ( b = 0; b < 255; b++) {
+ d = ReadPX(NAV_ADDR_PX + b);
+ csum ^= d;
+ BufferPX[b] = d;
+ }
+ BufferPX[255] = csum;
+ for ( b = 0; b < 256; b++)
+ TxChar(BufferPX[b]);
+ TxChar(ACK);
+ break;
+ case '3':
+ csum = 0;
+ for ( b = 0; b < 63; b++) {
+ d = ReadPX(STATS_ADDR_PX + b);
+ csum ^= d;
+ BufferPX[b] = d;
+ }
+ BufferPX[63] = csum;
+ for ( b = 0; b < 64; b++)
+ TxChar(BufferPX[b]);
+ TxChar(ACK);
+ break;
+ default:
+ break;
+ } // switch
+
+ WritePXImagefile();
+ LEDBlue_OFF;
+
+} // UAVXNavCommand
+
+void GetWayPointPX(int8 wp) {
+ static uint16 w;
+
+ if ( wp > NoOfWayPoints )
+ CurrWP = wp = 0;
+ if ( wp == 0 ) { // force to Origin
+ WPLatitude = OriginLatitude;
+ WPLongitude = OriginLongitude;
+ WPAltitude = (int16)P[NavRTHAlt];
+ WPLoiter = 30000; // mS
+ } else {
+ w = NAV_WP_START + (wp-1) * WAYPOINT_REC_SIZE;
+ WPLatitude = Read32PX(w + 0);
+ WPLongitude = Read32PX(w + 4);
+ WPAltitude = Read16PX(w + 8);
+
+#ifdef NAV_ENFORCE_ALTITUDE_CEILING
+ if ( WPAltitude > NAV_CEILING )
+ WPAltitude = NAV_CEILING;
+#endif // NAV_ENFORCE_ALTITUDE_CEILING
+ WPLoiter = (int16)ReadPX(w + 10) * 1000L; // mS
+ }
+
+ F.WayPointCentred = F.WayPointAchieved = false;
+
+} // GetWaypointPX
+
+void InitNavigation(void) {
+ static uint8 i;
+
+ HoldLatitude = HoldLongitude = WayHeading = 0;
+
+ for ( i = 0; i < (uint8)3; i++ )
+ NavEp[i] = NavIntE[i] = NavCorr[i] = NavCorrp[i] = 0;
+
+ NavState = HoldingStation;
+ AttitudeHoldResetCount = 0;
+ CurrMaxRollPitch = 0;
+ F.WayPointAchieved = F.WayPointCentred = false;
+ F.NavComputed = false;
+
+ if ( ReadPX(NAV_NO_WP) <= 0 ) {
+ NavProximityRadius = ConvertMToGPS(NAV_PROXIMITY_RADIUS);
+ NavProximityAltitude = NAV_PROXIMITY_ALTITUDE * 10L; // Decimetres
+ } else {
+ // need minimum values in UAVXNav?
+ NavProximityRadius = ConvertMToGPS(ReadPX(NAV_PROX_RADIUS));
+ NavProximityAltitude = ReadPX(NAV_PROX_ALT) * 10L; // Decimetres
+ }
+
+ NoOfWayPoints = ReadPX(NAV_NO_WP);
+
+ if ( NoOfWayPoints <= 0 )
+ CurrWP = 0;
+ else
+ CurrWP = 1;
+ GetWayPointPX(0);
+
+} // InitNavigation
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/baro.c Fri Feb 18 22:28:05 2011 +0000
@@ -0,0 +1,613 @@
+// ===============================================================================================
+// = 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 =
+// ===============================================================================================
+
+// 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/
+
+#include "UAVXArm.h"
+
+// Barometers Freescale TI ADC and Bosch BMP085 3.8MHz, Bosch SMD500 400KHz
+
+#define BARO_MIN_CLIMB 150.0 // M minimum available barometer climb from origin
+#define BARO_MIN_DESCENT -50.0 //M minimum available barometer descent from origin
+
+void GetBaroAltitude(void);
+void InitBarometer(void);
+
+void ShowBaroType(void);
+void BaroTest(void);
+
+#define BaroROCFilter HardFilter
+
+uint16 BaroPressure, BaroTemperature;
+boolean AcquiringPressure;
+int16 BaroOffsetDAC;
+
+#define BARO_BUFF_SIZE 4
+
+struct {
+ uint8 Head, Tail;
+ int24 B[BARO_BUFF_SIZE];
+} BaroQ;
+
+int32 OriginBaroPressure, CompBaroPressure;
+real32 BaroRelAltitude, BaroRelAltitudeP;
+i16u BaroVal;
+int8 BaroType;
+int16 BaroClimbAvailable, BaroDescentAvailable;
+int16 AltitudeUpdateRate;
+int8 BaroRetries;
+
+real32 FakeBaroRelAltitude;
+int8 SimulateCycles = 0;
+
+void ShowBaroType(void) {
+ switch ( BaroType ) {
+ case BaroMPX4115:
+ TxString("MPX4115\r\n");
+ break;
+ case BaroSMD500:
+ TxString("SMD500\r\n");
+ break;
+ case BaroBMP085:
+ TxString("BMP085\r\n");
+ break;
+ case BaroUnknown:
+ TxString("None\r\n");
+ break;
+ default:
+ break;
+ }
+} // ShowBaro
+
+void BaroTest(void) {
+ TxString("\r\nAltitude test\r\n");
+
+ TxString("Initialising\r\n");
+
+ InitBarometer();
+
+ while ( F.BaroAltitudeValid && ! F.NewBaroValue )
+ GetBaroAltitude();
+
+ TxString("\r\nType:\t");
+ ShowBaroType();
+
+ TxString("Init Retries:\t");
+ TxVal32((int32)BaroRetries - 2, 0, ' '); // always minimum of 2
+ if ( BaroRetries >= BARO_INIT_RETRIES )
+ TxString(" FAILED Init.\r\n");
+ else
+ TxNextLine();
+
+ if ( BaroType == BaroMPX4115 ) {
+ TxString("Range :\t");
+ TxVal32((int32) BaroDescentAvailable * 10.0, 1, ' ');
+ TxString("-> ");
+ TxVal32((int32) BaroClimbAvailable * 10.0, 1, 'M');
+ TxString(" {Offset ");
+ TxVal32((int32)BaroOffsetDAC, 0,'}');
+ if (( BaroClimbAvailable < BARO_MIN_CLIMB ) || (BaroDescentAvailable > BARO_MIN_DESCENT))
+ TxString(" Bad climb or descent range - offset adjustment?");
+ TxNextLine();
+ }
+
+ if ( !F.BaroAltitudeValid ) goto BAerror;
+
+ while ( !F.NewBaroValue )
+ GetBaroAltitude();
+ F.NewBaroValue = false;
+
+ TxString("Alt.: \t");
+ TxVal32(BaroRelAltitude * 10.0, 1, ' ');
+ TxString("M\r\n");
+
+ TxString("\r\nR.Finder: \t");
+ if ( F.RangefinderAltitudeValid ) {
+ GetRangefinderAltitude();
+ TxVal32(RangefinderAltitude * 100.0, 2, ' ');
+ TxString("M\r\n");
+ } else
+ TxString("no rangefinder\r\n");
+
+ TxString("\r\nAmbient :\t");
+ TxVal32((int32)AmbientTemperature.i16, 1, ' ');
+ TxString("C\r\n");
+
+ return;
+BAerror:
+ TxString("FAIL\r\n");
+} // BaroTest
+
+void GetBaroAltitude(void) {
+ static real32 Temp, AltChange;
+
+ if ( BaroType == BaroMPX4115 )
+ GetFreescaleBaroAltitude();
+ else
+ GetBoschBaroAltitude();
+
+ if ( F.NewBaroValue ) {
+#ifdef SIMULATE
+ if ( State == InFlight ) {
+ if ( ++SimulateCycles >= AltitudeUpdateRate ) {
+ FakeBaroRelAltitude += ( DesiredThrottle - CruiseThrottle ) + Comp[Alt];
+ if ( FakeBaroRelAltitude < -5.0 )
+ FakeBaroRelAltitude = 0.0;
+
+ SimulateCycles = 0;
+
+ ROC = FakeBaroRelAltitude - BaroRelAltitudeP;
+ BaroRelAltitudeP = FakeBaroRelAltitude;
+ }
+ BaroRelAltitude = FakeBaroRelAltitude;
+ }
+#else
+
+ AltChange = BaroRelAltitude - BaroRelAltitudeP;
+ Temp = AltChange * AltitudeUpdateRate;
+
+ if ( fabs( Temp ) > BARO_SANITY_CHECK_MPS ) {
+ BaroRelAltitude = BaroRelAltitudeP; // use previous value
+ Temp = 0;
+ Stats[BaroFailS]++;
+ }
+
+ Temp = Limit( Temp , -BARO_SANITY_CHECK_MPS, BARO_SANITY_CHECK_MPS );
+ ROC = ROC * 0.9 + Temp * 0.1;
+ BaroRelAltitudeP = BaroRelAltitude;
+
+#endif // SIMULATE
+
+ if ( State == InFlight ) {
+ if ( ROC > Stats[MaxROCS] )
+ Stats[MaxROCS] = ROC;
+ else
+ if ( ROC < Stats[MinROCS] )
+ Stats[MinROCS] = ROC;
+
+ if ( BaroRelAltitude > Stats[BaroRelAltitudeS] )
+ Stats[BaroRelAltitudeS] = BaroRelAltitude;
+ }
+ }
+
+} // GetBaroAltitude
+
+void InitBarometer(void) {
+ BaroRelAltitude = BaroRelAltitudeP = CompBaroPressure = OriginBaroPressure = 0;
+ BaroType = BaroUnknown;
+
+ Comp[Alt] = AltDiffSum = AltDSum = 0;
+ F.BaroAltitudeValid= true; // optimistic
+
+ if ( IsFreescaleBaroActive() )
+ InitFreescaleBarometer();
+ else
+ if ( IsBoschBaroActive() )
+ InitBoschBarometer();
+ else {
+ F.BaroAltitudeValid = F.HoldingAlt = false;
+ Stats[BaroFailS]++;
+ }
+} // InitBarometer
+
+// -----------------------------------------------------------
+
+// Freescale ex Motorola MPX4115 Barometer with ADS7823 12bit ADC
+
+void SetFreescaleMCP4725(int16);
+void SetFreescaleOffset(void);
+void ReadFreescaleBaro(void);
+real32 FreescaleToDM(int24);
+void GetFreescaleBaroAltitude(void);
+boolean IsFreescaleBaroActive(void);
+void InitFreescaleBarometer(void);
+
+void SetFreescaleMCP4725(int16 d) {
+ static i16u dd;
+ static uint8 r;
+
+ dd.u16 = d << 4; // left align
+
+ I2CBARO.start();
+ r = I2CBARO.write(MCP4725_WR) != I2C_ACK;
+ r = I2CBARO.write(MCP4725_CMD) != I2C_ACK;
+ r = I2CBARO.write(dd.b1) != I2C_ACK;
+ r = I2CBARO.write(dd.b0) != I2C_ACK;
+ I2CBARO.stop();
+
+} // SetFreescaleMCP4725
+
+void SetFreescaleOffset(void) {
+ // Steve Westerfeld
+ // 470 Ohm, 1uF RC 0.47mS use 2mS for settling?
+
+ TxString("\r\nOffset \tPressure\r\n");
+
+ BaroOffsetDAC = MCP4725_MAX;
+
+ SetFreescaleMCP4725(BaroOffsetDAC);
+
+ Delay1mS(20); // initial settling
+ ReadFreescaleBaro();
+
+ while ( (BaroVal.u16 < (uint16)(((uint24)ADS7823_MAX*4L*7L)/10L) )
+ && (BaroOffsetDAC > 20) ) { // first loop gets close
+ BaroOffsetDAC -= 20; // approach at 20 steps out of 4095
+ SetFreescaleMCP4725(BaroOffsetDAC);
+ Delay1mS(20);
+ ReadFreescaleBaro();
+ TxVal32(BaroOffsetDAC,0,HT);
+ TxVal32(BaroVal.u16,0,' ');
+ TxNextLine();
+ LEDYellow_TOG;
+ }
+
+ BaroOffsetDAC += 20; // move back up to come at it a little slower
+ SetFreescaleMCP4725(BaroOffsetDAC);
+ Delay1mS(100);
+ ReadFreescaleBaro();
+
+ while ( (BaroVal.u16 < (uint16)(((uint24)ADS7823_MAX*4L*3L)/4L) ) && (BaroOffsetDAC > 2) ) {
+ BaroOffsetDAC -= 2;
+ SetFreescaleMCP4725(BaroOffsetDAC);
+ Delay1mS(10);
+ ReadFreescaleBaro();
+ TxVal32(BaroOffsetDAC,0,HT);
+ TxVal32(BaroVal.u16,0,' ');
+ TxNextLine();
+ LEDYellow_TOG;
+ }
+
+ Delay1mS(200); // wait for caps to settle
+ F.BaroAltitudeValid = BaroOffsetDAC > 0;
+
+} // SetFreescaleOffset
+
+void ReadFreescaleBaro(void) {
+ static char B[8];
+ static i16u B0, B1, B2, B3;
+
+ mS[BaroUpdate] = mSClock() + ADS7823_TIME_MS;
+
+ I2CBARO.start(); // start conversion
+
+ if ( I2CBARO.write(ADS7823_WR) != I2C_ACK ) goto FSError;
+ if ( I2CBARO.write(ADS7823_CMD) != I2C_ACK ) goto FSError;
+
+ I2CBARO.read(ADS7823_RD, B, 8); // read block of 4 baro samples
+
+ B0.b0 = B[1];
+ B0.b1 = B[0];
+ B1.b0 = B[3];
+ B1.b1 = B[2];
+ B2.b0 = B[5];
+ B2.b1 = B[4];
+ B3.b0 = B[7];
+ B3.b1 = B[6];
+
+ BaroVal.u16 = (uint16)16380 - ( B0.u16 + B1.u16 + B2.u16 + B3.u16 );
+
+ F.BaroAltitudeValid = true;
+
+ return;
+
+FSError:
+ I2CBARO.stop();
+
+ 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
+ return( -( (real32)p * 0.8 ) / (real32)P[BaroScale] );
+} // FreescaleToDM
+
+void GetFreescaleBaroAltitude(void) {
+ static int24 BaroPressure;
+
+ if ( mSClock() >= mS[BaroUpdate] ) {
+ ReadFreescaleBaro();
+ if ( F.BaroAltitudeValid ) {
+ BaroPressure = (int24)BaroVal.u16; // sum of 4 samples
+
+ BaroRelAltitude = FreescaleToDM(BaroPressure - OriginBaroPressure);
+
+ F.NewBaroValue = F.BaroAltitudeValid;
+ }
+ }
+
+} // GetFreescaleBaroAltitude
+
+boolean IsFreescaleBaroActive(void) { // check for Freescale Barometer
+
+ I2CBARO.start();
+ if ( I2CBARO.write(ADS7823_ID) != I2C_ACK ) goto FreescaleInactive;
+
+ BaroType = BaroMPX4115;
+ I2CBARO.stop();
+
+ TrackMinI2CRate(400000);
+
+ return(true);
+
+FreescaleInactive:
+ I2CBARO.stop();
+ return(false);
+
+} // IsFreescaleBaroActive
+
+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
+ BaroPressure = 0;
+
+ BaroRetries = 0;
+ do {
+ BaroPressureP = BaroPressure;
+
+ SetFreescaleOffset();
+
+ while ( mSClock() < mS[BaroUpdate] ) {};
+ ReadFreescaleBaro();
+ BaroPressure = (int24)BaroVal.u16;
+ } while ( ( ++BaroRetries < BARO_INIT_RETRIES )
+ && ( abs((int16)(BaroPressure - BaroPressureP)) > Error ) );
+
+ F.BaroAltitudeValid = BaroRetries < BARO_INIT_RETRIES;
+
+ OriginBaroPressure = BaroPressure;
+
+ BaroRelAltitudeP = BaroRelAltitude = 0.0;
+
+ MinAltitude = FreescaleToDM((int24)ADS7823_MAX*4);
+ BaroOriginAltitude = FreescaleToDM(OriginBaroPressure);
+ BaroDescentAvailable = MinAltitude - BaroOriginAltitude;
+ BaroClimbAvailable = -BaroOriginAltitude;
+
+ //F.BaroAltitudeValid &= (( BaroClimbAvailable >= BARO_MIN_CLIMB )
+ // && (BaroDescentAvailable <= BARO_MIN_DESCENT));
+
+#ifdef SIMULATE
+ FakeBaroRelAltitude = 0;
+#endif // SIMULATE
+
+} // InitFreescaleBarometer
+
+// -----------------------------------------------------------
+
+// Bosch SMD500 and BMP085 Barometers
+
+void StartBoschBaroADC(boolean);
+int24 CompensatedBoschPressure(uint16, uint16);
+
+void GetBoschBaroAltitude(void);
+boolean IsBoschBaroActive(void);
+void InitBoschBarometer(void);
+
+// SMD500 9.5mS (T) 34mS (P)
+// BMP085 4.5mS (T) 25.5mS (P) OSRS=3
+#define BOSCH_TEMP_TIME_MS 11 // 10 increase to make P+T acq time ~50mS
+//#define BMP085_PRESS_TIME_MS 26
+//#define SMD500_PRESS_TIME_MS 34
+#define BOSCH_PRESS_TIME_MS 38
+#define BOSCH_PRESS_TEMP_TIME_MS 50 // pressure and temp time + overheads
+
+void StartBoschBaroADC(boolean ReadPressure) {
+ static uint8 TempOrPress;
+
+ if ( ReadPressure ) {
+ TempOrPress = BOSCH_PRESS;
+ mS[BaroUpdate] = mSClock() + BOSCH_PRESS_TIME_MS;
+ } else {
+ mS[BaroUpdate] = mSClock() + BOSCH_TEMP_TIME_MS;
+ if ( BaroType == BaroBMP085 )
+ TempOrPress = BOSCH_TEMP_BMP085;
+ else
+ TempOrPress = BOSCH_TEMP_SMD500;
+ }
+
+ I2CBARO.start();
+ if ( I2CBARO.write(BOSCH_ID) != I2C_ACK ) goto SBerror;
+
+ // access control register, start measurement
+ if ( I2CBARO.write(BOSCH_CTL) != I2C_ACK ) goto SBerror;
+
+ // select 32kHz input, measure temperature
+ if ( I2CBARO.write(TempOrPress) != I2C_ACK ) goto SBerror;
+ I2CBARO.stop();
+
+ F.BaroAltitudeValid = true;
+ return;
+
+SBerror:
+ I2CBARO.stop();
+ F.BaroAltitudeValid = F.HoldingAlt = false;
+ return;
+} // StartBoschBaroADC
+
+void ReadBoschBaro(void) {
+ // Possible I2C protocol error - split read of ADC
+ I2CBARO.start();
+ if ( I2CBARO.write(BOSCH_ID) != I2C_ACK ) goto RVerror;
+ if ( I2CBARO.write(BOSCH_ADC_MSB) != I2C_ACK ) goto RVerror;
+ I2CBARO.start(); // restart
+ if ( I2CBARO.write(BOSCH_ID+1) != I2C_ACK ) goto RVerror;
+ BaroVal.b1 = I2CBARO.read(I2C_NACK);
+ I2CBARO.stop();
+
+ I2CBARO.start();
+ if ( I2CBARO.write(BOSCH_ID) != I2C_ACK ) goto RVerror;
+ if ( I2CBARO.write(BOSCH_ADC_LSB) != I2C_ACK ) goto RVerror;
+ I2CBARO.start(); // restart
+ if ( I2CBARO.write(BOSCH_ID+1) != I2C_ACK ) goto RVerror;
+ BaroVal.b0 = I2CBARO.read(I2C_NACK);
+ I2CBARO.stop();
+
+ F.BaroAltitudeValid = true;
+ return;
+
+RVerror:
+ I2CBARO.stop();
+
+ F.BaroAltitudeValid = F.HoldingAlt = false;
+ if ( State == InFlight ) {
+ Stats[BaroFailS]++;
+ F.BaroFailure = true;
+ }
+ return;
+} // ReadBoschBaro
+
+#define BOSCH_BMP085_TEMP_COEFF 62L
+#define BOSCH_SMD500_TEMP_COEFF 50L
+
+int24 CompensatedBoschPressure(uint16 BaroPress, uint16 BaroTemp) {
+ static int24 BaroTempComp;
+
+ if ( BaroType == BaroBMP085 )
+ BaroTempComp = (BaroTemp * BOSCH_BMP085_TEMP_COEFF + 64L) >> 7;
+ else
+ BaroTempComp = (BaroTemp * BOSCH_SMD500_TEMP_COEFF + 8L) >> 4;
+
+ return ((int24)BaroPress + BaroTempComp - OriginBaroPressure);
+
+} // CompensatedBoschPressure
+
+void GetBoschBaroAltitude(void) {
+ static int24 Temp;
+
+ if ( mSClock() >= mS[BaroUpdate] ) {
+ ReadBoschBaro();
+ if ( F.BaroAltitudeValid )
+ if ( AcquiringPressure ) {
+ BaroPressure = (int24)BaroVal.u16;
+ AcquiringPressure = false;
+ } else {
+ BaroTemperature = (int24)BaroVal.u16;
+ AcquiringPressure = true;
+
+ Temp = CompensatedBoschPressure(BaroPressure, BaroTemperature);
+ CompBaroPressure -= BaroQ.B[BaroQ.Head];
+ BaroQ.B[BaroQ.Head] = Temp;
+ CompBaroPressure += Temp;
+ BaroQ.Head = (BaroQ.Head + 1) & (BARO_BUFF_SIZE -1);
+
+ // Pressure queue has 4 entries corresponding to an average delay at 20Hz of 0.1Sec
+ // decreasing pressure is increase in altitude negate and rescale to decimetre altitude
+
+ BaroRelAltitude = - ( (real32)CompBaroPressure * (real32)P[BaroScale] ) / 1280.0;
+
+ F.NewBaroValue = F.BaroAltitudeValid;
+ }
+ else {
+ AcquiringPressure = true;
+ Stats[BaroFailS]++;
+ }
+
+ StartBoschBaroADC(AcquiringPressure);
+ }
+} // GetBoschBaroAltitude
+
+boolean IsBoschBaroActive(void) { // check for Bosch Barometers
+ static uint8 r;
+
+ I2CBARO.start();
+ if ( I2CBARO.write(BOSCH_ID) != I2C_ACK ) goto BoschInactive;
+ if ( I2CBARO.write(BOSCH_TYPE) != I2C_ACK ) goto BoschInactive;
+ I2CBARO.start(); // restart
+ if ( I2CBARO.write(BOSCH_ID+1) != I2C_ACK ) goto BoschInactive;
+ r = I2CBARO.read(I2C_NACK);
+ I2CBARO.stop();
+
+ if (r == BOSCH_ID_BMP085 )
+ BaroType = BaroBMP085;
+ else
+ BaroType = BaroSMD500;
+
+ TrackMinI2CRate(400000);
+
+ return(true);
+
+BoschInactive:
+ return(false);
+
+} // IsBoschBaroActive
+
+void InitBoschBarometer(void) {
+ int8 s;
+ int24 Temp, CompBaroPressureP;
+
+ AltitudeUpdateRate = 1000L / BOSCH_PRESS_TEMP_TIME_MS;
+
+ F.NewBaroValue = false;
+ CompBaroPressure = 0;
+
+ TxString("Temp. \tPressure\r\n");
+
+ BaroRetries = 0;
+ do { // occasional I2C misread of Temperature so keep doing it until the Origin is stable!!
+ CompBaroPressureP = CompBaroPressure;
+ CompBaroPressure = BaroQ.Head = 0;
+
+ AcquiringPressure = true;
+ StartBoschBaroADC(AcquiringPressure); // Pressure
+
+ for ( s = 0; s < 4; s++ ) {
+ while ( mSClock() < mS[BaroUpdate] );
+ ReadBoschBaro(); // Pressure
+ BaroPressure = BaroVal.u16;
+
+ AcquiringPressure = !AcquiringPressure;
+ StartBoschBaroADC(AcquiringPressure); // Temperature
+ while ( mSClock() < mS[BaroUpdate] );
+ ReadBoschBaro();
+ BaroTemperature = BaroVal.u16;
+
+ TxVal32(BaroTemperature,0,HT);
+ TxVal32(BaroPressure,0,0);
+ TxNextLine();
+
+ Temp = CompensatedBoschPressure(BaroPressure, BaroTemperature);
+ BaroQ.B[s] = Temp;
+ CompBaroPressure += Temp;
+
+ AcquiringPressure = !AcquiringPressure;
+ StartBoschBaroADC(AcquiringPressure);
+ }
+
+ } while ( ( ++BaroRetries < BARO_INIT_RETRIES ) && ( abs(CompBaroPressure - CompBaroPressureP) > 12 ) ); // stable within ~0.5M
+
+ OriginBaroPressure = SRS32(CompBaroPressure, 2);
+
+ F.BaroAltitudeValid = BaroRetries < BARO_INIT_RETRIES;
+ BaroRelAltitudeP = BaroRelAltitude = 0.0;
+
+#ifdef SIMULATE
+ FakeBaroRelAltitude = 0.0;
+#endif // SIMULATE
+
+} // InitBoschBarometer
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/compass.c Fri Feb 18 22:28:05 2011 +0000
@@ -0,0 +1,525 @@
+// ===============================================================================================
+// = 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 =
+// ===============================================================================================
+
+// 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/
+
+#include "UAVXArm.h"
+
+// Local magnetic declination not included
+// http://www.ngdc.noaa.gov/geomagmodels/Declination.jsp
+
+
+void ReadCompass(void);
+void GetHeading(void);
+void CalibrateCompass(void);
+void ShowCompassType(void);
+void DoCompassTest(void);
+void InitCompass(void);
+
+MagStruct Mag[3] = {{ 0,0 },{ 0,0 },{ 0,0 }};
+real32 MagDeviation, CompassOffset;
+real32 MagHeading, Heading, Headingp, FakeHeading;
+real32 HeadingSin, HeadingCos;
+uint8 CompassType;
+
+void ReadCompass(void) {
+ switch ( CompassType ) {
+ case HMC5843:
+ ReadHMC5843();
+ break;
+ case HMC6352:
+ ReadHMC6352();
+ break;
+ default:
+ Heading = 0;
+ break;
+ } // switch
+
+} // ReadCompass
+
+void CalibrateCompass(void) {
+ switch ( CompassType ) {
+ case HMC5843:
+ CalibrateHMC5843();
+ break;
+ case HMC6352:
+ CalibrateHMC6352();
+ break;
+ default:
+ break;
+ } // switch
+} // CalibrateCompass
+
+void ShowCompassType(void) {
+ switch ( CompassType ) {
+ case HMC5843:
+ TxString("HMC5843");
+ break;
+ case HMC6352:
+ TxString("HMC6352");
+ break;
+ default:
+ break;
+ }
+ } // ShowCompassType
+
+void DoCompassTest(void) {
+ switch ( CompassType ) {
+ case HMC5843:
+ DoHMC5843Test();
+ break;
+ case HMC6352:
+ DoHMC6352Test();
+ break;
+ default:
+ TxString("\r\nCompass test\r\nCompass not detected?\r\n");
+ break;
+ } // switch
+} // DoCompassTest
+
+void GetHeading(void) {
+
+ const real32 CompassA = COMPASS_UPDATE_S / ( OneOnTwoPiCompassF + COMPASS_UPDATE_S );
+
+ ReadCompass();
+
+ Heading = Make2Pi( MagHeading - MagDeviation - CompassOffset );
+ if ( fabs(Heading - Headingp ) > PI )
+ Headingp = Heading;
+
+ Heading = Headingp + (Heading - Headingp) * CompassA;
+ Headingp = Heading;
+
+#ifdef SIMULATE
+#if ( defined AILERON | defined ELEVON )
+ if ( State == InFlight )
+ FakeHeading -= FakeDesiredRoll/5 + FakeDesiredYaw/5;
+#else
+ if ( State == InFlight ) {
+ if ( Abs(FakeDesiredYaw) > 5 )
+ FakeHeading -= FakeDesiredYaw/5;
+ }
+
+ FakeHeading = Make2Pi((int16)FakeHeading);
+ Heading = FakeHeading;
+#endif // AILERON | ELEVON
+#endif // SIMULATE
+} // GetHeading
+
+void InitCompass(void) {
+ if ( IsHMC5843Active() )
+ CompassType = HMC5843;
+ else
+ if ( HMC6352Active() )
+ CompassType = HMC6352;
+ else {
+ CompassType = NoCompass;
+ F.CompassValid = false;
+ }
+
+ switch ( CompassType ) {
+ case HMC5843:
+ InitHMC5843();
+ break;
+ case HMC6352:
+ InitHMC6352();
+ break;
+ default:
+ MagHeading = 0;
+ } // switch
+
+ ReadCompass();
+ mS[CompassUpdate] = mSClock();
+ Heading = Headingp = Make2Pi( MagHeading - MagDeviation - CompassOffset );
+
+} // InitCompass
+
+//________________________________________________________________________________________
+
+// HMC5843 3 Axis Magnetometer
+
+void ReadHMC5843(void);
+void GetHMC5843Parameters(void);
+void DoHMC5843Test(void);
+void CalibrateHMC5843(void);
+void InitHMC5843(void);
+boolean HMC5843Active(void);
+
+void ReadHMC5843(void) {
+ static char b[6];
+ static i16u X, Y, Z;
+ static uint8 r;
+ static real32 mx, my;
+ static real32 CRoll, SRoll, CPitch, SPitch;
+
+ I2CCOMPASS.start();
+ r = I2CCOMPASS.write(HMC5843_ID);
+ r = I2CCOMPASS.write(0x03); // point to data
+ I2CCOMPASS.stop();
+
+ I2CCOMPASS.read(HMC5843_ID, b, 6);
+
+ X.b1 = b[0];
+ X.b0 = b[1];
+ Y.b1 = b[2];
+ Y.b0 =b[3];
+ Z.b1 = b[4];
+ Z.b0 = b[5];
+
+ if ( F.Using9DOF ) { // SparkFun/QuadroUFO 9DOF Breakout pins front edge components up
+ Mag[BF].V = X.i16;
+ Mag[LR].V = -Y.i16;
+ Mag[UD].V = -Z.i16;
+ } else { // SparkFun Magnetometer Breakout pins right edge components up
+ Mag[BF].V = -X.i16;
+ Mag[LR].V = Y.i16;
+ Mag[UD].V = -Z.i16;
+ }
+
+ CRoll = cos(Angle[Roll]);
+ SRoll = sin(Angle[Roll]);
+ CPitch = cos(Angle[Pitch]);
+ SPitch = sin(Angle[Pitch]);
+
+ 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;
+
+ // Magnetic Heading
+ MagHeading = MakePi(atan2( -my, mx ));
+
+ F.CompassValid = true;
+ return;
+
+} // ReadHMC5843
+
+void CalibrateHMC5843(void) {
+
+} // DoHMC5843Test
+
+void DoHMC5843Test(void) {
+ TxString("\r\nCompass test (HMC5843)\r\n\r\n");
+
+ ReadHMC5843();
+
+ TxString("Mag:\t");
+ 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");
+
+ Heading = Headingp = Make2Pi( MagHeading - MagDeviation - CompassOffset );
+ TxVal32(Heading * RADDEG * 10.0, 1, 0);
+ TxString(" deg (True)\r\n");
+} // DoHMC5843Test
+
+void InitHMC5843(void) {
+ static uint8 r;
+
+ I2CCOMPASS.start();
+ r = I2CCOMPASS.write(HMC5843_ID);
+ r = I2CCOMPASS.write(0x02);
+ r = I2CCOMPASS.write(0x00); // Set continuous mode (default to 10Hz)
+ I2CCOMPASS.stop();
+
+ Delay1mS(50);
+
+} // InitHMC5843Magnetometer
+
+boolean IsHMC5843Active(void) {
+ I2CCOMPASS.start();
+ F.CompassValid = !(I2CCOMPASS.write(HMC5843_ID) != I2C_ACK);
+ I2CCOMPASS.stop();
+
+ if ( F.CompassValid )
+ TrackMinI2CRate(400000);
+
+ return ( F.CompassValid );
+
+} // IsHMC5843Active
+
+//________________________________________________________________________________________
+
+// HMC6352 Compass
+
+void ReadHMC6352(void);
+uint8 WriteByteHMC6352(uint8);
+void GetHMC6352Parameters(void);
+void DoHMC6352Test(void);
+void CalibrateHMC6352(void);
+void InitHMC6352(void);
+boolean IsHMC6352Active(void);
+
+void ReadHMC6352(void) {
+ static i16u v;
+
+ I2CCOMPASS.start();
+ F.CompassMissRead = I2CCOMPASS.write(HMC6352_ID + 1) != I2C_ACK;
+ v.b1 = I2CCOMPASS.read(I2C_ACK);
+ v.b0 = I2CCOMPASS.read(I2C_NACK);
+ I2CCOMPASS.stop();
+
+ MagHeading = Make2Pi( ((real32)v.i16 * PI) / 1800.0 - CompassOffset ); // Radians
+} // ReadHMC6352
+
+uint8 WriteByteHMC6352(uint8 d) {
+ I2CCOMPASS.start();
+ if ( I2CCOMPASS.write(HMC6352_ID) != I2C_ACK ) goto WError;
+ if ( I2CCOMPASS.write(d) != I2C_ACK ) goto WError;
+ I2CCOMPASS.stop();
+
+ return( I2C_ACK );
+WError:
+ I2CCOMPASS.stop();
+ return ( I2C_NACK );
+} // WriteByteHMC6352
+
+char CP[9];
+
+#define TEST_COMP_OPMODE 0x70 // standby mode to reliably read EEPROM
+
+void GetHMC6352Parameters(void) {
+ uint8 r;
+
+ I2CCOMPASS.start();
+ if ( I2CCOMPASS.write(HMC6352_ID) != 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(20);
+
+ for (r = 0; r <= (uint8)8; r++) { // must have this timing - not block read!
+
+ I2CCOMPASS.start();
+ if ( I2CCOMPASS.write(HMC6352_ID) != I2C_ACK ) goto CTerror;
+ if ( I2CCOMPASS.write('r') != I2C_ACK ) goto CTerror;
+ if ( I2CCOMPASS.write(r) != I2C_ACK ) goto CTerror;
+ I2CCOMPASS.stop();
+
+ Delay1mS(10);
+
+ I2CCOMPASS.start();
+ if ( I2CCOMPASS.write(HMC6352_ID+1) != I2C_ACK ) goto CTerror;
+ 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_ID) != 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");
+ TxValH(CP[0]);
+ if ( CP[0] != (uint8)0x42 )
+ TxString("\t Error expected 0x42 for HMC6352");
+ TxNextLine();
+
+ Temp = (CP[1]*256)|CP[2];
+ TxString("\t1:2:\tXOffset\t");
+ TxVal32((int32)Temp, 0, 0);
+ TxNextLine();
+
+ Temp = (CP[3]*256)|CP[4];
+ TxString("\t3:4:\tYOffset\t");
+ TxVal32((int32)Temp, 0, 0);
+ TxNextLine();
+
+ TxString("\t5:\tDelay\t");
+ TxVal32((int32)CP[5], 0, 0);
+ TxNextLine();
+
+ TxString("\t6:\tNSum\t");
+ TxVal32((int32)CP[6], 0, 0);
+ TxNextLine();
+
+ TxString("\t7:\tSW Ver\t");
+ TxString(" 0x");
+ TxValH(CP[7]);
+ TxNextLine();
+
+ TxString("\t8:\tOpMode:");
+ switch ( ( CP[8] >> 5 ) & 0x03 ) {
+ case 0:
+ TxString(" 1Hz");
+ break;
+ case 1:
+ TxString(" 5Hz");
+ break;
+ case 2:
+ TxString(" 10Hz");
+ break;
+ case 3:
+ TxString(" 20Hz");
+ break;
+ }
+
+ if ( CP[8] & 0x10 ) TxString(" S/R");
+
+ switch ( CP[8] & 0x03 ) {
+ case 0:
+ TxString(" Standby");
+ break;
+ case 1:
+ TxString(" Query");
+ break;
+ case 2:
+ TxString(" Continuous");
+ break;
+ case 3:
+ TxString(" Not-allowed");
+ break;
+ }
+ TxNextLine();
+
+ InitCompass();
+ if ( !F.CompassValid ) goto CTerror;
+
+ Delay1mS(50);
+
+ ReadHMC6352();
+ if ( F.CompassMissRead ) goto CTerror;
+
+ TxNextLine();
+ TxVal32(MagHeading * RADDEG * 10.0, 1, 0);
+ TxString(" deg (Magnetic)\r\n");
+ Heading = Headingp = Make2Pi( MagHeading - MagDeviation - CompassOffset );
+ TxVal32(Heading * RADDEG * 10.0, 1, 0);
+ TxString(" deg (True)\r\n");
+
+ return;
+CTerror:
+ I2CCOMPASS.stop();
+ TxString("FAIL\r\n");
+} // DoHMC6352Test
+
+void CalibrateHMC6352(void) { // calibrate the compass by rotating the ufo through 720 deg smoothly
+ TxString("\r\nCalib. compass - Press CONTINUE button (x) to Start\r\n");
+ while ( PollRxChar() != 'x' ); // UAVPSet uses 'x' for CONTINUE button
+
+ // Do Set/Reset now
+ if ( WriteByteHMC6352('O') != I2C_ACK ) goto CCerror;
+
+ Delay1mS(7);
+
+ // set Compass device to Calibration mode
+ if ( WriteByteHMC6352('C') != I2C_ACK ) goto CCerror;
+
+ 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;
+
+ TxString("\r\nCalibration complete\r\n");
+
+ Delay1mS(50);
+
+ InitCompass();
+
+ return;
+
+CCerror:
+ TxString("Calibration FAILED\r\n");
+} // CalibrateHMC6352
+
+void InitHMC6352(void) {
+
+ // 20Hz continuous read with periodic reset.
+#ifdef SUPPRESS_COMPASS_SR
+#define COMP_OPMODE 0x62
+#else
+#define COMP_OPMODE 0x72
+#endif // SUPPRESS_COMPASS_SR
+
+ // Set device to Compass mode
+ I2CCOMPASS.start();
+ if ( I2CCOMPASS.write(HMC6352_ID) != 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;
+ I2CCOMPASS.stop();
+
+ Delay1mS(1);
+
+ // save operation mode in Flash
+ if ( WriteByteHMC6352('L') != I2C_ACK ) goto CTerror;
+
+ Delay1mS(1);
+
+ // Do Bridge Offset Set/Reset now
+ if ( WriteByteHMC6352('O') != I2C_ACK ) goto CTerror;
+
+ Delay1mS(50);
+
+ F.CompassValid = true;
+
+ return;
+CTerror:
+ F.CompassValid = false;
+ Stats[CompassFailS]++;
+ F.CompassFailure = true;
+
+ I2CCOMPASS.stop();
+} // InitHMC6352
+
+boolean HMC6352Active(void) {
+
+ I2CCOMPASS.start();
+ F.CompassValid = !(I2CCOMPASS.write(HMC6352_ID) != I2C_ACK);
+ I2CCOMPASS.stop();
+
+ if ( F.CompassValid )
+ TrackMinI2CRate(100000);
+
+ return ( F.CompassValid );
+
+} // HMC6352Active
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/control.c Fri Feb 18 22:28:05 2011 +0000
@@ -0,0 +1,437 @@
+// ===============================================================================================
+// = 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 =
+// ===============================================================================================
+
+// 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/
+
+#include "UAVXArm.h"
+
+void DoAltitudeHold(void);
+void UpdateAltitudeSource(void);
+void AltitudeHold(void);
+void InertialDamping(void);
+void DoOrientationTransform(void);
+void DoControl(void);
+
+void CheckThrottleMoved(void);
+void LightsAndSirens(void);
+void InitControl(void);
+
+real32 Angle[3], Anglep[3], Rate[3], Ratep[3]; // Milliradians
+real32 Comp[4];
+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 CurrMaxRollPitch;
+
+int16 AttitudeHoldResetCount;
+real32 AltDiffSum, AltD, AltDSum;
+real32 DesiredAltitude, Altitude, AltitudeP;
+real32 ROC;
+
+uint32 AltuSp;
+int16 DescentLimiter;
+
+real32 GRollKp, GRollKi, GRollKd, GPitchKp, GPitchKi, GPitchKd;
+
+boolean FirstPass;
+int8 BeepTick = 0;
+
+void DoAltitudeHold(void) { // Syncronised to baro intervals independant of active altitude source
+
+ static int16 NewAltComp;
+ static real32 AltP, AltI, AltD;
+ static real32 LimAltE, AltE;
+ static real32 AltdT, AltdTR;
+ static uint32 Now;
+
+ Now = uSClock();
+ AltdT = ( Now - AltuSp ) * 0.000001;
+ AltdT = Limit(AltdT, 0.01, 0.1); // limit range for restarts
+ AltdTR = 1.0 / AltdT;
+ AltuSp = Now;
+
+ AltE = DesiredAltitude - Altitude;
+ LimAltE = Limit(AltE, -ALT_BAND_M, ALT_BAND_M);
+
+ AltP = LimAltE * K[AltKp];
+ AltP = Limit(AltP, -ALT_MAX_THR_COMP, ALT_MAX_THR_COMP);
+
+ AltDiffSum += LimAltE;
+ AltDiffSum = Limit(AltDiffSum, -ALT_INT_WINDUP_LIMIT, ALT_INT_WINDUP_LIMIT);
+ AltI = AltDiffSum * K[AltKi] * AltdT;
+ AltI = Limit(AltDiffSum, -K[AltIntLimit], 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);
+
+ 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);
+
+
+ if ( ROC > Stats[MaxROCS] )
+ Stats[MaxROCS] = ROC;
+ else
+ if ( ROC < Stats[MinROCS] )
+ Stats[MinROCS] = ROC;
+
+} // DoAltitudeHold
+
+void UpdateAltitudeSource(void) {
+ if ( F.UsingRangefinderAlt )
+ Altitude = RangefinderAltitude;
+ else
+ Altitude = BaroRelAltitude;
+
+} // UpdateAltitudeSource
+
+void AltitudeHold() {
+ static int16 NewCruiseThrottle;
+
+ GetBaroAltitude();
+ GetRangefinderAltitude();
+ CheckThrottleMoved();
+
+ if ( F.AltHoldEnabled ) {
+ if ( F.NewBaroValue ) { // sync on Baro which MUST be working
+ F.NewBaroValue = false;
+
+ UpdateAltitudeSource();
+
+ if ( ( NavState != HoldingStation ) && F.AllowNavAltitudeHold ) { // Navigating - using CruiseThrottle
+ F.HoldingAlt = true;
+ DoAltitudeHold();
+ } else
+ if ( F.ThrottleMoving ) {
+ F.HoldingAlt = false;
+ DesiredAltitude = Altitude;
+ Comp[Alt] = Decay1(Comp[Alt]);
+ } else {
+ F.HoldingAlt = true;
+ if ( fabs(ROC) < ALT_HOLD_MAX_ROC_MPS ) {
+ NewCruiseThrottle = DesiredThrottle + Comp[Alt];
+ CruiseThrottle = HardFilter(CruiseThrottle, NewCruiseThrottle);
+ CruiseThrottle = Limit( CruiseThrottle , IdleThrottle, MaxCruiseThrottle );
+ }
+ DoAltitudeHold();
+ }
+ }
+ } else {
+ Comp[Alt] = Decay1(Comp[Alt]);
+ 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;
+
+ if ( F.UsingPolar ) {
+ OSO = OSin[PolarOrientation];
+ OCO = OCos[PolarOrientation];
+ } else {
+ OSO = OSin[Orientation];
+ OCO = OCos[Orientation];
+ }
+
+ if ( !F.NavigationActive )
+ NavCorr[Roll] = NavCorr[Pitch] = NavCorr[Yaw] = 0;
+
+ // -PS+RC
+ ControlRoll = (int16)( -DesiredPitch * OSO + DesiredRoll * OCO );
+
+ // PC+RS
+ ControlPitch = (int16)( DesiredPitch * OCO + DesiredRoll * OSO );
+
+} // DoOrientationTransform
+
+void GainSchedule(boolean UseAngle) {
+
+ /*
+ // rudimentary gain scheduling (linear)
+ static int16 AttDiff, ThrDiff;
+
+ if ( (!F.NavigationActive) || ( F.NavigationActive && (NavState == HoldingStation ) ) )
+ {
+ // also density altitude?
+
+ if ( P[Acro] > 0) // due to Foliage 2009 and Alexinparis 2010
+ {
+ AttDiff = CurrMaxRollPitch - ATTITUDE_HOLD_LIMIT;
+ GS = GS * ( 1000.0 - (AttDiff * (int16)P[Acro]) );
+ GS *= 0.001;
+ GS = Limit(GS, 0, 1.0);
+ }
+
+ if ( P[GSThrottle] > 0 )
+ {
+ ThrDiff = DesiredThrottle - CruiseThrottle;
+ GS = (int32)GS * ( 1000.0 + (ThrDiff * (int16)P[GSThrottle]) );
+ GS *= 0.001;
+ }
+ }
+
+ */
+
+ GS = 1.0; // Temp
+
+ GRollKp = K[RollKp];
+ GRollKi = K[RollKi];
+ GRollKd = K[RollKd];
+
+ GPitchKp = K[PitchKp];
+ GPitchKi = K[PitchKi];
+ GPitchKd = K[PitchKd];
+
+} // GainSchedule
+
+void DoControl(void) {
+ GetAttitude();
+ AltitudeHold();
+ InertialDamping();
+
+#ifdef SIMULATE
+
+ FakeDesiredRoll = DesiredRoll + NavRCorr;
+ FakeDesiredPitch = DesiredPitch + NavPCorr;
+ FakeDesiredYaw = DesiredYaw + NavYCorr;
+ Angle[Roll] = SlewLimit(Angle[Roll], FakeDesiredRoll * 16.0, 4.0);
+ Angle[Pitch] = SlewLimit(Angle[Pitch], FakeDesiredPitch * 16.0, 4.0);
+ Angle[Yaw] = SlewLimit(Angle[Yaw], FakeDesiredYaw, 4.0);
+ Rl = FakeDesiredRoll;
+ Pl = FakeDesiredPitch;
+ Yl = DesiredYaw;
+
+#else
+
+ DoOrientationTransform();
+
+ GainSchedule(F.UsingAngleControl);
+
+#ifdef DISABLE_EXTRAS
+ // for commissioning
+ Comp[BF] = Comp[LR] = Comp[UD] = Comp[Alt] = 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];
+
+ // Pitch
+
+ 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
+
+ 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
+
+ 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];
+ }
+
+ // Yaw
+
+ Rate[Yaw] -= NavCorr[Yaw];
+ if ( abs(DesiredYaw) > 5 )
+ Rate[Yaw] -= DesiredYaw;
+
+ Yl = Rate[Yaw] * K[YawKp] + Angle[Yaw] * K[YawKi] + (Rate[Yaw]-Ratep[Yaw]) * K[YawKd] * dTR;
+ Ratep[Yaw] = Rate[Yaw];
+
+#ifdef TRICOPTER
+ Yl = SlewLimit(Ylp, Yl, 2.0);
+ Ylp = Yl;
+ Yl = Limit(Yl, -K[YawLimit] * 4.0, K[YawLimit] * 4.0);
+#else
+ Yl = Limit(Yl, -K[YawLimit], K[YawLimit]);
+#endif // TRICOPTER
+
+#endif // SIMULATE
+
+} // DoControl
+
+static int8 RCStart = RC_INIT_FRAMES;
+
+void LightsAndSirens(void) {
+ static int24 Ch5Timeout;
+
+ LEDYellow_TOG;
+ if ( F.Signal ) LEDGreen_ON;
+ else LEDGreen_OFF;
+
+ Beeper_OFF;
+ Ch5Timeout = mSClock() + 500; // mS.
+
+ do {
+
+ ProcessCommand();
+
+ if ( F.Signal ) {
+ LEDGreen_ON;
+ if ( F.RCNewValues ) {
+ UpdateControls();
+ if ( --RCStart == 0 ) { // wait until RC filters etc. have settled
+ UpdateParamSetChoice();
+ MixAndLimitCam();
+ RCStart = 1;
+ }
+
+ InitialThrottle = StickThrottle;
+ StickThrottle = 0;
+ OutSignals();
+ if ( mSClock() > Ch5Timeout ) {
+ if ( F.Navigate || F.ReturnHome || !F.ParametersValid ) {
+ Beeper_TOG;
+ LEDRed_TOG;
+ } else
+ if ( Armed )
+ LEDRed_TOG;
+
+ Ch5Timeout += 500;
+ }
+ }
+ } else {
+ LEDRed_ON;
+ LEDGreen_OFF;
+ }
+ ReadParameters();
+ GetIRAttitude(); // only active if IRSensors selected
+ } while ((!F.Signal) || (Armed && FirstPass) || F.Ch5Active || F.GyroFailure || (!F.AccelerationsValid) ||
+ ( InitialThrottle >= RC_THRES_START ) || (!F.ParametersValid) );
+
+ FirstPass = false;
+
+ Beeper_OFF;
+ LEDRed_OFF;
+ LEDGreen_ON;
+ LEDYellow_ON;
+
+ mS[LastBattery] = mSClock();
+ mS[FailsafeTimeout] = mSClock() + FAILSAFE_TIMEOUT_MS;
+
+ F.LostModel = false;
+ FailState = MonitoringRx;
+
+} // LightsAndSirens
+
+void InitControl(void) {
+ static uint8 i;
+
+ 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;
+
+ Comp[Alt] = AltSum = Ylp = ControlRollP = ControlPitchP = AltitudeP = 0.0;
+
+} // InitControl
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/gps.c Fri Feb 18 22:28:05 2011 +0000
@@ -0,0 +1,562 @@
+// ===============================================================================================
+// = 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 =
+// ===============================================================================================
+
+// 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/
+
+#include "UAVXArm.h"
+
+void UpdateField(void);
+int32 ConvertGPSToM(int32);
+int32 ConvertMToGPS(int32);
+int24 ConvertInt(uint8, uint8);
+real32 ConvertReal(uint8, uint8);
+int32 ConvertLatLonM(uint8, uint8);
+void ConvertUTime(uint8, uint8);
+void ConvertUDate(uint8, uint8);
+void ParseGPGGASentence(void);
+void ParseGPRMCSentence(void);
+void SetGPSOrigin(void);
+void ParseGPSSentence(void);
+void RxGPSPacket(uint8);
+void SetGPSOrigin(void);
+void DoGPS(void);
+void GPSTest(void);
+void UpdateGPS(void);
+void InitGPS(void);
+
+const uint8 NMEATags[MAX_NMEA_SENTENCES][5]= {
+ // NMEA
+ {'G','P','G','G','A'}, // full positioning fix
+ {'G','P','R','M','C'}, // main current position and heading
+};
+
+NMEAStruct NMEA;
+
+uint8 GPSPacketTag;
+struct tm GPSTime;
+int32 GPSStartTime, GPSSeconds;
+int32 GPSLatitude, GPSLongitude;
+int32 OriginLatitude, OriginLongitude;
+int32 DesiredLatitude, DesiredLongitude;
+int32 LatitudeP, LongitudeP, HoldLatitude, HoldLongitude;
+real32 GPSAltitude, GPSRelAltitude, GPSOriginAltitude;
+real32 GPSLongitudeCorrection;
+real32 GPSHeading, GPSMagHeading, GPSMagDeviation, GPSVel, GPSVelp, GPSROC;
+int8 GPSNoOfSats;
+int8 GPSFix;
+int16 GPSHDilute;
+
+int32 FakeGPSLongitude, FakeGPSLatitude;
+
+uint8 ll, tt, ss, RxCh;
+uint8 GPSCheckSumChar, GPSTxCheckSum;
+uint8 nll, cc, lo, hi;
+boolean EmptyField;
+int16 ValidGPSSentences;
+real32 GPSdT, GPSdTR;
+uint32 GPSuSp;
+
+int32 SumGPSRelAltitude, SumBaroRelAltitude;
+
+int32 ConvertGPSToM(int32 c) { // approximately 1.8553257183 cm per LSB at the Equator
+ // conversion max is 21Km
+ return ( ((int32)c * (int32)1855)/((int32)100000) );
+} // ConvertGPSToM
+
+int32 ConvertMToGPS(int32 c) {
+ // conversion max is 21Km
+ return ( ((int32)c * (int32)100000)/((int32)1855) );
+} // ConvertMToGPS
+
+int24 ConvertInt(uint8 lo, uint8 hi) {
+ static uint8 i;
+ static int24 r;
+
+ r = 0;
+ if ( !EmptyField )
+ for (i = lo; i <= hi ; i++ )
+ r = r * 10 + NMEA.s[i] - '0';
+
+ return (r);
+} // ConvertInt
+
+real32 ConvertReal(uint8 lo, uint8 hi) {
+ int16 i, n, dp;
+ boolean Positive;
+ int16 whole;
+ real32 rval;
+
+ if (EmptyField)
+ rval=0.0;
+ else {
+ if (NMEA.s[lo]=='-') {
+ Positive=false;
+ lo++;
+ } else
+ Positive=true;
+
+ dp=lo;
+ while ((NMEA.s[dp] != '.')&&(dp<=hi))
+ dp++;
+
+ whole=ConvertInt(lo, dp-1);
+ rval=ConvertInt(dp + 1, hi);
+
+ n=hi-dp;
+ for (i=1;i<=n;i++)
+ rval/=10.0;
+
+ if (Positive)
+ rval=(whole+rval);
+ else
+ rval=-(whole+rval);
+ }
+
+ return(rval);
+} // ConvertReal
+
+int32 ConvertLatLonM(uint8 lo, uint8 hi) { // NMEA coordinates normally assumed as DDDMM.MMMMM ie 5 decimal minute digits
+ // but code can deal with 4 and 5 decimal minutes
+ // Positions are stored at 5 decimal minute NMEA resolution which is
+ // approximately 1.855 cm per LSB at the Equator.
+ static int32 dd, mm, dm, r;
+ static uint8 dp;
+
+ r = 0;
+ if ( !EmptyField ) {
+ dp = lo + 4; // could do this in initialisation for Lat and Lon?
+ while ( NMEA.s[dp] != '.') dp++;
+
+ dd = ConvertInt(lo, dp - 3);
+ mm = ConvertInt(dp - 2 , dp - 1);
+ if ( ( hi - dp ) > (uint8)4 )
+ dm = ConvertInt(dp + 1, dp + 5);
+ else
+ dm = ConvertInt(dp + 1, dp + 4) * 10L;
+
+ r = dd * 6000000 + mm * 100000 + dm;
+ }
+
+ return(r);
+} // ConvertLatLonM
+
+void ConvertUTime(uint8 lo, uint8 hi) {
+
+ if ( !EmptyField ) {
+ GPSTime.tm_hour = ConvertInt(lo, lo+1);
+ GPSTime.tm_min = ConvertInt(lo+2, lo+3);
+ GPSTime.tm_sec = ConvertInt(lo+4, lo+5);
+ GPSSeconds = (int32)GPSTime.tm_hour * 3600 + GPSTime.tm_min * 60 + GPSTime.tm_sec;
+ }
+} // ConvertUTime
+
+void ConvertUDate(uint8 lo, uint8 hi) {
+
+ static time_t seconds;
+
+// if ( !EmptyField && !F.RTCValid )
+ {
+ GPSTime.tm_mday = ConvertInt(lo, lo + 1);
+ GPSTime.tm_mon = ConvertInt(lo + 2, lo + 3) - 1;
+ GPSTime.tm_year = ConvertInt( lo + 4, hi ) + 100;
+ seconds = mktime ( &GPSTime );
+ set_time( seconds );
+
+ F.RTCValid = true;
+ }
+
+} // ConvertUDate
+
+void UpdateField(void) {
+ static uint8 ch;
+
+ lo = cc;
+
+ ch = NMEA.s[cc];
+ while (( ch != ',' ) && ( ch != '*' ) && ( cc < nll ))
+ ch = NMEA.s[++cc];
+
+ hi = cc - 1;
+ cc++;
+ EmptyField = hi < lo;
+} // UpdateField
+
+void ParseGPGGASentence(void) { // full position $GPGGA fix
+
+ UpdateField();
+
+ UpdateField(); //UTime
+ ConvertUTime(lo,hi);
+
+ UpdateField(); //Lat
+ GPSLatitude = ConvertLatLonM(lo, hi);
+ UpdateField(); //LatH
+ if (NMEA.s[lo] == 'S') GPSLatitude = -GPSLatitude;
+
+ UpdateField(); //Lon
+ GPSLongitude = ConvertLatLonM(lo, hi);
+ UpdateField(); //LonH
+ if (NMEA.s[lo] == 'W') GPSLongitude = -GPSLongitude;
+
+ UpdateField(); //Fix
+ GPSFix = (uint8)(ConvertInt(lo,hi));
+
+ UpdateField(); //Sats
+ GPSNoOfSats = (uint8)(ConvertInt(lo,hi));
+
+ UpdateField(); // HDilute
+ GPSHDilute = ConvertInt(lo, hi-3) * 100 + ConvertInt(hi-1, hi); // Cm
+
+ UpdateField(); // Alt
+ GPSAltitude = real32(ConvertInt(lo, hi-2) * 10L + ConvertInt(hi, hi)); // Metres
+
+ //UpdateField(); // AltUnit - assume Metres!
+
+ //UpdateField(); // GHeight
+ //UpdateField(); // GHeightUnit
+
+ F.GPSValid = (GPSFix >= GPS_MIN_FIX) && ( GPSNoOfSats >= GPS_MIN_SATELLITES );
+
+ if ( State == InFlight ) {
+ if ( GPSHDilute > Stats[MaxHDiluteS] ) {
+ Stats[MaxHDiluteS] = GPSHDilute;
+ F.GPSFailure = GPSHDilute > 150;
+ } else
+ if ( GPSHDilute < Stats[MinHDiluteS] )
+ Stats[MinHDiluteS] = GPSHDilute;
+
+ if ( GPSNoOfSats > Stats[GPSMaxSatsS] )
+ Stats[GPSMaxSatsS] = GPSNoOfSats;
+ else
+ if ( GPSNoOfSats < Stats[GPSMinSatsS] )
+ Stats[GPSMinSatsS] = GPSNoOfSats;
+ }
+} // ParseGPGGASentence
+
+void ParseGPRMCSentence() { // main current position and heading
+
+ // uint32 UTime;
+
+ UpdateField();
+
+ UpdateField(); //UTime
+ //UTime = ConvertUTime(lo,hi);
+ // GPSMissionTime =(int) (UTime-GPSStartTime);
+
+ UpdateField();
+ if ( NMEA.s[lo] == 'A' ) {
+ UpdateField(); //Lat
+ GPSLatitude = ConvertLatLonM(lo,hi);
+ UpdateField(); //LatH
+ if (NMEA.s[lo] == 'S')
+ GPSLatitude= -GPSLatitude;
+
+ UpdateField(); //Lon
+ GPSLongitude = ConvertLatLonM(lo,hi);
+
+ UpdateField(); //LonH
+ if ( NMEA.s[lo] == 'W' )
+ GPSLongitude = -GPSLongitude;
+
+ UpdateField(); // Groundspeed (Knots)
+ GPSVel = ConvertReal(lo, hi) * 0.514444444; // KTTOMPS
+
+ UpdateField(); // True course made good (Degrees)
+ GPSHeading = ConvertReal(lo, hi) * DEGRAD;
+
+ UpdateField(); //UDate
+ ConvertUDate(lo, hi);
+
+ UpdateField(); // Magnetic Deviation (Degrees)
+ GPSMagDeviation = ConvertReal(lo, hi) * DEGRAD;
+
+ UpdateField(); // East/West
+ if ( NMEA.s[lo] == 'W')
+ GPSMagHeading = GPSHeading - GPSMagDeviation; // need to check sign????
+ else
+ GPSMagHeading = GPSHeading + GPSMagDeviation;
+ F.HaveGPRMC = true;
+ } else
+ F.HaveGPRMC = false;
+
+} // ParseGPRMCSentence
+
+void SetGPSOrigin(void) {
+ if ( ( ValidGPSSentences == GPS_ORIGIN_SENTENCES ) && F.GPSValid ) {
+ GPSStartTime = GPSSeconds;
+ OriginLatitude = DesiredLatitude = HoldLatitude = LatitudeP = GPSLatitude;
+ OriginLongitude = DesiredLongitude = HoldLongitude = LongitudeP = GPSLongitude;
+ GPSVel = 0;
+
+#ifdef SIMULATE
+ FakeGPSLongitude = GPSLongitude;
+ FakeGPSLatitude = GPSLatitude;
+#endif // SIMULATE
+
+ mS[LastGPS] = mSClock();
+
+ GPSLongitudeCorrection = cos(fabs((real32)(GPSLatitude/600000L) * DEGRAD));
+
+ GPSOriginAltitude = GPSAltitude;
+
+ Write16PX(NAV_ORIGIN_ALT, (int16)(GPSAltitude/100));
+ Write32PX(NAV_ORIGIN_LAT, GPSLatitude);
+ Write32PX(NAV_ORIGIN_LON, GPSLongitude);
+
+ if ( !F.NavValid ) {
+ DoBeep100mS(2,0);
+ Stats[NavValidS] = true;
+ F.NavValid = true;
+ }
+ F.AcquireNewPosition = true;
+ }
+} // SetGPSOrigin
+
+void ParseGPSSentence(void) {
+ static uint32 Now;
+
+#define FAKE_NORTH_WIND 0L
+#define FAKE_EAST_WIND 0L
+#define SCALE_VEL 64L
+
+ cc = 0;
+ nll = NMEA.length;
+
+ switch ( GPSPacketTag ) {
+ case GPGGAPacketTag:
+ ParseGPGGASentence();
+ break;
+ case GPRMCPacketTag:
+ ParseGPRMCSentence();
+ break;
+ }
+
+ if ( F.GPSValid ) {
+ // all coordinates in 0.00001 Minutes or ~1.8553cm relative to Origin
+ // There is a lot of jitter in position - could use Kalman Estimator?
+
+ Now = uSClock();
+ GPSdT = ( Now - GPSuSp) * 0.0000001;
+ GPSdTR = 1.0 / GPSdT;
+ GPSuSp = Now;
+
+ if ( ValidGPSSentences < GPS_ORIGIN_SENTENCES ) { // repetition to ensure GPGGA altitude is captured
+ F.GPSValid = false;
+
+ if ( ( GPSPacketTag == GPGGAPacketTag ) && ( GPSHDilute <= GPS_MIN_HDILUTE ))
+ ValidGPSSentences++;
+ else
+ ValidGPSSentences = 0;
+ }
+
+#ifdef SIMULATE
+
+ if ( State == InFlight ) { // don't bother with GPS longitude correction for now?
+ CosH = int16cos(Heading);
+ SinH = int16sin(Heading);
+ GPSLongitude = FakeGPSLongitude + ((int32)(-FakeDesiredPitch) * SinH)/SCALE_VEL;
+ GPSLatitude = FakeGPSLatitude + ((int32)(-FakeDesiredPitch) * CosH)/SCALE_VEL;
+
+ A = Make2Pi(Heading + HALFMILLIPI);
+ CosH = int16cos(A);
+ SinH = int16sin(A);
+ GPSLongitude += ((int32)FakeDesiredRoll * SinH) / SCALE_VEL;
+ GPSLongitude += FAKE_EAST_WIND; // wind
+ GPSLatitude += ((int32)FakeDesiredRoll * CosH) / SCALE_VEL;
+ GPSLatitude += FAKE_NORTH_WIND; // wind
+
+ FakeGPSLongitude = GPSLongitude;
+ FakeGPSLatitude = GPSLatitude;
+
+ GPSRelAltitude = BaroRelAltitude;
+ }
+
+#else
+ if (F.NavValid )
+ GPSRelAltitude = GPSAltitude - GPSOriginAltitude;
+
+#endif // SIMULATE
+
+ // possibly add GPS filtering here
+
+ if ( State == InFlight ) {
+ if ( GPSRelAltitude > Stats[GPSAltitudeS] )
+ Stats[GPSAltitudeS] = GPSRelAltitude;
+
+ if ( GPSVel * 10.0 > Stats[GPSVelS] )
+ Stats[GPSVelS] = GPSVel * 10.0;
+
+ if (( BaroRelAltitude > 5.0 ) && ( BaroRelAltitude < 15.0 )) { // 5-15M
+ SumGPSRelAltitude += GPSRelAltitude;
+ SumBaroRelAltitude += BaroRelAltitude;
+ }
+ }
+ } else
+ if ( State == InFlight )
+ Stats[GPSInvalidS]++;
+
+} // ParseGPSSentence
+
+void RxGPSPacket(uint8 RxCh) {
+
+ switch ( RxState ) {
+ case WaitCheckSum:
+ if (GPSCheckSumChar < (uint8)2) {
+ GPSTxCheckSum *= 16;
+ if ( RxCh >= 'A' )
+ GPSTxCheckSum += ( RxCh - ('A' - 10) );
+ else
+ GPSTxCheckSum += ( RxCh - '0' );
+
+ GPSCheckSumChar++;
+ } else {
+ NMEA.length = ll;
+ F.GPSPacketReceived = GPSTxCheckSum == RxCheckSum;
+ RxState = WaitSentinel;
+ }
+ break;
+ case WaitBody:
+ if ( RxCh == '*' ) {
+ GPSCheckSumChar = GPSTxCheckSum = 0;
+ RxState = WaitCheckSum;
+ } else
+ if ( RxCh == '$' ) { // abort partial Sentence
+ ll = tt = RxCheckSum = 0;
+ RxState = WaitTag;
+ } else {
+ RxCheckSum ^= RxCh;
+ NMEA.s[ll++] = RxCh;
+ if ( ll > (uint8)( GPSRXBUFFLENGTH-1 ) )
+ RxState = WaitSentinel;
+ }
+
+ break;
+ case WaitTag:
+ RxCheckSum ^= RxCh;
+ while ( ( RxCh != NMEATags[ss][tt] ) && ( ss < MAX_NMEA_SENTENCES ) ) ss++;
+ if ( RxCh == NMEATags[ss][tt] )
+ if ( tt == (uint8)NMEA_TAG_INDEX ) {
+ GPSPacketTag = ss;
+ RxState = WaitBody;
+ } else
+ tt++;
+ else
+ RxState = WaitSentinel;
+ break;
+ case WaitSentinel: // highest priority skipping unused sentence types
+ if ( RxCh == '$' ) {
+ ll = tt = ss = RxCheckSum = 0;
+ RxState = WaitTag;
+ }
+ break;
+ }
+} // RxGPSPacket
+
+void UpdateGPS(void) {
+ if ( F.GPSPacketReceived ) {
+ LEDBlue_TOG;
+ F.GPSPacketReceived = false;
+ ParseGPSSentence();
+ if ( F.GPSValid ) {
+ F.NavComputed = false;
+ mS[GPSTimeout] = mSClock() + GPS_TIMEOUT_MS;
+ } else {
+ NavCorr[Pitch] = DecayX(NavCorr[Pitch], 2);
+ NavCorr[Roll] = DecayX(NavCorr[Roll], 2);
+ NavCorr[Yaw] = 0;
+ }
+ } else
+ if ( mSClock() > mS[GPSTimeout] )
+ F.GPSValid = false;
+
+ if ( F.GPSValid )
+ LEDRed_OFF;
+ else
+ LEDRed_ON;
+
+} // UpdateGPS
+
+void GPSTest(void) {
+
+ static uint8 i;
+
+ TxString("\r\nGPS test\r\n\r\n");
+
+ UpdateGPS();
+
+ UpdateRTC();
+ i = 0;
+ while ( RTCString[i] != NULL )
+ TxChar(RTCString[i++]);
+ TxNextLine();
+ TxString("Fix: \t");
+ TxVal32(GPSFix,0,0);
+ TxNextLine();
+ TxString("Sats: \t");
+ TxVal32(GPSNoOfSats,0,0);
+ TxNextLine();
+ TxString("HDilute: \t");
+ TxVal32(GPSHDilute,2,0);
+ TxNextLine();
+ TxString("Alt: \t");
+ TxVal32(GPSAltitude,1,0);
+ TxNextLine();
+ TxString("Lat: \t");
+ TxVal32(GPSLatitude/600, 4, 0);
+ TxNextLine();
+ TxString("Lon: \t");
+ TxVal32(GPSLongitude/600, 4, 0);
+ if ( F.HaveGPRMC ) {
+ TxString("\r\nVel. :\t");
+ TxVal32(GPSVel * 10.0 , 1, 0);
+ TxString("\r\nHeading:\t");
+ TxVal32(GPSHeading * RADDEG * 10.0 , 1, 0);
+ TxString("\r\nMDev. :\t");
+ TxVal32(GPSMagDeviation * RADDEG * 10.0 , 1, 0);
+ TxNextLine();
+ }
+ TxNextLine();
+} // GPSTest
+
+void InitGPS(void) {
+ cc = 0;
+
+ GPSuSp = uSClock();
+
+ GPSLongitudeCorrection = 1.0;
+ GPSSeconds = GPSFix = GPSNoOfSats = GPSHDilute = 0;
+ GPSRelAltitude = GPSAltitude = GPSMagDeviation = GPSHeading = GPSVel = 0.0;
+ GPSPacketTag = GPSUnknownPacketTag;
+
+ GPSTime.tm_hour = GPSTime.tm_min = GPSTime.tm_sec = GPSTime.tm_mday = GPSTime.tm_mon = GPSTime.tm_year = 0;
+
+ OriginLatitude = DesiredLatitude = HoldLatitude = LatitudeP = GPSLatitude = 0;
+ OriginLongitude = DesiredLongitude = HoldLongitude = LongitudeP = GPSLongitude = 0;
+
+ Write32PX(NAV_ORIGIN_LAT, 0);
+ Write32PX(NAV_ORIGIN_LON, 0);
+ Write16PX(NAV_ORIGIN_ALT, 0);
+
+ ValidGPSSentences = 0;
+
+ SumGPSRelAltitude = SumBaroRelAltitude = 0;
+
+ F.NavValid = F.GPSValid = F.GPSPacketReceived = false;
+ RxState = WaitSentinel;
+
+} // InitGPS
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/gyro.c Fri Feb 18 22:28:05 2011 +0000
@@ -0,0 +1,364 @@
+// ===============================================================================================
+// = 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 =
+// ===============================================================================================
+
+// 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/
+
+#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
+ 1.0 // Infrared Sensors
+ // add others as required
+};
+
+void ReadGyros(void);
+void GetGyroRates(void);
+void CheckGyroFault(uint8, uint8, uint8);
+void ErectGyros(void);
+void InitGyros(void);
+void GyroTest(void);
+void ShowGyroType(void);
+
+real32 GyroADC[3], GyroNeutral[3], Gyro[3]; // Radians
+uint8 GyroType;
+
+void GetGyroRates(void) {
+ static uint8 g;
+
+ ReadGyros();
+ for ( g = 0; g < (uint8)3; g++ )
+ Gyro[g] = ( GyroADC[g] - GyroNeutral[g] ) ;
+} // GetGyroRates
+
+void ReadGyros(void) {
+ switch ( GyroType ) {
+ case ITG3200Gyro:
+ ReadITG3200Gyro();
+ break;
+ case IRSensors:
+ GetIRAttitude();
+ break;
+ default :
+ ReadAnalogGyros();
+ break;
+ } // switch
+} // ReadGyros
+
+void ErectGyros(void) {
+ static int16 i, g;
+
+ LEDRed_ON;
+
+ for ( g = 0; g <(int8)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 <(int8)3; g++ ) {
+ GyroNeutral[g] *= 0.03125;
+ Gyro[g] = 0.0;
+ }
+
+ LEDRed_OFF;
+
+} // ErectGyros
+
+void GyroTest(void) {
+ TxString("\r\nGyro Test - ");
+ ShowGyroType();
+
+ ReadGyros();
+
+ TxString("\r\n\tRoll: \t");
+ TxVal32(GyroADC[Roll] * 1000.0, 3, 0);
+ TxNextLine();
+ TxString("\tPitch: \t");
+ TxVal32(GyroADC[Pitch] * 1000.0, 3, 0);
+ TxNextLine();
+ TxString("\tYaw: \t");
+ TxVal32(GyroADC[Yaw] * 1000.0, 3, 0);
+ TxNextLine();
+
+ TxString("Expect ~0.0 ( ~0.5 for for analog gyros)\r\n");
+
+ switch ( GyroType ) {
+ case ITG3200Gyro:
+ ITG3200Test();
+ break;
+ default:
+ break;
+ } // switch
+
+ if ( F.GyroFailure )
+ TxString("\r\nFAILED\r\n");
+
+} // GyroTest
+
+void InitGyros(void) {
+ if ( ITG3200GyroActive() )
+ GyroType = ITG3200Gyro;
+ else
+ GyroType = P[GyroRollPitchType];
+
+ switch ( GyroType ) {
+ case ITG3200Gyro:
+ InitITG3200Gyro();
+ break;
+ case IRSensors:
+ InitIRSensors();
+ default:
+ InitAnalogGyros();
+ break;
+ } // switch
+
+ Delay1mS(50);
+ ErectGyros();
+
+} // InitGyros
+
+void ShowGyroType(void) {
+ switch ( GyroType ) {
+ case MLX90609Gyro:
+ TxString("MLX90609");
+ break;
+ case ADXRS150Gyro:
+ TxString("ADXRS613/150");
+ break;
+ case IDG300Gyro:
+ TxString("IDG300");
+ break;
+ case LY530Gyro:
+ TxString("ST-AY530");
+ break;
+ case ADXRS300Gyro:
+ TxString("ADXRS610/300");
+ break;
+ case ITG3200Gyro:
+ TxString("ITG3200");
+ break;
+ case IRSensors:
+ TxString("IR Sensors");
+ break;
+ default:
+ TxString("unknown");
+ break;
+ } // switch
+} // ShowGyroType
+
+//________________________________________________________________________________________
+
+// Analog Gyros
+
+void ReadAnalogGyros(void);
+void InitAnalogGyros(void);
+void CheckAnalogGyroFault(uint8, uint8, uint8);
+void AnalogGyroTest(void);
+
+void ReadAnalogGyros(void) {
+ static uint8 g;
+
+ GyroADC[Roll] = RollADC.read();
+ GyroADC[Pitch] = PitchADC.read();
+ GyroADC[Yaw] = YawADC.read();
+
+ for ( g = 0; g < (uint8)3; g++ )
+ GyroADC[g] *= GyroToRadian[GyroType];
+} // ReadAnalogGyros
+
+void InitAnalogGyros(void) {
+ // nothing to do
+ F.GyroFailure = false;
+} // InitAnalogGyros
+
+//________________________________________________________________________________________
+
+// ITG3200 3-axis I2C Gyro
+
+void ReadITG3200(void);
+uint8 ReadByteITG3200(uint8);
+void WriteByteITG3200(uint8, uint8);
+void InitITG3200(void);
+void ITG3200Test(void);
+boolean ITG3200Active(void);
+
+real32 ITG3200Temperature;
+
+void ReadITG3200Gyro(void) {
+ static char G[6];
+ static uint8 g;
+ static i16u GX, GY, GZ;
+
+ I2CGYRO.start();
+ if ( I2CGYRO.write(ITG3200_W) != I2C_ACK ) goto SGerror;
+ if ( I2CGYRO.write(ITG3200_GX_H) != I2C_ACK ) goto SGerror;
+ I2CGYRO.stop();
+
+ I2CGYRO.read(ITG3200_R, G, 6);
+
+ 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;
+ }
+
+ for ( g = 0; g < (uint8)3; g++ )
+ GyroADC[g] *= GyroToRadian[ITG3200Gyro];
+
+ return;
+
+SGerror:
+ I2CGYRO.stop();
+ // GYRO FAILURE - FATAL
+ Stats[GyroFailS]++;
+ F.GyroFailure = true;
+ return;
+} // ReadITG3200Gyro
+
+uint8 ReadByteITG3200(uint8 a) {
+ static uint8 d;
+
+ I2CGYRO.start();
+ if ( I2CGYRO.write(ITG3200_W) != I2C_ACK ) goto SGerror;
+ if ( I2CGYRO.write(a) != I2C_ACK ) goto SGerror;
+
+ I2CGYRO.start();
+ if ( I2CGYRO.write(ITG3200_R) != I2C_ACK ) goto SGerror;
+ d = I2CGYRO.read(I2C_NACK);
+ I2CGYRO.stop();
+
+ return ( d );
+
+SGerror:
+ I2CGYRO.stop();
+ // 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_W) != I2C_ACK ) goto SGerror;
+ if ( I2CGYRO.write(a) != I2C_ACK ) goto SGerror;
+ if ( I2CGYRO.write(d) != I2C_ACK ) goto SGerror;
+ I2CGYRO.stop();
+ return;
+
+SGerror:
+ I2CGYRO.stop();
+ // GYRO FAILURE - FATAL
+ Stats[GyroFailS]++;
+ F.GyroFailure = true;
+ return;
+} // WriteByteITG3200
+
+void InitITG3200Gyro(void) {
+ F.GyroFailure = false; // reset optimistically!
+
+ 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.
+
+ Delay1mS(50);
+
+ ReadITG3200Gyro();
+
+} // InitITG3200Gyro
+
+void ITG3200Test(void) {
+ static uint8 MyID, SMPLRT_DIV, DLPF_FS, PWR_MGM;
+ static int16 TEMP,GYRO_X, GYRO_Y, GYRO_Z;
+
+ MyID = ReadByteITG3200(ITG3200_WHO);
+
+ TxString("\tWHO_AM_I \t0x");
+ TxValH(MyID);
+ TxNextLine();
+ Delay1mS(1);
+ SMPLRT_DIV = ReadByteITG3200(ITG3200_SMPL);
+ DLPF_FS = ReadByteITG3200(ITG3200_DLPF);
+ TEMP = (int16)ReadByteITG3200(ITG3200_TMP_H)<<8 | ReadByteITG3200(ITG3200_TMP_L);
+ GYRO_X = (int16)ReadByteITG3200(ITG3200_GX_H)<<8 | ReadByteITG3200(ITG3200_GX_L);
+ GYRO_Y = (int16)ReadByteITG3200(ITG3200_GY_H)<<8 | ReadByteITG3200(ITG3200_GY_L);
+ GYRO_Z = (int16)ReadByteITG3200(ITG3200_GZ_H)<<8 | ReadByteITG3200(ITG3200_GZ_L);
+ PWR_MGM = ReadByteITG3200(ITG3200_PWR_M);
+
+ ITG3200Temperature = 35.0 + ((TEMP + 13200.0 ) / 280.0);
+
+ TxString("\tSMPLRT_DIV\t");
+ TxVal32( SMPLRT_DIV,0,0);
+ TxNextLine();
+ TxString("\tDLPF \t");
+ TxVal32( DLPF_FS & 7,0,0 );
+ TxString(" FS \t");
+ TxVal32( (DLPF_FS>>3)&3, 0, 0);
+ TxNextLine();
+ TxString("\tTEMP \t");
+ TxVal32( TEMP, 0, 0);
+ TxNextLine();
+ TxString("\tGYRO_X \t");
+ TxVal32( GYRO_X, 0, 0);
+ TxNextLine();
+ TxString("\tGYRO_Y \t");
+ TxVal32( GYRO_Y, 0, 0);
+ TxNextLine();
+ TxString("\tGYRO_Z \t");
+ TxVal32( GYRO_Z, 0, 0);
+ TxNextLine();
+ TxString("\tPWR_MGM \t0x");
+ TxValH( PWR_MGM );
+ TxNextLine();
+
+ TxNextLine();
+
+} // ITG3200Test
+
+boolean ITG3200GyroActive(void) {
+ I2CGYRO.start();
+ F.GyroFailure = I2CGYRO.write(ITG3200_ID) != I2C_ACK;
+ I2CGYRO.stop();
+
+ if ( !F.GyroFailure )
+ TrackMinI2CRate(400000);
+
+ return ( !F.GyroFailure );
+
+} // ITG3200GyroActive
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/harness.c Fri Feb 18 22:28:05 2011 +0000
@@ -0,0 +1,111 @@
+// ===============================================================================================
+// = 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 =
+// ===============================================================================================
+
+// 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/
+
+#include "UAVXArm.h"
+
+void UpdateRTC(void);
+void InitHarness(void);
+
+LocalFileSystem Flash("local");
+
+// connections to ARM
+ // 1 GND
+ // 2 4.5-9V
+ // 3 VBat
+ // 4 NReset
+
+//SPI SPI0(p5, p6, p7); // 5 SPI MOSI, 6 SPI MOSO, 7 SPI CLK
+//DigitalOut SPICS(p8); // 8
+SDFileSystem SDCard(p5, p6, p7, p8, "SDCard");
+
+//I2C I2C1(p9, p10); // 9 Tx / I2C SDA, 10 Rx / I2C SCL
+SerialBuffered TelemetrySerial(p9, p10);
+DigitalIn Armed(p11); // 11 SPI MOSI
+DigitalOut PWMCamPitch(p12); // 12 SPI MOSO
+
+Serial GPSSerial(p13, p14); // 13 Tx1 / SPI CLK, 14 Rx1
+
+AnalogIn PitchADC(p15); // 15 AN0
+AnalogIn RollADC(p16); // 16 AN1
+AnalogIn YawADC(p18); // 17 AN2 (has DAC capability)
+
+AnalogIn RangefinderADC(p17); // 18 AN3
+AnalogIn BatteryCurrentADC(p19); // 19 AN4
+AnalogIn BatteryVoltsADC(p20); // 20 AN5
+
+PwmOut Out0(p21); // 21
+PwmOut Out1(p22); // 22
+PwmOut Out2(p23); // 23
+PwmOut Out3(p24); // 24
+
+//PwmOut Out4(p25); // 25
+//PwmOut Out5(p26); // 26
+
+DigitalOut DebugPin(p25); // 25
+
+I2C I2C0(p28, p27); // 27, 28
+
+DigitalIn RCIn(p29); // 29 CAN
+DigitalOut PWMCamRoll(p30); // 30 CAN
+
+//Serial TelemetrySerial(USBTX, USBRX);
+ // 31 USB +, 32 USB -
+ // 34 -37 Ethernet
+ // 38 IF +
+ // 39 IF -
+ // 40 3.3V Out
+
+// order L-R end of card
+DigitalOut BlueLED(LED1);
+DigitalOut GreenLED(LED2);
+DigitalOut RedLED(LED3);
+DigitalOut YellowLED(LED4);
+
+InterruptIn RCInterrupt(p29);
+
+char RTCString[32], RTCLogfile[32];
+struct tm* RTCTime;
+
+void UpdateRTC(void)
+{
+ time_t s = time(NULL);
+ RTCTime = localtime(&s);
+ strftime(RTCString, 32, "%a %b %d %H:%M:%S %Y", RTCTime );
+} // UpdateRTCString
+
+void InitHarness(void){
+
+ I2C0.frequency(I2C_MAX_RATE_HZ);
+
+ TelemetrySerial.baud(115200);
+ TelemetrySerial.set_tx_buffer_size(512);
+
+ GPSSerial.baud(115200); // reduce baud rate to lighten interrupt traffic?
+
+ PWMCamRoll.write(false);
+ PWMCamPitch.write(false);
+
+ Armed.mode(PullUp);
+
+ CheckSDCardValid();
+
+ UpdateRTC();
+
+} // InitHarness
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/i2c.c Fri Feb 18 22:28:05 2011 +0000
@@ -0,0 +1,163 @@
+// ===============================================================================================
+// = 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 =
+// ===============================================================================================
+
+// 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/
+
+#include "UAVXArm.h"
+
+uint32 MinI2CRate = I2C_MAX_RATE_HZ;
+
+void TrackMinI2CRate(uint32 r) {
+ if ( r < MinI2CRate )
+ MinI2CRate = r;
+} // TrackMinI2CRate
+
+void ShowI2CDeviceName(uint8 d)
+{
+ TxChar(' ');
+ switch ( d ) {
+ case ADXL345_ID: TxString("ADXL345 Acc"); break;
+ case ITG3200_ID: TxString("ITG3200 Gyro"); break;
+ case HMC5843_ID: TxString("HMC5843 Magnetometer"); break;
+ case HMC6352_ID: TxString("HMC6352 Compass"); break;
+ case ADS7823_ID: TxString("ADS7823 ADC"); break;
+ case MCP4725_ID: TxString("MCP4725 DAC"); break;
+ case BOSCH_ID: TxString("Bosch Baro"); break;
+ case TMP100_ID: TxString("TMP100 Temp"); break;
+ case PCA9551_ID: TxString("PCA9551 LED");break;
+ case LISL_ID: TxString("LIS3L Acc"); break;
+ default: break;
+ } // switch
+ TxChar(' ');
+
+} // ShowI2CDeviceName
+
+uint8 ScanI2CBus(void) {
+ uint8 s;
+ uint8 d;
+
+ d = 0;
+
+ TxString("Buss 0\r\n");
+ for ( s = 0x10 ; s <= 0xf6 ; s += 2 ) {
+ I2C0.start();
+ if ( I2C0.write(s) == I2C_ACK ) {
+ d++;
+ TxString("\t0x");
+ TxValH(s);
+ ShowI2CDeviceName( s );
+ TxNextLine();
+ }
+ I2C0.stop();
+
+ Delay1mS(2);
+ }
+
+ /*
+ TxString("Buss 1\r\n");
+ for ( s = 0x10 ; s <= 0xf6 ; s += 2 ) {
+ I2C1.start();
+ if ( I2C1.write(s) == I2C_ACK ) {
+ d++;
+ TxString("\t0x");
+ TxValH(s);
+ TxNextLine();
+ }
+ I2C1.stop();
+
+ Delay1mS(2);
+ }
+ */
+
+ PCA9551Test();
+
+ return(d);
+} // ScanI2CBus
+
+void ProgramSlaveAddress(uint8 addr) {
+ static uint8 s;
+
+ for (s = 0x10 ; s < 0xf0 ; s += 2 ) {
+ I2CESC.start();
+ if ( I2CESC.read(s) == I2C_ACK )
+ if ( s == addr ) { // ESC is already programmed OK
+ I2CESC.stop();
+ TxString("\tESC at SLA 0x");
+ TxValH(addr);
+ TxString(" is already programmed OK\r\n");
+ return;
+ } else {
+ if ( I2CESC.read(0x87) == I2C_ACK ) // select register 0x07
+ if ( I2CESC.write( addr ) == I2C_ACK ) { // new slave address
+ I2CESC.stop();
+ TxString("\tESC at SLA 0x");
+ TxValH(s);
+ TxString(" reprogrammed to SLA 0x");
+ TxValH(addr);
+ TxNextLine();
+ return;
+ }
+ }
+ I2CESC.stop();
+ }
+ TxString("\tESC at SLA 0x");
+ TxValH(addr);
+ TxString(" no response - check cabling and pullup resistors!\r\n");
+} // ProgramSlaveAddress
+
+boolean CheckESCBus(void) {
+ return ( true );
+} // CheckESCBus
+
+void ConfigureESCs(void) {
+ int8 m;
+
+ if ( (int8)P[ESCType] == ESCYGEI2C ) {
+ TxString("\r\nProgram YGE ESCs\r\n");
+ for ( m = 0 ; m < NoOfI2CESCOutputs ; m++ ) {
+ TxString("Connect ONLY ");
+ switch ( m ) {
+#ifdef HEXACOPTER
+ not yet!
+#else
+ case 0 :
+ TxString("Front");
+ break;
+ case 1 :
+ TxString("Back");
+ break;
+ case 2 :
+ TxString("Right");
+ break;
+ case 3 :
+ TxString("Left");
+ break;
+#endif // HEXACOPTER
+ }
+ TxString(" ESC, then press any key \r\n");
+ while ( PollRxChar() != 'x' ); // UAVPSet uses 'x' for any key button
+ // TxString("\r\n");
+ ProgramSlaveAddress( 0x62 + ( m*2 ));
+ }
+ TxString("\r\nConnect ALL ESCs and power-cycle the Quadrocopter\r\n");
+ } else
+ TxString("\r\nYGEI2C not selected as ESC?\r\n");
+} // ConfigureESCs
+
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ir.c Fri Feb 18 22:28:05 2011 +0000
@@ -0,0 +1,85 @@
+// ===============================================================================================
+// = 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 =
+// ===============================================================================================
+
+// 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/
+
+#include "UAVXArm.h"
+
+// IR Sensors ( Scheme from "Leveller" Autopilot ~2005 )
+
+void GetIRAttitude(void);
+void TrackIRMaxMin(real32);
+void InitIRSensors(void);
+
+// FMA Roll/Pitch connector Pin1 Gnd, Pin 2 3.3V, Pin3 -Pitch, Pin4 Roll.
+// FMA Z-Axis connector Pin1 Gnd, Pin 2 3.3V, Pin3 Z , Pin4 Unused.
+// DIYDrones connector Pin1 Gnd, Pin 2 3.3V, Pin3 Z, Pin4 -Pitch, Pin5 Roll.
+
+real32 IR[3], IRMax, IRMin, IRSwing;
+
+
+void TrackIRMaxMin(real32 m) {
+ if ( m > IRMax ) IRMax = m;
+ else
+ if ( m < IRMin ) IRMin = m;
+
+ IRSwing = Max( IRSwing, fabs(m) );
+ IRSwing -= 0.00001; // zzz
+
+} // TrackIRMaxMin
+
+void GetIRAttitude() {
+ #define IR_NEUTRAL 1.0
+ static uint8 i;
+
+ if ( GyroType == IRSensors ) {
+
+ IR[Pitch] = -PitchADC.read() * 2.0 - IR_NEUTRAL;
+ IR[Roll] = RollADC.read() * 2.0 - IR_NEUTRAL;
+ IR[Yaw] = YawADC.read() * 2.0 - IR_NEUTRAL;
+
+ for ( i = 0; i < (uint8)3; i++ )
+ TrackIRMaxMin(IR[i]);
+
+ for ( i = 0; i < (uint8)2; i++ )
+ Angle[i] = asin ( Limit( IR[i] / IRSwing, -1.0, 1.0 ) );
+
+ Angle[Yaw] = 0.0;
+ Rate[Yaw] = 0.0;
+
+ }
+
+} // GetIRAttitude
+
+void InitIRSensors() {
+ static uint8 i;
+
+ LEDYellow_ON;
+ IRSwing = 0.5;
+
+ IRMax = -1.0;
+ IRMin= 1.0;
+
+ for ( i = 0; i < 20; i++ ) {
+ GetIRAttitude();
+ Delay1mS(100);
+ }
+
+ LEDYellow_OFF;
+
+} // InitIRSensors
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/irq.c Fri Feb 18 22:28:05 2011 +0000
@@ -0,0 +1,190 @@
+// ===============================================================================================
+// = 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 =
+// ===============================================================================================
+
+// 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/
+
+#include "UAVXArm.h"
+
+// Interrupt Routines
+
+
+const int16 MIN_PPM_SYNC_PAUSE = 2400; // uS
+
+// no less than 1500
+
+extern void InitTimersAndInterrupts(void);
+
+void enableTxIrq0(void);
+void disableTxIrq0(void);
+void enableTxIrq1(void);
+void disableTxIrq1(void);
+
+void RCISR(void);
+void RCNullISR(void);
+void RCTimeoutISR(void);
+void CamPWMOnISR(void);
+void CamRollPWMOffISR(void);
+void CamPitchPWMOffISR(void);
+void TelemetryInISR(void);
+void GPSInISR(void);
+
+Timer timer;
+Timeout CamRollTimeout, CamPitchTimeout;
+Ticker CameraTicker;
+Timeout RCTimeout;
+
+uint32 mS[CompassUpdate + 1];
+
+uint32 PrevEdge, CurrEdge;
+uint8 Intersection, PrevPattern, CurrPattern;
+uint32 Width;
+int16 PPM[MAX_CONTROLS];
+int8 PPM_Index;
+int24 PauseTime;
+uint8 RxState;
+int8 State, FailState;
+int8 SignalCount;
+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;
+
+ timer.start();
+
+ GPSSerial.attach(GPSInISR, GPSSerial.RxIrq);
+
+ InitTelemetryPacket();
+ //TelemetrySerial.attach(TelemetryInISR, TelemetrySerial.RxIrq);
+
+ RCPositiveEdge = true;
+
+ RCTimeout.attach_us(& RCTimeoutISR, 50000);
+
+#ifdef SOFTWARE_CAM_PWM
+ CameraTicker.attach_us(&CamPWMOnISR, 22500);
+#endif // SOFTWARE_CAM_PWM
+
+ for (i = Clock; i<= CompassUpdate; i++)
+ mS[i] = 0;
+
+} // InitTimersAndInterrupts
+
+void RCISR(void) {
+ CurrEdge = timer.read_us();
+
+ Width = CurrEdge - PrevEdge;
+ PrevEdge = CurrEdge;
+
+ if ( Width > MIN_PPM_SYNC_PAUSE ) { // A pause > 5ms
+ PPM_Index = 0; // Sync pulse detected - next CH is CH1
+ F.RCFrameOK = true;
+ F.RCNewValues = false;
+ PauseTime = Width;
+ } else
+ if (PPM_Index < RC_CONTROLS) { // Width in 1us ticks.
+ if ( ( Width >= RC_MIN_WIDTH_US ) && ( Width <= RC_MAX_WIDTH_US ) )
+ PPM[PPM_Index] = Width - 1000;
+ else {
+ // preserve old value i.e. default hold
+ RCGlitches++;
+ F.RCFrameOK = false;
+ }
+
+ PPM_Index++;
+ // MUST demand rock solid RC frames for autonomous functions not
+ // to be cancelled by noise-generated partially correct frames
+ if ( PPM_Index == RC_CONTROLS ) {
+ if ( F.RCFrameOK ) {
+ F.RCNewValues = true;
+ SignalCount++;
+ } else {
+ F.RCNewValues = false;
+ SignalCount -= RC_GOOD_RATIO;
+ }
+
+ SignalCount = Limit(SignalCount, -RC_GOOD_BUCKET_MAX, RC_GOOD_BUCKET_MAX);
+ F.Signal = SignalCount > 0;
+
+ if ( F.Signal )
+ mS[LastValidRx] = timer.read_ms();
+
+ RCTimeout.attach_us(& RCTimeoutISR, RC_SIGNAL_TIMEOUT_MS*1000);
+ }
+ }
+
+} // RCISR
+
+void RCNullISR(void) {
+// discard edge
+} // RCNullISR
+
+void CamPWMOnISR(void) {
+
+ PWMCamRoll.write(true);
+ CamRollTimeout.attach_us(&CamRollPWMOffISR, CamRollPulseWidth);
+ PWMCamPitch.write(true);
+ CamPitchTimeout.attach_us(&CamPitchPWMOffISR, CamPitchPulseWidth);
+
+} // CamPWMOnISR
+
+void CamRollPWMOffISR(void) {
+
+ PWMCamRoll.write(false);
+ CamRollTimeout.detach();
+
+} // CamRollPWMOffISR
+
+void CamPitchPWMOffISR(void) {
+
+ PWMCamPitch.write(false);
+ CamPitchTimeout.detach();
+
+} // CamPitchPWMOffISR
+
+void RCTimeoutISR(void) {
+ if ( F.Signal ) {
+ F.Signal = false;
+ SignalCount = -RC_GOOD_BUCKET_MAX;
+ }
+} // RCTimeoutISR
+
+void GPSInISR(void) {
+ RxGPSPacket( GPSSerial.getc());
+} // GPSInISR
+
+void TelemetryInISR(void) {
+ RxTelemetryPacket(TelemetrySerial.getc());
+} // TelemetryInISR
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/leds.c Fri Feb 18 22:28:05 2011 +0000
@@ -0,0 +1,265 @@
+// ===============================================================================================
+// = 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 =
+// ===============================================================================================
+
+// 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/
+
+#include "UAVXArm.h"
+
+void SendLEDs(void);
+void SaveLEDs(void);
+void RestoreLEDs(void);
+void LEDsOn(uint16);
+void LEDsOff(uint16);
+void LEDChaser(void);
+
+void PCA9551Test(void);
+void WritePCA9551(uint8);
+boolean IsPCA9551Active(void);
+
+void DoLEDs(void);
+void PowerOutput(int8);
+void LEDsAndBuzzer(void);
+void InitLEDs(void);
+
+uint16 LEDShadow, SavedLEDs, LEDPattern = 0;
+uint16 PrevLEDShadow;
+uint8 PrevPCA9551LEDShadow;
+
+boolean PrevHolding = false;
+const uint16 LEDChase[8] = {
+ YellowM,
+ RedM,
+ GreenM,
+ BlueM,
+ DRV1M,
+ DRV3M,
+ DRV2M,
+ DRV0M
+};
+
+void WritePCA9551(uint8 S) {
+ const uint8 M = 0; // On
+ // const uint8 M = 2; // PWM0 rate
+ // const uint8 M = 3; // PWM1 rate
+
+ const uint8 LOFF = 1; // High impedance
+ static uint8 L03, L47, i;
+
+ L03 = L47 = 0;
+ for ( i = 0; i < 4; i++ ) {
+ L03 <<= 2;
+ if ( S & 0x80 )
+ L03 |= M;
+ else
+ L03 |= LOFF;
+ S <<= 1;
+ }
+
+ for ( i = 0; i <4; i++ ) {
+ L47 <<= 2;
+ if ( S & 0x80 )
+ L47 |= M;
+ else
+ L47 |= LOFF;
+ S <<= 1;
+ }
+
+ I2CLED.start();
+ I2CLED.write(PCA9551_ID);
+ I2CLED.write(0x15);
+ I2CLED.write(L03);
+ I2CLED.write(L47);
+ I2CLED.stop();
+
+} // WritePCA9551
+
+void SendLEDs(void) { // 39.3 uS @ 40MHz
+ static uint8 PCA9551LEDShadow;
+
+ if ( LEDShadow != PrevLEDShadow ) {
+
+ BlueLED = LEDShadow & BlueM;
+ RedLED = LEDShadow & RedM;
+ GreenLED = LEDShadow & GreenM;
+ YellowLED = LEDShadow & YellowM;
+
+ PCA9551LEDShadow = uint8(LEDShadow & 0x00ff);
+
+ if ( F.UsingLEDDriver && ( PCA9551LEDShadow != PrevPCA9551LEDShadow ) ) {
+ WritePCA9551(PCA9551LEDShadow);
+ PrevPCA9551LEDShadow = PCA9551LEDShadow;
+ }
+
+ PrevLEDShadow = LEDShadow;
+ }
+
+} // SendLEDs
+
+
+void SaveLEDs(void) { // one level only
+ SavedLEDs = LEDShadow;
+} // SaveLEDs
+
+void RestoreLEDs(void) {
+ LEDShadow = SavedLEDs;
+ SendLEDs();
+} // RestoreLEDs
+
+void LEDsOn(uint16 l) {
+ LEDShadow |= l;
+ SendLEDs();
+} // LEDsOn
+
+void LEDsOff(uint16 l) {
+ LEDShadow &= ~l;
+ SendLEDs();
+} // LEDsOff
+
+void LEDChaser(void) {
+#define LED_NO (uint8)6 // skip buzzer
+//#define LED_NO (uint8)7 // all LEDs
+
+ if ( mSClock() > mS[LEDChaserUpdate] ) {
+ if ( F.HoldingAlt ) {
+ LEDShadow ^= LEDChase[LEDPattern];
+ if ( LEDPattern < LED_NO ) LEDPattern++;
+ else LEDPattern = 0;
+ LEDShadow |= LEDChase[LEDPattern];
+ SendLEDs();
+ } else {
+ LEDShadow = SavedLEDs;
+ SendLEDs();
+ }
+
+ mS[LEDChaserUpdate] = mSClock() + 50;
+ }
+} // LEDChaser
+
+void DoLEDs(void) {
+ if ( F.AccelerationsValid ) LEDYellow_ON;
+ else LEDYellow_OFF;
+
+ if ( F.Signal ) {
+ LEDRed_OFF;
+ LEDGreen_ON;
+ } else {
+ LEDGreen_OFF;
+ LEDRed_ON;
+ }
+} // DoLEDs
+
+void PowerOutput(int8 d) {
+ int8 s;
+ uint16 m;
+
+ m = 1 << d;
+ for ( s=0; s < 10; s++ ) { // 10 flashes (count MUST be even!)
+ LEDShadow ^= m;
+ SendLEDs();
+ Delay1mS(50);
+ }
+} // PowerOutput
+
+void LEDsAndBuzzer(void) {
+ int8 s, m;
+ uint16 mask, LEDSave;
+
+ LEDSave = LEDShadow;
+ LEDShadow = 0;
+ SendLEDs();
+
+ TxString("\r\nOutput test\r\n");
+ TxString("Sequence Drv0/Buzz, Drv1, Drv2, Drv3, Aux0, Aux1, Aux2, Aux3, Y, R, G, B\r\n");
+ mask = (uint8)1;
+ for ( m = 1; m <= 12; m++ ) {
+ for ( s = 0; s < 10; s++ ) { // 10 flashes (count MUST be even!)
+ LEDShadow ^= mask;
+ SendLEDs();
+ Delay1mS(100);
+ }
+ mask <<= 1;
+ }
+ LEDShadow = LEDSave;
+ SendLEDs();
+ TxString("Test Finished\r\n");
+} // LEDsAndBuzzer
+
+//______________________________________________________________________________________________
+
+// LED Driver
+
+void PCA9551Test(void) {
+ static char b[8], i, r;
+
+ TxString("\r\nPCA9551Test\r\n");
+
+ I2CLED.start();
+ I2CGYRO.write(PCA9551_ID);
+ I2CGYRO.write(0x11);
+ I2CLED.stop();
+
+ I2CLED.read(PCA9551_ID, b, 7);
+
+ TxString("0:\t0b");
+ TxBin8(b[6]);
+ TxNextLine();
+ for (i = 0; i <6; i++ ) {
+ TxVal32(i+1, 0,0);
+ TxString(":\t0b");
+ TxBin8(b[i]);
+ TxNextLine();
+ }
+ TxNextLine();
+} // PCA9551Test
+
+boolean IsPCA9551Active(void) {
+
+ const char b[7] = {0x11,0x25,0x80,0x25,0x80,0x00,0x00}; // Period 1Sec., PWM 50%, ON
+
+ F.UsingLEDDriver = true; // optimistic
+
+ I2CLED.start();
+ if ( I2CGYRO.write(PCA9551_ID) != I2C_ACK ) goto LError;
+ I2CLED.stop();
+
+ I2CLED.write(PCA9551_ID, b, 7);
+
+ TrackMinI2CRate(400000);
+
+ return ( true );
+
+LError:
+ F.UsingLEDDriver = false;
+ return ( F.UsingLEDDriver );
+
+} //IsPCA9551Active
+
+void InitLEDs(void) {
+
+ boolean r;
+
+ r = IsPCA9551Active();
+
+ LEDShadow = SavedLEDs = LEDPattern = 0;
+ PrevLEDShadow = 0x0fff;
+
+ PrevPCA9551LEDShadow = 0;
+ WritePCA9551( PrevPCA9551LEDShadow );
+
+} // InitLEDs
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/math.c Fri Feb 18 22:28:05 2011 +0000
@@ -0,0 +1,118 @@
+// ===============================================================================================
+// = 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 =
+// ===============================================================================================
+
+// 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/
+
+#include "UAVXArm.h"
+
+int16 SRS16(int16, uint8);
+int32 SRS32(int32, uint8);
+real32 Make2Pi(real32);
+real32 MakePi(real32);
+int16 Table16(int16, const int16 *);
+
+real32 VDot(real32 v1[3], real32 v2[3]);
+void VCross(real32 VOut[3], real32 v1[3], real32 v2[3]);
+void VScale(real32 VOut[3], real32 v[3], real32 s);
+void VAdd(real32 VOut[3],real32 v1[3], real32 v2[3]);
+void VSub(real32 VOut[3],real32 v1[3], real32 v2[3]);
+
+int16 SRS16(int16 x, uint8 s) {
+ static i16u Temp;
+
+ if ( s == (uint8)8 ) {
+ Temp.i16 = x;
+ return( (int16) Temp.i1 );
+ } else
+ return((x<0) ? -((-x)>>s) : (x>>s));
+} // SRS16
+
+int32 SRS32(int32 x, uint8 s) {
+ static i32u Temp;
+
+ if ( s == (uint8)8 ) {
+ Temp.i32 = x;
+ return( (int32)Temp.i3_1 );
+ } else
+ return((x<0) ? -((-x)>>s) : (x>>s));
+} // SRS32
+
+real32 Make2Pi(real32 A) {
+ while ( A < 0 ) A += TWOPI;
+ while ( A >= TWOPI ) A -= TWOPI;
+ return( A );
+} // Make2Pi
+
+real32 MakePi(real32 A) {
+ while ( A < -PI ) A += TWOPI;
+ while ( A >= PI ) A -= TWOPI;
+ return( A );
+} // MakePi
+
+int16 Table16(int16 Val, const int16 *T) {
+ static uint8 Index,Offset;
+ static int16 Temp, Low, High;
+
+ Index = (uint8) (Val >> 4);
+ Offset = (uint8) (Val & 0x0f);
+ Low = T[Index];
+ High = T[++Index];
+ Temp = (High-Low) * Offset;
+
+ return( Low + SRS16(Temp, 4) );
+} // Table16
+
+
+real32 VDot(real32 v1[3], real32 v2[3]) {
+ static real32 op;
+ static uint8 i;
+
+ op = 0.0;
+ for ( i = 0; i < (uint8)3; i++ )
+ op += v1[i] * v2[i];
+
+ return op;
+} // VDot
+
+void VCross(real32 VOut[3], real32 v1[3], real32 v2[3]) {
+ VOut[0]= (v1[1] * v2[2]) - (v1[2] * v2[1]);
+ VOut[1]= (v1[2] * v2[0]) - (v1[0] * v2[2]);
+ VOut[2]= (v1[0] * v2[1]) - (v1[1] * v2[0]);
+} // VCross
+
+void VScale(real32 VOut[3], real32 v[3], real32 s) {
+ static uint8 i;
+
+ for ( i = 0; i < (uint8)3; i++ )
+ VOut[i] = v[i] * s;
+} // VScale
+
+void VAdd(real32 VOut[3],real32 v1[3], real32 v2[3]) {
+ static uint8 i;
+
+ for ( i = 0; i < (uint8)3; i++ )
+ VOut[i] = v1[i] + v2[i];
+} // VAdd
+
+void VSub(real32 VOut[3],real32 v1[3], real32 v2[3]) {
+ static uint8 i;
+
+ for ( i = 0; i < (uint8)3; i++ )
+ VOut[i] = v1[i] - v2[i];
+} // VSub
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Fri Feb 18 22:28:05 2011 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/e2ac27c8e93e
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/menu.c Fri Feb 18 22:28:05 2011 +0000
@@ -0,0 +1,525 @@
+// ===============================================================================================
+// = 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 =
+// ===============================================================================================
+
+// 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/
+
+#include "UAVXArm.h"
+
+void ShowPrompt(void);
+void ShowRxSetup(void);
+void ShowSetup(boolean);
+uint8 MakeUpper(uint8);
+void ProcessCommand(void);
+
+const uint8 SerHello[] = "UAVXArm "
+ " Copyright 2008 G.K. Egan & 2007 W. Mahringer\r\n"
+ "This is FREE SOFTWARE and comes with ABSOLUTELY NO WARRANTY "
+ "see http://www.gnu.org/licenses/!\r\n";
+const uint8 SerHelp[] = "\r\nCommands:\r\n"
+ "A..Attitude test\r\n"
+ "C..Compass test\r\n"
+ "D..Load default parameter set\r\n"
+ "G..GPS test\r\n"
+ "H..Barometer/Rangefinder test\r\n"
+ "I..I2C bus scan\r\n"
+ "K..Calibrate compass scan\r\n"
+// "M..Modify parameters\r\n"
+ "P..Rx test\r\n"
+ "S..Setup\r\n"
+ "T..All LEDs and buzzer test\r\n"
+ "V..Battery test\r\n"
+ "X..Flight stats\r\n"
+ "Y..Program YGE I2C ESC\r\n"
+ "1-8..Individual LED/buzzer test\r\n"; // last line must be in this form for UAVPSet
+
+const uint8 RxChMnem[] = "TAERG12";
+
+uint8 MakeUpper(uint8 ch) {
+ if ( ( ch >='a') && ( ch <='z' ) )
+ ch = (ch - 'a') + 'A';
+
+ return (ch);
+} // MakeUpper
+
+void ShowPrompt(void) {
+ TxString("\r\n>");
+} // ShowPrompt
+
+void ShowRxSetup(void) {
+ if ( F.UsingSerialPPM )
+ if ( PPMPosPolarity[P[TxRxType]] )
+ TxString("Serial PPM frame (Positive Polarity)");
+ else
+ TxString("Serial PPM frame (Negative Polarity)");
+ else
+ TxString("Odd Rx Channels PPM");
+} // ShowRxSetup
+
+void ShowSetup(boolean h) {
+ int8 i;
+
+ TxNextLine();
+ if ( h )
+ ParamSet = 1;
+
+ TxString(SerHello);
+
+ UpdateRTC();
+ i = 0;
+ while ( RTCString[i] != NULL )
+ TxChar(RTCString[i++]);
+ TxNextLine();
+
+ TxString("Clock: 92MHz LPC1768 (mbed)\r\n");
+
+ TxString("Aircraft: ");
+ switch ( UAVXAirframe ) {
+ case QuadAF:
+ TxString("QUADROCOPTER\r\n");
+ break;
+ case TriAF:
+ TxString("TRICOPTER\r\n");
+ break;
+ case VTAF:
+ TxString("VTCOPTER\r\n");
+ break;
+ case HeliAF:
+ TxString("HELICOPTER\r\n");
+ break;
+ case ElevAF:
+ TxString("FLYING WING\r\n");
+ break;
+ case AilAF:
+ TxString("AILERON\r\n");
+ break;
+ default:
+ TxString("UNKNOWN\r\n");
+ }
+
+ if ( F.CompassValid ) {
+ TxString("Compass: ");
+ ShowCompassType();
+ TxString(" ( offset ");
+ TxVal32((int16)P[CompassOffsetQtr] * 90,0,0);
+ TxString("deg. )\r\n");
+ }
+
+ TxString("Baro: ");
+ ShowBaroType();
+
+ TxString("Accelerometers: ");
+ ShowAccType();
+ TxNextLine();
+
+ TxString("Gyros: ");
+ ShowGyroType();
+ TxNextLine();
+
+ TxString("Motor ESCs: ");
+ switch ( P[ESCType] ) {
+ case ESCPPM:
+ TxString("PPM ");
+ break;
+ case ESCHolger:
+ TxString("Holger I2C {");
+ break;
+ case ESCX3D:
+ TxString("X-3D I2C {");
+ break;
+ case ESCYGEI2C:
+ TxString("YGE I2C {");
+ break;
+ }
+
+ if ( P[ESCType] != ESCPPM ) {
+ for ( i = 0; i < NoOfI2CESCOutputs; i++ )
+ if ( ESCI2CFail[i] )
+ TxString(" Fail");
+ else
+ TxString(" OK");
+ TxString(" }");
+ }
+ TxNextLine();
+
+ if ( F.HaveBatterySensor )
+ TxString("External Volt/Amp Sensor Fitted\r\n");
+#ifdef RX6CH
+ TxString("6 CHANNEL VERSION - 5 ACTIVE CHANNELS ONLY\r\n");
+#endif // RX6CH
+
+ TxString("Tx/Rx: ");
+
+ switch ( P[TxRxType] ) {
+ case FutabaCh3:
+ TxString("Futaba Th 3 {");
+ break;
+ case FutabaCh2:
+ TxString("Futaba Th 2 {");
+ break;
+ case FutabaDM8:
+ TxString("Futaba DM8 & AR7000 {");
+ break;
+ case JRPPM:
+ TxString("JR PPM {");
+ break;
+ case JRDM9:
+ TxString("JR DM9 & AR7000{");
+ break;
+ case JRDXS12:
+ TxString("JR DSX12 & AR7000 {");
+ break;
+ case DX7AR7000:
+ TxString("Spektrum DX7 & AR7000 {");
+ break;
+ case DX7AR6200:
+ TxString("Spektrum DX7 & AR6200 {");
+ break;
+ case CustomTxRx:
+ TxString("Custom {");
+ break;
+ case FutabaCh3_6_7:
+ TxString("Futaba Th 2 Swap 6&7 {");
+ break;
+ case DX7AR6000:
+ TxString("Spektrum DX7 & AR6000 {");
+ break;
+ case DX6iAR6200:
+ TxString("Spektrum DX6i & AR6200 {");
+ break;
+ case FutabaCh3_R617FS:
+ TxString("Futaba Th 3 & R617FS {");
+ break;
+ case GraupnerMX16s:
+ TxString("Graupner MX16s {");
+ break;
+ case DX7aAR7000:
+ TxString("Spektrum DX7a & AR7000 {");
+ break;
+ case FrSkyDJT_D8R:
+ TxString("FrSky DJT & D8R-SP Composite {");
+ break;
+ case ExternalDecoder:
+ TxString("External Decoder {");
+ break;
+ case UnknownTxRx:
+ TxString("UNKNOWN {");
+ break;
+ default:
+ ;
+ } // switch
+
+ if ( F.UsingSerialPPM )
+ ShowRxSetup();
+ else
+ if ( P[TxRxType] != UnknownTxRx ) {
+ for ( i = 0; i < RC_CONTROLS; i++)
+ TxChar(RxChMnem[RMap[i]]);
+
+ TxString("} connect {");
+
+ for ( i = 0; i < RC_CONTROLS; i+=2) {
+ TxChar(RxChMnem[RMap[i]]);
+ TxChar(' ');
+ }
+ }
+ TxChar('}');
+ if (( P[TxRxType] == DX7AR6200 ) || ( P[TxRxType] == DX6iAR6200))
+ TxString(" Mix Rudder to Aux1/Flaps ");
+ if ( F.UsingTxMode2 )
+ TxString(" Tx Mode 2");
+ else
+ TxString(" Tx Mode 1");
+ TxNextLine();
+
+ TxString("Selected parameter set: "); // must be exactly this string as UAVPSet expects it
+ TxChar('0' + ParamSet);
+ TxNextLine();
+
+#ifdef MULTICOPTER
+ TxString("Forward Flight: ");
+ TxVal32((int16)Orientation * 75L, 1, 0);
+ TxString("deg CW from K1 motor(s)\r\n");
+#endif // MULTICOPTER
+
+ if ( F.UsingAngleControl )
+ TxString("\tSticks control roll/pitch angle directly\r\n");
+ else
+ TxString("\tSticks control rate of change of roll/pitch angle\r\n");
+
+ if ( F.UsingRTHAutoDescend )
+ TxString("\tAuto descend ENABLED\r\n");
+ else
+ TxString("\tAuto descend disabled\r\n");
+
+ if ( F.AllowTurnToWP )
+ TxString("\tTurn toward Way Point\r\n");
+ else
+ TxString("\tHold heading\r\n");
+
+ if ( F.AllowNavAltitudeHold )
+ TxString("\tAllow Nav altitude hold\r\n");
+ else
+ TxString("\tWARNING - Manual Nav altitude hold\r\n");
+
+ if ( !F.SDCardValid )
+ TxString("\tSD Card NOT loaded - DISCONNECT power and load card if required.\r\n");
+
+ TxString("\r\nALARM (if any):\r\n");
+ if ( P[TxRxType] == UnknownTxRx )
+ TxString("\tTx/Rx TYPE not set\r\n");
+#ifdef TESTING
+ TxString("\tTEST VERSION - No Motors\r\n");
+#endif // TESTING
+
+ if ( !F.ParametersValid )
+ TxString("\tINVALID flight parameters (PID)!\r\n");
+
+ if ( !F.BaroAltitudeValid )
+ TxString("\tBarometer OFFLINE\r\n");
+ if ( BaroRetries >= BARO_INIT_RETRIES )
+ TxString("\tBaro Init: FAILED\r\n");
+
+ if ( !F.RangefinderAltitudeValid )
+ TxString("\tRangefinder OFFLINE\r\n");
+
+ if ( F.GyroFailure )
+ TxString("\tGyro FAILURE\r\n");
+
+ if ( !F.AccelerationsValid )
+ TxString("\tAccelerometers OFFLINE\r\n");
+
+ if ( !F.CompassValid )
+ TxString("\tCompass OFFLINE\r\n");
+
+ if ( !F.Signal )
+ TxString("\tBad EPAs or Tx switched off?\r\n");
+ if ( Armed && FirstPass )
+ TxString("\tUAVX is armed - DISARM!\r\n");
+
+ if ( F.Navigate || F.ReturnHome )
+ TxString("\tNavigate/RTH is selected - DESELECT!\r\n");
+
+ if ( InitialThrottle >= RC_THRES_START )
+ TxString("\tThrottle may be open - CLOSE!\r\n");
+
+ ShowPrompt();
+} // ShowSetup
+
+void ProcessCommand(void) {
+ static int8 p;
+ static uint8 ch;
+ static int8 d;
+
+ if ( 1 ) {//!Armed.read() ) {
+ ch = PollRxChar();
+ if ( ch != NUL ) {
+ ch = MakeUpper(ch);
+
+ switch ( ch ) {
+ case 'D':
+ UseDefaultParameters();
+ ShowPrompt();
+ break;
+ case 'L' : // List parameters
+ TxString("\r\nParameter list for set #"); // do not change (UAVPset!)
+ TxChar('0' + ParamSet);
+ ReadParameters();
+ for ( p = 0 ; p < MAX_PARAMETERS; p++ ) {
+ TxString("\r\nRegister ");
+ TxValU((uint8)(p+1));
+ TxString(" = ");
+ TxValS(P[p]);
+ }
+ ShowPrompt();
+ break;
+ case 'M' : // modify parameters
+ // no reprogramming in flight!!!!!!!!!!!!!!!
+ LEDBlue_ON;
+ TxString("\r\nRegister ");
+ p = (uint16)(RxNumU()-1);
+ // Attempts to block use of old versions of UAVPSet not compatible with UAVX
+ // assumes parameters are written sequentially from 0..(MAX_PARAMETERS-1)
+
+ TxString(" = ");
+ d = RxNumS();
+ if ( p < MAX_PARAMETERS ) {
+ // Keep RAM based set up to date.
+ if ( ParamSet == (uint8)1 ) {
+ WritePX(p, d);
+ if ( DefaultParams[p][1] )
+ WritePX(MAX_PARAMETERS + p, d);
+ } else {
+ if ( !DefaultParams[p][1] )
+ WritePX(MAX_PARAMETERS + p, d);
+ }
+ ParametersChanged = true;
+ }
+ if ( p < (MAX_PARAMETERS-1) )
+ F.ParametersValid = false;
+ else
+ if ( p == (MAX_PARAMETERS-1) ) {
+ WritePXImagefile();
+ F.ParametersValid = true; // ALL parameters must be written
+ }
+ LEDBlue_OFF;
+ ShowPrompt();
+ break;
+ case 'N' : // neutral values
+ TxString("\r\nNeutral R:");
+ TxValS(NewAccNeutral[BF]);
+
+ TxString(" P:");
+ TxValS(NewAccNeutral[LR]);
+
+ TxString(" V:");
+ TxValS(NewAccNeutral[UD]);
+ ShowPrompt();
+ break;
+ case 'Z' : // set Paramset
+ p = RxNumU();
+ if ( p != (int8)ParamSet ) {
+ ParamSet = p;
+ ParametersChanged = true;
+ ReadParameters();
+ }
+ break;
+ case 'W' : // comms with UAVXNav utility NOT UAVPSet
+ UAVXNavCommand();
+ //ShowPrompt();
+ break;
+ case 'R': // receiver values
+ TxString("\r\nT:");
+ TxValU(ToPercent(RC[ThrottleRC], RC_MAXIMUM));
+ TxString(",R:");
+ TxValS(ToPercent(((RC[RollRC]- RC_NEUTRAL) * 2L), RC_MAXIMUM));
+ TxString(",P:");
+ TxValS(ToPercent(((RC[PitchRC]- RC_NEUTRAL) * 2L), RC_MAXIMUM));
+ TxString(",Y:");
+ TxValS(ToPercent(((RC[YawRC]- RC_NEUTRAL) * 2L), RC_MAXIMUM));
+ TxString(",5:");
+ TxValU(ToPercent(RC[RTHRC], RC_MAXIMUM));
+ TxString(",6:");
+ TxValS(ToPercent(((RC[CamPitchRC] - RC_NEUTRAL) * 2L), RC_MAXIMUM));
+ TxString(",7:");
+ TxValU(ToPercent(RC[NavGainRC], RC_MAXIMUM));
+ ShowPrompt();
+ break;
+ case 'S' : // show status
+ ShowSetup(false);
+ break;
+ case 'X' : // flight stats
+ ShowStats();
+ ShowPrompt();
+ break;
+ case 'A' : // linear sensor
+ AttitudeTest();
+ ShowPrompt();
+ break;
+ case 'G': // GPS
+ GPSTest();
+ ShowPrompt();
+ break;
+ case 'H': // barometer
+ BaroTest();
+ ShowPrompt();
+ break;
+ case 'I':
+ TxString("\r\nI2C devices ...\r\n");
+ TxVal32(ScanI2CBus(),0,0);
+ TxString(" device(s) found\r\n");
+ ShowPrompt();
+ break;
+ case 'P' : // Receiver test
+ ReceiverTest();
+ ShowPrompt();
+ break;
+ case 'C' : // Compass test
+ DoCompassTest();
+ ShowPrompt();
+ break;
+
+ case 'K' : //Calibrate compass
+ CalibrateCompass();
+ ShowPrompt();
+ break;
+
+ case 'Y': // configure YGE30i EScs
+ ConfigureESCs();
+ ShowPrompt();
+ break;
+ case '1':
+ case '2':
+ case '3':
+ case '4':
+ case '5':
+ case '6':
+ case '7':
+ case '8':
+ TxString("\r\nOutput test\r\n");
+ TxChar(ch);
+ TxChar(':');
+ switch ( ch ) {
+ case '1':
+ TxString("Aux2");
+ break;
+ case '2':
+ TxString("Blue");
+ break;
+ case '3':
+ TxString("Red");
+ break;
+ case '4':
+ TxString("Green");
+ break;
+ case '5':
+ TxString("Aux1");
+ break;
+ case '6':
+ TxString("Yellow");
+ break;
+ case '7':
+ TxString("Aux3");
+ break;
+ case '8':
+ TxString("Beeper");
+ break;
+ }
+ TxNextLine();
+ PowerOutput(ch-'1');
+ ShowPrompt();
+ break;
+ case 'T':
+ LEDsAndBuzzer();
+ ShowPrompt();
+ break;
+
+ case 'V' : // Battery test
+ BatteryTest();
+ ShowPrompt();
+ break;
+ case '?' : // help
+ TxString(SerHelp);
+ ShowPrompt();
+ break;
+ default:
+ break;
+ }
+ }
+ }
+} // ProcessCommand
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/nonvolatile.c Fri Feb 18 22:28:05 2011 +0000
@@ -0,0 +1,221 @@
+// ===============================================================================================
+// = 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 =
+// ===============================================================================================
+
+// 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/
+
+#include "UAVXArm.h"
+
+// transfers to Flash seem to be ~35KByte/Sec
+
+void CheckSDCardValid(void);
+
+void CreateLogfile(void);
+void CloseLogfile(void);
+void TxLogChar(uint8);
+
+void WritePXImagefile(void);
+boolean ReadPXImagefile(void);
+
+int8 ReadPX(uint16);
+int16 Read16PX(uint16);
+int32 Read32PX(uint16);
+void WritePX(uint16, int8);
+void Write16PX(uint16, int16);
+void Write32PX(uint16, int32);
+
+FILE *pxfile = NULL;
+FILE *newpxfile = NULL;
+
+const int PX_LENGTH = 2048;
+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) {
+ static uint16 a;
+ static int32 CheckSum;
+ static int8 v, i;
+
+ UpdateRTC();
+
+ if ( F.PXImageStale ) {
+ if ( F.SDCardValid )
+ newpxfile = fopen("/SDCard/Params.txt", "w");
+ else
+ newpxfile = fopen("/local/Params.txt", "w");
+
+ if ( newpxfile != NULL ) {
+ CheckSum = 0;
+ for ( a = 0; a < PX_LENGTH; a++ ) {
+ v = PX[a];
+ CheckSum += (int32)v;
+ fprintf(newpxfile, "%i \r\n", v);
+ }
+ fprintf(newpxfile, "%li \r\n ", -CheckSum );
+
+ i=0;
+ while ( RTCString[i] != 0 )
+ fprintf(newpxfile, "%c", RTCString[i++]);
+ fprintf(newpxfile, "\r\n");
+
+ F.PXImageStale = false;
+ fclose(newpxfile);
+ }
+ }
+
+} // WritePXIMagefile
+
+boolean ReadPXImagefile(void) {
+ static uint16 a;
+ static int8 r;
+ static int32 v, CheckSum;
+ static boolean OK;
+
+ if ( F.SDCardValid )
+ pxfile = fopen("/SDCard/Params.txt", "r");
+ else
+ pxfile = fopen("/local/Params.txt", "r");
+
+
+ OK = false;
+ if ( pxfile != NULL ) {
+ CheckSum = 0;
+ a = 0;
+ for ( a = 0; a < PX_LENGTH ; a++ ) {
+ r = fscanf(pxfile, "%i ", &v);
+ CheckSum += v;
+ PXNew[a] = (int8)v;
+ }
+ r = fscanf(pxfile, "%li ", &v);
+ CheckSum += (int32)v;
+ OK = CheckSum == 0;
+
+ fclose(pxfile);
+
+ if ( OK ) {
+ for ( a = 0; a < PX_LENGTH; a++ )
+ PX[a] = PXNew[a];
+ F.PXImageStale = false;
+ }
+ }
+
+ return( OK );
+
+} // ReadPXImagefile
+
+
+void CreateLogfile(void) {
+ static uint8 i;
+
+ UpdateRTC();
+ if ( F.SDCardValid )
+ strftime(RTCLogfile, 32, "/SDCard/L%H-%M.log", RTCTime );
+ else
+ strftime(RTCLogfile, 32, "/local/L%H-%M.log", RTCTime );
+
+ logfile = fopen(RTCLogfile, "w");
+
+ LogfileIsOpen = logfile != NULL;
+ if ( LogfileIsOpen ) {
+ i=0;
+ while ( RTCString[i] != 0 )
+ TxLogChar(RTCString[i++]);
+ TxLogChar(CR);
+ TxLogChar(LF);
+ }
+ LogChars = 0;
+
+} // CreateLogfile
+
+void CloseLogfile(void) {
+ fclose(logfile);
+} // CloseLog
+
+void TxLogChar(uint8 ch) {
+
+#ifndef SUPPRESS_SDCARD
+ if ( LogfileIsOpen )
+ fprintf(logfile, "%c", ch);
+#endif // !SUPPRESS_SDCARD
+} // TxLogChar
+
+
+int8 ReadPX(uint16 a) {
+ static int8 b;
+ b = PX[a];
+ return(b);
+} // ReadPX
+
+int16 Read16PX(uint16 a) {
+ static i16u Temp16;
+
+ Temp16.b0 = ReadPX(a);
+ Temp16.b1 = ReadPX(a + 1);
+
+ return ( Temp16.i16 );
+} // Read16P
+
+int32 Read32PX(uint16 a) {
+ static i32u Temp32;
+
+ Temp32.b0 = ReadPX(a);
+ Temp32.b1 = ReadPX(a + 1);
+ Temp32.b2 = ReadPX(a + 2);
+ Temp32.b3 = ReadPX(a + 3);
+
+ return ( Temp32.i32 );
+} // Read32P
+
+void WritePX(uint16 a, int8 d) {
+ if ( PX[a] != d ) {
+ PX[a] = d;
+ F.PXImageStale = true;
+ }
+} // WritePX
+
+void Write16PX(uint16 a, int16 d) {
+ static i16u Temp16;
+
+ Temp16.i16 = d;
+ WritePX(a, Temp16.b0);
+ WritePX(a + 1, Temp16.b1);
+
+} // Write16P
+
+void Write32PX(uint16 a, int32 d) {
+ static i32u Temp32;
+
+ Temp32.i32 = d;
+ WritePX(a, Temp32.b0);
+ WritePX(a + 1, Temp32.b1);
+ WritePX(a + 2, Temp32.b2);
+ WritePX(a + 3, Temp32.b3);
+
+} // Write16PX
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/outputs.c Fri Feb 18 22:28:05 2011 +0000
@@ -0,0 +1,326 @@
+// ===============================================================================================
+// = 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 =
+// ===============================================================================================
+
+// 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/
+
+#include "UAVXArm.h"
+
+const real32 PWMScale = 1000.0 / OUT_MAXIMUM;
+
+void DoMulticopterMix(real32);
+void CheckDemand(real32);
+void MixAndLimitMotors(void);
+void MixAndLimitCam(void);
+void OutSignals(void);
+void InitI2CESCs(void);
+void StopMotors(void);
+void ExercisePWM(void);
+void InitMotors(void);
+
+boolean OutToggle;
+real32 PWM[8];
+real32 PWMSense[8];
+int16 ESCI2CFail[6];
+int16 CurrThrottle;
+int16 CamRollPulseWidth, CamPitchPulseWidth;
+int16 ESCMin, ESCMax;
+
+#define PWM_PERIOD_US (1000000/PWM_UPDATE_HZ)
+
+#ifdef MULTICOPTER
+
+uint8 TC(int16 T) {
+ return ( Limit(T, ESCMin, ESCMax) );
+} // TC
+
+void DoMulticopterMix(real32 CurrThrottle) {
+ static real32 Temp;
+
+#ifdef Y6COPTER
+ PWM[FrontTC] = PWM[LeftTC] = PWM[RightTC] = CurrThrottle;
+#else
+ PWM[FrontC] = PWM[LeftC] = PWM[RightC] = PWM[BackC] = CurrThrottle;
+#endif
+
+#ifdef TRICOPTER // usually flown K1 motor to the rear - use orientation of 24
+ Temp = Pl * 0.5;
+ PWM[FrontC] -= Pl; // front motor
+ PWM[LeftC] += (Temp - Rl); // right rear
+ PWM[RightC] += (Temp + Rl); // left rear
+
+ PWM[BackC] = -PWMSense[RudderC] * Yl + OUT_NEUTRAL; // yaw servo
+ if ( fabs(K[Balance]) > 0.5 )
+ PWM[FrontC] = PWM[FrontC] * K[Balance];
+#else
+#ifdef VTCOPTER // usually flown VTail (K1+K4) to the rear - use orientation of 24
+ Temp = Pl * 0.5;
+
+ PWM[LeftC] += (Temp - Rl); // right rear
+ PWM[RightC] += (Temp + Rl); // left rear
+
+ PWM[FrontLeftC] -= Pl - PWMSense[RudderC] * Yl;
+ PWM[FrontRightC] -= Pl + PWMSense[RudderC] * Yl;
+ if ( fabs(K[Balance]) > 0.01 ) {
+ PWM[FrontLeftC] = PWM[FrontLeftC] * K[Balance];
+ PWM[FrontRightC] = PWM[FrontRightC] * K[Balance];
+ }
+#else
+#ifdef Y6COPTER
+
+ Temp = Pl * 0.5;
+ PWM[FrontTC] -= Pl; // front motor
+ PWM[LeftTC] += (Temp - Rl); // right rear
+ PWM[RightTC] += (Temp + Rl); // left rear
+
+ PWM[FrontBC] = PWM[FrontTC];
+ PWM[LeftBC] = PWM[LeftTC];
+ PWM[RightBC] = PWM[RightTC];
+
+ if ( fabs(K[Balance]) > 0.01 ) {
+ PWM[FrontTC] = PWM[FrontTC] * K[Balance];
+ PWM[FrontBC] = PWM[FrontTC];
+ }
+
+ Temp = Yl * 0.5;
+ PWM[FrontTC] -= Temp;
+ PWM[LeftTC] -= Temp;
+ PWM[RightTC] -= Temp;
+
+ PWM[FrontBC] += Temp;
+ PWM[LeftBC] += Temp;
+ PWM[RightBC] += Temp;
+
+#else
+ PWM[LeftC] += -Rl + Yl;
+ PWM[RightC] += Rl + Yl;
+ PWM[FrontC] += -Pl - Yl;
+ PWM[BackC] += Pl - Yl;
+#endif
+#endif
+#endif
+
+} // DoMulticopterMix
+
+boolean MotorDemandRescale;
+
+void CheckDemand(real32 CurrThrottle) {
+ static real32 Scale, ScaleHigh, ScaleLow, MaxMotor, DemandSwing;
+
+#ifdef Y6COPTER
+ MaxMotor = Max(PWM[FrontTC], PWM[LeftTC]);
+ MaxMotor = Max(MaxMotor, PWM[RightTC]);
+ MaxMotor = Max(MaxMotor, PWM[FrontBC]);
+ MaxMotor = Max(MaxMotor, PWM[LeftBC]);
+ MaxMotor = Max(MaxMotor, PWM[RightBC]);
+#else
+ MaxMotor = Max(PWM[FrontC], PWM[LeftC]);
+ MaxMotor = Max(MaxMotor, PWM[RightC]);
+#ifndef TRICOPTER
+ MaxMotor = Max(MaxMotor, PWM[BackC]);
+#endif // TRICOPTER
+#endif // Y6COPTER
+
+ DemandSwing = MaxMotor - CurrThrottle;
+
+ if ( DemandSwing > 0.0 ) {
+ ScaleHigh = (OUT_MAXIMUM - CurrThrottle) / DemandSwing;
+ ScaleLow = (CurrThrottle - IdleThrottle) / DemandSwing;
+ Scale = Min(ScaleHigh, ScaleLow); // just in case!
+ if ( Scale < 0.0 ) Scale = 1.0 / OUT_MAXIMUM;
+ if ( Scale < 1.0 ) {
+ MotorDemandRescale = true;
+ Rl *= Scale; // could get rid of the divides
+ Pl *= Scale;
+#ifndef TRICOPTER
+ Yl *= Scale;
+#endif // TRICOPTER
+ } else
+ MotorDemandRescale = false;
+ } else
+ MotorDemandRescale = false;
+
+} // CheckDemand
+
+#endif // MULTICOPTER
+
+void MixAndLimitMotors(void) {
+ static real32 Temp, TempElevon, TempElevator;
+ static uint8 m;
+
+ if ( DesiredThrottle < IdleThrottle )
+ CurrThrottle = 0;
+ else
+ CurrThrottle = DesiredThrottle;
+
+#ifdef MULTICOPTER
+ if ( State == InFlight )
+ CurrThrottle += (-Comp[UD] + Comp[Alt]); // vertical compensation not optional
+
+ Temp = OUT_MAXIMUM * 0.9; // 10% headroom for control
+ CurrThrottle = Limit(CurrThrottle, 0, Temp );
+
+ if ( CurrThrottle > IdleThrottle ) {
+ DoMulticopterMix(CurrThrottle);
+
+ CheckDemand(CurrThrottle);
+
+ if ( MotorDemandRescale )
+ DoMulticopterMix(CurrThrottle);
+ } else {
+#ifdef Y6COPTER
+ for ( m = 0; m < (uint8)6; m++ )
+ PWM[m] = CurrThrottle;
+#else
+ PWM[FrontC] = PWM[LeftC] = PWM[RightC] = CurrThrottle;
+#ifdef TRICOPTER
+ PWM[BackC] = -PWMSense[RudderC] * Yl + OUT_NEUTRAL; // yaw servo
+#else
+ PWM[BackC] = CurrThrottle;
+#endif // !TRICOPTER
+#endif // Y6COPTER
+ }
+#else
+ CurrThrottle += Comp[Alt]; // simple - faster to climb with no elevator yet
+
+ PWM[ThrottleC] = CurrThrottle;
+ PWM[RudderC] = -PWMSense[RudderC] * Yl + OUT_NEUTRAL;
+
+#if ( defined AILERON | defined HELICOPTER )
+ PWM[AileronC] = PWMSense[AileronC] * Rl + OUT_NEUTRAL;
+ PWM[ElevatorC] = PWMSense[ElevatorC] * Pl + OUT_NEUTRAL;
+#else // ELEVON
+ TempElevator = PWMSense[2] * Pl + OUT_NEUTRAL;
+ PWM[RightElevonC] = PWMSense[RightElevonC] * (TempElevator + Rl);
+ PWM[LeftElevonC] = PWMSense[LeftElevonC] * (TempElevator - Rl);
+#endif
+#endif
+
+} // MixAndLimitMotors
+
+void MixAndLimitCam(void) {
+ static real32 Temp;
+
+ // use only roll/pitch angle estimates
+ Temp = Angle[Roll] * K[CamRollKp];
+ PWM[CamRollC] = Temp + K[CamRollTrim];
+ PWM[CamRollC] = PWM[CamRollC] + OUT_NEUTRAL; // PWMSense[CamRollC] *
+
+ Temp = Angle[Pitch] * K[CamPitchKp];
+ PWM[CamPitchC] = Temp + DesiredCamPitchTrim;
+ PWM[CamPitchC] = PWM[CamPitchC] + OUT_NEUTRAL; //PWMSense[CamPitchC] *
+
+ CamRollPulseWidth = 1000 + (int16)( PWM[CamRollC] * PWMScale );
+ CamPitchPulseWidth = 1000 + (int16)( PWM[CamPitchC] * PWMScale );
+
+} // MixAndLimitCam
+
+#if ( defined Y6COPTER )
+#include "outputs_y6.h"
+#else
+#if ( defined TRICOPTER | defined MULTICOPTER | defined VTCOPTER )
+#include "outputs_copter.h"
+#else
+#include "outputs_conventional.h"
+#endif // Y6COPTER
+#endif // TRICOPTER | MULTICOPTER
+
+void InitI2CESCs(void) {
+ static int8 m;
+ static uint8 r;
+
+#ifdef MULTICOPTER
+
+ if ( P[ESCType] == ESCHolger )
+ for ( m = 0 ; m < NoOfI2CESCOutputs ; m++ ) {
+ I2CESC.start();
+ r = I2CESC.write(0x52 + ( m*2 )); // one cmd, one data byte per motor
+ r += I2CESC.write(0);
+ ESCI2CFail[m] += r;
+ I2CESC.stop();
+ }
+ else
+ if ( P[ESCType] == ESCYGEI2C )
+ for ( m = 0 ; m < NoOfI2CESCOutputs ; m++ ) {
+ I2CESC.start();
+ r = I2CESC.write(0x62 + ( m*2 )); // one cmd, one data byte per motor
+ r += I2CESC.write(0);
+ ESCI2CFail[m] += r;
+ I2CESC.stop();
+ }
+ else
+ if ( P[ESCType] == ESCX3D ) {
+ I2CESC.start();
+ r = I2CESC.write(0x10); // one command, 4 data bytes
+ r += I2CESC.write(0);
+ r += I2CESC.write(0);
+ r += I2CESC.write(0);
+ r += I2CESC.write(0);
+ ESCI2CFail[0] += r;
+ I2CESC.stop();
+ }
+#endif // MULTICOPTER
+} // InitI2CESCs
+
+void StopMotors(void) {
+#ifdef MULTICOPTER
+#ifdef Y6COPTER
+ PWM[FrontTC] = PWM[LeftTC] = PWM[RightTC] =
+ PWM[FrontBC] = PWM[LeftBC] = PWM[RightBC] = ESCMin;
+#else
+ PWM[FrontC] = PWM[LeftC] = PWM[RightC] = ESCMin;
+#ifndef TRICOPTER
+ PWM[BackC] = ESCMin;
+#endif // !TRICOPTER
+#endif // Y6COPTER
+#else
+ PWM[ThrottleC] = ESCMin;
+#endif // MULTICOPTER
+
+ Out0.pulsewidth_us(1000 + (int16)( PWM[FrontC] * PWMScale ) );
+ Out1.pulsewidth_us(1000 + (int16)( PWM[LeftC] * PWMScale ) );
+ Out2.pulsewidth_us(1000 + (int16)( PWM[RightC] * PWMScale ) );
+ Out3.pulsewidth_us(1000 + (int16)( PWM[BackC] * PWMScale ) );
+
+ CamRollPulseWidth = 1000 + (int16)( PWM[CamRollC] * PWMScale );
+ CamPitchPulseWidth = 1000 + (int16)( PWM[CamPitchC] * PWMScale );
+
+#ifndef SOFTWARE_CAM_PWM
+ Out4.pulsewidth_us(CamRollPulseWidth);
+ Out5.pulsewidth_us(CamPitchPulseWidth);
+#endif // !SOFTWARE_CAM_PWM
+
+ F.MotorsArmed = false;
+} // StopMotors
+
+void InitMotors(void) {
+ static uint8 m;
+
+ Out0.period_us(PWM_PERIOD_US);
+
+ StopMotors();
+
+#ifndef Y6COPTER
+#ifdef TRICOPTER
+ PWM[BackC] = OUT_NEUTRAL;
+#endif // !TRICOPTER
+ PWM[CamRollC] = OUT_NEUTRAL;
+ PWM[CamPitchC] = OUT_NEUTRAL;
+#endif // Y6COPTER
+
+} // InitMotors
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/outputs_conventional.h Fri Feb 18 22:28:05 2011 +0000
@@ -0,0 +1,54 @@
+// ===============================================================================================
+// = 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 =
+// ===============================================================================================
+
+// 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/
+
+
+void OutSignals(void) {
+ static int8 m;
+ static uint8 r;
+
+ for ( m = 0; m < (uint8)6; m++ )
+ PWM[m] = Limit(PWM[m], ESCMin, ESCMax);
+
+#if !( defined SIMULATE | defined TESTING )
+
+ if ( !F.MotorsArmed )
+ StopMotors();
+
+#ifdef ELEVON
+ Out0.pulsewidth_us(1000 + (int16)( PWM[ThrottleC] * PWMScale ) );
+ Out1.pulsewidth_us(1000 + (int16)( PWM[RightElevonC] * PWMScale ) );
+ Out2.pulsewidth_us(1000 + (int16)( PWM[LeftElevonC] * PWMScale ) );
+ Out3.pulsewidth_us(1000 + (int16)( PWM[RudderC] * PWMScale ) );
+#else
+ Out0.pulsewidth_us(1000 + (int16)( PWM[ThrottleC] * PWMScale ) );
+ Out1.pulsewidth_us(1000 + (int16)( PWM[AileronC] * PWMScale ) );
+ Out2.pulsewidth_us(1000 + (int16)( PWM[ElevatorC] * PWMScale ) );
+ Out3.pulsewidth_us(1000 + (int16)( PWM[RudderC] * PWMScale ) );
+#endif
+
+} // OutSignals
+
+
+
+
+
+
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/outputs_copter.h Fri Feb 18 22:28:05 2011 +0000
@@ -0,0 +1,99 @@
+// ===============================================================================================
+// = 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 =
+// ===============================================================================================
+
+// 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/
+
+
+void OutSignals(void) {
+ static int8 m;
+ static uint8 r;
+
+#if !( defined SIMULATE | defined TESTING )
+
+ if ( !F.MotorsArmed )
+ StopMotors();
+
+ PWM[FrontC] = TC(PWM[FrontC]);
+ PWM[LeftC] = TC(PWM[LeftC]);
+ PWM[RightC] = TC(PWM[RightC]);
+#ifdef TRICOPTER
+ PWM[BackC] = Limit(PWM[BackC], 1, OUT_MAXIMUM);
+#else
+ PWM[BackC] = TC(PWM[BackC]);
+#endif
+
+ Out0.pulsewidth_us(1000 + (int16)( PWM[FrontC] * PWMScale ) );
+ Out1.pulsewidth_us(1000 + (int16)( PWM[LeftC] * PWMScale ) );
+ Out2.pulsewidth_us(1000 + (int16)( PWM[RightC] * PWMScale ) );
+ Out3.pulsewidth_us(1000 + (int16)( PWM[BackC] * PWMScale ) );
+
+#ifdef MULTICOPTER
+ // in X3D and Holger-Mode, K2 (left motor) is SDA, K3 (right) is SCL.
+ // ACK (r) not checked as no recovery is possible.
+ // Octocopters may have ESCs paired with common address so ACK is meaningless.
+ // All motors driven with fourth motor ignored for Tricopter.
+
+ if ( P[ESCType] == ESCHolger )
+ for ( m = 0 ; m < NoOfI2CESCOutputs ; m++ ) {
+ I2CESC.start();
+ r = I2CESC.write(0x52 + ( m*2 )); // one command, one data byte per motor
+ r += I2CESC.write( PWM[m] );
+ ESCI2CFail[m] += r;
+ I2CESC.stop();
+ }
+ else
+ if ( P[ESCType] == ESCYGEI2C )
+ for ( m = 0 ; m < NoOfI2CESCOutputs ; m++ ) {
+ I2CESC.start();
+ r = I2CESC.write(0x62 + ( m*2) ); // one cmd, one data byte per motor
+ r += I2CESC.write( PWM[m] * 0.5 );
+ ESCI2CFail[m] += r;
+ I2CESC.stop();
+ }
+ else
+ if ( P[ESCType] == ESCX3D ) {
+ I2CESC.start();
+ r = I2CESC.write(0x10); // one command, 4 data bytes
+ r += I2CESC.write( PWM[FrontC] );
+ r += I2CESC.write( PWM[BackC] );
+ r += I2CESC.write( PWM[LeftC] );
+ r += I2CESC.write( PWM[RightC] );
+ ESCI2CFail[0] += r;
+ // other ESCs if a Hexacopter
+ I2CESC.stop();
+ }
+#endif // MULTICOPTER
+
+#else
+
+ PWM[FrontC] = Limit(PWM[FrontC], ESCMin, ESCMax);
+ PWM[LeftC] = Limit(PWM[LeftC], ESCMin, ESCMax);
+ PWM[RightC] = Limit(PWM[RightC], ESCMin, ESCMax);
+#ifdef TRICOPTER
+ PWM[BackC] = Limit(PWM[BackC], 1, OUT_MAXIMUM);
+#else
+ PWM[BackC] = Limit(PWM[BackC], ESCMin, ESCMax);
+#endif
+
+#endif // !(SIMULATE | TESTING)
+
+} // OutSignals
+
+
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/outputs_y6.h Fri Feb 18 22:28:05 2011 +0000
@@ -0,0 +1,60 @@
+// ===============================================================================================
+// = 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 =
+// ===============================================================================================
+
+// 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/
+
+
+void OutSignals(void) { // The PWM pulses are in two parts these being a 1mS preamble followed by a 0-1mS part.
+ // Interrupts are enabled during the first part which uses TMR0. TMR0 is monitored until
+ // there is just sufficient time for one remaining interrupt latency before disabling
+ // interrupts. We do this because there appears to be no atomic method of detecting the
+ // remaining time AND conditionally disabling the interupt.
+ static int8 m;
+ static uint8 r, s;
+ static i16u SaveTimer0;
+ static uint24 SaveClockmS;
+
+ PWM[FrontTC] = TC(PWM[FrontTC]);
+ PWM[LeftTC] = TC(PWM[LeftTC]);
+ PWM[RightTC] = TC(PWM[RightTC]);
+ PWM[FrontBC] = TC(PWM[FrontBC]);
+ PWM[LeftBC] = TC(PWM[LeftBC]);
+ PWM[RightBC] = TC(PWM[RightBC]);
+
+#if !( defined SIMULATE | defined TESTING )
+
+ if ( !F.MotorsArmed )
+ StopMotors();
+
+ Out0.pulsewidth_us(1000 + (int16)( PWM[FrontTC] * PWMScale ) );
+ Out1.pulsewidth_us(1000 + (int16)( PWM[LeftTC] * PWMScale ) );
+ Out2.pulsewidth_us(1000 + (int16)( PWM[RightTC] * PWMScale ) );
+ Out3.pulsewidth_us(1000 + (int16)( PWM[FrontBC] * PWMScale ) );
+
+ #ifdef USING_PWM4AND5
+ Out4.pulsewidth_us(1000 + (int16)( PWM[LeftBC] * PWMScale ) );
+ Out5.pulsewidth_us(1000 + (int16)( PWM[RightBC] * PWMScale ) );
+ #endif // USING_PWM4AND5
+
+#endif // !(SIMULATE | TESTING)
+
+} // OutSignals
+
+
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/params.c Fri Feb 18 22:28:05 2011 +0000
@@ -0,0 +1,335 @@
+// ===============================================================================================
+// = 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 =
+// ===============================================================================================
+
+// 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/
+
+#include "UAVXArm.h"
+
+void ReadParameters(void);
+void UseDefaultParameters(void);
+void UpdateWhichParamSet(void);
+boolean ParameterSanityCheck(void);
+void InitParameters(void);
+
+const uint8 ESCLimits [] = { OUT_MAXIMUM, OUT_HOLGER_MAXIMUM, OUT_X3D_MAXIMUM, OUT_YGEI2C_MAXIMUM };
+
+#ifdef MULTICOPTER
+#include "uavx_multicopter.h"
+#else
+#ifdef HELICOPTER
+#include "uavx_helicopter.h"
+#else
+#ifdef ELEVONS
+#include "uavx_elevon.h"
+#else
+#include "uavx_aileron.h"
+#endif
+#endif
+#endif
+
+uint8 ParamSet;
+boolean ParametersChanged, SaveAllowTurnToWP;
+
+int8 P[MAX_PARAMETERS];
+real32 K[MAX_PARAMETERS]; // Arm rescaled legacy parameters as appropriate
+
+real32 OSin[48], OCos[48];
+
+uint8 Orientation , PolarOrientation;
+uint8 UAVXAirframe;
+
+void Legacy(void) {
+ static uint8 p;
+
+ for ( p = 0; p <MAX_PARAMETERS; p++ ) // brute force
+ K[p] = (float)P[p];
+
+ // Rate Control
+ K[RollKp] *= 2.6;
+ K[RollKi] *= 20.7;
+ K[RollKd] = -K[RollKd] * 0.021;
+ K[RollIntLimit] *= DEGRAD;
+
+ K[PitchKp] *= 2.6;
+ K[PitchKi] *= 20.7;
+ K[PitchKd] = -K[PitchKd] * 0.021;
+ K[PitchIntLimit] *= DEGRAD;
+
+ K[YawKp] *= 2.6;
+ K[YawKi] *= 41.4;
+ K[YawKd] = -K[YawKd] * 0.0004;
+
+ // 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;
+
+ // Altitude Hold
+ K[AltKp] *= 0.625;
+ K[AltKi] *= 25.0;
+ K[AltKd] *= 0.125;
+
+ // Navigation
+ K[NavKi] *= 0.08;
+ K[NavKd] *= 0.0008;
+
+ K[Balance] = ( 128.0 + (float)P[Balance])/128.0;
+
+ K[CompassKp] = P[CompassKp] / 4096.0;
+ K[YawIntLimit] = P[YawIntLimit] * 256.0 /1000.0;
+
+ // Camera
+ K[CamRollKp] *= 5.0;
+ K[CamPitchKp] *= 5.0;
+
+ // Acceleration Neutrals
+ K[MiddleBF] = P[MiddleBF] * 0.001; // mG
+ K[MiddleLR] = P[MiddleLR] * 0.001;
+ K[MiddleUD] = P[MiddleUD] * 0.001;
+
+ K[LowVoltThres] *= 0.2;
+
+} // Legacy
+
+void ReadParameters(void) {
+ static int8 i, b, a;
+
+ if ( ParametersChanged ) { // overkill if only a single parameter has changed but is not in flight loop
+ a = (ParamSet - 1)* MAX_PARAMETERS;
+ for ( i = 0; i < MAX_PARAMETERS; i++)
+ P[i] = ReadPX(a + i);
+
+ Legacy();
+
+ ESCMax = ESCLimits[P[ESCType]];
+ if ( P[ESCType] == ESCPPM )
+ ESCMin = 1;
+ else {
+ ESCMin = 0;
+ for ( i = 0; i < NoOfI2CESCOutputs; i++ )
+ ESCI2CFail[i] = 0;
+ InitI2CESCs();
+ }
+
+ b = P[ServoSense];
+ for ( i = 0; i < 8; i++ ) {
+ if ( b & 1 )
+ PWMSense[i] = -1;
+ else
+ PWMSense[i] = 1;
+ b >>=1;
+ }
+
+ F.UsingPositionHoldLock = ( (P[ConfigBits] & UsePositionHoldLockMask ) != 0);
+ F.UsingPolarCoordinates = ( (P[ConfigBits] & UsePolarMask ) != 0);
+
+ for ( i = 0; i < CONTROLS; i++) // make reverse map
+ RMap[Map[P[TxRxType]][i]] = i;
+
+ IdleThrottle = Limit((int16)P[PercentIdleThr], 10, 30); // 10-30%
+ IdleThrottle = (IdleThrottle * OUT_MAXIMUM )/100L;
+ CruiseThrottle = ((int16)P[PercentCruiseThr] * OUT_MAXIMUM )/100L;
+ MaxCruiseThrottle = (RC_MAXIMUM * 60L * OUT_MAXIMUM)/100L; // 60%
+
+ NavNeutralRadius = Limit((int16)P[NeutralRadius], 0, NAV_MAX_NEUTRAL_RADIUS);
+ NavNeutralRadius = ConvertMToGPS(NavNeutralRadius);
+
+ NavYCorrLimit = Limit((int16)P[NavYawLimit], 5, 50);
+
+ MagDeviation = (real32)P[NavMagVar] * DEGRAD;
+ CompassOffset = ((real32)((int16)P[CompassOffsetQtr] * 90L) * DEGRAD );
+ InitCompass();
+
+#ifdef MULTICOPTER
+ Orientation = P[Orient];
+ if (Orientation == 0xff ) // uninitialised
+ Orientation = 0;
+#else
+ Orientation = 0;
+#endif // MULTICOPTER
+
+ F.UsingSerialPPM = ( P[TxRxType] == FrSkyDJT_D8R ) || ( P[TxRxType] == ExternalDecoder ) || ( (P[ConfigBits] & RxSerialPPMMask ) != 0);
+ DoRxPolarity();
+ PPM_Index = PrevEdge = 0;
+
+ F.UsingPolar = ((P[ConfigBits] & UsePolarMask) != 0);
+ F.RFInInches = ((P[ConfigBits] & RFInchesMask) != 0);
+
+ F.UsingTxMode2 = ((P[ConfigBits] & TxMode2Mask) != 0);
+
+ if ( P[GyroRollPitchType] == IRSensors )
+ F.UsingAngleControl = true;
+ else
+ F.UsingAngleControl = ((P[ConfigBits] & UseAngleControlMask) != 0);
+
+ F.UsingRTHAutoDescend = ((P[ConfigBits] & UseRTHDescendMask) != 0);
+ NavRTHTimeoutmS = (uint24)P[DescentDelayS]*1000L;
+
+ BatteryVolts = K[LowVoltThres];
+ BatteryCurrent = 0;
+
+ F.ParametersValid = ParameterSanityCheck();
+
+ ParametersChanged = false;
+
+ }
+} // ReadParameters
+
+void UseDefaultParameters(void) { // loads a representative set of initial parameters as a base for tuning
+ int8 p, d;
+ static int16 a;
+
+ for ( a = 0; a < PX_LENGTH; a++ )
+ PX[a] = 0xff;
+
+ for ( p = 0; p < MAX_PARAMETERS; p++ ) {
+ d = DefaultParams[p][0];
+ WritePX(p, d);
+ WritePX(p+MAX_PARAMETERS, d);
+ }
+
+ WritePX(NAV_NO_WP, 0); // set NoOfWaypoints to zero
+
+ WritePXImagefile();
+
+ TxString("\r\nDefault Parameters Loaded\r\n");
+ TxString("Do a READ CONFIG to refresh the UAVPSet parameter display\r\n");
+} // UseDefaultParameters
+
+void UpdateParamSetChoice(void) {
+#define STICK_WINDOW 30
+
+ uint8 NewParamSet, NewAllowNavAltitudeHold, NewAllowTurnToWP;
+ int8 Selector;
+
+ NewParamSet = ParamSet;
+ NewAllowNavAltitudeHold = F.AllowNavAltitudeHold;
+ NewAllowTurnToWP = F.AllowTurnToWP;
+
+ if ( F.UsingTxMode2 )
+ Selector = DesiredRoll;
+ else
+ Selector = -DesiredYaw;
+
+ if ( (abs(DesiredPitch) > STICK_WINDOW) && (abs(Selector) > STICK_WINDOW) ) {
+ if ( DesiredPitch > STICK_WINDOW ) { // bottom
+ if ( Selector < -STICK_WINDOW ) // left
+ { // bottom left
+ NewParamSet = 1;
+ NewAllowNavAltitudeHold = true;
+ } else
+ if ( Selector > STICK_WINDOW ) // right
+ { // bottom right
+ NewParamSet = 2;
+ NewAllowNavAltitudeHold = true;
+ }
+ } else
+ if ( DesiredPitch < -STICK_WINDOW ) { // top
+ if ( Selector < -STICK_WINDOW ) { // left
+ NewAllowNavAltitudeHold = false;
+ NewParamSet = 1;
+ } else
+ if ( Selector > STICK_WINDOW ) { // right
+ NewAllowNavAltitudeHold = false;
+ NewParamSet = 2;
+ }
+ }
+
+ if ( ( NewParamSet != ParamSet ) || ( NewAllowNavAltitudeHold != F.AllowNavAltitudeHold ) ) {
+ ParamSet = NewParamSet;
+ F.AllowNavAltitudeHold = NewAllowNavAltitudeHold;
+ LEDBlue_ON;
+ DoBeep100mS(2, 2);
+ if ( ParamSet == (uint8)2 )
+ DoBeep100mS(2, 2);
+ if ( F.AllowNavAltitudeHold )
+ DoBeep100mS(4, 4);
+ ParametersChanged |= true;
+ Beeper_OFF;
+ LEDBlue_OFF;
+ }
+ }
+
+ if ( F.UsingTxMode2 )
+ Selector = -DesiredYaw;
+ else
+ Selector = DesiredRoll;
+
+ if ( (abs(RC[ThrottleC]) < STICK_WINDOW) && (abs(Selector) > STICK_WINDOW ) ) {
+ if ( Selector < -STICK_WINDOW ) // left
+ NewAllowTurnToWP = false;
+ else
+ if ( Selector > STICK_WINDOW ) // left
+ NewAllowTurnToWP = true; // right
+
+ if ( NewAllowTurnToWP != F.AllowTurnToWP ) {
+ F.AllowTurnToWP = NewAllowTurnToWP;
+ LEDBlue_ON;
+ // if ( F.AllowTurnToWP )
+ DoBeep100mS(4, 2);
+
+ LEDBlue_OFF;
+ }
+ }
+
+ SaveAllowTurnToWP = F.AllowTurnToWP;
+
+} // UpdateParamSetChoice
+
+boolean ParameterSanityCheck(void) {
+ static boolean Fail;
+
+ Fail = (P[RollKp] == 0) ||
+ (P[PitchKp] == 0) ||
+ (P[YawKp] == 0);
+
+ return ( !Fail );
+} // ParameterSanityCheck
+
+void InitParameters(void) {
+ static int8 i;
+ static real32 A;
+
+ F.ParametersValid = false;
+ if ( !ReadPXImagefile() )
+ UseDefaultParameters();
+
+ UAVXAirframe = AF_TYPE;
+
+ for (i = 0; i < 48; i++) {
+ A = ((real32)i * PI)/24.0;
+ OSin[i] = sin(A);
+ OCos[i] = cos(A);
+ }
+ Orientation = 0;
+
+ ALL_LEDS_ON;
+
+ ParametersChanged = true;
+ ParamSet = 1;
+ ReadParameters();
+
+ ALL_LEDS_OFF;
+} // InitParameters
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/rc.c Fri Feb 18 22:28:05 2011 +0000
@@ -0,0 +1,391 @@
+// ===============================================================================================
+// = 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 =
+// ===============================================================================================
+
+// 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/
+
+#include "UAVXArm.h"
+
+void DoRxPolarity(void);
+void InitRC(void);
+void MapRC(void);
+void CheckSticksHaveChanged(void);
+void UpdateControls(void);
+void CaptureTrims(void);
+void CheckThrottleMoved(void);
+void ReceiverTest(void);
+
+const uint8 Map[CustomTxRx+1][CONTROLS] = {
+ { 2,0,1,3,4,5,6 }, // Futaba Thr 3 Throttle
+ { 1,0,3,2,4,5,6 }, // Futaba Thr 2 Throttle
+ { 4,2,1,0,5,3,6 }, // Futaba 9C Spektrum DM8/AR7000
+ { 0,1,2,3,4,5,6 }, // JR XP8103/PPM
+ { 6,0,3,5,2,4,1 }, // JR 9XII Spektrum DM9 ?
+
+ { 5,0,3,6,2,1,4 }, // JR DXS12
+ { 5,0,3,6,2,1,4 }, // Spektrum DX7/AR7000
+ { 4,0,3,5,2,1,6 }, // Spektrum DX7/AR6200
+
+ { 2,0,1,3,4,6,5 }, // Futaba Thr 3 Sw 6/7
+ { 0,1,2,3,4,5,6 }, // Spektrum DX7/AR6000
+ { 0,1,2,3,4,5,6 }, // Graupner MX16S
+ { 4,0,2,3,1,5,6 }, // Spektrum DX6i/AR6200
+ { 2,0,1,3,4,5,6 }, // Futaba Th 3/R617FS
+ { 4,0,2,3,5,1,6 }, // Spektrum DX7a/AR7000
+ { 2,0,1,3,4,6,5 }, // External decoder (Futaba Thr 3 6/7 swap)
+ { 0,1,2,3,4,5,6 }, // FrSky DJT/D8R-SP
+ { 5,0,3,6,2,1,4 }, // UNDEFINED Spektrum DX7/AR7000
+ { 2,0,1,3,4,5,6 } // Custom
+//{ 4,0,2,1,3,5,6 } // Custom
+ };
+
+// Rx signalling polarity used only for serial PPM frames usually
+// by tapping internal Rx circuitry.
+const boolean PPMPosPolarity[CustomTxRx+1] =
+ {
+ false, // Futaba Ch3 Throttle
+ false, // Futaba Ch2 Throttle
+ true, // Futaba 9C Spektrum DM8/AR7000
+ true, // JR XP8103/PPM
+ true, // JR 9XII Spektrum DM9/AR7000
+
+ true, // JR DXS12
+ true, // Spektrum DX7/AR7000
+ true, // Spektrum DX7/AR6200
+ false, // Futaba Thr 3 Sw 6/7
+ true, // Spektrum DX7/AR6000
+ true, // Graupner MX16S
+ true, // Graupner DX6i/AR6200
+ true, // Futaba Thr 3/R617FS
+ true, // Spektrum DX7a/AR7000
+ false, // External decoder (Futaba Ch3 Throttle)
+ true, // FrSky DJT/D8R-SP
+ true, // UNKNOWN using Spektrum DX7/AR7000
+ true // custom Tx/Rx combination
+ };
+
+// Reference Internal Quadrocopter Channel Order
+// 0 Throttle
+// 1 Aileron
+// 2 Elevator
+// 3 Rudder
+// 4 Gear
+// 5 Aux1
+// 6 Aux2
+
+int8 RMap[CONTROLS];
+
+int16x8x4Q PPMQ;
+int16 PPMQSum[CONTROLS];
+int16 RC[CONTROLS], RCp[CONTROLS];
+int16 Trim[3];
+int16 ThrLow, ThrNeutral, ThrHigh;
+
+boolean RCPositiveEdge;
+
+void DoRxPolarity(void) {
+
+ RCInterrupt.rise(&RCNullISR);
+ RCInterrupt.fall(&RCNullISR);
+
+ if ( F.UsingSerialPPM ) // serial PPM frame from within an Rx
+ if ( PPMPosPolarity[P[TxRxType]] )
+ RCInterrupt.rise(&RCISR);
+ else
+ RCInterrupt.fall(&RCISR);
+ else {
+ RCInterrupt.rise(&RCISR);
+ RCInterrupt.fall(&RCISR);
+ }
+
+} // DoRxPolarity
+
+void InitRC(void) {
+ static int8 c, i, q;
+
+ DoRxPolarity();
+
+ SignalCount = -RC_GOOD_BUCKET_MAX;
+ F.Signal = F.RCNewValues = false;
+
+ for (c = 0; c < RC_CONTROLS; c++) {
+ PPM[c] = 0;
+ RC[c] = RCp[c] = RC_NEUTRAL;
+
+ for (q = 0; q <= PPMQMASK; q++)
+ PPMQ.B[q][c] = RC_NEUTRAL;
+ PPMQSum[c] = RC_NEUTRAL * 4;
+ }
+ PPMQ.Head = 0;
+
+ DesiredRoll = DesiredPitch = DesiredYaw = DesiredThrottle = StickThrottle = 0;
+
+ for ( i = 0; i < 3; i++ )
+ Trim[i] = 0;
+
+ PPM_Index = PrevEdge = RCGlitches = 0;
+} // InitRC
+
+void MapRC(void) { // re-maps captured PPM to Rx channel sequence
+ static int8 c;
+ static int16 LastThrottle, i;
+ static uint16 Sum;
+
+ LastThrottle = RC[ThrottleC];
+
+ for ( c = 0 ; c < RC_CONTROLS ; c++ ) {
+ Sum = PPM[c];
+ PPMQSum[c] -= PPMQ.B[PPMQ.Head][c];
+ PPMQ.B[PPMQ.Head][c] = Sum;
+ PPMQSum[c] += Sum;
+ PPMQ.Head = (PPMQ.Head + 1) & PPMQMASK;
+ }
+
+ for ( c = 0 ; c < RC_CONTROLS ; c++ ) {
+ i = Map[P[TxRxType]][c];
+ RC[c] = ( PPMQSum[i] * RC_MAXIMUM ) / 4000; // needs rethink - throwing away precision
+ }
+
+ if ( THROTTLE_SLEW_LIMIT > 0 )
+ RC[ThrottleRC] = SlewLimit(LastThrottle, RC[ThrottleRC], THROTTLE_SLEW_LIMIT);
+
+} // MapRC
+
+void CheckSticksHaveChanged(void) {
+ static boolean Change;
+ static uint8 c;
+
+ if ( F.ReturnHome || F.Navigate ) {
+ if ( mSClock() > mS[StickChangeUpdate] ) {
+ mS[StickChangeUpdate] = mSClock() + 500;
+ if ( !F.ForceFailsafe && ( State == InFlight )) {
+ Stats[RCFailsafesS]++;
+ mS[NavStateTimeout] = mSClock() + NAV_RTH_LAND_TIMEOUT_MS;
+ mS[DescentUpdate] = mSClock() + ALT_DESCENT_UPDATE_MS;
+ DescentComp = 0; // for no Baro case
+ }
+
+ F.ForceFailsafe = State == InFlight; // abort if not navigating
+ }
+ } else {
+ if ( mSClock() > mS[StickChangeUpdate] ) {
+ mS[StickChangeUpdate] = mSClock() + 500;
+
+ Change = false;
+ for ( c = ThrottleC; c <= (uint8)RTHC; c++ ) {
+ Change |= abs( RC[c] - RCp[c]) > RC_STICK_MOVEMENT;
+ RCp[c] = RC[c];
+ }
+ }
+
+ if ( Change ) {
+ mS[RxFailsafeTimeout] = mSClock() + RC_NO_CHANGE_TIMEOUT_MS;
+ mS[NavStateTimeout] = mSClock();
+ F.ForceFailsafe = false;
+ if ( FailState == MonitoringRx ) {
+ if ( F.LostModel ) {
+ Beeper_OFF;
+ F.LostModel = false;
+ DescentComp = 0;
+ }
+ }
+ } else
+ if ( mSClock() > mS[RxFailsafeTimeout] ) {
+ if ( !F.ForceFailsafe && ( ( State == InFlight ) || ( ( mSClock() - mS[RxFailsafeTimeout]) > 120000 ) ) ) {
+ Stats[RCFailsafesS]++;
+ mS[NavStateTimeout] = mSClock() + NAV_RTH_LAND_TIMEOUT_MS;
+ mS[DescentUpdate] = mSClock() + ALT_DESCENT_UPDATE_MS;
+ DescentComp = 0; // for no Baro case
+ F.ForceFailsafe = true;
+ }
+
+ // F.ForceFailsafe = State == InFlight; // abort if not navigating
+ }
+ }
+
+} // CheckSticksHaveChanged
+
+void UpdateControls(void) {
+ static int16 HoldRoll, HoldPitch, RollPitchScale;
+ static boolean NewCh5Active;
+
+ F.RCNewValues = false;
+
+ MapRC(); // remap channel order for specific Tx/Rx
+
+ StickThrottle = RC[ThrottleRC];
+
+ //_________________________________________________________________________________________
+
+ // Navigation
+
+ F.ReturnHome = F.Navigate = F.UsingPolar = false;
+ NewCh5Active = RC[RTHRC] > RC_NEUTRAL;
+
+ if ( F.UsingPositionHoldLock )
+ if ( NewCh5Active & !F.Ch5Active )
+ F.AllowTurnToWP = true;
+ else
+ F.AllowTurnToWP = SaveAllowTurnToWP;
+ else
+ if ( RC[RTHRC] > ((3L*RC_MAXIMUM)/4) )
+ F.ReturnHome = true;
+ else
+ if ( RC[RTHRC] > (RC_NEUTRAL/2) )
+ F.Navigate = true;
+
+ F.Ch5Active = NewCh5Active;
+
+#ifdef RX6CH
+ DesiredCamPitchTrim = RC_NEUTRAL;
+ // NavSensitivity set in ReadParametersPX
+#else
+ DesiredCamPitchTrim = RC[CamPitchRC] - RC_NEUTRAL;
+ NavSensitivity = RC[NavGainRC];
+ NavSensitivity = Limit(NavSensitivity, 0, RC_MAXIMUM);
+#endif // !RX6CH
+
+ //_________________________________________________________________________________________
+
+ // Altitude Hold
+
+ F.AltHoldEnabled = NavSensitivity > NAV_SENS_ALTHOLD_THRESHOLD;
+
+ if ( NavState == HoldingStation ) { // Manual
+ if ( StickThrottle < RC_THRES_STOP ) // to deal with usual non-zero EPA
+ StickThrottle = 0;
+ } else // Autonomous
+ if ( F.AllowNavAltitudeHold && F.AltHoldEnabled )
+ StickThrottle = CruiseThrottle;
+
+ if ( (! F.HoldingAlt) && (!(F.Navigate || F.ReturnHome )) ) // cancel any current altitude hold setting
+ DesiredAltitude = Altitude;
+
+ //_________________________________________________________________________________________
+
+ // Attitude
+
+#ifdef ATTITUDE_NO_LIMITS
+ DesiredRoll = RC[RollRC] - RC_NEUTRAL;
+ DesiredPitch = RC[PitchRC] - RC_NEUTRAL;
+#else
+ RollPitchScale = MAX_ROLL_PITCH - (NavSensitivity >> 2);
+ DesiredRoll = SRS16((RC[RollRC] - RC_NEUTRAL) * RollPitchScale, 7);
+ DesiredPitch = SRS16((RC[PitchRC] - RC_NEUTRAL) * RollPitchScale, 7);
+#endif // ATTITUDE_NO_LIMITS
+ DesiredYaw = RC[YawRC] - RC_NEUTRAL;
+
+ HoldRoll = DesiredRoll - Trim[Roll];
+ HoldRoll = abs(HoldRoll);
+ HoldPitch = DesiredPitch - Trim[Pitch];
+ HoldPitch = abs(HoldPitch);
+ CurrMaxRollPitch = Max(HoldRoll, HoldPitch);
+
+ if ( CurrMaxRollPitch > ATTITUDE_HOLD_LIMIT )
+ if ( AttitudeHoldResetCount > ATTITUDE_HOLD_RESET_INTERVAL )
+ F.AttitudeHold = false;
+ else {
+ AttitudeHoldResetCount++;
+ F.AttitudeHold = true;
+ }
+ else {
+ F.AttitudeHold = true;
+ if ( AttitudeHoldResetCount > 1 )
+ AttitudeHoldResetCount -= 2; // Faster decay
+ }
+
+ //_________________________________________________________________________________________
+
+ // Rx has gone to failsafe
+
+ //zzz CheckSticksHaveChanged();
+
+ F.NewCommands = true;
+
+} // UpdateControls
+
+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);
+
+ HoldYaw = 0;
+} // CaptureTrims
+
+void CheckThrottleMoved(void) {
+ if ( mSClock() < mS[ThrottleUpdate] )
+ ThrNeutral = DesiredThrottle;
+ else {
+ ThrLow = ThrNeutral - THROTTLE_MIDDLE;
+ ThrLow = Max(ThrLow, THROTTLE_MIN_ALT_HOLD);
+ ThrHigh = ThrNeutral + THROTTLE_MIDDLE;
+ if ( ( DesiredThrottle <= ThrLow ) || ( DesiredThrottle >= ThrHigh ) ) {
+ mS[ThrottleUpdate] = mSClock() + THROTTLE_UPDATE_MS;
+ F.ThrottleMoving = true;
+ } else
+ F.ThrottleMoving = false;
+ }
+} // CheckThrottleMoved
+
+void ReceiverTest(void) {
+ static int8 s;
+
+ TxString("\r\nRx Test \r\n");
+
+ TxString("\r\nRx: ");
+ ShowRxSetup();
+ TxString("\r\n");
+
+ TxString("RAW Rx frame values - neutrals NOT applied\r\n");
+ TxString("Channel order is: ");
+ for ( s = 0; s < RC_CONTROLS; s++)
+ TxChar(RxChMnem[RMap[s]]);
+
+ if ( F.Signal )
+ TxString("\r\nSignal OK ");
+ else
+ TxString("\r\nSignal FAIL ");
+ TxVal32(mSClock() - mS[LastValidRx], 0, 0);
+ TxString(" mS ago\r\n");
+
+ // Be wary as new RC frames are being received as this
+ // is being displayed so data may be from overlapping frames
+
+ for ( s = 0; s < RC_CONTROLS ; s++ ) {
+ TxChar(s+'1');
+ TxString(": ");
+ TxChar(RxChMnem[RMap[s]]);
+ TxString(":\t");
+
+ TxVal32((int32)PPM[s] + 1000, 3, 0);
+ TxChar(HT);
+ TxVal32(PPM[s] / 10, 0, '%');
+ if ( ( PPM[s] < -200 ) || ( PPM[s] > 1200 ) )
+ TxString(" FAIL");
+
+ TxNextLine();
+ }
+
+ TxString("Glitches:\t");
+ TxVal32(RCGlitches,0,0);
+ TxNextLine();
+
+} // ReceiverTest
+
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/sb_globals.cpp Fri Feb 18 22:28:05 2011 +0000 @@ -0,0 +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]; +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/sb_globals.h Fri Feb 18 22:28:05 2011 +0000 @@ -0,0 +1,62 @@ +/* + 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 SB_GLOBALS_G +#define SB_GLOBALS_G + +#define SERIALBUFFERED_BUFFER_SIZE 256 + +#define SET_REGISTER(reg, val) *(uint32_t *)(uart_base+reg)=val +#define GET_REGISTER(reg) *(uint32_t *)(uart_base+reg) + +#define SERIALBUFFERED_RBR 0x00 +#define SERIALBUFFERED_THR 0x00 +#define SERIALBUFFERED_DLL 0x00 +#define SERIALBUFFERED_IER 0x04 +#define SERIALBUFFERED_DML 0x04 +#define SERIALBUFFERED_IIR 0x08 +#define SERIALBUFFERED_FCR 0x08 +#define SERIALBUFFERED_LCR 0x0C +#define SERIALBUFFERED_LSR 0x14 +#define SERIALBUFFERED_SCR 0x1C +#define SERIALBUFFERED_ACR 0x20 +#define SERIALBUFFERED_ICR 0x24 +#define SERIALBUFFERED_FDR 0x28 +#define SERIALBUFFERED_TER 0x30 + +extern char *_tx_buffer[4]; +extern int _tx_buffer_size[4]; +extern int _tx_buffer_in[4]; +extern int _tx_buffer_out[4]; +extern bool _tx_buffer_full[4]; +extern bool _tx_buffer_overflow[4]; +extern bool _tx_buffer_used_malloc[4]; + +extern char *_rx_buffer[4]; +extern int _rx_buffer_size[4]; +extern int _rx_buffer_in[4]; +extern int _rx_buffer_out[4]; +extern bool _rx_buffer_full[4]; +extern bool _rx_buffer_overflow[4]; +extern bool _rx_buffer_used_malloc[4]; + +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/serial.c Fri Feb 18 22:28:05 2011 +0000
@@ -0,0 +1,279 @@
+// ===============================================================================================
+// = 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 =
+// ===============================================================================================
+
+// 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/
+
+#include "UAVXArm.h"
+
+// USART routines
+
+// Much of this legacy code to support UAVPset, the original GUI for UAVP.
+// It is to be replaced by packet based comms ASAP.
+
+
+void TxString(const uint8*);
+void TxChar(uint8);
+void TxValU(uint8);
+void TxValS(int8);
+void TxBin8(uint8);
+void TxNextLine(uint8);
+void TxNibble( uint8);
+void TxValH( uint8);
+void TxValH16( uint16);
+uint8 RxChar(void);
+uint8 PollRxChar(void);
+uint8 RxNumU(void);
+int8 RxNumS(void);
+void TxVal32(int32, int8, uint8);
+void TxChar( uint8);
+void TxESCu8( uint8);
+void Sendi16( int16);
+void TxESCi8( int8);
+void TxESCi16( int16);
+void TxESCi24( int24);
+void TxESCi32( int32);
+
+void TxString(const uint8 *pch) {
+ while ( *pch != '\0' )
+ TxChar(*pch++);
+} // TxString
+
+void TxChar(uint8 ch) {
+
+ TxCheckSum ^= ch;
+
+ while ( !TelemetrySerial.writeable() ) {};
+ TelemetrySerial.putc(ch);
+
+ if ( EchoToLogFile )
+ TxLogChar(ch);
+
+} // TxChar
+
+void TxValU(uint8 v) {
+ // UAVPSet requires 3 digits exactly ( 000 to 999 )
+ TxChar((v / 100) + '0');
+ v %= 100;
+
+ TxChar((v / 10) + '0');
+ v %= 10;
+
+ TxChar(v + '0');
+} // TxValU
+
+void TxValS(int8 v) {
+ // UAVPSet requires sign and 3 digits exactly (-999 to 999)
+ if ( v < 0 ) {
+ TxChar('-'); // send sign
+ v = -v;
+ } else
+ TxChar('+'); // send sign
+
+ TxValU(v);
+} // TxValS
+
+void TxNextLine(void) {
+ TxChar(CR);
+ TxChar(LF);
+} // TxNextLine
+
+void TxBin8(uint8 v) {
+
+ static uint8 i;
+
+ for (i = 0; i <8; i++) {
+ if ( v & 0x80 )
+ TxChar('1');
+ else TxChar('0');
+ v <<= 1;
+ }
+} // TxBin8
+
+void TxNibble(uint8 v) {
+ if ( v > (uint8)9)
+ TxChar('A' + v - 10);
+ else
+ TxChar('0' + v);
+} // TxNibble
+
+void TxValH(uint8 v) {
+ TxNibble(v >> 4);
+ TxNibble(v & 0x0f);
+} // TxValH
+
+void TxValH16(uint16 v) {
+ TxValH(v >> 8);
+ TxValH(v);
+} // TxValH16
+
+uint8 PollRxChar(void) {
+ uint8 ch;
+
+ if ( TelemetrySerial.readable() ) { // a character is waiting in the buffer
+ ch = TelemetrySerial.getc(); // get the character
+ TxChar(ch); // echo it for UAVPSet
+ return(ch); // and return it
+ }
+ return( NUL ); // nothing in buffer
+
+} // PollRxChar
+
+uint8 RxChar(void) {
+ uint8 ch;
+
+ while ( !TelemetrySerial.readable() ) {};
+
+ ch = TelemetrySerial.getc(); // get the character
+
+ return(ch);
+} // RxChar
+
+
+uint8 RxNumU(void) {
+ // UAVPSet sends 2 digits
+ uint8 ch;
+ uint8 n;
+
+ n = 0;
+ do
+ ch = PollRxChar();
+ while ( (ch < '0') || (ch > '9') );
+ n = (ch - '0') * 10;
+ do
+ ch = PollRxChar();
+ while ( (ch < '0') || (ch > '9') );
+ n += ch - '0';
+ return(n);
+} // RxNumU
+
+
+int8 RxNumS(void) {
+ // UAVPSet sends sign and 2 digits
+ uint8 ch;
+ int8 n;
+ boolean Neg;
+ n = 0;
+
+ Neg = false;
+ do
+ ch = PollRxChar();
+ while ( ((ch < '0') || (ch > '9')) &&
+ (ch != '-') );
+ if ( ch == '-' ) {
+ Neg = true;
+ do
+ ch = PollRxChar();
+ while ( (ch < '0') || (ch > '9') );
+ }
+ n = (ch - '0') * 10;
+
+ do
+ ch = PollRxChar();
+ while ( (ch < '0') || (ch > '9') );
+ n += ch - '0';
+ if ( Neg )
+ n = -n;
+ return(n);
+} // RxNumS
+
+void TxVal32(int32 V, int8 dp, uint8 Separator) {
+ uint8 S[16];
+ int8 c, zeros, i;
+ int32 NewV, Rem;
+
+ if (V<0) {
+ TxChar('-');
+ V=-V;
+ }
+// else
+// TxChar(' ');
+
+ c=0;
+ do {
+ NewV=V/10;
+ Rem=V-(NewV*10);
+ S[c++]=Rem + '0';
+ V=NewV;
+ } while (V>0);
+
+ if ((c < ( dp + 1 ) ) && (dp > 0 )) {
+ TxChar('0');
+ TxChar('.');
+ }
+
+ zeros = (int8)dp-c-1;
+ if ( zeros >= 0 )
+ for (i = zeros; i>=0; i--)
+ TxChar('0');
+
+ do {
+ c--;
+ TxChar(S[c]);
+ if ((c==dp)&&(c>0))
+ TxChar('.');
+ } while ( c > 0 );
+
+ if ( Separator != NUL )
+ TxChar(Separator);
+} // TxVal32
+
+void TxESCu8(uint8 ch) {
+ if ((ch==SOH)||(ch==EOT)||(ch==ESC))
+ TxChar(ESC);
+ TxChar(ch);
+} // TxESCu8
+
+void TxESCi8(int8 b) {
+ if (((uint8)b==SOH)||((uint8)b==EOT)||((uint8)b==ESC))
+ TxChar(ESC);
+ TxChar(b);
+} // TxESCu8
+
+void Sendi16(int16 v) {
+ i16u Temp;
+
+ Temp.i16 = v;
+ TxChar(Temp.b0);
+ TxChar(Temp.b1);
+} // Sendi16
+
+void TxESCi16(int16 v) {
+ i16u Temp;
+
+ Temp.i16 = v;
+ TxESCu8(Temp.b0);
+ TxESCu8(Temp.b1);
+} // Sendi16
+
+void TxESCi24(int24 v) {
+ i24u Temp;
+
+ Temp.i24 = v;
+ TxESCu8(Temp.b0);
+ TxESCu8(Temp.b1);
+ TxESCu8(Temp.b2);
+} // Sendi16
+
+void TxESCi32(int32 v) {
+ i32u Temp;
+
+ Temp.i32 = v;
+ TxESCi16(Temp.w0);
+ TxESCi16(Temp.w1);
+} // TxESCi32
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/stats.c Fri Feb 18 22:28:05 2011 +0000
@@ -0,0 +1,137 @@
+// ===============================================================================================
+// = 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 =
+// ===============================================================================================
+
+// 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/
+
+#include "UAVXArm.h"
+
+void ZeroStats(void);
+void ReadStatsPX(void);
+void WriteStatsPX(void);
+void ShowStats(void);
+
+int16 Stats[MAX_STATS];
+
+#define INIT_MIN 1000L
+
+void ZeroStats(void)
+{
+ int8 s;
+
+ for (s = 0 ; s < MAX_STATS ; s++ )
+ Stats[s] = 0;
+
+ Stats[MinHDiluteS] = INIT_MIN;
+ Stats[MaxHDiluteS] = 0;
+ Stats[MinROCS] = INIT_MIN;
+ Stats[MaxROCS] = 0;
+ Stats[GPSMinSatsS] = INIT_MIN;
+ Stats[GPSMaxSatsS] = 0;
+ Stats[MinTempS] = INIT_MIN;
+ Stats[MaxTempS] = 0;
+
+} // ZeroStats
+
+void ReadStatsPX(void)
+{
+ int8 s;
+
+ for (s = 0 ; s < MAX_STATS ; s++ )
+ Stats[s] = Read16PX(STATS_ADDR_PX + s*2);
+} // InitStats
+
+void WriteStatsPX()
+{
+ int8 s, i;
+ int16 Temp;
+
+ if ( P[ESCType] != ESCPPM )
+ for ( i = 0; i < NoOfI2CESCOutputs; i++ )
+ Stats[ESCI2CFailS] += ESCI2CFail[i];
+
+ for (s = 0 ; s < MAX_STATS ; s++ )
+ Write16PX(STATS_ADDR_PX + s*2, Stats[s]);
+
+ Temp = ToPercent(CruiseThrottle, OUT_MAXIMUM);
+ WritePX(PercentCruiseThr, Temp);
+
+} // WriteStatsPX
+
+void ShowStats(void)
+{
+ TxString("\r\nFlight Statistics\r\n");
+
+ if ( Stats[BadS] != 0 )
+ {
+ TxString("Misc(gke): \t");TxVal32((int32)Stats[BadS],0,' '); TxVal32((int32)Stats[BadNumS],0,' ');TxNextLine();
+ }
+
+ TxString("\r\nSensor/Rx Failures (Count)\r\n");
+ TxString("I2CBus: \t");TxVal32((int32)Stats[I2CFailS],0,0); TxNextLine();
+ TxString("GPS: \t");TxVal32((int32)Stats[GPSInvalidS],0,0); TxNextLine();
+ TxString("Acc: \t");TxVal32((int32)Stats[AccFailS], 0, 0); TxNextLine();
+ TxString("Gyro: \t");TxVal32((int32)Stats[GyroFailS], 0, 0); TxNextLine();
+ TxString("Comp: \t");TxVal32((int32)Stats[CompassFailS], 0, 0); TxNextLine();
+ TxString("Baro: \t");TxVal32((int32)Stats[BaroFailS],0 , 0); TxNextLine();
+ if ( P[ESCType] != ESCPPM )
+ {
+ TxString("I2CESC: \t");TxVal32((int32)Stats[ESCI2CFailS],0 , 0); TxNextLine();
+ }
+ TxString("Rx: \t");TxVal32((int32)Stats[RCGlitchesS],0,' '); TxNextLine();
+ TxString("Failsafes:\t");TxVal32((int32)Stats[RCFailsafesS],0,' '); TxNextLine();
+
+ TxString("\r\nBaro\r\n"); // can only display to 3276M
+ TxString("Alt: \t");TxVal32((int32)Stats[BaroRelAltitudeS], 1, ' '); TxString("M \r\n");
+ if ( Stats[MinROCS] < INIT_MIN )
+ {
+ TxString("ROC: \t");TxVal32((int32)Stats[MinROCS], 1, ' ');
+ TxVal32((int32)Stats[MaxROCS], 1, ' '); TxString("M/S\r\n");
+ }
+
+ if ( Stats[MinTempS] < INIT_MIN )
+ {
+ TxString("Ambient: \t");TxVal32((int32)Stats[MinTempS], 1, ' ');
+ TxVal32((int32)Stats[MaxTempS], 1, ' '); TxString("C\r\n");
+ }
+
+ TxString("\r\nGPS\r\n");
+ TxString("Alt: \t");TxVal32((int32)Stats[GPSAltitudeS], 1, ' '); TxString("M\r\n");
+ #ifdef GPS_INC_GROUNDSPPXD
+ TxString("Vel: \t");TxVal32(ConvertGPSToM((int32)Stats[GPSVelS]), 1, ' '); TxString("M/S\r\n");
+ #endif // GPS_INC_GROUNDSPPXD
+
+ if ( Stats[GPSMinSatsS] < INIT_MIN )
+ {
+ TxString("Sats: \t");TxVal32((int32)Stats[GPSMinSatsS], 0, ' ');
+ TxVal32((int32)Stats[GPSMaxSatsS], 0, 0); TxNextLine();
+ }
+
+ if ( Stats[MinHDiluteS] < INIT_MIN )
+ {
+ TxString("HDilute: \t");TxVal32((int32)Stats[MinHDiluteS], 2, ' ');
+ TxVal32((int32)Stats[MaxHDiluteS], 2, 0); TxNextLine();
+ }
+
+ if ( Stats[NavValidS] )
+ TxString("Navigation ENABLED\r\n");
+ else
+ TxString("Navigation DISABLED (No fix at launch)\r\n");
+
+} // ShowStats
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/telemetry.c Fri Feb 18 22:28:05 2011 +0000
@@ -0,0 +1,613 @@
+// ===============================================================================================
+// = 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 =
+// ===============================================================================================
+
+// 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/
+
+#include "UAVXArm.h"
+
+void RxTelemetryPacket(uint8);
+void InitTelemetryPacket(void);
+void BuildTelemetryPacket(uint8);
+
+void SendPacketHeader(void);
+void SendPacketTrailer(void);
+
+void SendTelemetry(void);
+void SendCycle(void);
+void SendControl(void);
+void SendFlightPacket(void);
+void SendNavPacket(void);
+void SendControlPacket(void);
+void SendStatsPacket(void);
+void SendParamPacket(uint8);
+void SendMinPacket(void);
+void SendArduStation(void);
+void SendCustom(void);
+void SensorTrace(void);
+
+uint8 UAVXCurrPacketTag;
+uint8 RxPacketLength, RxPacketByteCount;
+uint8 RxCheckSum;
+uint8 RxPacketTag, ReceivedPacketTag;
+uint8 PacketRxState;
+boolean CheckSumError, TelemetryPacketReceived;
+
+int16 RxLengthErrors, RxTypeErrors, RxCheckSumErrors;
+
+uint8 UAVXPacket[256];
+
+FILE *logfile = NULL;
+boolean EchoToLogFile = false;
+uint32 LogChars;
+boolean LogfileIsOpen = false;
+
+uint8 TxCheckSum;
+
+void InitTelemetryPacket(void) {
+ RxPacketByteCount = 0;
+ RxCheckSum = 0;
+ RxPacketTag = UnknownPacketTag;
+ RxPacketLength = 2; // set as minimum
+ PacketRxState = WaitRxSentinel;
+} // InitTelemetryPacket
+
+void BuildTelemetryPacket(uint8 ch) {
+ static boolean RxPacketError;
+
+ UAVXPacket[RxPacketByteCount++] = ch;
+
+ if (RxPacketByteCount == 1) {
+ RxPacketTag = ch;
+ PacketRxState=WaitRxBody;
+ } else
+ if (RxPacketByteCount == 2) {
+ RxPacketLength = ch; // ignore
+ PacketRxState = WaitRxBody;
+ } else
+ if (RxPacketByteCount >= (RxPacketLength + 3)) {
+ RxPacketError = CheckSumError = RxCheckSum != 0;
+
+ if (CheckSumError)
+ RxCheckSumErrors++;
+
+ if (!RxPacketError) {
+ TelemetryPacketReceived = true;
+ ReceivedPacketTag=RxPacketTag;
+ }
+ PacketRxState = WaitRxSentinel;
+ // InitPollPacket();
+ } else
+ PacketRxState = WaitRxBody;
+} // BuildTelemetryPacket
+
+void RxTelemetryPacket(uint8 ch) {
+
+ RxCheckSum ^= ch;
+ switch (PacketRxState) {
+ case WaitRxSentinel:
+ if (ch == SOH) {
+ InitTelemetryPacket();
+ CheckSumError = false;
+ PacketRxState = WaitRxBody;
+ }
+ break;
+ case WaitRxBody:
+ if (ch == ESC)
+ PacketRxState = WaitRxESC;
+ else
+ if (ch == SOH) { // unexpected start of packet
+ RxLengthErrors++;
+
+ InitTelemetryPacket();
+ PacketRxState = WaitRxBody;
+ } else
+ if (ch == EOT) { // unexpected end of packet
+ RxLengthErrors++;
+ PacketRxState = WaitRxSentinel;
+ } else
+ BuildTelemetryPacket(ch);
+ break;
+ case WaitRxESC:
+ BuildTelemetryPacket(ch);
+ break;
+ default:
+ PacketRxState = WaitRxSentinel;
+ break;
+ }
+} // ParseTelemetryPacket
+
+
+#define NAV_STATS_INTERLEAVE 10
+static int8 StatsNavAlternate = 0;
+
+void CheckTelemetry(void) {
+
+ // check incoming - switch on associated action
+
+ // do routine telemetry Tx
+ if ( mSClock() > mS[TelemetryUpdate] )
+ switch ( P[TelemetryType] ) {
+ case UAVXTelemetry:
+ mS[TelemetryUpdate] = mSClock() + UAVX_TEL_INTERVAL_MS;
+ SendCycle();
+ break;
+ case ArduStationTelemetry:
+ mS[TelemetryUpdate] = mSClock() + UAVX_CONTROL_TEL_INTERVAL_MS;
+ SendArduStation();
+ break;
+ case UAVXControlTelemetry:
+ mS[TelemetryUpdate] = mSClock() + UAVX_CONTROL_TEL_INTERVAL_MS;
+ SendControlPacket();
+ break;
+ case UAVXMinTelemetry:
+ mS[TelemetryUpdate] = mSClock() + UAVX_MIN_TEL_INTERVAL_MS;
+ SendMinPacket();
+ break;
+ case CustomTelemetry:
+ mS[TelemetryUpdate] = mSClock() + CUSTOM_TEL_INTERVAL_MS;
+ SendCustom();
+ break;
+ case GPSTelemetry:
+ break;
+ }
+
+} // 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;
+} // SendPacketHeader
+
+void SendPacketTrailer(void) {
+ TxESCu8(TxCheckSum);
+ TxChar(EOT);
+
+ TxChar(CR);
+ TxChar(LF);
+
+ EchoToLogFile = false;
+} // SendPacketTrailer
+
+void ShowAttitude(void) {
+ // Stick units
+ TxESCi16(DesiredRoll);
+ TxESCi16(DesiredPitch);
+ TxESCi16(DesiredYaw);
+
+ // mRadian and mG
+ TxESCi16(Gyro[Roll] * 1000.0); // Rate
+ TxESCi16(Gyro[Pitch] * 1000.0);
+ TxESCi16(Gyro[Yaw] * 1000.0);
+
+ TxESCi16(Angle[Roll] * 1000.0);
+ TxESCi16(Angle[Pitch] * 1000.0);
+ TxESCi16(Angle[Yaw] * 1000.0);
+
+ TxESCi16(Acc[LR] * 1000.0);
+ TxESCi16(Acc[BF] * 1000.0);
+ TxESCi16(Acc[UD] * 1000.0);
+} // ShowAttitude
+
+void SendFlightPacket(void) {
+ static int8 b;
+
+ SendPacketHeader();
+
+ TxESCu8(UAVXFlightPacketTag);
+ TxESCu8(48 + TELEMETRY_FLAG_BYTES);
+ for ( b = 0; b < TELEMETRY_FLAG_BYTES; b++ )
+ TxESCu8(F.AllFlags[b]);
+
+ TxESCu8(State);
+
+ // dV, dA, mAH
+ TxESCi16(BatteryVolts * 10.0); // to do scaling
+ TxESCi16(BatteryCurrent * 10.0);
+ TxESCi16(BatteryChargeUsedAH * 1000.0);
+
+ TxESCi16(RCGlitches);
+ TxESCi16(DesiredThrottle);
+
+ ShowAttitude();
+
+ TxESCi8((int8)Comp[LR]);
+ TxESCi8((int8)Comp[BF]);
+ TxESCi8((int8)Comp[UD]);
+ TxESCi8((int8)Comp[Alt]);
+
+ for ( b = 0; b < 4; b++ )
+ TxESCu8((uint8)PWM[b]);
+
+ TxESCu8((uint8)PWM[CamRollC]);
+ TxESCu8((uint8)PWM[CamPitchC]);
+
+ TxESCi24(mSClock() - mS[StartTime]);
+
+ SendPacketTrailer();
+} // SendFlightPacket
+
+void SendControlPacket(void) {
+ static int8 b;
+
+ SendPacketHeader();
+
+ TxESCu8(UAVXControlPacketTag);
+ TxESCu8(35);
+
+ TxESCi16(DesiredThrottle);
+
+ ShowAttitude();
+
+ for ( b = 0; b < 6; b++ ) // motor/servo channels
+ TxESCu8((uint8)PWM[b]);
+
+ TxESCi24( time(NULL) );
+
+ SendPacketTrailer();
+
+} // SendControlPacket
+
+void SendNavPacket(void) {
+ SendPacketHeader();
+
+ TxESCu8(UAVXNavPacketTag);
+ TxESCu8(59);
+
+ TxESCu8(NavState);
+ TxESCu8(FailState);
+ TxESCu8(GPSNoOfSats);
+ TxESCu8(GPSFix);
+
+ TxESCu8(CurrWP);
+
+ TxESCi16(ROC * 10.0 ); // dm/S
+ TxESCi24(BaroRelAltitude * 10.0);
+
+ TxESCi16(GPSHeading * 1000.0);
+ TxESCi16(RangefinderAltitude * 100.0); // cm
+
+ TxESCi16(GPSHDilute);
+ TxESCi16(Heading * 1000.0);
+ TxESCi16(WayHeading * 1000.0);
+
+ TxESCi16(GPSVel * 10.0);
+ TxESCi16(0); // was GPSROC;
+
+ TxESCi24(GPSRelAltitude * 10.0); // dm
+ TxESCi32(GPSLatitude); // 5 decimal minute units
+ TxESCi32(GPSLongitude);
+
+ TxESCi24(DesiredAltitude * 10.0 );
+ TxESCi32(DesiredLatitude);
+ TxESCi32(DesiredLongitude);
+
+ TxESCi24(mS[NavStateTimeout] - mSClock()); // mS
+
+ TxESCi16(AmbientTemperature.i16); // 0.1C
+ TxESCi32( time(NULL) ); //GPSSeconds);
+
+ TxESCu8(NavSensitivity);
+ TxESCi8(NavCorr[Roll]);
+ TxESCi8(NavCorr[Pitch]);
+ TxESCi8(NavCorr[Yaw]);
+
+ SendPacketTrailer();
+
+} // SendNavPacket
+
+void SendStatsPacket(void) {
+ 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]);
+
+ 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]);
+
+ TxESCu8(UAVXAirframe | 0x80);
+ TxESCu8(Orientation);
+ TxESCi16(Stats[BadNumS]);
+
+ SendPacketTrailer();
+
+} // SendStatsPacket
+
+void SendMinPacket(void) {
+ static int8 b;
+
+ SendPacketHeader();
+
+ TxESCu8(UAVXMinPacketTag);
+ TxESCu8(33 + TELEMETRY_FLAG_BYTES);
+ for ( b = 0; b < TELEMETRY_FLAG_BYTES; b++ )
+ TxESCu8(F.AllFlags[b]);
+
+ TxESCu8(State);
+ TxESCu8(NavState);
+ TxESCu8(FailState);
+
+ TxESCi16(BatteryVolts * 10.0); // to do scaling
+ TxESCi16(BatteryCurrent * 10.0);
+ TxESCi16(BatteryChargeUsedAH * 1000.0);
+
+ TxESCi16(Angle[Roll] * 1000.0);
+ TxESCi16(Angle[Pitch] * 1000.0);
+
+ TxESCi24(BaroRelAltitude * 10.0);
+ TxESCi16(RangefinderAltitude * 100.0); // cm
+
+ TxESCi16(Heading * 1000.0);
+
+ TxESCi32(GPSLatitude); // 5 decimal minute units
+ TxESCi32(GPSLongitude);
+
+ TxESCu8(UAVXAirframe | 0x80 ); // ARM has top bit set
+ TxESCu8(Orientation);
+
+ TxESCi24(mSClock() - mS[StartTime]);
+
+ SendPacketTrailer();
+
+} // SendMinPacket
+
+void SendParamPacket(uint8 p) {
+
+ static uint8 b;
+
+ SendPacketHeader();
+
+ TxESCu8(UAVXParamsPacketTag);
+ TxESCu8(MAX_PARAMETERS+1);
+ TxESCu8(p);
+ for (b = 0; b < (uint8)MAX_PARAMETERS; b++ )
+ TxESCi8(PX[MAX_PARAMETERS*2 + b]);
+
+ SendPacketTrailer();
+
+} // SendParamPacket
+
+void SendCycle(void) {
+
+ switch ( UAVXCurrPacketTag ) {
+ case UAVXFlightPacketTag:
+ SendFlightPacket();
+
+ UAVXCurrPacketTag = UAVXNavPacketTag;
+ break;
+
+ case UAVXNavPacketTag:
+ if ( ++StatsNavAlternate < NAV_STATS_INTERLEAVE)
+ SendNavPacket();
+ else {
+ SendStatsPacket();
+ StatsNavAlternate = 0;
+ }
+
+ UAVXCurrPacketTag = UAVXFlightPacketTag;
+ break;
+
+ default:
+ UAVXCurrPacketTag = UAVXFlightPacketTag;
+ break;
+ }
+
+} // SendCycle
+
+void SendArduStation(void) {
+
+ static int8 Count = 0;
+ /*
+ Definitions of the low rate telemetry (1Hz):
+ LAT: Latitude
+ LON: Longitude
+ SPD: Speed over ground from GPS
+ CRT: Climb Rate in M/S
+ ALT: Altitude in meters
+ ALH: The altitude is trying to hold
+ CRS: Course over ground in degrees.
+ BER: Bearing is the heading you want to go
+ WPN: Waypoint number, where WP0 is home.
+ DST: Distance from Waypoint
+ BTV: Battery Voltage.
+ RSP: Roll setpoint used to debug, (not displayed here).
+
+ Definitions of the high rate telemetry (~4Hz):
+ ASP: Airspeed, right now is the raw data.
+ TTH: Throttle in 100% the autopilot is applying.
+ RLL: Roll in degrees + is right - is left
+ PCH: Pitch in degrees
+ SST: Switch Status, used for debugging, but is disabled in the current version.
+ */
+
+ if ( ++Count == 4 ) {
+ TxString("!!!");
+ TxString("LAT:");
+ TxVal32(GPSLatitude / 6000, 3, 0);
+ TxString(",LON:");
+ TxVal32(GPSLongitude / 6000, 3, 0);
+ TxString(",ALT:");
+ TxVal32(Altitude / 10,0,0);
+ TxString(",ALH:");
+ TxVal32(DesiredAltitude / 10, 0, 0);
+ TxString(",CRT:");
+ TxVal32(ROC / 100, 0, 0);
+ TxString(",CRS:");
+ TxVal32(Heading * RADDEG, 0, 0); // scaling to degrees?
+ TxString(",BER:");
+ TxVal32(WayHeading * RADDEG, 0, 0);
+ TxString(",SPD:");
+ TxVal32(GPSVel, 0, 0);
+ TxString(",WPN:");
+ TxVal32(CurrWP,0,0);
+ TxString(",DST:");
+ TxVal32(0, 0, 0); // distance to WP
+ TxString(",BTV:");
+ TxVal32((BatteryVoltsADC * 61)/205, 1, 0);
+ TxString(",RSP:");
+ TxVal32(DesiredRoll, 0, 0);
+
+ Count = 0;
+ } else {
+ TxString("+++");
+ TxString("ASP:");
+ TxVal32(GPSVel / 100, 0, 0);
+ TxString(",RLL:");
+ TxVal32(Angle[Roll] / 35, 0, 0); // scale to degrees?
+ TxString(",PCH:");
+ TxVal32(Angle[Pitch] / 35, 0, 0);
+ TxString(",THH:");
+ TxVal32( ((int24)DesiredThrottle * 100L) / RC_MAXIMUM, 0, 0);
+ }
+
+ TxString(",***\r\n");
+
+} // SendArduStation
+
+void SendCustom(void)
+{ // user defined telemetry human readable OK for small amounts of data < 1mS
+
+ EchoToLogFile = true;
+
+ // 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)
+ // ->
+
+ TxVal32(mSClock(), 3, HT);
+
+ if ( F.HoldingAlt ) // are we holding
+ TxChar('H');
+ else
+ TxChar('N');
+ TxChar(HT);
+
+ if (F.UsingRangefinderAlt ) // are we using the rangefinder
+ TxChar('R');
+ else
+ TxChar('B');
+ TxChar(HT);
+
+ TxVal32(SRS32(Comp[Alt],1), 1, HT); // ~% throttle compensation
+
+ TxVal32(GPSRelAltitude, 1, HT);
+ TxVal32(BaroRelAltitude, 1, HT);
+ TxVal32(RangefinderAltitude, 2, HT);
+
+ 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
+
+ // <-
+
+ TxChar(CR);
+ TxChar(LF);
+
+ EchoToLogFile = false;
+} // SendCustom
+
+void SensorTrace(void) {
+#ifdef TESTING
+
+ if ( DesiredThrottle > 20 ) {
+ EchoToLogFile = false; // direct to USART
+
+ TxValH16(((int24)Heading * 180)/MILLIPI);
+ TxChar(';');
+
+ TxValH16(BaroRelAltitude);
+ TxChar(';');
+ TxValH16(RangefinderAltitude);
+ TxChar(';');
+ TxValH16(0);
+ TxChar(';');
+
+ TxValH16(DesiredThrottle);
+ TxChar(';');
+ TxValH16(DesiredRoll);
+ TxChar(';');
+ TxValH16(DesiredPitch);
+ TxChar(';');
+ TxValH16(DesiredYaw);
+ TxChar(';');
+
+ TxValH16(Rate[Roll]);
+ TxChar(';');
+ TxValH16(Rate[Pitch]);
+ TxChar(';');
+ TxValH16(Rate[Yaw]);
+ TxChar(';');
+
+ TxValH16(Angle[Roll]);
+ TxChar(';');
+ TxValH16(Angle[Pitch]);
+ TxChar(';');
+ TxValH16(Angle[Yaw]);
+ TxChar(';');
+
+ TxValH16(Acc[LR]);
+ TxChar(';');
+ TxValH16(Acc[FB]);
+ TxChar(';');
+ TxValH16(Acc[DU]);
+ TxChar(';');
+
+ TxValH16(Comp[LR]);
+ TxChar(';');
+ TxValH16(Comp[FB]);
+ TxChar(';');
+ TxValH16(Comp[DU]);
+ TxChar(';');
+ TxValH16(Comp[Alt]);
+ TxChar(';');
+ TxNextLine();
+ }
+#endif // TESTING
+} // SensorTrace
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/temperature.c Fri Feb 18 22:28:05 2011 +0000
@@ -0,0 +1,71 @@
+// ===============================================================================================
+// = 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 =
+// ===============================================================================================
+
+// 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/
+
+#include "UAVXArm.h"
+void GetTemperature(void);
+void InitTemperature(void);
+
+i16u AmbientTemperature;
+
+void GetTemperature(void) {
+
+ I2CTEMP.start();
+ if ( I2CTEMP.write(TMP100_RD) != I2C_ACK ) goto Terror;
+ AmbientTemperature.b1 = I2CTEMP.read(I2C_ACK);
+ AmbientTemperature.b0 = I2CTEMP.read(I2C_NACK);
+ I2CTEMP.stop();
+
+ // Top 9 bits 0.5C res. scale to 0.1C
+ AmbientTemperature.i16 = SRS16( AmbientTemperature.i16, 7) * 5;
+ if ( AmbientTemperature.i16 > Stats[MaxTempS])
+ Stats[MaxTempS] = AmbientTemperature.i16;
+ else
+ if ( AmbientTemperature.i16 < Stats[MinTempS] )
+ Stats[MinTempS] = AmbientTemperature.i16;
+ return;
+
+Terror:
+ I2CTEMP.stop();
+ AmbientTemperature.i16 = 0;
+
+ return;
+} // GetTemperature
+
+void InitTemperature(void) {
+ I2CTEMP.start();
+ if( I2CTEMP.write(TMP100_WR) != I2C_ACK ) goto Terror;
+ if( I2CTEMP.write(TMP100_CMD) != I2C_ACK ) goto Terror;
+ if( I2CTEMP.write(TMP100_CFG) != I2C_ACK ) goto Terror;
+ I2CTEMP.stop();
+
+ I2CTEMP.start();
+ if( I2CTEMP.write(TMP100_WR) != I2C_ACK ) goto Terror;
+ if( I2CTEMP.write(TMP100_TMP) != I2C_ACK ) goto Terror; // Select temperature
+ I2CTEMP.stop();
+
+ GetTemperature();
+
+ return;
+
+ Terror:
+ I2CTEMP.stop();
+ AmbientTemperature.i16 = 0;
+
+} // InitTemperature
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/uavx_aileron.h Fri Feb 18 22:28:05 2011 +0000
@@ -0,0 +1,97 @@
+// ===============================================================================================
+// = 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 =
+// ===============================================================================================
+
+// 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/
+
+ const int8 DefaultParams[MAX_PARAMETERS][2] = {
+ {20,0}, // RollKp, 01
+ {12,0}, // RollKi, 02
+ {50, 0}, // RollKd, 03
+ {0,true}, // HorizDampKp, 04c //-1
+ {5,0}, // RollIntLimit, 05
+ {20,0}, // PitchKp, 06
+ {12,0}, // PitchKi, 07
+ {50,0}, // PitchKd, 08
+ {8,0}, // AltKp, 09
+ {5,0}, // PitchIntLimit, 10
+
+ {30,0}, // YawKp, 11
+ {25,0}, // YawKi, 12
+ {0,0}, // YawKd, 13
+ {25,0}, // YawLimit, 14
+ {2,0}, // YawIntLimit, 15
+ {8,true}, // ConfigBits, 16c
+ {0,true}, // was TimeSlots, 17c
+ {48,true}, // LowVoltThres, 18c
+ {20,true}, // CamRollKp, 19c
+ {45,true}, // PercentCruiseThr,20c
+
+ {0,true}, // VertDampKp, 21c //-1
+ {0,true}, // MiddleDU, 22c
+ {20,true}, // PercentIdleThr, 23c
+ {0,true}, // MiddleLR, 24c
+ {0,true}, // MiddleFB, 25c
+ {20,true}, // CamPitchKp, 26c
+ {10,0}, // CompassKp, 27
+ {8,0}, // AltKi, 28
+ {0,0}, // was NavRadius, 29
+ {8,0}, // NavKi, 30
+
+ {0,0}, // GSThrottle, 31
+ {0,0}, // Acro, 32
+ {10,0}, // NavRTHAlt, 33
+ {0,true}, // NavMagVar, 34c
+ {ITG3200Gyro,true}, // GyroType, 35c
+ {ESCPPM,true}, // ESCType, 36c
+ {UnknownTxRx,true}, // TxRxType 37c
+ {2,true}, // NeutralRadius 38
+ {30,true}, // PercentNavSens6Ch 39
+ {1,true}, // CamRollTrim, 40c
+
+ {-16,0}, // NavKd 41
+ {1,true}, // VertDampDecay 42c
+ {1,true}, // HorizDampDecay 43c
+ {56,true}, // BaroScale 44c
+ {UAVXTelemetry,true}, // TelemetryType 45c
+ {-8,0}, // MaxDescentRateDmpS 46
+ {30,true}, // DescentDelayS 47c
+ {1,0}, // NavIntLimit 48
+ {2,0}, // AltIntLimit 49
+ {11,true}, // was GravComp 50c
+
+ {0,true}, // was CompSteps 51c
+ {0,true}, // ServoSense 52c
+ {3,true}, // CompassOffsetQtr 53c
+ {49,true}, // BatteryCapacity 54c
+ {LY530Gyro,true}, // was GyroYawType 55c
+ {4,0}, // AltKd 56
+ #if (defined TRICOPTER) | (defined VTCOPTER )
+ {24,true}, // Orient 57
+ #else
+ {0,true}, // Orient 57
+ #endif // TRICOPTER | VTCOPTER
+
+ {12,true}, // NavYawLimit 58
+ {0,0}, // Balance 59
+ {0,0}, // 60 - 64 unused currently
+ {0,0},
+
+ {0,0},
+ {0,0},
+ {0,0}
+ };
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/uavx_elevon.h Fri Feb 18 22:28:05 2011 +0000
@@ -0,0 +1,97 @@
+// ===============================================================================================
+// = 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 =
+// ===============================================================================================
+
+// 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/
+
+ const int8 DefaultParams[MAX_PARAMETERS][2] = {
+ {20,0}, // RollKp, 01
+ {12,0}, // RollKi, 02
+ {50, 0}, // RollKd, 03
+ {0,true}, // HorizDampKp, 04c //-1
+ {5,0}, // RollIntLimit, 05
+ {20,0}, // PitchKp, 06
+ {12,0}, // PitchKi, 07
+ {50,0}, // PitchKd, 08
+ {8,0}, // AltKp, 09
+ {5,0}, // PitchIntLimit, 10
+
+ {30,0}, // YawKp, 11
+ {25,0}, // YawKi, 12
+ {0,0}, // YawKd, 13
+ {25,0}, // YawLimit, 14
+ {2,0}, // YawIntLimit, 15
+ {8,true}, // ConfigBits, 16c
+ {0,true}, // was TimeSlots, 17c
+ {48,true}, // LowVoltThres, 18c
+ {20,true}, // CamRollKp, 19c
+ {45,true}, // PercentCruiseThr,20c
+
+ {0,true}, // VertDampKp, 21c //-1
+ {0,true}, // MiddleDU, 22c
+ {20,true}, // PercentIdleThr, 23c
+ {0,true}, // MiddleLR, 24c
+ {0,true}, // MiddleFB, 25c
+ {20,true}, // CamPitchKp, 26c
+ {10,0}, // CompassKp, 27
+ {8,0}, // AltKi, 28
+ {0,0}, // was NavRadius, 29
+ {8,0}, // NavKi, 30
+
+ {0,0}, // GSThrottle, 31
+ {0,0}, // Acro, 32
+ {10,0}, // NavRTHAlt, 33
+ {0,true}, // NavMagVar, 34c
+ {ITG3200Gyro,true}, // GyroType, 35c
+ {ESCPPM,true}, // ESCType, 36c
+ {UnknownTxRx,true}, // TxRxType 37c
+ {2,true}, // NeutralRadius 38
+ {30,true}, // PercentNavSens6Ch 39
+ {1,true}, // CamRollTrim, 40c
+
+ {16,0}, // NavKd 41
+ {1,true}, // VertDampDecay 42c
+ {1,true}, // HorizDampDecay 43c
+ {56,true}, // BaroScale 44c
+ {UAVXTelemetry,true}, // TelemetryType 45c
+ {8,0}, // MaxDescentRateDmpS 46
+ {30,true}, // DescentDelayS 47c
+ {1,0}, // NavIntLimit 48
+ {2,0}, // AltIntLimit 49
+ {11,true}, // was GravComp 50c
+
+ {0,true}, // was CompSteps 51c
+ {0,true}, // ServoSense 52c
+ {3,true}, // CompassOffsetQtr 53c
+ {49,true}, // BatteryCapacity 54c
+ {LY530Gyro,true}, // was GyroYawType 55c
+ {4,0}, // AltKd 56
+ #if (defined TRICOPTER) | (defined VTCOPTER )
+ {24,true}, // Orient 57
+ #else
+ {0,true}, // Orient 57
+ #endif // TRICOPTER | VTCOPTER
+
+ {12,true}, // NavYawLimit 58
+ {0,0}, // Balance 59
+ {0,0}, // 60 - 64 unused currently
+ {0,0},
+
+ {0,0},
+ {0,0},
+ {0,0}
+ };
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/uavx_helicopter.h Fri Feb 18 22:28:05 2011 +0000
@@ -0,0 +1,97 @@
+// ===============================================================================================
+// = 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 =
+// ===============================================================================================
+
+// 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/
+
+ const int8 DefaultParams[MAX_PARAMETERS][2] = {
+ {20,0}, // RollKp, 01
+ {12,0}, // RollKi, 02
+ {50, 0}, // RollKd, 03
+ {0,true}, // HorizDampKp, 04c //-1
+ {5,0}, // RollIntLimit, 05
+ {20,0}, // PitchKp, 06
+ {12,0}, // PitchKi, 07
+ {50,0}, // PitchKd, 08
+ {8,0}, // AltKp, 09
+ {5,0}, // PitchIntLimit, 10
+
+ {30,0}, // YawKp, 11
+ {25,0}, // YawKi, 12
+ {0,0}, // YawKd, 13
+ {25,0}, // YawLimit, 14
+ {2,0}, // YawIntLimit, 15
+ {8,true}, // ConfigBits, 16c
+ {0,true}, // was TimeSlots, 17c
+ {48,true}, // LowVoltThres, 18c
+ {20,true}, // CamRollKp, 19c
+ {45,true}, // PercentCruiseThr,20c
+
+ {0,true}, // VertDampKp, 21c //-1
+ {0,true}, // MiddleDU, 22c
+ {20,true}, // PercentIdleThr, 23c
+ {0,true}, // MiddleLR, 24c
+ {0,true}, // MiddleFB, 25c
+ {20,true}, // CamPitchKp, 26c
+ {10,0}, // CompassKp, 27
+ {8,0}, // AltKi, 28
+ {0,0}, // was NavRadius, 29
+ {8,0}, // NavKi, 30
+
+ {0,0}, // GSThrottle, 31
+ {0,0}, // Acro, 32
+ {10,0}, // NavRTHAlt, 33
+ {0,true}, // NavMagVar, 34c
+ {ITG3200Gyro,true}, // GyroType, 35c
+ {ESCPPM,true}, // ESCType, 36c
+ {UnknownTxRx,true}, // TxRxType 37c
+ {2,true}, // NeutralRadius 38
+ {30,true}, // PercentNavSens6Ch 39
+ {1,true}, // CamRollTrim, 40c
+
+ {16,0}, // NavKd 41
+ {1,true}, // VertDampDecay 42c
+ {1,true}, // HorizDampDecay 43c
+ {56,true}, // BaroScale 44c
+ {UAVXTelemetry,true}, // TelemetryType 45c
+ {8,0}, // MaxDescentRateDmpS 46
+ {30,true}, // DescentDelayS 47c
+ {1,0}, // NavIntLimit 48
+ {2,0}, // AltIntLimit 49
+ {11,true}, // was GravComp 50c
+
+ {0,true}, // was CompSteps 51c
+ {0,true}, // ServoSense 52c
+ {3,true}, // CompassOffsetQtr 53c
+ {49,true}, // BatteryCapacity 54c
+ {LY530Gyro,true}, // was GyroYawType 55c
+ {4,0}, // AltKd 56
+ #if (defined TRICOPTER) | (defined VTCOPTER )
+ {24,true}, // Orient 57
+ #else
+ {0,true}, // Orient 57
+ #endif // TRICOPTER | VTCOPTER
+
+ {12,true}, // NavYawLimit 58
+ {0,0}, // Balance 59
+ {0,0}, // 60 - 64 unused currently
+ {0,0},
+
+ {0,0},
+ {0,0},
+ {0,0}
+ };
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/uavx_multicopter.h Fri Feb 18 22:28:05 2011 +0000
@@ -0,0 +1,97 @@
+// ===============================================================================================
+// = 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 =
+// ===============================================================================================
+
+// 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/
+
+ const int8 DefaultParams[MAX_PARAMETERS][2] = {
+ {20,0}, // RollKp, 01
+ {12,0}, // RollKi, 02
+ {50, 0}, // RollKd, 03
+ {0,true}, // HorizDampKp, 04c //-1
+ {5,0}, // RollIntLimit, 05
+ {20,0}, // PitchKp, 06
+ {12,0}, // PitchKi, 07
+ {50,0}, // PitchKd, 08
+ {8,0}, // AltKp, 09
+ {5,0}, // PitchIntLimit, 10
+
+ {30,0}, // YawKp, 11
+ {25,0}, // YawKi, 12
+ {0,0}, // YawKd, 13
+ {25,0}, // YawLimit, 14
+ {2,0}, // YawIntLimit, 15
+ {8,true}, // ConfigBits, 16c
+ {0,true}, // was TimeSlots, 17c
+ {48,true}, // LowVoltThres, 18c
+ {20,true}, // CamRollKp, 19c
+ {45,true}, // PercentCruiseThr,20c
+
+ {0,true}, // VertDampKp, 21c //-1
+ {0,true}, // MiddleDU, 22c
+ {20,true}, // PercentIdleThr, 23c
+ {0,true}, // MiddleLR, 24c
+ {0,true}, // MiddleFB, 25c
+ {20,true}, // CamPitchKp, 26c
+ {10,0}, // CompassKp, 27
+ {8,0}, // AltKi, 28
+ {0,0}, // was NavRadius, 29
+ {8,0}, // NavKi, 30
+
+ {0,0}, // GSThrottle, 31
+ {0,0}, // Acro, 32
+ {10,0}, // NavRTHAlt, 33
+ {0,true}, // NavMagVar, 34c
+ {ITG3200Gyro,true}, // DesGyroType, 35c
+ {ESCPPM,true}, // ESCType, 36c
+ {UnknownTxRx,true}, // TxRxType 37c
+ {2,true}, // NeutralRadius 38
+ {30,true}, // PercentNavSens6Ch 39
+ {1,true}, // CamRollTrim, 40c
+
+ {16,0}, // NavKd 41
+ {1,true}, // VertDampDecay 42c
+ {1,true}, // HorizDampDecay 43c
+ {56,true}, // BaroScale 44c
+ {UAVXTelemetry,true}, // TelemetryType 45c
+ {8,0}, // MaxDescentRateDmpS 46
+ {30,true}, // DescentDelayS 47c
+ {1,0}, // NavIntLimit 48
+ {2,0}, // AltIntLimit 49
+ {11,true}, // was GravComp 50c
+
+ {0,true}, // was CompSteps 51c
+ {0,true}, // ServoSense 52c
+ {3,true}, // CompassOffsetQtr 53c
+ {49,true}, // BatteryCapacity 54c
+ {ITG3200Gyro,true}, // was GyroYawType 55c
+ {4,0}, // AltKd 56
+ #if (defined TRICOPTER) | (defined VTCOPTER )
+ {24,true}, // Orient 57
+ #else
+ {0,true}, // Orient 57
+ #endif // TRICOPTER | VTCOPTER
+
+ {12,true}, // NavYawLimit 58
+ {0,0}, // Balance 59
+ {0,0}, // 60 - 64 unused currently
+ {0,0},
+
+ {0,0},
+ {0,0},
+ {0,0}
+ };
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/utils.c Fri Feb 18 22:28:05 2011 +0000
@@ -0,0 +1,170 @@
+// ===============================================================================================
+// = 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 =
+// ===============================================================================================
+
+// 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/
+
+#include "UAVXArm.h"
+
+void InitMisc(void);
+void Delay1mS(int16);
+void Delay100mS(int16);
+void DoBeep100mS(uint8, uint8);
+void DoStartingBeeps(uint8);
+void CheckAlarms(void);
+real32 SlewLimit(real32, real32, real32);
+real32 DecayX(real32, real32);
+void LPFilter(real32*, real32*, real32, real32);
+void Timing(uint8, uint32);
+
+TimingRec Times[(UnknownT+1)];
+
+void InitMisc(void) {
+ int8 i;
+
+ State = Starting; // For trace preconditions
+
+ for ( i = 0; i <= UnknownT; i++ )
+ Times[i].T = Times[i].Count = 0;
+
+ for ( i = 0; i < FLAG_BYTES ; i++ )
+ F.AllFlags[i] = false;
+
+ F.ParametersValid = F.AcquireNewPosition = F.AllowNavAltitudeHold = true;
+
+#ifdef SIMULATE
+ F.Simulation = true;
+#endif // SIMULATE
+
+ BatteryCharge = 0;
+
+ IdleThrottle = ((10L*OUT_MAXIMUM)/100);
+ InitialThrottle = RC_MAXIMUM;
+ ESCMin = OUT_MINIMUM;
+ ESCMax = OUT_MAXIMUM;
+
+ ALL_LEDS_OFF;
+ LEDRed_ON;
+ Beeper_OFF;
+} // InitMisc
+
+void Delay1mS(int16 d) {
+ static int32 Timeout;
+
+ Timeout = timer.read_us() + ((int32)d * 1000 );
+ while ( timer.read_us() < Timeout ) {};
+
+} // Delay1mS
+
+void Delay100mS(int16 d) {
+ Delay1mS( 100 * d );
+} // Delay100mS
+
+void DoBeep100mS(uint8 t, uint8 d) {
+ Beeper_ON;
+ Delay100mS(t);
+ Beeper_OFF;
+ Delay100mS(d);
+} // DoBeep100mS
+
+void DoStartingBeeps(uint8 b) {
+ uint8 i;
+
+ for ( i = 0; i < b; i++ )
+ DoBeep100mS(2, 8);
+
+ DoBeep100mS(8,0);
+} // DoStartingBeeps
+
+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;
+ } else
+ if ( State == Shutdown ) {
+ BeeperOffTime = 4750;
+ BeeperOnTime = 250;
+ } else
+ if ( F.LostModel ) {
+ BeeperOffTime = 125;
+ BeeperOnTime = 125;
+ }
+
+ if ( (mSClock() > mS[BeeperUpdate]) && BEEPER_IS_ON ) {
+ mS[BeeperUpdate] = mSClock() + BeeperOffTime;
+ Beeper_OFF;
+ LEDRed_OFF;
+ } else
+ if ( (mSClock() > mS[BeeperUpdate]) && BEEPER_IS_OFF ) {
+ mS[BeeperUpdate] = mSClock() + BeeperOnTime;
+ Beeper_ON;
+ LEDRed_ON;
+ }
+ }
+#ifdef NAV_ACQUIRE_BEEPER
+ else
+ if ( (State == InFlight) && (!F.AcquireNewPosition) && (mSClock() > mS[BeeperTimeout]) )
+ Beeper_OFF;
+#endif // NAV_ACQUIRE_BEEPER
+
+} // CheckAlarms
+
+real32 DecayX(real32 i, real32 d) {
+ if ( i < 0 ) {
+ i += d;
+ if ( i >0 )
+ i = 0;
+ } else
+ if ( i > 0 ) {
+ i -= d;
+ if ( i < 0 )
+ i = 0;
+ }
+ return (i);
+} // DecayX
+
+void LPFilter(real32* i, real32* ip, real32 FilterF, real32 dT) {
+ static real32 FilterA;
+
+ FilterA = dT / ( FilterF + dT );
+
+ *i = *ip + (*i - *ip) * FilterA;
+ *ip = *i;
+
+} // LPFilter
+
+real32 SlewLimit(real32 Old, real32 New, real32 Slew) {
+ real32 Low, High;
+
+ Low = Old - Slew;
+ High = Old + Slew;
+ return(( New < Low ) ? Low : (( New > High ) ? High : New));
+} // SlewLimit
+
+void Timing(uint8 w, uint32 T) {
+
+ Times[w].T += timer.read_us() - T;
+ Times[w].Count++;
+
+} // Timing
+