AHRS姿态融合代码

国外飞控有关姿态融合的一个函数，AHRS，带地磁纠正偏航角

/**
 ******************************************************************************
 * @addtogroup AHRS AHRS
 * @brief The AHRS Modules perform
 *
 * @{
 * @addtogroup AHRS_Main
 * @brief Main function which does the hardware dependent stuff
 * @{
 *
 *
 * @file       ahrs.c
 * @author     The OpenPilot Team http://www.openpilot.org Copyright （C） 2010.
 * @brief      INSGPS Test Program
 * @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
 */

/* OpenPilot Includes */
#include “ahrs.h“
#include 
#include “pios.h“
#include “ahrs_timer.h“
#include “ahrs_spi_comm.h“
#include “insgps.h“
#include “CoordinateConversions.h“
#include 
#include “fifo_buffer.h“

#define DEG_TO_RAD         （M_PI / 180.0）
#define RAD_TO_DEG         （180.0 / M_PI）

#define INSGPS_GPS_TIMEOUT 2   /* 2 seconds triggers reinit of position */
#define INSGPS_GPS_MINSAT  6   /* 2 seconds triggers reinit of position */
#define INSGPS_GPS_MINPDOP 3.5 /* minimum PDOP for postition updates    */
#define INSGPS_MAGLEN      1000
#define INSGPS_MAGTOL      0.5 /* error in magnetic vector length to use  */

#define GYRO_OOB（x） （（x > （1000 * DEG_TO_RAD）） || （x < （-1000 * DEG_TO_RAD）））
#define ACCEL_OOB（x） （（（x > 12*9.81） || （x < -12*9.81）））
#define ISNAN（x） （x != x）
// down-sampled data index
#define ACCEL_RAW_X_IDX			2
#define ACCEL_RAW_Y_IDX			0
#define ACCEL_RAW_Z_IDX			4
#define GYRO_RAW_X_IDX			1
#define GYRO_RAW_Y_IDX			3
#define GYRO_RAW_Z_IDX			5
#define GYRO_TEMP_RAW_XY_IDX	6
#define GYRO_TEMP_RAW_Z_IDX		7
#define MAG_RAW_X_IDX			1
#define MAG_RAW_Y_IDX			0
#define MAG_RAW_Z_IDX			2

// For debugging the raw sensors
//#define DUMP_RAW
//#define DUMP_EKF
//#define PIP_DUMP_RAW

volatile int8_t ahrs_algorithm;

/* INS functions */
void ins_outdoor_update（）;
void ins_indoor_update（）;
void simple_update（）;

/* Data accessors */
void adc_callback（float *）;
bool get_accel_gyro_data（）;
void process_mag_data（）;
void reset_values（）;
void calibrate_sensors（void）;

/* Communication functions */
void send_calibration（void）;
void send_attitude（void）;
void send_velocity（void）;
void send_positi

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件      42418  2012-03-23 18:04  AHRS\ahrs.c

     文件       1865  2012-03-23 18:04  AHRS\ahrs_timer.c

     文件      62531  2012-03-23 18:04  AHRS\insgps13state.c

     文件      69599  2012-03-23 18:04  AHRS\insgps16state.c

     文件      13458  2012-03-23 18:04  AHRS\Makefile

     文件      11588  2012-03-23 18:04  AHRS\pios_board.c

     文件        839  2012-03-23 18:04  AHRS\test.c

     文件       2510  2012-03-23 18:04  AHRS\inc\ahrs.h

     文件       2977  2012-03-23 18:04  AHRS\inc\ahrs_fsm.h

     文件       1489  2012-03-23 18:04  AHRS\inc\ahrs_timer.h

     文件       3071  2012-03-23 18:04  AHRS\inc\insgps.h

     文件       1666  2012-03-23 18:04  AHRS\inc\pios_config.h

     目录          0  2012-05-21 11:28  AHRS\inc

     目录          0  2012-05-21 11:28  AHRS

----------- ---------  ---------- -----  ----

               214011                    14