Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-23 21:36:28 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

fixed actuators_mkk and ami601 drivers on lpC21

Browse Source
This commit is contained in:
Antoine Drouin
2010-09-06 10:32:53 +00:00
parent b5022c14bb
commit 0a9d04e0d8
12 changed files with 144 additions and 84 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/Poine/booz2_a1.xml
+2
View File
@@ -214,7 +214,9 @@
<target name="test_baro" board="booz_1.0"/>
<target name="test_rc_spektrum" board="booz_1.0"/>
<target name="test_rc_ppm" board="booz_1.0"/>
<target name="test_esc_mkk_simple" board="booz_1.0"/>
<target name="test_actuators_mkk" board="booz_1.0"/>
<target name="test_ami601" board="booz_1.0"/>
</firmware>
conf/autopilot/booz_test_progs.makefile
+55 -1
View File
@@ -166,6 +166,28 @@ test_rc_ppm.srcs += $(SRC_BOOZ)/booz_radio_control.c \
$(SRC_BOOZ_ARCH)/radio_control/booz_radio_control_ppm_arch.c
#
# simple test of mikrokopter motor controllers
#
test_esc_mkk_simple.ARCHDIR = $(ARCHI)
test_esc_mkk_simple.ARCH = arm7tdmi
test_esc_mkk_simple.TARGET = test_esc_mkk_simple
test_esc_mkk_simple.TARGETDIR = test_esc_mkk_simple
test_esc_mkk_simple.CFLAGS = -I$(SRC_LISA) -I$(ARCHI) -DPERIPHERALS_AUTO_INIT
test_esc_mkk_simple.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
test_esc_mkk_simple.srcs = test/test_esc_mkk_simple.c \
$(SRC_ARCH)/armVIC.c
test_esc_mkk_simple.CFLAGS += -DUSE_LED
test_esc_mkk_simple.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
test_esc_mkk_simple.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
test_esc_mkk_simple.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
test_esc_mkk_simple.CFLAGS += -DACTUATORS_MKK_DEV=i2c0
test_esc_mkk_simple.CFLAGS += -DUSE_I2C0
test_esc_mkk_simple.CFLAGS += -DI2C0_SCLL=150 -DI2C0_SCLH=150 -DI2C0_VIC_SLOT=10
test_esc_mkk_simple.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
#
# test actuators mkk
#
@@ -194,5 +216,37 @@ test_actuators_mkk.srcs += $(SRC_BOOZ)/booz2_commands.c
test_actuators_mkk.srcs += $(SRC_BOOZ)/actuators/booz_actuators_mkk.c
test_actuators_mkk.CFLAGS += -DACTUATORS_MKK_DEVICE=i2c0
test_actuators_mkk.srcs += $(SRC_BOOZ)/actuators/booz_supervision.c
test_actuators_mkk.CFLAGS += -DACTUATORS_MKK_DEV=i2c0
test_actuators_mkk.CFLAGS += -DUSE_I2C0
test_actuators_mkk.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
test_actuators_mkk.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
test_actuators_mkk.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=150 -DI2C0_SCLH=150 -DI2C0_VIC_SLOT=10
#
# test ami601
#
test_ami601.ARCHDIR = $(ARCHI)
test_ami601.ARCH = arm7tdmi
test_ami601.TARGET = test_ami601
test_ami601.TARGETDIR = test_ami601
test_ami601.CFLAGS = -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) -DPERIPHERALS_AUTO_INIT
test_ami601.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
test_ami601.srcs = test/peripherals/test_ami601.c \
$(SRC_ARCH)/armVIC.c
test_ami601.CFLAGS += -DUSE_LED
test_ami601.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=$(SYS_TIME_LED)
test_ami601.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
test_ami601.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
test_ami601.CFLAGS += -DUSE_$(MODEM_PORT) -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD)
test_ami601.srcs += $(SRC_ARCH)/uart_hw.c
test_ami601.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT)
test_ami601.srcs += downlink.c pprz_transport.c
test_ami601.CFLAGS += -DUSE_AMI601
test_ami601.srcs += $(SRC_BOOZ)/peripherals/booz_ami601.c
test_ami601.CFLAGS += -DUSE_I2C1 -DI2C1_SCLL=150 -DI2C1_SCLH=150 -DI2C1_VIC_SLOT=11
test_ami601.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
conf/autopilot/subsystems/rotorcraft/imu_b2_v1.0.makefile
+1 -1
View File
@@ -49,7 +49,7 @@ ap.srcs += $(SRC_BOOZ)/peripherals/booz_max1168.c \
ap.CFLAGS += -DUSE_AMI601
ap.srcs += $(SRC_BOOZ)/peripherals/booz_ami601.c
ap.CFLAGS += -DUSE_I2C1 -DI2C1_SCLL=150 -DI2C1_SCLH=150 -DI2C1_VIC_SLOT=11 -DI2C1_BUF_LEN=16
ap.CFLAGS += -DUSE_I2C1 -DI2C1_SCLL=150 -DI2C1_SCLH=150 -DI2C1_VIC_SLOT=11
#
sw/airborne/arm7/i2c_hw.c
+2 -2
View File
@@ -309,8 +309,8 @@ void i2c1_hw_init ( void ) {
I2C1SCLH = I2C1_SCLH_D;
// initialize the interrupt vector
VICIntSelect &= ~VIC_BIT(VIC_I2C1); // I2C0 selected as IRQ
VICIntEnable = VIC_BIT(VIC_I2C1); // I2C0 interrupt enabled
VICIntSelect &= ~VIC_BIT(VIC_I2C1); // I2C1 selected as IRQ
VICIntEnable = VIC_BIT(VIC_I2C1); // I2C1 interrupt enabled
_VIC_CNTL(I2C1_VIC_SLOT) = VIC_ENABLE | VIC_I2C1;
_VIC_ADDR(I2C1_VIC_SLOT) = (uint32_t)i2c1_ISR; // address of the ISR
}
sw/airborne/arm7/sys_time_hw.c
+1 -1
View File
@@ -47,7 +47,7 @@ uint32_t sys_time_chrono; /* T0TC ticks */
#endif
#ifdef USE_AMI601
#include "peripherals/booz_ami601.h"
#include "booz/peripherals/booz_ami601.h"
#else
#define AMI601_IT 0x00
#endif
sw/airborne/booz/actuators/booz_actuators_mkk.c
+1
View File
@@ -43,6 +43,7 @@ void actuators_init(void) {
actuators_mkk.trans[i].type = I2CTransTx;
actuators_mkk.trans[i].len_w = 1;
actuators_mkk.trans[i].slave_addr = actuators_addr[i];
actuators_mkk.trans[i].stop_after_transmit = TRUE;
actuators_mkk.trans[i].status = I2CTransSuccess;
}
sw/airborne/booz/peripherals/booz_ami601.c
+2 -2
View File
@@ -16,10 +16,11 @@ void ami601_init( void ) {
ami601_values[i] = 0;
}
ami601_i2c_trans.status = I2CTransSuccess;
ami601_i2c_trans.slave_addr = AMI601_SLAVE_ADDR;
ami601_i2c_trans.stop_after_transmit = TRUE;
ami601_nb_err = 0;
ami601_status = AMI601_IDLE;
}
void ami601_read( void ) {
@@ -30,7 +31,6 @@ void ami601_read( void ) {
ami601_status = AMI601_SENDING_REQ;
ami601_i2c_trans.type = I2CTransTx;
ami601_i2c_trans.len_w = 3;
ami601_i2c_trans.slave_addr = AMI601_SLAVE_ADDR;
ami601_i2c_trans.buf[0] = 0x55;
ami601_i2c_trans.buf[1] = 0xAA;
ami601_i2c_trans.buf[2] = 0x14;
sw/airborne/booz/peripherals/booz_ami601.h
+1 -2
View File
@@ -53,7 +53,7 @@ extern volatile uint32_t ami601_nb_err;
ami601_foo3 = ami601_i2c_trans.buf[2]; /* ERR ? */ \
uint8_t i; \
for (i=0; i< AMI601_NB_CHAN; i++) { \
ami601_values[i] = ami601_i2c_trans.buf[3 + 2 * i]; \
ami601_values[i] = ami601_i2c_trans.buf[3 + 2 * i]; \
ami601_values[i] += ami601_i2c_trans.buf[3 + 2 * i + 1] * 256; \
} \
ami601_status = AMI601_DATA_AVAILABLE; \
@@ -70,7 +70,6 @@ extern volatile uint32_t ami601_nb_err;
ami601_status = AMI601_READING_MEASURE; \
ami601_i2c_trans.type = I2CTransRx; \
ami601_i2c_trans.len_r = 15; \
ami601_i2c_trans.slave_addr = AMI601_SLAVE_ADDR; \
i2c_submit(&i2c1, &ami601_i2c_trans); \
}
sw/airborne/i2c.h
+30 -30
View File
@@ -129,40 +129,40 @@ extern void i2c_init(struct i2c_periph* p);
extern bool_t i2c_submit(struct i2c_periph* p, struct i2c_transaction* t);
#define I2CReceive(_p, _t, _s_addr, _len) { \
_t.type = I2CTransRx; \
_t.slave_addr = _s_addr; \
_t.len_r = _len; \
_t.len_w = 0; \
_t.stop_after_transmit = TRUE; \
i2c_submit(&(_p),&(_t)); \
}
_t.type = I2CTransRx; \
_t.slave_addr = _s_addr; \
_t.len_r = _len; \
_t.len_w = 0; \
_t.stop_after_transmit = TRUE; \
i2c_submit(&(_p),&(_t)); \
}
#define I2CTransmit(_p, _t, _s_addr, _len) { \
_t.type = I2CTransTx; \
_t.slave_addr = _s_addr; \
_t.len_r = 0; \
_t.len_w = _len; \
_t.stop_after_transmit = TRUE; \
i2c_submit(&(_p),&(_t)); \
}
#define I2CTransmit(_p, _t, _s_addr, _len) { \
_t.type = I2CTransTx; \
_t.slave_addr = _s_addr; \
_t.len_r = 0; \
_t.len_w = _len; \
_t.stop_after_transmit = TRUE; \
i2c_submit(&(_p),&(_t)); \
}
#define I2CTransmitNoStop(_p, _t, _s_addr, _len) { \
_t.type = I2CTransTx; \
_t.slave_addr = _s_addr; \
_t.len_r = 0; \
_t.len_w = _len; \
_t.stop_after_transmit = FALSE; \
i2c_submit(&(_p),&(_t)); \
}
#define I2CTransmitNoStop(_p, _t, _s_addr, _len) { \
_t.type = I2CTransTx; \
_t.slave_addr = _s_addr; \
_t.len_r = 0; \
_t.len_w = _len; \
_t.stop_after_transmit = FALSE; \
i2c_submit(&(_p),&(_t)); \
}
#define I2CTransceive(_p, _t, _s_addr, _len_w, _len_r) { \
_t.type = I2CTransTxRx; \
_t.slave_addr = _s_addr; \
_t.len_r = _len_r; \
_t.len_w = _len_w; \
_t.stop_after_transmit = TRUE; \
i2c_submit(&(_p),&(_t)); \
}
_t.type = I2CTransTxRx; \
_t.slave_addr = _s_addr; \
_t.len_r = _len_r; \
_t.len_w = _len_w; \
_t.stop_after_transmit = TRUE; \
i2c_submit(&(_p),&(_t)); \
}
#endif /* I2C_H */
sw/airborne/math/pprz_algebra_float.h
+44 -44
View File
@@ -360,15 +360,15 @@ struct FloatRates {
#define FLOAT_QUAT_NORM(_q) (sqrtf(SQUARE(_q.qi) + SQUARE(_q.qx)+ \
SQUARE(_q.qy) + SQUARE(_q.qz))) \
#define FLOAT_QUAT_NORMALISE(q) { \
float norm = FLOAT_QUAT_NORM(q); \
if (norm > FLT_MIN) { \
q.qi = q.qi / norm; \
q.qx = q.qx / norm; \
q.qy = q.qy / norm; \
q.qz = q.qz / norm; \
} \
if (norm > FLT_MIN) { \
q.qi = q.qi / norm; \
q.qx = q.qx / norm; \
q.qy = q.qy / norm; \
q.qz = q.qz / norm; \
} \
}
#define FLOAT_QUAT_INVERT(_qo, _qi) QUAT_INVERT(_qo, _qi)
@@ -380,9 +380,9 @@ struct FloatRates {
/* _a2c = _a2b comp _b2c , aka _a2c = _a2b * _b2c */
#define FLOAT_QUAT_COMP_NORM_SHORTEST(_a2c, _a2b, _b2c) { \
FLOAT_QUAT_COMP(_a2c, _a2b, _b2c); \
FLOAT_QUAT_WRAP_SHORTEST(_a2c); \
FLOAT_QUAT_NORMALISE(_a2c); \
FLOAT_QUAT_COMP(_a2c, _a2b, _b2c); \
FLOAT_QUAT_WRAP_SHORTEST(_a2c); \
FLOAT_QUAT_NORMALISE(_a2c); \
}
/* _a2c = _a2b comp _b2c , aka _a2c = _a2b * _b2c */
@@ -396,10 +396,10 @@ struct FloatRates {
#define FLOAT_QUAT_MULT(_a2c, _a2b, _b2c) FLOAT_QUAT_COMP(_a2c, _a2b, _b2c)
/* _a2b = _a2c comp_inv _b2c , aka _a2b = _a2c * inv(_b2c) */
#define FLOAT_QUAT_COMP_INV_NORM_SHORTEST(_a2b, _a2c, _b2c) { \
FLOAT_QUAT_COMP_INV(_a2b, _a2c, _b2c); \
FLOAT_QUAT_WRAP_SHORTEST(_a2b); \
FLOAT_QUAT_NORMALISE(_a2b); \
#define FLOAT_QUAT_COMP_INV_NORM_SHORTEST(_a2b, _a2c, _b2c) { \
FLOAT_QUAT_COMP_INV(_a2b, _a2c, _b2c); \
FLOAT_QUAT_WRAP_SHORTEST(_a2b); \
FLOAT_QUAT_NORMALISE(_a2b); \
}
/* _a2b = _a2c comp_inv _b2c , aka _a2b = _a2c * inv(_b2c) */
@@ -411,11 +411,11 @@ struct FloatRates {
}
/* _b2c = _a2b inv_comp _a2c , aka _b2c = inv(_a2b) * _a2c */
#define FLOAT_QUAT_INV_COMP_NORM_SHORTEST(_b2c, _a2b, _a2c) { \
FLOAT_QUAT_INV_COMP(_b2c, _a2b, _a2c); \
FLOAT_QUAT_WRAP_SHORTEST(_b2c); \
FLOAT_QUAT_NORMALISE(_b2c); \
}
#define FLOAT_QUAT_INV_COMP_NORM_SHORTEST(_b2c, _a2b, _a2c) { \
FLOAT_QUAT_INV_COMP(_b2c, _a2b, _a2c); \
FLOAT_QUAT_WRAP_SHORTEST(_b2c); \
FLOAT_QUAT_NORMALISE(_b2c); \
}
/* _b2c = _a2b inv_comp _a2c , aka _b2c = inv(_a2b) * _a2c */
#define FLOAT_QUAT_INV_COMP(_b2c, _a2b, _a2c) { \
@@ -425,28 +425,28 @@ struct FloatRates {
(_b2c).qz = (_a2b).qi*(_a2c).qz - (_a2b).qx*(_a2c).qy + (_a2b).qy*(_a2c).qx - (_a2b).qz*(_a2c).qi; \
}
#define FLOAT_QUAT_DIFFERENTIAL(q_out, w, dt) { \
const float v_norm = sqrt((w).p*(w).p + (w).q*(w).q + (w).r*(w).r); \
const float c2 = cos(dt*v_norm/2.0); \
const float s2 = sin(dt*v_norm/2.0); \
if (v_norm < 1e-8) { \
(q_out).qi = 1; \
(q_out).qx = 0; \
(q_out).qy = 0; \
(q_out).qz = 0; \
} else { \
(q_out).qi = c2; \
(q_out).qx = (w).p/v_norm * s2; \
(q_out).qy = (w).q/v_norm * s2; \
(q_out).qz = (w).r/v_norm * s2; \
} \
}
#define FLOAT_QUAT_DIFFERENTIAL(q_out, w, dt) { \
const float v_norm = sqrt((w).p*(w).p + (w).q*(w).q + (w).r*(w).r); \
const float c2 = cos(dt*v_norm/2.0); \
const float s2 = sin(dt*v_norm/2.0); \
if (v_norm < 1e-8) { \
(q_out).qi = 1; \
(q_out).qx = 0; \
(q_out).qy = 0; \
(q_out).qz = 0; \
} else { \
(q_out).qi = c2; \
(q_out).qx = (w).p/v_norm * s2; \
(q_out).qy = (w).q/v_norm * s2; \
(q_out).qz = (w).r/v_norm * s2; \
} \
}
#define FLOAT_QUAT_ROTATE_FRAME(q_out, q_in, q_rot) { \
struct FloatQuat q_temp; \
FLOAT_QUAT_INV_COMP(q_temp, q_rot, q_in); \
print_quat(q_temp); \
FLOAT_QUAT_COMP(q_out, q_temp, q_rot); \
struct FloatQuat q_temp; \
FLOAT_QUAT_INV_COMP(q_temp, q_rot, q_in); \
print_quat(q_temp); \
FLOAT_QUAT_COMP(q_out, q_temp, q_rot); \
}
#define FLOAT_QUAT_VMULT(v_out, q, v_in) { \
@@ -475,7 +475,7 @@ struct FloatRates {
}
/* _qd = -0.5*omega(_r) * _q */
#define FLOAT_QUAT_DERIVATIVE(_qd, _r, _q) { \
#define FLOAT_QUAT_DERIVATIVE(_qd, _r, _q) { \
(_qd).qi = -0.5*( (_r).p*(_q).qx + (_r).q*(_q).qy + (_r).r*(_q).qz); \
(_qd).qx = -0.5*(-(_r).p*(_q).qi - (_r).r*(_q).qy + (_r).q*(_q).qz); \
(_qd).qy = -0.5*(-(_r).q*(_q).qi + (_r).r*(_q).qx - (_r).p*(_q).qz); \
@@ -483,9 +483,9 @@ struct FloatRates {
}
/* _qd = -0.5*omega(_r) * _q */
#define FLOAT_QUAT_DERIVATIVE_LAGRANGE(_qd, _r, _q) { \
const float K_LAGRANGE = 1.; \
const float c = K_LAGRANGE * ( 1 - FLOAT_QUAT_NORM(_q)) / -0.5; \
#define FLOAT_QUAT_DERIVATIVE_LAGRANGE(_qd, _r, _q) { \
const float K_LAGRANGE = 1.; \
const float c = K_LAGRANGE * ( 1 - FLOAT_QUAT_NORM(_q)) / -0.5; \
(_qd).qi = -0.5*( c*(_q).qi + (_r).p*(_q).qx + (_r).q*(_q).qy + (_r).r*(_q).qz); \
(_qd).qx = -0.5*(-(_r).p*(_q).qi + c*(_q).qx - (_r).r*(_q).qy + (_r).q*(_q).qz); \
(_qd).qy = -0.5*(-(_r).q*(_q).qi + (_r).r*(_q).qx + c*(_q).qy - (_r).p*(_q).qz); \
@@ -497,7 +497,7 @@ struct FloatRates {
const float phi2 = (_e).phi/2.0; \
const float theta2 = (_e).theta/2.0; \
const float psi2 = (_e).psi/2.0; \
\
\
const float s_phi2 = sinf( phi2 ); \
const float c_phi2 = cosf( phi2 ); \
const float s_theta2 = sinf( theta2 ); \
sw/airborne/test/Makefile
+3
View File
@@ -14,6 +14,9 @@ test_geodetic: test_geodetic.c ../math/pprz_geodetic_float.c ../math/pprz_geodet
test_algebra: test_algebra.c ../math/pprz_trig_int.c
$(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
test_martin: test_martin.c ../math/pprz_trig_int.c
$(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
test_bla: test_bla.c ../math/pprz_trig_int.c
$(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
sw/airborne/test/test_esc_mkk_simple.c
+2 -1
View File
@@ -59,7 +59,8 @@ static inline void main_periodic_task( void ) {
trans.buf[0] = 0x04;
trans.len_w = 1;
trans.slave_addr = 0x58;
i2c_submit(&i2c1,&trans);
trans.stop_after_transmit = TRUE;
i2c_submit(&ACTUATORS_MKK_DEV,&trans);
LED_PERIODIC();