diff --git a/flight/AHRS/insgps.c b/flight/AHRS/insgps.c index 3262e0a6a..140ca211a 100644 --- a/flight/AHRS/insgps.c +++ b/flight/AHRS/insgps.c @@ -1315,17 +1315,17 @@ void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV], HPHR += HP[k] * H[m][k]; for (k = 0; k < NUMX; k++) - K[m][k] = HP[k] / HPHR; // find K = HP/HPHR + K[k][m] = HP[k] / HPHR; // find K = HP/HPHR for (i = 0; i < NUMX; i++) { // Find P(m)= P(m-1) + K*HP for (j = i; j < NUMX; j++) P[i][j] = P[j][i] = - P[i][j] - K[m][i] * HP[j]; + P[i][j] - K[i][m] * HP[j]; } Error = Z[m] - Y[m]; for (i = 0; i < NUMX; i++) // Find X(m)= X(m-1) + K*Error - X[i] = X[i] + K[m][i] * Error; + X[i] = X[i] + K[i][m] * Error; } } diff --git a/flight/Libraries/buffer.c b/flight/Libraries/buffer.c deleted file mode 100644 index 3cb1f5d98..000000000 --- a/flight/Libraries/buffer.c +++ /dev/null @@ -1,234 +0,0 @@ -/** - ****************************************************************************** - * - * @addtogroup OpenPilotModules OpenPilot Modules - * @{ - * @addtogroup Flight_Libraries Miscellaneous library functions - * @brief Miscellaneous library functions shared between PIOS / OpenPilot / AHRS - * @{ - * - * @file buffer.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Simplies buffering data - * @see The GNU Public License (GPL) Version 3 - * - *****************************************************************************/ - -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License - * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -//***************************************************************************** -// -// File Name : 'buffer.c' -// Title : Multipurpose byte buffer structure and methods -// Author : Pascal Stang - Copyright (C) 2001-2002 -// Created : 9/23/2001 -// Revised : 9/23/2001 -// Version : 1.0 -// Target MCU : any -// Editor Tabs : 4 -// -// This code is distributed under the GNU Public License -// which can be found at http://www.gnu.org/licenses/gpl.txt -// -//***************************************************************************** - -#include "buffer.h" - -/** - * @brief Initialize a cBuffer structure - * @param[in] buffer Points to the buffer structure - * @param[in] start Allocated memory to store data - * @param[in] size Maximum size of buffer - * @return None - */ -void bufferInit(cBuffer * buffer, uint8_t * start, uint32_t size) -{ - // set start pointer of the buffer - buffer->dataptr = start; - buffer->size = size; - // initialize index and length - buffer->dataindex = 0; - buffer->datalength = 0; -} - -/** - * @brief Return remaining space in buffer - * @param[in] buffer Pointer to buffer structure - * @return Amount of space remaining on buffer - */ -uint32_t bufferRemainingSpace(cBuffer * buffer) -{ - return buffer->size - buffer->datalength; -} - -/** - * @brief Return amount of data - * @param[in] buffer Pointer to buffer structure - * @return Amount of data queued in buffer - */ -uint32_t bufferBufferedData(cBuffer * buffer) -{ - return buffer->datalength; -} - -/** - * @brief Pop one element from buffer - * @param[in] buffer Pointer to the buffer structure - * @return None - */ -uint8_t bufferGetFromFront(cBuffer * buffer) -{ - unsigned char data = 0; - - // check to see if there's data in the buffer - if (buffer->datalength) { - // get the first character from buffer - data = buffer->dataptr[buffer->dataindex]; - // move index down and decrement length - buffer->dataindex++; - if (buffer->dataindex >= buffer->size) { - buffer->dataindex %= buffer->size; - } - buffer->datalength--; - } - // return - return data; -} - -/** - * @brief Copy number of elements into another buffer - * @param[in] buffer Pointer to the buffer structure - * @param[in] dest Point to destimation, must be allocated enough space for size - * @param[in] size Number of elements to get - * @return - * @arg -1 for success - * @arg 0 error - */ -uint8_t bufferGetChunkFromFront(cBuffer * buffer, uint8_t * dest, uint32_t size) -{ - if (size > buffer->datalength) - return -1; - - for (uint32_t i = 0; i < size; i++) { - dest[i] = bufferGetFromFront(buffer); - } - - return 0; -} - -/** - * @brief Shift index to trash data from front of buffer - * @param[in] buffer Pointer to buffer structure - * @param[in] numbytes Number of bytes to drop - * @return None - */ -void bufferDumpFromFront(cBuffer * buffer, uint32_t numbytes) -{ - // dump numbytes from the front of the buffer - // are we dumping less than the entire buffer? - if (numbytes < buffer->datalength) { - // move index down by numbytes and decrement length by numbytes - buffer->dataindex += numbytes; - if (buffer->dataindex >= buffer->size) { - buffer->dataindex %= buffer->size; - } - buffer->datalength -= numbytes; - } else { - // flush the whole buffer - buffer->datalength = 0; - } -} - -/** - * @brief Get element indexed from the front of buffer - * @param[in] buffer Point to the buffer structure - * @param[in] index Index into the buffer relative to front - * @return None - */ -uint8_t bufferGetAtIndex(cBuffer * buffer, uint32_t index) -{ - // return character at index in buffer - return buffer->dataptr[(buffer->dataindex + index) % (buffer->size)]; -} - -/** - * @brief Queue a character to end of buffer - * @param[in] buffer Point to the buffer structure - * @param[in] data Byte to add - * @return - * @arg -1 for success - * @arg 0 error - */ -uint8_t bufferAddToEnd(cBuffer * buffer, uint8_t data) -{ - // make sure the buffer has room - if (buffer->datalength < buffer->size) { - // save data byte at end of buffer - buffer->dataptr[(buffer->dataindex + buffer->datalength) % buffer->size] = data; - // increment the length - buffer->datalength++; - // return success - return -1; - } else - return 0; -} - -/** - * @brief Queue a block of character to end of buffer - * @param[in] buffer Point to the buffer structure - * @param[in] data Pointer to data to add - * @param[in] size Number of bytes to add - * @return - * @arg -1 for success - * @arg 0 error - */ -uint8_t bufferAddChunkToEnd(cBuffer * buffer, const uint8_t * data, uint32_t size) -{ - // TODO: replace with memcpy and logic, for now keeping it simple - for (uint32_t i = 0; i < size; i++) { - if (bufferAddToEnd(buffer, data[i]) == 0) - return 0; - } - return -1; -} - -/** - * @brief Check to see if the buffer has room - * @param[in] buffer Point to the buffer structure - * @return - * @arg True there is room available in buffer - * @arg False buffer is full - */ -unsigned char bufferIsNotFull(cBuffer * buffer) -{ - return (buffer->datalength < buffer->size); -} - -/** - * @brief Trash all data in buffer - * @param[in] buffer Point to the buffer structure - */ -void bufferFlush(cBuffer * buffer) -{ - // flush contents of the buffer - buffer->datalength = 0; -} - -/** - * @} - * @} - */ diff --git a/flight/Libraries/inc/buffer.h b/flight/Libraries/inc/buffer.h deleted file mode 100644 index f0f77ba53..000000000 --- a/flight/Libraries/inc/buffer.h +++ /dev/null @@ -1,105 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup OpenPilotModules OpenPilot Modules - * @{ - * @addtogroup GSPModule GPS Module - * @brief Process GPS information - * @{ - * - * @file buffer.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief see below - * As with all modules only the initialize function is exposed all other - * interactions with the module take place through the event queue and - * objects. - * @see The GNU Public License (GPL) Version 3 - * - *****************************************************************************/ - -//***************************************************************************** -// -// File Name : 'buffer.h' -// Title : Multipurpose byte buffer structure and methods -// Author : Pascal Stang - Copyright (C) 2001-2002 -// Created : 9/23/2001 -// Revised : 11/16/2002 -// Version : 1.1 -// Target MCU : any -// Editor Tabs : 4 -// -/// \code #include "buffer.h" \endcode -/// \par Overview -/// This byte-buffer structure provides an easy and efficient way to store -/// and process a stream of bytes.  You can create as many buffers as you -/// like (within memory limits), and then use this common set of functions to -/// access each buffer.  The buffers are designed for FIFO operation (first -/// in, first out).  This means that the first byte you put in the buffer -/// will be the first one you get when you read out the buffer.  Supported -/// functions include buffer initialize, get byte from front of buffer, add -/// byte to end of buffer, check if buffer is full, and flush buffer.  The -/// buffer uses a circular design so no copying of data is ever necessary. -/// This buffer is not dynamically allocated, it has a user-defined fixed -/// maximum size.  This buffer is used in many places in the avrlib code. -// -// This code is distributed under the GNU Public License -// which can be found at http://www.gnu.org/licenses/gpl.txt -// -//***************************************************************************** -//@{ - -#ifndef BUFFER_H -#define BUFFER_H - -#include "stdint.h" - -// structure/typdefs - -//! cBuffer structure -typedef struct struct_cBuffer { - unsigned char *dataptr; ///< the physical memory address where the buffer is stored - unsigned short size; ///< the allocated size of the buffer - unsigned short datalength; ///< the length of the data currently in the buffer - unsigned short dataindex; ///< the index into the buffer where the data starts -} cBuffer; - -// function prototypes - -//! initialize a buffer to start at a given address and have given size -void bufferInit(cBuffer * buffer, uint8_t * start, uint32_t size); - -//! check free space in buffer -uint32_t bufferRemainingSpace(cBuffer * buffer); - -//! get the first byte from the front of the buffer -uint8_t bufferGetFromFront(cBuffer * buffer); - -//! get the number of bytes buffered -uint32_t bufferBufferedData(cBuffer * buffer); - -//! copy number of elements into another buffer -uint8_t bufferGetChunkFromFront(cBuffer * buffer, uint8_t * dest, uint32_t size); - -//! dump (discard) the first numbytes from the front of the buffer -void bufferDumpFromFront(cBuffer * buffer, uint32_t numbytes); - -//! get a byte at the specified index in the buffer (kind of like array access) -// ** note: this does not remove the byte that was read from the buffer -uint8_t bufferGetAtIndex(cBuffer * buffer, uint32_t index); - -//! add a byte to the end of the buffer -uint8_t bufferAddToEnd(cBuffer * buffer, uint8_t data); - -//! queue a block of character to end of buffer -uint8_t bufferAddChunkToEnd(cBuffer * buffer, const uint8_t * data, uint32_t size); - -//! check if the buffer is full/not full (returns non-zero value if not full) -uint8_t bufferIsNotFull(cBuffer * buffer); - -//! flush (clear) the contents of the buffer -void bufferFlush(cBuffer * buffer); - -#endif - -/** - * @} - */