Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-31 03:57:45 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

PPZUAV-IMU universal module driver without interrupts

Browse Source
This commit is contained in:
Christophe De Wagter
2011-05-04 21:57:33 +02:00
parent ea9a9cc4fa
commit 15bf773a94
5 changed files with 315 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/CDW/PPZIMUTinyFw.xml → conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
+54 -24
View File
@@ -8,8 +8,8 @@
<servos> <servos>
<servo name="THROTTLE" no="0" min="1000" neutral="1000" max="2000"/> <servo name="THROTTLE" no="0" min="1000" neutral="1000" max="2000"/>
<servo name="AILERON_LEFT" no="1" min="1000" neutral="1500" max="2000"/> <servo name="AILERON_LEFT" no="2" min="1000" neutral="1500" max="2000"/>
<servo name="AILERON_RIGHT" no="2" min="2000" neutral="1500" max="1000"/> <servo name="AILERON_RIGHT" no="6" min="2000" neutral="1500" max="1000"/>
<servo name="ELEVATOR" no="3" min="2000" neutral="1500" max="1000"/> <servo name="ELEVATOR" no="3" min="2000" neutral="1500" max="1000"/>
<servo name="RUDDER" no="4" min="1100" neutral="1500" max="1900"/> <servo name="RUDDER" no="4" min="1100" neutral="1500" max="1900"/>
</servos> </servos>
@@ -42,26 +42,39 @@
<define name="BRAKE_PITCH" value="0.0f"/> <define name="BRAKE_PITCH" value="0.0f"/>
<define name="MAX_BRAKE_RATE" value="150"/> <define name="MAX_BRAKE_RATE" value="150"/>
<define name="LIMIT(X,XL,XH)" value="( ((X)>(XH)) ? (XH) : ( ((X)>(XL)) ? (X) : (XL) ) )"/> <define name="RATELIMIT(X,RATE)" value="( _rate_limited_value + Chop( (X) - _rate_limited_value, -(RATE), (RATE) )); \
</section> int16_t _rate_limited_value = 0;"/>
</section>
<command_laws> <command_laws>
<!-- Brake Rate Limiter -->
<let var="brake_value" value="Chop(-@BRAKE, 0, MAX_PPRZ)"/>
<!--<let var="brake_value" value="RATELIMIT( $brake_value , MAX_BRAKE_RATE )"/>
<let var="test; \
static int16_t _var_brake_value = 0; \
_var_brake_value += LIMIT(_var_brake_value_nofilt - _var_brake_value,-MAX_BRAKE_RATE,MAX_BRAKE_RATE); \
int verwaarloos_deze_warning_CDW" value="0"/>
-->
<!-- Differential Aileron Depending on Brake Value --> <!-- Differential Aileron Depending on Brake Value -->
<set servo="AILERON_LEFT" value="@ROLL"/> <let var="aileron_up_rate" value="(AILERON_RATE_UP * (MAX_PPRZ - $brake_value)) + (AILERON_RATE_UP_BRAKE * $brake_value)"/>
<set servo="AILERON_RIGHT" value="@ROLL"/> <let var="aileron_down_rate" value="(AILERON_RATE_DOWN * (MAX_PPRZ - $brake_value)) + (AILERON_RATE_DOWN_BRAKE * $brake_value)"/>
<let var="aileron_up" value="(@ROLL * (((float)$aileron_up_rate) / ((float)MAX_PPRZ)))"/>
<let var="aileron_down" value="(@ROLL * (((float)$aileron_down_rate) / ((float)MAX_PPRZ)))"/>
<let var="leftturn" value="(@ROLL >= 0? 1 : 0)"/>
<let var="rightturn" value="(1 - $leftturn)"/>
<set servo="AILERON_LEFT" value="($aileron_up * $leftturn) + ($aileron_down * $rightturn) - $brake_value*(BRAKE_AILEVON)"/>
<set servo="AILERON_RIGHT" value="($aileron_up * $rightturn) + ($aileron_down * $leftturn) + $brake_value*(BRAKE_AILEVON)"/>
<!-- Differential Thurst --> <!-- Differential Thurst -->
<set servo="THROTTLE" value="@THROTTLE"/> <set servo="THROTTLE" value="@THROTTLE"/>
<!-- Pitch with Brake-Trim Function --> <!-- Pitch with Brake-Trim Function -->
<set servo="ELEVATOR" value="@PITCH"/> <set servo="ELEVATOR" value="-@PITCH * PITCH_GAIN + BRAKE_PITCH * $brake_value"/>
</command_laws> </command_laws>
<section name="INS" prefix="INS_">
<define name="ROLL_NEUTRAL_DEFAULT" value="0" unit="deg"/>
<define name="PITCH_NEUTRAL_DEFAULT" value="0" unit="deg"/>
</section>
<section name="AUTO1" prefix="AUTO1_"> <section name="AUTO1" prefix="AUTO1_">
<define name="MAX_ROLL" value="0.7"/> <define name="MAX_ROLL" value="0.7"/>
<define name="MAX_PITCH" value="0.7"/> <define name="MAX_PITCH" value="0.7"/>
@@ -75,6 +88,11 @@
<define name="CATASTROPHIC_BAT_LEVEL" value="9.1" unit="V"/> <define name="CATASTROPHIC_BAT_LEVEL" value="9.1" unit="V"/>
</section> </section>
<section name="INS" prefix="INS_">
<define name="ROLL_NEUTRAL_DEFAULT" value="0" unit="deg"/>
<define name="PITCH_NEUTRAL_DEFAULT" value="0" unit="deg"/>
</section>
<section name="MISC"> <section name="MISC">
<define name="NOMINAL_AIRSPEED" value="13." unit="m/s"/> <define name="NOMINAL_AIRSPEED" value="13." unit="m/s"/>
<define name="CARROT" value="5." unit="s"/> <define name="CARROT" value="5." unit="s"/>
@@ -116,17 +134,19 @@
<define name="COURSE_DGAIN" value="0.3"/> <define name="COURSE_DGAIN" value="0.3"/>
<define name="COURSE_PRE_BANK_CORRECTION" value="0.2"/> <define name="COURSE_PRE_BANK_CORRECTION" value="0.2"/>
<define name="ROLL_MAX_SETPOINT" value="0.9" unit="radians"/> <define name="ROLL_MAX_SETPOINT" value="0.75" unit="radians"/>
<define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/> <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
<define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/> <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
<define name="PITCH_PGAIN" value="-12000."/> <define name="PITCH_PGAIN" value="-12000."/>
<define name="PITCH_DGAIN" value="0"/> <define name="PITCH_DGAIN" value="1.5"/>
<define name="ELEVATOR_OF_ROLL" value="1000."/> <define name="ELEVATOR_OF_ROLL" value="1000."/>
<define name="ROLL_ATTITUDE_GAIN" value="-11500"/> <define name="ROLL_SLEW" value="1."/>
<define name="ROLL_RATE_GAIN" value="-600."/>
<define name="ROLL_ATTITUDE_GAIN" value="-7500"/>
<define name="ROLL_RATE_GAIN" value="0."/>
</section> </section>
<section name="AGGRESSIVE" prefix="AGR_"> <section name="AGGRESSIVE" prefix="AGR_">
@@ -157,26 +177,37 @@
<modules> <modules>
<load name="baro_bmp.xml" > <!--
<define name="SENSOR_SYNC_SEND" /> <load name="light.xml">
<define name="BMP_I2C_DEV" value="i2c0" /> <define name="LIGHT_LED_STROBE" value="3"/>
<define name="LIGHT_LED_NAV" value="2"/>
<define name="STROBE_LIGHT_MODE_DEFAULT" value="6"/>
<define name="NAV_LIGHT_MODE_DEFAULT" value="4"/>
</load> </load>
-->
<!-- <load name="digital_cam_i2c.xml"/> -->
<load name="ins_ppzuavimu.xml" />
<!--
<load name="digital_cam.xml" >
<define name="DC_SHUTTER_LED" value="3"/>
</load>
-->
</modules> </modules>
<firmware name="fixedwing"> <firmware name="fixedwing">
<target name="ap" board="tiny_2.11"> <target name="ap" board="tiny_2.11">
<define name="STRONG_WIND"/> <define name="STRONG_WIND"/>
<define name="WIND_INFO"/> <define name="WIND_INFO"/>
<define name="WIND_INFO_RET"/> <define name="WIND_INFO_RET"/>
<define name="LOITER_TRIM"/> <define name="LOITER_TRIM"/>
<define name="ALT_KALMAN"/> <define name="ALT_KALMAN"/>
<define name="NB_CHANNELS" value="5" />
</target> </target>
<target name="sim" board="pc"/> <target name="sim" board="pc"/>
<subsystem name="radio_control" type="ppm"/> <subsystem name="radio_control" type="ppm"/>
<subsystem name="gps" type="ublox_lea5h" />
<!-- Communication --> <!-- Communication -->
<subsystem name="telemetry" type="xbee_api"> <subsystem name="telemetry" type="xbee_api">
@@ -187,11 +218,10 @@
<subsystem name="control"/> <subsystem name="control"/>
<!-- Sensors --> <!-- Sensors -->
<subsystem name="navigation"/> <subsystem name="navigation"/>
<!-- <subsystem name="gps" type="ublox_lea5h"/> --> <subsystem name="gps" type="ublox_lea4p"/>
<subsystem name="i2c"/> <subsystem name="i2c"/>
<subsystem name="imu" type="aspirin" />
</firmware> </firmware>
</airframe> </airframe>
conf/modules/ins_ppzuavimu.xml
+18
View File
@@ -0,0 +1,18 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="ins_ppzuavimu" dir="ins">
<!-- <depend conflict="ins" -->
<header>
<file name="ins_ppzuavimu.h"/>
</header>
<init fun="ppzuavimu_module_init()"/>
<periodic fun="ppzuavimu_module_periodic()" freq="60"/>
<event fun="ppzuavimu_module_event()"/>
<makefile>
<define name="AHRS_TYPE_H" value="\\\"modules/ins/ins_ppzuavimu.h\\\"" />
<define name="USE_I2C" />
<file name="ins_ppzuavimu.c"/>
<define name="PPZUAVIMU_I2C_DEVICE" value="i2c0" />
<define name="USE_I2C0" />
</makefile>
</module>
sw/airborne/modules/ins/ins_ppzuavimu.c
+205
View File
@@ -0,0 +1,205 @@
/*
* 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.
*
*/
#include <math.h>
#include "estimator.h"
#include "mcu_periph/i2c.h"
// Downlink
#include "mcu_periph/uart.h"
#include "messages.h"
#include "downlink.h"
#ifndef DOWNLINK_DEVICE
#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
#endif
// Peripherials
#define HMC5843_NO_IRQ
#include "../../peripherals/itg3200.h"
#include "../../peripherals/adxl345.h"
#include "../../peripherals/hmc5843.h"
// INS
#include "ins_module.h"
INS_FORMAT ins_roll_neutral;
INS_FORMAT ins_pitch_neutral;
// Results
int32_t mag_x, mag_y, mag_z;
bool_t mag_valid;
int32_t gyr_x, gyr_y, gyr_z;
bool_t gyr_valid;
int32_t acc_x, acc_y, acc_z;
bool_t acc_valid;
// Communication
struct i2c_transaction ppzuavimu_hmc5843;
struct i2c_transaction ppzuavimu_itg3200;
struct i2c_transaction ppzuavimu_adxl345;
void ppzuavimu_module_init( void )
{
/////////////////////////////////////////////////////////////////////
// ITG3200
ppzuavimu_itg3200.type = I2CTransTx;
ppzuavimu_itg3200.slave_addr = ITG3200_ADDR;
/* set gyro range to 2000deg/s and low pass at 20Hz (< 60Hz/2) internal sampling at 1kHz */
ppzuavimu_itg3200.buf[0] = ITG3200_REG_DLPF_FS;
ppzuavimu_itg3200.buf[1] = (0x03<<3) | (0x04<<0);
ppzuavimu_itg3200.len_w = 2;
i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_itg3200);
while(ppzuavimu_itg3200.status == I2CTransPending);
/* set sample rate to 66Hz: so at 60Hz there is always a new sample ready and you loose little */
ppzuavimu_itg3200.buf[0] = ITG3200_REG_SMPLRT_DIV;
ppzuavimu_itg3200.buf[1] = 0x0E;
i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_itg3200);
while(ppzuavimu_itg3200.status == I2CTransPending);
/* switch to gyroX clock */
ppzuavimu_itg3200.buf[0] = ITG3200_REG_PWR_MGM;
ppzuavimu_itg3200.buf[1] = 0x01;
i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_itg3200);
while(ppzuavimu_itg3200.status == I2CTransPending);
/* no interrupts for now */
ppzuavimu_itg3200.buf[0] = ITG3200_REG_INT_CFG;
ppzuavimu_itg3200.buf[1] = 0x00;
i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_itg3200);
while(ppzuavimu_itg3200.status == I2CTransPending);
/////////////////////////////////////////////////////////////////////
// ADXL345
/* set data rate to 100Hz */
ppzuavimu_adxl345.slave_addr = ADXL345_ADDR;
ppzuavimu_adxl345.type = I2CTransTx;
ppzuavimu_adxl345.buf[0] = ADXL345_REG_BW_RATE;
ppzuavimu_adxl345.buf[1] = 0x0a; // normal power and 100Hz sampling, 50Hz BW
ppzuavimu_adxl345.len_w = 2;
i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_adxl345);
while(ppzuavimu_adxl345.status == I2CTransPending);
/* switch to measurement mode */
ppzuavimu_adxl345.type = I2CTransTx;
ppzuavimu_adxl345.buf[0] = ADXL345_REG_POWER_CTL;
ppzuavimu_adxl345.buf[1] = 1<<3;
ppzuavimu_adxl345.len_w = 2;
i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_adxl345);
while(ppzuavimu_adxl345.status == I2CTransPending);
/* Set range to 4g */
ppzuavimu_adxl345.type = I2CTransTx;
ppzuavimu_adxl345.buf[0] = ADXL345_REG_DATA_FORMAT;
ppzuavimu_adxl345.buf[1] = 1<<3 | 0<<2 | 0x01; // bit 3 is full resolution bit, bit 2 is left justify bit 0,1 are range: 00=2g 01=4g 10=8g 11=16g
ppzuavimu_adxl345.len_w = 2;
i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_adxl345);
while(ppzuavimu_adxl345.status == I2CTransPending);
/////////////////////////////////////////////////////////////////////
// HMC5843
ppzuavimu_hmc5843.slave_addr = HMC5843_ADDR;
ppzuavimu_hmc5843.type = I2CTransTx;
ppzuavimu_hmc5843.buf[0] = HMC5843_REG_CFGA; // set to rate to 50Hz
ppzuavimu_hmc5843.buf[1] = 0x00 | (0x06 << 2);
ppzuavimu_hmc5843.len_w = 2;
i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_hmc5843);
while(ppzuavimu_hmc5843.status == I2CTransPending);
ppzuavimu_hmc5843.type = I2CTransTx;
ppzuavimu_hmc5843.buf[0] = HMC5843_REG_CFGB; // set to gain to 1 Gauss
ppzuavimu_hmc5843.buf[1] = 0x01<<5;
ppzuavimu_hmc5843.len_w = 2;
i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_hmc5843);
while(ppzuavimu_hmc5843.status == I2CTransPending);
ppzuavimu_hmc5843.type = I2CTransTx;
ppzuavimu_hmc5843.buf[0] = HMC5843_REG_MODE; // set to continuous mode
ppzuavimu_hmc5843.buf[1] = 0x00;
ppzuavimu_hmc5843.len_w = 2;
i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_hmc5843);
while(ppzuavimu_hmc5843.status == I2CTransPending);
}
void ppzuavimu_module_periodic( void )
{
// Start reading the latest gyroscope data
ppzuavimu_itg3200.type = I2CTransTxRx;
ppzuavimu_itg3200.len_r = 6;
ppzuavimu_itg3200.len_w = 1;
ppzuavimu_itg3200.buf[0] = ITG3200_REG_GYRO_XOUT_H;
i2c_submit(&PPZUAVIMU_I2C_DEVICE, &ppzuavimu_itg3200);
// Start reading the latest accelerometer data
ppzuavimu_adxl345.type = I2CTransTxRx;
ppzuavimu_adxl345.len_r = 6;
ppzuavimu_adxl345.len_w = 1;
ppzuavimu_adxl345.buf[0] = ADXL345_REG_DATA_X0;
i2c_submit(&PPZUAVIMU_I2C_DEVICE, &ppzuavimu_adxl345);
// Start reading the latest magnetometer data
ppzuavimu_hmc5843.type = I2CTransTxRx;
ppzuavimu_hmc5843.len_r = 6;
ppzuavimu_hmc5843.len_w = 1;
ppzuavimu_hmc5843.buf[0] = HMC5843_REG_DATXM;
i2c_submit(&PPZUAVIMU_I2C_DEVICE, &ppzuavimu_hmc5843);
RunOnceEvery(6,DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel,&gyr_x,&gyr_y,&gyr_z));
RunOnceEvery(6,DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel,&acc_x,&acc_y,&acc_z));
RunOnceEvery(6,DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,&mag_x,&mag_y,&mag_z));
}
void ppzuavimu_module_event( void )
{
// If the itg3200 I2C transaction has succeeded: convert the data
if (ppzuavimu_itg3200.status == I2CTransSuccess)
{
gyr_x = (int16_t) ((ppzuavimu_itg3200.buf[0] << 8) | ppzuavimu_itg3200.buf[1]);
gyr_y = (int16_t) ((ppzuavimu_itg3200.buf[2] << 8) | ppzuavimu_itg3200.buf[3]);
gyr_z = (int16_t) ((ppzuavimu_itg3200.buf[4] << 8) | ppzuavimu_itg3200.buf[5]);
gyr_valid = TRUE;
}
// If the adxl345 I2C transaction has succeeded: convert the data
if (ppzuavimu_adxl345.status == I2CTransSuccess)
{
acc_x = (int16_t) ((ppzuavimu_adxl345.buf[1] << 8) | ppzuavimu_adxl345.buf[0]);
acc_y = (int16_t) ((ppzuavimu_adxl345.buf[3] << 8) | ppzuavimu_adxl345.buf[2]);
acc_z = (int16_t) ((ppzuavimu_adxl345.buf[5] << 8) | ppzuavimu_adxl345.buf[4]);
acc_valid = TRUE;
}
// If the hmc5843 I2C transaction has succeeded: convert the data
if (ppzuavimu_hmc5843.status == I2CTransSuccess)
{
mag_x = (int16_t) ((ppzuavimu_hmc5843.buf[0] << 8) | ppzuavimu_hmc5843.buf[1]);
mag_y = (int16_t) ((ppzuavimu_hmc5843.buf[2] << 8) | ppzuavimu_hmc5843.buf[3]);
mag_z = (int16_t) ((ppzuavimu_hmc5843.buf[4] << 8) | ppzuavimu_hmc5843.buf[5]);
mag_valid = TRUE;
}
}
sw/airborne/modules/ins/ins_ppzuavimu.h
+36
View File
@@ -0,0 +1,36 @@
/*
* 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.
*
*/
#ifndef PPZUAVIMU_H
#define PPZUAVIMU_H
#include "std.h"
// pitch/roll neutrals
#include "ins_module.h"
extern int32_t mag_x, mag_y, mag_z;
extern int32_t gyr_x, gyr_y, gyr_z;
extern int32_t acc_x, acc_y, acc_z;
extern void ppzuavimu_module_init( void );
extern void ppzuavimu_module_periodic( void );
extern void ppzuavimu_module_event( void );
#endif // PPZUAVIMU_H
sw/airborne/peripherals/adxl345.h
+2
View File
@@ -1,6 +1,8 @@
#ifndef ADXL345_H #ifndef ADXL345_H
#define ADXL345_H #define ADXL345_H
#define ADXL345_ADDR 0xA6
#define ADXL345_ADDR_ALT 0x3A
#define ADXL345_REG_BW_RATE 0x2C #define ADXL345_REG_BW_RATE 0x2C
#define ADXL345_REG_POWER_CTL 0x2D #define ADXL345_REG_POWER_CTL 0x2D