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

*** empty log message ***

Browse Source
This commit is contained in:
Antoine Drouin
2007-10-04 07:40:42 +00:00
parent f607f105f1
commit eb05a8c7d7
6 changed files with 191 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/booz.xml
+11 -1
View File
@@ -6,6 +6,11 @@ ARCHI=arm7
FLASH_MODE = IAP
#
# controller CPU
#
ctl.ARCHDIR = $(ARCHI)
ctl.ARCH = arm7tdmi
ctl.TARGET = ctl
@@ -35,6 +40,10 @@ ctl.srcs += link_imu.c
ctl.CFLAGS += -DRADIO_CONTROL -DRADIO_CONTROL_TYPE=RC_FUTABA
ctl.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
#
# AHRS CPU
#
imu.ARCHDIR = $(ARCHI)
imu.ARCH = arm7tdmi
imu.TARGET = imu
@@ -59,7 +68,8 @@ imu.srcs += $(SRC_ARCH)/micromag.c
imu.srcs += imu_v3.c
imu.srcs += ahrs_new.c
//imu.srcs += ahrs_new.c
imu.srcs += multitilt.c
imu.srcs += link_imu.c
sw/airborne/AMI601.c
+144
View File
@@ -0,0 +1,144 @@
#include "AMI601.h"
#include "i2c.h"
#include "led.h"
#include <string.h>
#define FOO_DELAY() { \
uint16_t foo = 1; \
while (foo) foo++; \
}
#define AMI601_SLAVE_ADDR 0x60
float ami601_ax;
float ami601_ay;
float ami601_az;
float ami601_mx;
float ami601_my;
float ami601_mz;
uint8_t ami601_foo1;
uint8_t ami601_foo2;
uint8_t ami601_foo3;
uint16_t ami601_val[AMI601_NB_CHAN];
static volatile bool_t ami601_i2c_done;
/* TRG ( trigger) P0.28 */
#define AMI601_TRG_PIN 28
#define AMI601_TRG_IODIR IO0DIR
#define AMI601_TRG_IOSET IO0SET
#define AMI601_TRG_IOCLR IO0CLR
/* RST ( reset ) P0.29 */
#define AMI601_RST_PIN 29
#define AMI601_RST_IODIR IO0DIR
#define AMI601_RST_IOSET IO0SET
#define AMI601_RST_IOCLR IO0CLR
/* BUSY ( busy ) P0.30 EINT3 */
#define AMI601_BUSY_PIN
#define AMI601_BUSY_IODIR
#define AMI601_BUSY_IOSET
#define AMI601_BUSY_IOCLR
void ami601_init( void ) {
/* configure TRG pin as output and assert it*/
SetBit(AMI601_TRG_IODIR , AMI601_TRG_PIN);
SetBit(AMI601_TRG_IOSET , AMI601_TRG_PIN);
/* configure RST pin as output and set it low*/
SetBit(AMI601_RST_IODIR , AMI601_RST_PIN);
SetBit(AMI601_RST_IOCLR , AMI601_RST_PIN);
uint8_t i;
for (i=0; i< AMI601_NB_CHAN; i++) {
ami601_val[i] = 0;
}
ami601_i2c_done = TRUE;
FOO_DELAY();
/* assert reset */
SetBit(AMI601_RST_IOSET , AMI601_RST_PIN);
}
void ami601_periodic( void ) {
/* pulse TRG */
// SetBit(AMI601_TRG_IOCLR , AMI601_TRG_PIN);
// FOO_DELAY();
// SetBit(AMI601_TRG_IOSET , AMI601_TRG_PIN);
if (ami601_i2c_done) {
LED_ON(2);
const uint8_t read_cmd[] = { 0x55, 0xAA, 0X14};
memcpy((void*)i2c_buf, read_cmd, sizeof(read_cmd));
i2c_transmit(AMI601_SLAVE_ADDR, sizeof(read_cmd), &ami601_i2c_done);
ami601_i2c_done = FALSE;
while (!ami601_i2c_done);
FOO_DELAY();
i2c_receive(AMI601_SLAVE_ADDR, 15, &ami601_i2c_done);
while (!ami601_i2c_done);
LED_OFF(2);
ami601_foo1 = i2c_buf[0];
ami601_foo1 = i2c_buf[1];
ami601_foo1 = i2c_buf[2];
uint8_t i;
for (i=0; i< AMI601_NB_CHAN; i++) {
ami601_val[i] = i2c_buf[3 + 2 * i];
ami601_val[i] += i2c_buf[3 + 2 * i + 1] * 256;
}
}
}
void ami601_send_sleep( void ) {
}
#define AX_NEUTRAL 2323
#define AY_NEUTRAL 2323
#define AZ_NEUTRAL 2323
#define AX_GAIN 0.0283936
#define AY_GAIN -0.0283936
#define AZ_GAIN 0.0283936
#define MX_NEUTRAL 2048
#define MY_NEUTRAL 2048
#define MZ_NEUTRAL 2048
#define MX_GAIN -1.
#define MY_GAIN 1.
#define MZ_GAIN 1.
void ami601_scale_measures(void) {
ami601_ax = (ami601_val[3] - AX_NEUTRAL) * AX_GAIN;
ami601_ay = (ami601_val[5] - AY_NEUTRAL) * AY_GAIN;
ami601_az = (ami601_val[1] - AZ_NEUTRAL) * AZ_GAIN;
ami601_mx = (ami601_val[0] - MX_NEUTRAL) * MX_GAIN;
ami601_my = (ami601_val[4] - MY_NEUTRAL) * MY_GAIN;
ami601_mz = (ami601_val[2] - MZ_NEUTRAL) * MZ_GAIN;
}
sw/airborne/AMI601.h
+25
View File
@@ -0,0 +1,25 @@
#ifndef AMI601_H
#define AMI601_H
#include "std.h"
extern void ami601_init( void );
extern void ami601_periodic( void );
#define AMI601_NB_CHAN 6
extern uint16_t ami601_val[AMI601_NB_CHAN];
extern uint8_t ami601_foo1;
extern uint8_t ami601_foo2;
extern uint8_t ami601_foo3;
float ami601_ax;
float ami601_ay;
float ami601_az;
float ami601_mx;
float ami601_my;
float ami601_mz;
#endif /* AMI601_H */
sw/airborne/frames.h
+3
View File
@@ -6,5 +6,8 @@
#define AXIS_Z 2
#define AXIS_NB 3
#define AXIS_P 0
#define AXIS_Q 1
#define AXIS_R 2
#endif /* FRAMES_H */
sw/airborne/link_imu.c
+3 -1
View File
@@ -9,7 +9,7 @@ struct imu_state link_imu_state_foo;
#ifdef IMU /* IMU LPC code */
#include "imu_v3.h"
#include "ahrs_new.h"
//#include "ahrs_new.h"
#include "LPC21xx.h"
#include "armVIC.h"
@@ -90,6 +90,7 @@ void link_imu_init ( void ) {
}
void link_imu_send ( void ) {
/*
if (!isnan(ahrs_phi) &&
!isnan(ahrs_theta) &&
!isnan(ahrs_psi)) {
@@ -114,6 +115,7 @@ void link_imu_send ( void ) {
link_imu_state.sin_theta = 0.;
link_imu_state.status = IMU_CRASHED;
}
*/
Spi0InitBuf();
}
sw/airborne/main_imu.c
+5 -4
View File
@@ -11,7 +11,8 @@
#include "max1167.h"
#include "micromag.h"
#include "imu_v3.h"
#include "ahrs_new.h"
//#include "ahrs_new.h"
#include "multitilt.h"
#include "link_imu.h"
#include "messages.h"
@@ -96,7 +97,7 @@ static inline void main_event_task( void ) {
#ifdef SEND_ACCEL
DOWNLINK_SEND_IMU_ACCEL(&imu_accel[AXIS_X], &imu_accel[AXIS_Y], &imu_accel[AXIS_Z]);
#endif
ahrs_task();
// ahrs_task();
t1=T0TC;
// uint32_t dt = t1 - t0;
@@ -119,7 +120,7 @@ static inline void main_periodic_task( void ) {
}
#if 0
static inline void ahrs_task( void ) {
/* discard first 100 measures */
if (ahrs_step == AHRS_STEP_UNINIT) {
@@ -153,7 +154,7 @@ static inline void ahrs_task( void ) {
if (ahrs_step == AHRS_STEP_NB) ahrs_step = AHRS_STEP_ROLL;
}
}
#endif
/* SSPCR0 settings */
#define SSP_DDS 0x07 << 0 /* data size : 8 bits */