Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-29 02:38:07 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

Merge pull request #81 from paparazzi/imu_analog_gyro

Browse Source 
replace separate roll and pitch gyro subsystems with an imu subsystem
This commit is contained in:
Felix Ruess
2011-11-24 06:33:41 -08:00
parent 0a6903069a fe0151c005
commit 83601512c4
28 changed files with 162 additions and 255 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/jsbsim.xml
+7 -14
View File
@@ -50,10 +50,6 @@
<define name="IR2" value="ADC_2"/>
<define name="IR_TOP" value="ADC_0"/>
<define name="IR_NB_SAMPLES" value="16"/>
<define name="GYRO_ROLL" value="ADC_3"/>
<define name="GYRO_NB_SAMPLES" value="16"/>
</section>
<section name="INFRARED" prefix="IR_">
@@ -78,15 +74,10 @@
<define name="CORRECTION_RIGHT" value="1."/>
</section>
<section name="GYRO" prefix="GYRO_">
<define name="ADC_ROLL_NEUTRAL" value="520"/>
<define name="ADC_TEMP_NEUTRAL" value="476"/>
<define name="ADC_TEMP_SLOPE" value="0"/>
<define name="DYNAMIC_RANGE" value="300" unit="deg/s"/>
<define name="ADXRS300_RESISTOR_BRIDGE" value="(3.3/(3.3+1.8))"/>
<define name="ADXRS300_SENSITIVITY" value="5" unit="mV/(deg/s)"/>
<define name="ROLL_SCALE" value="3.3*1000./1024./(GYRO_ADXRS300_SENSITIVITY*GYRO_ADXRS300_RESISTOR_BRIDGE)" unit="deg/s/adc_unit"/>
<define name="ROLL_DIRECTION" value="-1."/>
<section name="IMU" prefix="IMU_">
<define name="GYRO_P_SIGN" value="-1."/>
<define name="GYRO_P_NEUTRAL" value="512"/>
<define name="GYRO_P_SENS" value="1." integer="16"/>
</section>
<section name="BAT">
@@ -215,7 +206,9 @@
<subsystem name="control"/>
<!-- Sensors -->
<subsystem name="gyro" type="roll"/>
<subsystem name="imu" type="analog">
<configure name="GYRO_P" value="ADC_3"/>
</subsystem>
<subsystem name="ahrs" type="infrared"/>
<subsystem name="gps" type="ublox_utm"/>
<subsystem name="navigation"/>
conf/airframes/microjet_example.xml
+14 -10
View File
@@ -68,13 +68,15 @@
<define name="CORRECTION_RIGHT" value="1."/>
</section>
<section name="GYRO" prefix="GYRO_">
<define name="ADC_ROLL_NEUTRAL" value="500"/>
<define name="DYNAMIC_RANGE" value="300" unit="deg/s"/>
<define name="ADXRS300_RESISTOR_BRIDGE" value="(3.3/(3.3+1.8))"/>
<define name="ADXRS300_SENSITIVITY" value="5" unit="mV/(deg/s)"/>
<define name="ROLL_SCALE" value="3.3*1000./1024./(GYRO_ADXRS300_SENSITIVITY*GYRO_ADXRS300_RESISTOR_BRIDGE)" unit="deg/s/adc_unit"/>
<define name="ROLL_DIRECTION" value="1."/>
<section name="IMU" prefix="IMU_">
<define name="GYRO_P_SIGN" value="1"/>
<define name="GYRO_P_NEUTRAL" value="500"/>
<!--define name="ADXRS300_RESISTOR_BRIDGE" value="(3.3/(3.3+1.8))"/-->
<!--define name="ADXRS300_SENSITIVITY" value="5" unit="mV/(deg/s)"/-->
<!--define name="ROLL_SCALE" value="3.3*1000./1024./(IMU_ADXRS300_SENSITIVITY*IMU_ADXRS300_RESISTOR_BRIDGE)" unit="deg/s/adc_unit"/-->
<!--define name="RATE_DEGS_TO_RADINT" value="M_PI/180./4096."/-->
<!--define name="GYRO_P_SENS" value="IMU_RATE_DEGS_TO_RADINT * IMU_ROLL_SCALE" integer="16"/-->
<define name="GYRO_P_SENS" value="0.137518981" integer="16"/>
</section>
<section name="BAT">
@@ -188,9 +190,11 @@
<subsystem name="control"/>
<!-- Sensors -->
<subsystem name="gyro" type="roll"/>
<subsystem name="ahrs" type="infrared"/>
<subsystem name="gps" type="ublox_utm"/>
<subsystem name="imu" type="analog">
<configure name="GYRO_P" value="ADC_3"/>
</subsystem>
<subsystem name="ahrs" type="infrared"/>
<subsystem name="gps" type="ublox_utm"/>
<subsystem name="navigation"/>
</firmware>
conf/airframes/microjet_raw_makefile.xml → conf/airframes/obsolete/microjet_raw_makefile.xml
View File
conf/airframes/osam_xsens_twog.xml
-10
View File
@@ -123,16 +123,6 @@
<define name="PITCH_NEUTRAL_DEFAULT" value="5.73" unit="deg"/>
</section>
<!--
<section name="GYRO" prefix="GYRO_">
<define name="ADC_ROLL_NEUTRAL" value="497"/>
<define name="DYNAMIC_RANGE" value="300" unit="deg/s"/>
<define name="ADXRS300_RESISTOR_BRIDGE" value="(3.3/(3.3+1.8))"/>
<define name="ADXRS300_SENSITIVITY" value="5" unit="mV/(dNOMINALeg/s)"/>
<define name="ROLL_SCALE" value="3.3*1000./1024./(GYRO_ADXRS300_SENSITIVITY*GYRO_ADXRS300_RESISTOR_BRIDGE)" unit="deg/s/adc_unit"/>
<define name="ROLL_DIRECTION" value="1."/>
</section>
-->
<section name="BAT">
<define name="MILLIAMP_AT_FULL_THROTTLE" value="10000."/>
<define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
conf/airframes/twinjet_example.xml
+7 -5
View File
@@ -22,7 +22,9 @@
<subsystem name="telemetry" type="transparent"/>
<subsystem name="control"/>
<subsystem name="ahrs" type="infrared"/>
<subsystem name="gyro" type="roll"/>
<subsystem name="imu" type="analog">
<configure name="GYRO_P" value="ADC_3"/>
</subsystem>
<subsystem name="gps" type="ublox"/>
<subsystem name="navigation"/>
</firmware>
@@ -99,10 +101,10 @@
<define name="CORRECTION_RIGHT" value="1."/>
</section>
<section name="GYRO" prefix="GYRO_">
<define name="ADC_ROLL_NEUTRAL" value="316"/>
<define name="ROLL_DIRECTION" value="1."/>
<define name="ROLL_SCALE" value="0.44"/>
<section name="IMU" prefix="IMU_">
<define name="GYRO_P_SIGN" value="1."/>
<define name="GYRO_P_NEUTRAL" value="316"/>
<define name="GYRO_P_SENS" value="1." integer="16"/>
</section>
<section name="HORIZONTAL CONTROL" prefix="H_CTL_">
conf/airframes/twinjet_overo.xml
+1 -8
View File
@@ -24,8 +24,7 @@
<subsystem name="radio_control" type="ppm"/>
<subsystem name="telemetry" type="transparent"/>
<subsystem name="control"/>
<subsystem name="ahrs" type="infrared"/>
<subsystem name="gyro" type="roll"/>
<subsystem name="ahrs" type="infrared"/>
<subsystem name="gps" type="ublox_hitl"/>
<subsystem name="navigation"/>
<subsystem name="joystick"/>
@@ -110,12 +109,6 @@
<define name="CORRECTION_RIGHT" value="1."/>
</section>
<section name="GYRO" prefix="GYRO_">
<define name="ADC_ROLL_NEUTRAL" value="316"/>
<define name="ROLL_DIRECTION" value="1."/>
<define name="ROLL_SCALE" value="0.44"/>
</section>
<section name="HORIZONTAL CONTROL" prefix="H_CTL_">
<define name="COURSE_PGAIN" value="-0.9"/>
conf/autopilot/subsystems/fixedwing/gyro_pitch.makefile
+8 -13
View File
@@ -1,14 +1,9 @@
# Hey Emacs, this is a -*- makefile -*-
# additional pitch stabilization with gyro
#
# this assumes you are already using a roll gyro
#
# default values for tiny and twog are:
# ADC_GYRO_PITCH = ADC_4
#
# to change just redefine these before including this file
#
ap.CFLAGS += -DADC_CHANNEL_GYRO_PITCH=$(ADC_GYRO_PITCH) -DUSE_$(ADC_GYRO_PITCH)
$(info Error: Please replace <subsystem name="gyro" type="pitch"/> with)
$(info <subsystem name="imu" type="analog">)
$(info <configure name="GYRO_P" value="ADC_3"/>)
$(info <configure name="GYRO_Q" value="ADC_4"/>)
$(info </subsystem>)
$(info and replace the GYRO section with the appropriate IMU section
$(info in your airframe file.)
$(error The gyro_pitch subsystem has been removed)
conf/autopilot/subsystems/fixedwing/gyro_roll.makefile
+7 -16
View File
@@ -1,17 +1,8 @@
# Hey Emacs, this is a -*- makefile -*-
$(info Error: Please replace <subsystem name="gyro" type="roll"/> with)
$(info <subsystem name="imu" type="analog">)
$(info <configure name="GYRO_P" value="ADC_3"/>)
$(info </subsystem>)
$(info and replace the GYRO section with the appropriate IMU section
$(info in your airframe file.)
# roll stabilization with gyro
#
# default values for tiny and twog are:
# ADC_GYRO_ROLL = ADC_3
# ADC_GYRO_NB_SAMPLES = 16
#
# to change just redefine these before including this file
#
ap.CFLAGS += -DADC_CHANNEL_GYRO_ROLL=$(ADC_GYRO_ROLL) -DUSE_$(ADC_GYRO_ROLL)
ap.CFLAGS += -DADC_CHANNEL_GYRO_NB_SAMPLES=$(ADC_GYRO_NB_SAMPLES)
ap.CFLAGS += -DUSE_GYRO -DADXRS150
ap.srcs += $(SRC_FIXEDWING)/gyro.c
$(error The gyro_pitch subsystem has been removed)
conf/autopilot/subsystems/fixedwing/imu_analog.makefile
+32 -4
View File
@@ -3,6 +3,9 @@
#
# Analog IMU connected to MCU ADC ports
#
# Only add the configure and define lines for the sensors you actually use.
# E.g. to replace the old gyro_pitch subsystem only add GYRO_P and GYRO_Q
#
#
# <subsystem name="imu" type="analog">
# <configure name="GYRO_P" value="ADC_0"/>
@@ -49,11 +52,36 @@ ifeq ($(ARCH), lpc21)
imu_CFLAGS += -DIMU_TYPE_H=\"subsystems/imu/imu_analog.h\" -DUSE_IMU
imu_CFLAGS += -DADC
imu_CFLAGS += -DUSE_$(GYRO_P) -DUSE_$(GYRO_Q) -DUSE_$(GYRO_R)
imu_CFLAGS += -DUSE_$(ACCEL_X) -DUSE_$(ACCEL_Y) -DUSE_$(ACCEL_Z)
imu_CFLAGS += -DADC_CHANNEL_GYRO_NB_SAMPLES=$(ADC_GYRO_NB_SAMPLES)
imu_CFLAGS += -DADC_CHANNEL_GYRO_P=$(GYRO_P) -DADC_CHANNEL_GYRO_Q=$(GYRO_Q) -DADC_CHANNEL_GYRO_R=$(GYRO_R)
imu_CFLAGS += -DADC_CHANNEL_ACCEL_X=$(ACCEL_X) -DADC_CHANNEL_ACCEL_Y=$(ACCEL_Y) -DADC_CHANNEL_ACCEL_Z=$(ACCEL_Z)
ifeq ($(ADC_ACCEL_NB_SAMPLES),)
ADC_ACCEL_NB_SAMPLES = $(ADC_GYRO_NB_SAMPLES)
endif
imu_CFLAGS += -DADC_CHANNEL_ACCEL_NB_SAMPLES=$(ADC_ACCEL_NB_SAMPLES)
ifneq ($(GYRO_P),)
imu_CFLAGS += -DADC_CHANNEL_GYRO_P=$(GYRO_P) -DUSE_$(GYRO_P)
endif
ifneq ($(GYRO_Q),)
imu_CFLAGS += -DADC_CHANNEL_GYRO_Q=$(GYRO_Q) -DUSE_$(GYRO_Q)
endif
ifneq ($(GYRO_R),)
imu_CFLAGS += -DADC_CHANNEL_GYRO_R=$(GYRO_R) -DUSE_$(GYRO_R)
endif
ifneq ($(ACCEL_X),)
imu_CFLAGS += -DADC_CHANNEL_ACCEL_X=$(ACCEL_X) -DUSE_$(ACCEL_X)
endif
ifneq ($(ACCEL_Y),)
imu_CFLAGS += -DADC_CHANNEL_ACCEL_Y=$(ACCEL_Y) -DUSE_$(ACCEL_Y)
endif
ifneq ($(ACCEL_Z),)
imu_CFLAGS += -DADC_CHANNEL_ACCEL_Z=$(ACCEL_Z) -DUSE_$(ACCEL_Z)
endif
imu_srcs += $(SRC_SUBSYSTEMS)/imu.c
imu_srcs += $(SRC_SUBSYSTEMS)/imu/imu_analog.c
conf/boards/tiny_2.11.makefile
-2
View File
@@ -40,8 +40,6 @@ ADC_IR_TOP = ADC_0
ADC_IR1 = ADC_1
ADC_IR2 = ADC_2
ADC_IR_NB_SAMPLES = 16
ADC_GYRO_ROLL = ADC_3
ADC_GYRO_PITCH = ADC_4
ADC_GYRO_NB_SAMPLES = 16
ADC_GENERIC_NB_SAMPLES = 16
conf/messages.xml
+1 -5
View File
@@ -244,11 +244,7 @@
<field name="utm_zone" type="uint8"/>
</message>
<message name="GYRO_RATES" id="36">
<field name="roll_adc" type="int16"/>
<field name="roll" type="float" unit="rad/s"/>
<field name="pitch" type="float" unit="rad/s"/>
</message>
<!-- 36 is free -->
<message name="ENERGY" id="37">
<field name="bat" type="float" unit="V"/>
conf/telemetry/aerocomm.xml
+1 -1
View File
@@ -18,7 +18,7 @@
<message name="DOWNLINK" period="7.1"/>
<message name="DL_VALUE" period="1.8"/>
<message name="IR_SENSORS" period="3.1"/>
<message name="GYRO_RATES" period="2.9"/>
<message name="IMU_GYRO" period="2.9"/>
<message name="SURVEY" period="2.7"/>
<message name="GPS_SOL" period="2.0"/>
<message name="AIRSPEED" period="0.5"/>
conf/telemetry/default.xml
+2 -2
View File
@@ -25,7 +25,7 @@
<message name="DOWNLINK" period="5.1"/>
<message name="DL_VALUE" period="1.5"/>
<message name="IR_SENSORS" period="1.2"/>
<message name="GYRO_RATES" period="1.1"/>
<message name="IMU_GYRO" period="1.1"/>
<message name="SURVEY" period="2.1"/>
<message name="GPS_SOL" period="2.0"/>
</mode>
@@ -47,7 +47,7 @@
<message name="DOWNLINK" period="5.1"/>
<message name="DL_VALUE" period="1.5"/>
<message name="IR_SENSORS" period="5.2"/>
<message name="GYRO_RATES" period="10.1"/>
<message name="IMU_GYRO" period="10.1"/>
<message name="SURVEY" period="2.1"/>
<message name="GPS_SOL" period="5.0"/>
</mode>
conf/telemetry/default_fixedwing_imu.xml
-2
View File
@@ -25,7 +25,6 @@
<message name="DOWNLINK" period="5.1"/>
<message name="DL_VALUE" period="1.5"/>
<message name="IR_SENSORS" period="1.2"/>
<message name="GYRO_RATES" period="1.1"/>
<message name="SURVEY" period="2.1"/>
<message name="GPS_SOL" period="2.0"/>
<message name="IMU_ACCEL" period=".8"/>
@@ -50,7 +49,6 @@
<message name="DOWNLINK" period="5.1"/>
<message name="DL_VALUE" period="1.5"/>
<message name="IR_SENSORS" period="5.2"/>
<message name="GYRO_RATES" period="10.1"/>
<message name="SURVEY" period="2.1"/>
<message name="GPS_SOL" period="5.0"/>
</mode>
conf/telemetry/default_fixedwing_imu_9k6.xml
-2
View File
@@ -25,7 +25,6 @@
<message name="DOWNLINK" period="5.1"/>
<message name="DL_VALUE" period="1.2"/>
<message name="IR_SENSORS" period="1.2"/>
<message name="GYRO_RATES" period="1.3"/>
<message name="SURVEY" period="2.1"/>
<message name="GPS_SOL" period="2.0"/>
<message name="IMU_ACCEL" period=".8"/>
@@ -50,7 +49,6 @@
<message name="DOWNLINK" period="5.1"/>
<message name="DL_VALUE" period="1.5"/>
<message name="IR_SENSORS" period="5.2"/>
<message name="GYRO_RATES" period="10.1"/>
<message name="SURVEY" period="2.1"/>
<message name="GPS_SOL" period="5.0"/>
</mode>
conf/telemetry/fw_h_ctl_a.xml
+1 -1
View File
@@ -19,7 +19,7 @@
<message name="DOWNLINK" period="5.1"/>
<message name="DL_VALUE" period="1.5"/>
<message name="IR_SENSORS" period="1.2"/>
<message name="GYRO_RATES" period="0.2"/>
<message name="IMU_GYRO" period="0.2"/>
<message name="SURVEY" period="2.1"/>
<message name="GPS_SOL" period="2.0"/>
</mode>
conf/telemetry/hitl.xml
-1
View File
@@ -17,7 +17,6 @@
<message name="DOWNLINK" period="5.1"/>
<message name="DL_VALUE" period="1.5"/>
<message name="IR_SENSORS" period="1.2"/>
<message name="GYRO_RATES" period="1.1"/>
<message name="SURVEY" period="2.1"/>
<message name="GPS_SOL" period="2.0"/>
</mode>
conf/telemetry/minimal.xml
+1 -1
View File
@@ -20,7 +20,7 @@
<message name="DOWNLINK" period="5.1"/>
<message name="DL_VALUE" period="1.5"/>
<message name="IR_SENSORS" period="1.2"/>
<message name="GYRO_RATES" period="2.1"/>
<message name="IMU_GYRO" period="2.1"/>
<message name="SURVEY" period="2.1"/>
<message name="GPS_SOL" period="2.0"/>
</mode>
conf/telemetry/osam_imu.xml
+1 -1
View File
@@ -18,7 +18,7 @@
<message name="DOWNLINK" period="5.1"/>
<message name="DL_VALUE" period="1.5"/>
<message name="IR_SENSORS" period="1.2"/>
<message name="GYRO_RATES" period="1.1"/>
<message name="IMU_GYRO" period="1.1"/>
<message name="SURVEY" period="2.1"/>
<message name="GPS_SOL" period="2.0"/>
<message name="GPS" period="1.0"/>
sw/airborne/ap_downlink.h
-8
View File
@@ -132,14 +132,6 @@
#define PERIODIC_SEND_ADC(_chan) {}
#ifdef IDG300
#include "gyro.h"
#define PERIODIC_SEND_GYRO_RATES(_chan) DOWNLINK_SEND_GYRO_RATES(_chan, &roll_rate_adc, &estimator_p, &estimator_q)
#elif defined ADXRS150
#define PERIODIC_SEND_GYRO_RATES(_chan) DOWNLINK_SEND_GYRO_RATES(_chan, &roll_rate_adc, &estimator_p, &temp_comp)
#else
#define PERIODIC_SEND_GYRO_RATES(_chan) {}
#endif
#define PERIODIC_SEND_CALIBRATION(_chan) DOWNLINK_SEND_CALIBRATION(_chan, &v_ctl_auto_throttle_sum_err, &v_ctl_auto_throttle_submode)
sw/airborne/firmwares/fixedwing/main_ap.c
-8
View File
@@ -39,7 +39,6 @@
#include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
#include "firmwares/fixedwing/guidance/guidance_v.h"
#include "subsystems/gps.h"
#include "gyro.h"
#include "ap_downlink.h"
#include "subsystems/nav.h"
#include "firmwares/fixedwing/autopilot.h"
@@ -392,10 +391,6 @@ static void navigation_task( void ) {
static inline void attitude_loop( void ) {
#ifdef USE_GYRO
gyro_update();
#endif
#ifdef USE_INFRARED
ahrs_update_infrared();
#endif /* USE_INFRARED */
@@ -540,9 +535,6 @@ void init_ap( void ) {
#endif /* SINGLE_MCU */
/************* Sensors initialization ***************/
#ifdef USE_GYRO
gyro_init();
#endif
#ifdef USE_GPS
gps_init();
#endif
sw/airborne/gyro.c
-81
View File
@@ -1,81 +0,0 @@
/*
* $Id$
*
* Copyright (C) 2006 Pascal Brisset, Antoine Drouin
*
* This file is part of paparazzi.
*
* paparazzi 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 2, or (at your option)
* any later version.
*
* paparazzi 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 paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*/
/** \file gyro.c
* \brief Basic code for gyro acquisition on ADC channels
*
*/
#include BOARD_CONFIG
#include "gyro.h"
#include "std.h"
#include "mcu_periph/adc.h"
#include "generated/airframe.h"
#include "estimator.h"
int16_t roll_rate_adc;
float temp_comp; /* Needed for the downlinked message */
static struct adc_buf buf_roll;
#define RadiansOfADC(_adc, scale) RadOfDeg((_adc * scale))
#if defined ADC_CHANNEL_GYRO_TEMP
static struct adc_buf buf_temp;
#endif
#if defined ADC_CHANNEL_GYRO_PITCH
int16_t pitch_rate_adc;
static struct adc_buf buf_pitch;
#endif
void gyro_init( void) {
#ifndef SITL
adc_buf_channel(ADC_CHANNEL_GYRO_ROLL, &buf_roll, ADC_CHANNEL_GYRO_NB_SAMPLES);
#if defined ADC_CHANNEL_GYRO_TEMP
adc_buf_channel(ADC_CHANNEL_GYRO_TEMP, &buf_temp, ADC_CHANNEL_GYRO_NB_SAMPLES);
#endif
#if defined ADC_CHANNEL_GYRO_PITCH
adc_buf_channel(ADC_CHANNEL_GYRO_PITCH, &buf_pitch, ADC_CHANNEL_GYRO_NB_SAMPLES);
#endif
#endif /* SITL */
}
void gyro_update( void ) {
#ifndef SITL
float pitch_rate = 0.;
roll_rate_adc = (buf_roll.sum/buf_roll.av_nb_sample) - GYRO_ADC_ROLL_NEUTRAL;
#if defined ADC_CHANNEL_GYRO_TEMP
temp_comp = buf_temp.sum/buf_temp.av_nb_sample - GYRO_ADC_TEMP_NEUTRAL;
roll_rate_adc += GYRO_ADC_TEMP_SLOPE * temp_comp;
#endif
#if defined IDG300
pitch_rate_adc = buf_pitch.sum/buf_pitch.av_nb_sample - GYRO_ADC_PITCH_NEUTRAL;
pitch_rate = GYRO_PITCH_DIRECTION * RadiansOfADC(pitch_rate_adc, GYRO_PITCH_SCALE);
#endif
float roll_rate = GYRO_ROLL_DIRECTION * RadiansOfADC(roll_rate_adc, GYRO_ROLL_SCALE);
EstimatorSetRate(roll_rate, pitch_rate);
#endif /* SITL */
}
sw/airborne/gyro.h
-55
View File
@@ -1,55 +0,0 @@
/*
* $Id$
*
* Copyright (C) 2006 Pascal Brisset, Antoine Drouin
*
* This file is part of paparazzi.
*
* paparazzi 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 2, or (at your option)
* any later version.
*
* paparazzi 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 paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*/
/** \file gyro.h
* \brief Basic code for gyro acquisition on ADC channels
*
*/
#ifndef GYRO_H
#define GYRO_H
#include <inttypes.h>
#ifdef GYRO
#error "The flag GYRO has been deprecated. Please replace it with USE_GYRO."
#endif
/** Raw (for debug), taking into accound neutral and temp compensation (if any) */
extern int16_t roll_rate_adc;
/** Hardware dependent code */
#if defined ADXRS150
extern float temp_comp;
#elif defined IDG300
extern int16_t pitch_rate_adc;
#endif
void gyro_init( void );
/** Sets roll_rate_adc and pitch_rate_adc (or temp_comp), and estimator_p */
void gyro_update( void );
#endif /* GYRO_H */
sw/airborne/subsystems/ahrs/ahrs_infrared.c
+7
View File
@@ -22,6 +22,7 @@
#include "subsystems/ahrs/ahrs_infrared.h"
#include "subsystems/sensors/infrared.h"
#include "subsystems/imu.h"
#include "subsystems/gps.h"
#include "estimator.h"
@@ -52,6 +53,12 @@ void ahrs_align(void) {
}
void ahrs_propagate(void) {
#ifdef ADC_CHANNEL_GYRO_P
ahrs_float.body_rate.p = RATE_FLOAT_OF_BFP(imu.gyro.p);
#endif
#ifdef ADC_CHANNEL_GYRO_Q
ahrs_float.body_rate.q = RATE_FLOAT_OF_BFP(imu.gyro.q);
#endif
}
void ahrs_update_accel(void) {
sw/airborne/subsystems/imu.c
+14 -3
View File
@@ -29,15 +29,26 @@ void imu_init(void) {
/* initialises neutrals */
RATES_ASSIGN(imu.gyro_neutral, IMU_GYRO_P_NEUTRAL, IMU_GYRO_Q_NEUTRAL, IMU_GYRO_R_NEUTRAL);
VECT3_ASSIGN(imu.accel_neutral, IMU_ACCEL_X_NEUTRAL, IMU_ACCEL_Y_NEUTRAL, IMU_ACCEL_Z_NEUTRAL);
//FIXME should not assume that every imu has a mag and this id defined?
#if defined IMU_ACCEL_X_NEUTRAL && defined IMU_ACCEL_Y_NEUTRAL && defined IMU_ACCEL_Z_NEUTRAL
INT_VECT3_ASSIGN(imu.accel_neutral, IMU_ACCEL_X_NEUTRAL, IMU_ACCEL_Y_NEUTRAL, IMU_ACCEL_Z_NEUTRAL);
#else
#warning Accelerometer neutrals are set to zero.
INT_VECT3_ZERO(imu.accel_neutral);
#endif
#if defined IMU_MAG_X_NEUTRAL && defined IMU_MAG_Y_NEUTRAL && defined IMU_MAG_Z_NEUTRAL
VECT3_ASSIGN(imu.mag_neutral, IMU_MAG_X_NEUTRAL, IMU_MAG_Y_NEUTRAL, IMU_MAG_Z_NEUTRAL);
#else
#warning Magnetomter neutrals are set to zero.
INT_VECT3_ZERO(imu.mag_neutral);
#endif
/*
Compute quaternion and rotation matrix
for conversions between body and imu frame
*/
#if defined IMU_BODY_TO_IMU_PHI && defined IMU_BODY_TO_IMU_THETA & defined IMU_BODY_TO_IMU_PSI
#if defined IMU_BODY_TO_IMU_PHI && defined IMU_BODY_TO_IMU_THETA & defined IMU_BODY_TO_IMU_PSI
struct Int32Eulers body_to_imu_eulers =
{ ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_PHI),
ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_THETA),
sw/airborne/subsystems/imu.h
+4
View File
@@ -70,20 +70,24 @@ extern struct Imu imu;
extern void imu_init(void);
#ifndef ImuScaleGyro
#define ImuScaleGyro(_imu) { \
RATES_COPY(_imu.gyro_prev, _imu.gyro); \
_imu.gyro.p = ((_imu.gyro_unscaled.p - _imu.gyro_neutral.p)*IMU_GYRO_P_SIGN*IMU_GYRO_P_SENS_NUM)/IMU_GYRO_P_SENS_DEN; \
_imu.gyro.q = ((_imu.gyro_unscaled.q - _imu.gyro_neutral.q)*IMU_GYRO_Q_SIGN*IMU_GYRO_Q_SENS_NUM)/IMU_GYRO_Q_SENS_DEN; \
_imu.gyro.r = ((_imu.gyro_unscaled.r - _imu.gyro_neutral.r)*IMU_GYRO_R_SIGN*IMU_GYRO_R_SENS_NUM)/IMU_GYRO_R_SENS_DEN; \
}
#endif
#ifndef ImuScaleAccel
#define ImuScaleAccel(_imu) { \
VECT3_COPY(_imu.accel_prev, _imu.accel); \
_imu.accel.x = ((_imu.accel_unscaled.x - _imu.accel_neutral.x)*IMU_ACCEL_X_SIGN*IMU_ACCEL_X_SENS_NUM)/IMU_ACCEL_X_SENS_DEN; \
_imu.accel.y = ((_imu.accel_unscaled.y - _imu.accel_neutral.y)*IMU_ACCEL_Y_SIGN*IMU_ACCEL_Y_SENS_NUM)/IMU_ACCEL_Y_SENS_DEN; \
_imu.accel.z = ((_imu.accel_unscaled.z - _imu.accel_neutral.z)*IMU_ACCEL_Z_SIGN*IMU_ACCEL_Z_SENS_NUM)/IMU_ACCEL_Z_SENS_DEN; \
}
#endif
#if defined IMU_MAG_45_HACK
#define ImuScaleMag(_imu) { \
sw/airborne/subsystems/imu/imu_analog.c
+24 -1
View File
@@ -21,7 +21,6 @@
#include "imu_analog.h"
#include "mcu_periph/adc.h"
#include "mcu_periph/uart.h"
volatile bool_t analog_imu_available;
int imu_overrun;
@@ -33,12 +32,24 @@ void imu_impl_init(void) {
analog_imu_available = FALSE;
imu_overrun = 0;
#ifdef ADC_CHANNEL_GYRO_P
adc_buf_channel(ADC_CHANNEL_GYRO_P, &analog_imu_adc_buf[0], ADC_CHANNEL_GYRO_NB_SAMPLES);
#endif
#ifdef ADC_CHANNEL_GYRO_Q
adc_buf_channel(ADC_CHANNEL_GYRO_Q, &analog_imu_adc_buf[1], ADC_CHANNEL_GYRO_NB_SAMPLES);
#endif
#ifdef ADC_CHANNEL_GYRO_R
adc_buf_channel(ADC_CHANNEL_GYRO_R, &analog_imu_adc_buf[2], ADC_CHANNEL_GYRO_NB_SAMPLES);
#endif
#ifdef ADC_CHANNEL_ACCEL_X
adc_buf_channel(ADC_CHANNEL_ACCEL_X, &analog_imu_adc_buf[3], ADC_CHANNEL_ACCEL_NB_SAMPLES);
#endif
#ifdef ADC_CHANNEL_ACCEL_Y
adc_buf_channel(ADC_CHANNEL_ACCEL_Y, &analog_imu_adc_buf[4], ADC_CHANNEL_ACCEL_NB_SAMPLES);
#endif
#ifdef ADC_CHANNEL_ACCEL_Z
adc_buf_channel(ADC_CHANNEL_ACCEL_Z, &analog_imu_adc_buf[5], ADC_CHANNEL_ACCEL_NB_SAMPLES);
#endif
}
@@ -52,12 +63,24 @@ void imu_periodic(void) {
last_head = analog_imu_adc_buf[0].head;
// Read All Measurements
#ifdef ADC_CHANNEL_GYRO_P
imu.gyro_unscaled.p = analog_imu_adc_buf[0].sum / ADC_CHANNEL_GYRO_NB_SAMPLES;
#endif
#ifdef ADC_CHANNEL_GYRO_Q
imu.gyro_unscaled.q = analog_imu_adc_buf[1].sum / ADC_CHANNEL_GYRO_NB_SAMPLES;
#endif
#ifdef ADC_CHANNEL_GYRO_R
imu.gyro_unscaled.r = analog_imu_adc_buf[2].sum / ADC_CHANNEL_GYRO_NB_SAMPLES;
#endif
#ifdef ADC_CHANNEL_ACCEL_X
imu.accel_unscaled.x = analog_imu_adc_buf[3].sum / ADC_CHANNEL_ACCEL_NB_SAMPLES;
#endif
#ifdef ADC_CHANNEL_ACCEL_Y
imu.accel_unscaled.y = analog_imu_adc_buf[4].sum / ADC_CHANNEL_ACCEL_NB_SAMPLES;
#endif
#ifdef ADC_CHANNEL_ACCEL_Z
imu.accel_unscaled.z = analog_imu_adc_buf[5].sum / ADC_CHANNEL_ACCEL_NB_SAMPLES;
#endif
analog_imu_available = TRUE;
}
sw/airborne/subsystems/imu/imu_analog.h
+30 -1
View File
@@ -22,10 +22,39 @@
#ifndef IMU_ANALOG_H
#define IMU_ANALOG_H
#include "subsystems/imu.h"
#define NB_ANALOG_IMU_ADC 6
// if not all gyros are used, override the ImuScaleGyro handler
#if defined ADC_CHANNEL_GYRO_P && defined ADC_CHANNEL_GYRO_Q && ! defined ADC_CHANNEL_GYRO_R
#define IMU_GYRO_R_NEUTRAL 0
#define ImuScaleGyro(_imu) { \
_imu.gyro.p = ((_imu.gyro_unscaled.p - _imu.gyro_neutral.p)*IMU_GYRO_P_SIGN*IMU_GYRO_P_SENS_NUM)/IMU_GYRO_P_SENS_DEN; \
_imu.gyro.q = ((_imu.gyro_unscaled.q - _imu.gyro_neutral.q)*IMU_GYRO_Q_SIGN*IMU_GYRO_Q_SENS_NUM)/IMU_GYRO_Q_SENS_DEN; \
}
#elif defined ADC_CHANNEL_GYRO_P && ! defined ADC_CHANNEL_GYRO_Q && ! defined ADC_CHANNEL_GYRO_R
#define IMU_GYRO_Q_NEUTRAL 0
#define IMU_GYRO_R_NEUTRAL 0
#define ImuScaleGyro(_imu) { \
_imu.gyro.p = ((_imu.gyro_unscaled.p - _imu.gyro_neutral.p)*IMU_GYRO_P_SIGN*IMU_GYRO_P_SENS_NUM)/IMU_GYRO_P_SENS_DEN; \
}
#endif
// if we don't have any accelerometers, set an empty ImuScaleAccel handler
#if ! defined ADC_CHANNEL_ACCEL_X && ! defined ADC_CHANNEL_ACCEL_Z && ! defined ADC_CHANNEL_ACCEL_Z
#define ImuScaleAccel(_imu) {}
#endif
/*
* we include imh.h after the definitions of ImuScale so we can override the default handlers
*/
#include "subsystems/imu.h"
extern volatile bool_t analog_imu_available;
extern int imu_overrun;