Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-08 02:15:53 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

moving booz to its own directory

Browse Source
This commit is contained in:
Antoine Drouin
2008-04-10 15:07:06 +00:00
parent 1311cd5ee5
commit d48bf6ab33
71 changed files with 0 additions and 4892 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/ahrs_comp_filter.c
-117
View File
@@ -1,117 +0,0 @@
/*
* $Id$
*
* Copyright (C) 2008 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.
*
*/
#include "booz_ahrs.h"
#include <math.h>
#include "6dof.h"
#define DT_PREDICT (1./250.)
#define K1_accel 0.075
#define K2_accel 0.00025
#define K1_mag 0.1
#define K2_mag 0.00025
FLOAT_T comp_filter_int_phi;
FLOAT_T comp_filter_int_theta;
FLOAT_T comp_filter_int_psi;
void booz_ahrs_init(void) {
booz_ahrs_status = BOOZ_AHRS_STATUS_UNINIT;
booz_ahrs_phi = 0.;
booz_ahrs_theta = 0.;
booz_ahrs_psi = 0.;
booz_ahrs_p = 0.;
booz_ahrs_q = 0.;
booz_ahrs_r = 0.;
booz_ahrs_bp = 0.;
booz_ahrs_bq = 0.;
booz_ahrs_br = 0.;
booz_ahrs_status = BOOZ_AHRS_STATUS_UNINIT;
}
void booz_ahrs_start(const float* accel, const float* gyro, const float* mag) {
booz_ahrs_p = 0.;
booz_ahrs_q = 0.;
booz_ahrs_r = 0.;
booz_ahrs_bp = gyro[AXIS_P];
booz_ahrs_bq = gyro[AXIS_Q];
booz_ahrs_br = gyro[AXIS_R];
PhiOfAccel(booz_ahrs_phi, accel);
ThetaOfAccel(booz_ahrs_theta, accel);
PsiOfMag(booz_ahrs_psi, mag);
booz_ahrs_status = BOOZ_AHRS_STATUS_RUNNING;
}
void booz_ahrs_predict(const float* gyro ) {
/* unbias gyro */
booz_ahrs_p = gyro[AXIS_P] - booz_ahrs_bp;
booz_ahrs_q = gyro[AXIS_Q] - booz_ahrs_bq;
booz_ahrs_r = gyro[AXIS_R] - booz_ahrs_br;
/* update integrated angles */
float s_phi = sin(booz_ahrs_phi);
float c_phi = cos(booz_ahrs_phi);
float t_theta = tan(booz_ahrs_theta);
float c_theta = cos(booz_ahrs_theta);
float phi_dot = booz_ahrs_p + s_phi*t_theta*booz_ahrs_q + c_phi*t_theta*booz_ahrs_r;
float theta_dot = c_phi*booz_ahrs_q - s_phi*booz_ahrs_r;
float psi_dot = s_phi/c_theta*booz_ahrs_q + c_phi/c_theta*booz_ahrs_r;
comp_filter_int_phi = booz_ahrs_phi + phi_dot * DT_PREDICT;
comp_filter_int_theta = booz_ahrs_theta + theta_dot * DT_PREDICT;
comp_filter_int_psi = booz_ahrs_psi + psi_dot * DT_PREDICT;
}
void booz_ahrs_update_accel( const float* accel) {
PhiOfAccel(booz_ahrs_measure_phi, accel);
ThetaOfAccel(booz_ahrs_measure_theta, accel);
float err_phi = booz_ahrs_measure_phi - comp_filter_int_phi;
float err_theta = booz_ahrs_measure_theta - comp_filter_int_theta;
booz_ahrs_phi = comp_filter_int_phi + err_phi * K1_accel;
booz_ahrs_theta = comp_filter_int_theta + err_theta * K1_accel;
booz_ahrs_bp -= err_phi * K2_accel;
booz_ahrs_bq -= err_theta * K2_accel;
}
void booz_ahrs_update_mag( const float* mag ) {
PsiOfMag(booz_ahrs_measure_psi, mag);
float err_psi = booz_ahrs_measure_psi - comp_filter_int_psi;
booz_ahrs_psi = comp_filter_int_psi + err_psi * K1_mag;
booz_ahrs_br -= err_psi * K2_mag;
}
sw/airborne/ahrs_comp_filter.h
-32
View File
@@ -1,32 +0,0 @@
/*
* $Id$
*
* Copyright (C) 2008 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.
*
*/
#ifndef AHRS_COMP_FLT_H
#define AHRS_COMP_FLT_H
extern FLOAT_T comp_filter_int_phi;
extern FLOAT_T comp_filter_int_theta;
extern FLOAT_T comp_filter_int_psi;
#endif /* AHRS_COMP_FLT_H */
sw/airborne/ahrs_multitilt.c
-162
View File
@@ -1,162 +0,0 @@
#include "booz_ahrs.h"
#include <string.h>
#include <math.h>
#include "6dof.h"
#define DT 4e-3
/* covariance matrix */
float mtt_P_phi[2][2];
float mtt_P_theta[2][2];
float mtt_P_psi[2][2];
/* process covariance noise */
//static const float Q_angle = 0.0005;
static const float Q_angle = 0.00025;
static const float Q_bias = 0.0005;
/* Measurement covariance */
//static const float R_accel = 0.3;
static const float R_accel = 0.5;
static inline void mtt_update_axis(float _err, float _P[2][2], float* angle, float* bias);
static inline void mtt_predict_axis(float* angle, float angle_dot, float P[2][2]);
void booz_ahrs_init(void) {
booz_ahrs_status = BOOZ_AHRS_STATUS_UNINIT;
booz_ahrs_phi = 0.;
booz_ahrs_theta = 0.;
booz_ahrs_psi = 0.;
booz_ahrs_p = 0.;
booz_ahrs_q = 0.;
booz_ahrs_r = 0.;
booz_ahrs_bp = 0.;
booz_ahrs_bq = 0.;
booz_ahrs_br = 0.;
}
void booz_ahrs_start(const float* accel, const float* gyro, const float* mag) {
/* reset covariance matrices */
const float cov_init[2][2] = {{1., 0.},
{0., 1.}};
memcpy(mtt_P_phi, cov_init, sizeof(cov_init));
memcpy(mtt_P_theta, cov_init, sizeof(cov_init));
memcpy(mtt_P_psi, cov_init, sizeof(cov_init));
/* initialise state */
booz_ahrs_p = 0.;
booz_ahrs_q = 0.;
booz_ahrs_r = 0.;
booz_ahrs_bp = gyro[AXIS_P];
booz_ahrs_bq = gyro[AXIS_Q];
booz_ahrs_br = gyro[AXIS_R];
PhiOfAccel(booz_ahrs_phi, accel);
ThetaOfAccel(booz_ahrs_theta, accel);
#ifndef DISABLE_MAGNETOMETER
PsiOfMag(booz_ahrs_psi, mag);
#endif
booz_ahrs_status = BOOZ_AHRS_STATUS_RUNNING;
}
static inline void mtt_predict_axis(float* angle, float angle_dot, float P[2][2]) {
(*angle) += angle_dot * DT;
const float P_dot00 = Q_angle - P[0][1] - P[1][0];
const float P_dot01 = - P[1][1];
const float P_dot10 = - P[1][1];
const float P_dot11 = Q_bias;
/* P += Pdot * dt */
P[0][0] += P_dot00 * DT;
P[0][1] += P_dot01 * DT;
P[1][0] += P_dot10 * DT;
P[1][1] += P_dot11 * DT;
}
void booz_ahrs_predict(const float* gyro ) {
/* unbias gyro */
booz_ahrs_p = gyro[AXIS_P] - booz_ahrs_bp;
booz_ahrs_q = gyro[AXIS_Q] - booz_ahrs_bq;
booz_ahrs_r = gyro[AXIS_R] - booz_ahrs_br;
/* update angles */
float s_phi = sin(booz_ahrs_phi);
float c_phi = cos(booz_ahrs_phi);
float t_theta = tan(booz_ahrs_theta);
float phi_dot = booz_ahrs_p + s_phi*t_theta*booz_ahrs_q + c_phi*t_theta*booz_ahrs_r;
float theta_dot = c_phi*booz_ahrs_q - s_phi*booz_ahrs_r;
mtt_predict_axis(&booz_ahrs_phi, phi_dot, mtt_P_phi);
mtt_predict_axis(&booz_ahrs_theta, theta_dot, mtt_P_theta);
#ifndef DISABLE_MAGNETOMETER
float c_theta = cos(booz_ahrs_theta);
float psi_dot = s_phi/c_theta*booz_ahrs_q + c_phi/c_theta*booz_ahrs_r;
mtt_predict_axis(&booz_ahrs_psi, psi_dot, mtt_P_psi);
#endif
}
static inline void mtt_update_axis(float _err, float _P[2][2], float* angle, float* bias) {
const float Pct_0 = _P[0][0];
const float Pct_1 = _P[1][0];
/* E = C P C' + R */
const float E = R_accel + Pct_0;
/* K = P C' inv(E) */
const float K_0 = Pct_0 / E;
const float K_1 = Pct_1 / E;
/* P = P - K C P */
const float t_0 = Pct_0;
const float t_1 = _P[0][1];
_P[0][0] -= K_0 * t_0;
_P[0][1] -= K_0 * t_1;
_P[1][0] -= K_1 * t_0;
_P[1][1] -= K_1 * t_1;
/* X += K * err */
(*angle) += K_0 * _err;
(*bias) += K_1 * _err;
}
void booz_ahrs_update_accel( const float* accel) {
PhiOfAccel(booz_ahrs_measure_phi, accel);
float err_phi = booz_ahrs_measure_phi - booz_ahrs_phi;
WRAP(err_phi, M_PI);
mtt_update_axis(err_phi, mtt_P_phi, &booz_ahrs_phi, &booz_ahrs_bp);
WRAP(booz_ahrs_phi, M_PI);
ThetaOfAccel(booz_ahrs_measure_theta, accel);
float err_theta = booz_ahrs_measure_theta - booz_ahrs_theta;
WRAP(err_theta, M_PI_2);
mtt_update_axis(err_theta, mtt_P_theta, &booz_ahrs_theta, &booz_ahrs_bq);
WRAP(booz_ahrs_theta, M_PI_2);
}
void booz_ahrs_update_mag( const float* mag ) {
#ifndef DISABLE_MAGNETOMETER
PsiOfMag(booz_ahrs_measure_psi, mag);
float err_psi = booz_ahrs_measure_psi - booz_ahrs_psi;
WRAP(err_psi, M_PI);
mtt_update_axis(err_psi, mtt_P_psi, &booz_ahrs_psi, &booz_ahrs_br);
WRAP(booz_ahrs_psi, M_PI);
#endif
}
sw/airborne/ahrs_multitilt.h
-38
View File
@@ -1,38 +0,0 @@
/*
* $Id$
*
* Copyright (C) 2008 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.
*
*/
#ifndef AHRS_MULTITILT_H
#define AHRS_MULTITILT_H
#include "std.h"
extern float mtt_P_phi[2][2];
extern float mtt_P_theta[2][2];
extern float mtt_P_psi[2][2];
#endif /* AHRS_MULTITILT_H */
sw/airborne/ahrs_quat_fast_ekf.h
-23
View File
@@ -1,23 +0,0 @@
#ifndef AHRS_QUAT_FAST_EKF_H
#define AHRS_QUAT_FAST_EKF_H
#include <inttypes.h>
#include "6dof.h"
/* ekf state : quaternion and gyro biases */
extern FLOAT_T afe_q0, afe_q1, afe_q2, afe_q3;
//extern FLOAT_T afe_bias_p, afe_bias_q, afe_bias_r;
/* we maintain unbiased rates */
//extern FLOAT_T afe_p, afe_q, afe_r;
/* we maintain eulers angles */
//extern FLOAT_T afe_phi, afe_theta, afe_psi;
extern FLOAT_T afe_P[7][7]; /* covariance */
extern void afe_init( const int16_t *mag, const FLOAT_T* accel, const FLOAT_T* gyro );
extern void afe_predict( const FLOAT_T* gyro );
extern void afe_update_phi( const FLOAT_T* accel);
extern void afe_update_theta( const FLOAT_T* accel);
extern void afe_update_psi( const int16_t* mag);
#endif /* AHRS_QUAT_FAST_EKF_H_H */
sw/airborne/ahrs_quat_fast_ekf_utils.h
-172
View File
@@ -1,172 +0,0 @@
#ifndef AHRS_QUAT_FAST_EKF_UTILS_H
#define AHRS_QUAT__FAST_EKF_UTILS_H
#include "ahrs_quat_fast_ekf.h"
/*
* Compute the five elements of the DCM that we use for our
* rotations and Jacobians. This is used by several other functions
* to speedup their computations.
*/
#define AFE_DCM_OF_QUAT() { \
afe_dcm00 = 1.0-2*(afe_q2*afe_q2 + afe_q3*afe_q3); \
afe_dcm01 = 2*(afe_q1*afe_q2 + afe_q0*afe_q3); \
afe_dcm02 = 2*(afe_q1*afe_q3 - afe_q0*afe_q2); \
afe_dcm12 = 2*(afe_q2*afe_q3 + afe_q0*afe_q1); \
afe_dcm22 = 1.0-2*(afe_q1*afe_q1 + afe_q2*afe_q2); \
}
/*
* Compute Euler angles from our DCM.
*/
#define AFE_PHI_OF_DCM() { booz_ahrs_phi = atan2( afe_dcm12, afe_dcm22 );}
#define AFE_THETA_OF_DCM() { booz_ahrs_theta = -asin( afe_dcm02 );}
#define AFE_PSI_OF_DCM() { booz_ahrs_psi = atan2( afe_dcm01, afe_dcm00 );}
#define AFE_EULER_OF_DCM() { AFE_PHI_OF_DCM(); AFE_THETA_OF_DCM(); AFE_PSI_OF_DCM()}
/*
* initialise the quaternion from the set of eulers
*/
#define AFE_QUAT_OF_EULER() { \
const FLOAT_T phi2 = booz_ahrs_phi / 2.0; \
const FLOAT_T theta2 = booz_ahrs_theta / 2.0; \
const FLOAT_T psi2 = booz_ahrs_psi / 2.0; \
\
const FLOAT_T sinphi2 = sin( phi2 ); \
const FLOAT_T cosphi2 = cos( phi2 ); \
\
const FLOAT_T sintheta2 = sin( theta2 ); \
const FLOAT_T costheta2 = cos( theta2 ); \
\
const FLOAT_T sinpsi2 = sin( psi2 ); \
const FLOAT_T cospsi2 = cos( psi2 ); \
\
afe_q0 = cosphi2 * costheta2 * cospsi2 + sinphi2 * sintheta2 * sinpsi2; \
afe_q1 = -cosphi2 * sintheta2 * sinpsi2 + sinphi2 * costheta2 * cospsi2; \
afe_q2 = cosphi2 * sintheta2 * cospsi2 + sinphi2 * costheta2 * sinpsi2; \
afe_q3 = cosphi2 * costheta2 * sinpsi2 - sinphi2 * sintheta2 * cospsi2; \
}
/*
* normalize quaternion
*/
#define AFE_NORM_QUAT() { \
FLOAT_T mag = afe_q0*afe_q0 + afe_q1*afe_q1 \
+ afe_q2*afe_q2 + afe_q3*afe_q3; \
mag = sqrt( mag ); \
afe_q0 /= mag; \
afe_q1 /= mag; \
afe_q2 /= mag; \
afe_q3 /= mag; \
}
/*
* Compute the Jacobian of the measurements to the system states.
*/
#define AFE_COMPUTE_H_PHI() { \
const FLOAT_T phi_err = 2 / (afe_dcm22*afe_dcm22 + afe_dcm12*afe_dcm12); \
afe_H[0] = (afe_q1 * afe_dcm22) * phi_err; \
afe_H[1] = (afe_q0 * afe_dcm22 + 2 * afe_q1 * afe_dcm12) * phi_err; \
afe_H[2] = (afe_q3 * afe_dcm22 + 2 * afe_q2 * afe_dcm12) * phi_err; \
afe_H[3] = (afe_q2 * afe_dcm22) * phi_err; \
}
#define AFE_COMPUTE_H_THETA() { \
const FLOAT_T theta_err = -2 / sqrt( 1 - afe_dcm02*afe_dcm02 ); \
afe_H[0] = -afe_q2 * theta_err; \
afe_H[1] = afe_q3 * theta_err; \
afe_H[2] = -afe_q0 * theta_err; \
afe_H[3] = afe_q1 * theta_err; \
}
#define AFE_COMPUTE_H_PSI() { \
const FLOAT_T psi_err = 2 / (afe_dcm00*afe_dcm00 + afe_dcm01*afe_dcm01); \
afe_H[0] = (afe_q3 * afe_dcm00) * psi_err; \
afe_H[1] = (afe_q2 * afe_dcm00) * psi_err; \
afe_H[2] = (afe_q1 * afe_dcm00 + 2 * afe_q2 * afe_dcm01) * psi_err; \
afe_H[3] = (afe_q0 * afe_dcm00 + 2 * afe_q3 * afe_dcm01) * psi_err; \
}
/* convert sensor reading into euler angle measurement */
static inline FLOAT_T afe_phi_of_accel( const FLOAT_T* accel ) {
return atan2(accel[AXIS_Y], accel[AXIS_Z]);
}
static inline FLOAT_T afe_theta_of_accel( const FLOAT_T* accel) {
FLOAT_T g2 =
accel[AXIS_X]*accel[AXIS_X] +
accel[AXIS_Y]*accel[AXIS_Y] +
accel[AXIS_Z]*accel[AXIS_Z];
return -asin( accel[AXIS_X] / sqrt( g2 ) );
}
/*
* The rotation matrix to rotate from NED frame to body frame without
* rotating in the yaw axis is:
*
* [ 1 0 0 ] [ cos(Theta) 0 -sin(Theta) ]
* [ 0 cos(Phi) sin(Phi) ] [ 0 1 0 ]
* [ 0 -sin(Phi) cos(Phi) ] [ sin(Theta) 0 cos(Theta) ]
*
* This expands to:
*
* [ cos(Theta) 0 -sin(Theta) ]
* [ sin(Phi)*sin(Theta) cos(Phi) sin(Phi)*cos(Theta)]
* [ cos(Phi)*sin(Theta) -sin(Phi) cos(Phi)*cos(Theta)]
*
* However, to untilt the compass reading, we need to use the
* transpose of this matrix.
*
* [ cos(Theta) sin(Phi)*sin(Theta) cos(Phi)*sin(Theta) ]
* [ 0 cos(Phi) -sin(Phi) ]
* [ -sin(Theta) sin(Phi)*cos(Theta) cos(Phi)*cos(Theta) ]
*
* Additionally,
* since we already have the DCM computed for our current attitude,
* we can short cut all of the trig. substituting
* in from the definition of euler2quat and quat2euler, we have:
*
* [ cos(Theta) -dcm12*dcm02 -dcm22*dcm02 ]
* [ 0 dcm22 -dcm12 ]
* [ dcm02 dcm12 dcm22 ]
*
*/
static inline FLOAT_T afe_psi_of_mag( const int16_t* mag ) {
const FLOAT_T ctheta = cos( booz_ahrs_theta );
#if 0
const FLOAT_T mn = ctheta * mag[0]
- (afe_dcm12 * mag[1] + afe_dcm22 * mag[2]) * afe_dcm02 / ctheta;
const FLOAT_T me =
(afe_dcm22 * mag[1] - afe_dcm12 * mag[2]) / ctheta;
#else
const FLOAT_T stheta = sin( booz_ahrs_theta );
const FLOAT_T cphi = cos( booz_ahrs_phi );
const FLOAT_T sphi = sin( booz_ahrs_phi );
const FLOAT_T mn =
ctheta* mag[0]+
sphi*stheta* mag[1]+
cphi*stheta* mag[2];
const FLOAT_T me =
0* mag[0]+
cphi* mag[1]+
-sphi* mag[2];
#endif
const FLOAT_T psi = -atan2( me, mn );
return psi;
}
#define AFE_WARP(x, b) { \
while( x < -b ) \
x += 2 * b; \
while( x > b ) \
x -= 2 * b; \
}
#endif /* AHRS_QUAT_FAST_EKF_UTILS_H */
sw/airborne/arm7/actuators_buss_twi_blmc_hw.c
-18
View File
@@ -1,18 +0,0 @@
#include "actuators.h"
uint8_t buss_twi_blmc_motor_power[BUSS_TWI_BLMC_NB];
volatile bool_t buss_twi_blmc_status;
volatile uint8_t buss_twi_blmc_nb_err;
volatile bool_t buss_twi_blmc_i2c_done;
volatile uint8_t buss_twi_blmc_idx;
const uint8_t buss_twi_blmc_addr[BUSS_TWI_BLMC_NB] = { 0x52, 0x54, 0x56, 0x58 };
void actuators_init ( void ) {
uint8_t i;
for (i=0; i<BUSS_TWI_BLMC_NB;i++)
buss_twi_blmc_motor_power[i] = 0;
buss_twi_blmc_status = BUSS_TWI_BLMC_STATUS_IDLE;
buss_twi_blmc_nb_err = 0;
buss_twi_blmc_i2c_done = TRUE;
}
sw/airborne/arm7/actuators_buss_twi_blmc_hw.h
-47
View File
@@ -1,47 +0,0 @@
#ifndef ACTUATORS_BUSS_TWI_BLMC_HW_H
#define ACTUATORS_BUSS_TWI_BLMC_HW_H
#include <string.h>
#include "std.h"
#include "i2c.h"
#define ChopServo(x,a,b) ((x)>(b)?(b):(x))
#define Actuator(i) buss_twi_blmc_motor_power[i]
#define ActuatorsCommit() { \
if ( buss_twi_blmc_status == BUSS_TWI_BLMC_STATUS_IDLE) { \
buss_twi_blmc_idx = 0; \
buss_twi_blmc_status = BUSS_TWI_BLMC_STATUS_BUSY; \
ActuatorsBussTwiBlmcSend(); \
} \
else \
buss_twi_blmc_nb_err++; \
}
#define SERVOS_TICS_OF_USEC(s) ((uint8_t)(s))
#define BUSS_TWI_BLMC_STATUS_IDLE 0
#define BUSS_TWI_BLMC_STATUS_BUSY 1
#define BUSS_TWI_BLMC_NB 4
extern uint8_t buss_twi_blmc_motor_power[BUSS_TWI_BLMC_NB];
extern volatile bool_t buss_twi_blmc_status;
extern volatile uint8_t buss_twi_blmc_nb_err;
extern volatile bool_t buss_twi_blmc_i2c_done;
extern volatile uint8_t buss_twi_blmc_idx;
extern const uint8_t buss_twi_blmc_addr[];
#define ActuatorsBussTwiBlmcNext() { \
buss_twi_blmc_idx++; \
if (buss_twi_blmc_idx < BUSS_TWI_BLMC_NB) { \
ActuatorsBussTwiBlmcSend(); \
} \
else \
buss_twi_blmc_status = BUSS_TWI_BLMC_STATUS_IDLE; \
}
#define ActuatorsBussTwiBlmcSend() { \
i2c_buf[0] = buss_twi_blmc_motor_power[buss_twi_blmc_idx]; \
i2c_transmit(buss_twi_blmc_addr[buss_twi_blmc_idx], 1, &buss_twi_blmc_i2c_done); \
}
#endif /* ACTUATORS_BUSS_TWI_BLMC_HW_H */
sw/airborne/arm7/booz_imu_hw.c
-105
View File
@@ -1,105 +0,0 @@
#include "booz_imu.h"
/* SSPCR0 settings */
#define SSP_DDS 0x07 << 0 /* data size : 8 bits */
#define SSP_FRF 0x00 << 4 /* frame format : SPI */
#define SSP_CPOL 0x00 << 6 /* clock polarity : data captured on first clock transition */
#define SSP_CPHA 0x00 << 7 /* clock phase : SCK idles low */
#define SSP_SCR 0x0F << 8 /* serial clock rate : divide by 16 */
/* SSPCR1 settings */
#define SSP_LBM 0x00 << 0 /* loopback mode : disabled */
#define SSP_SSE 0x00 << 1 /* SSP enable : disabled */
#define SSP_MS 0x00 << 2 /* master slave mode : master */
#define SSP_SOD 0x00 << 3 /* slave output disable : don't care when master */
static void SPI1_ISR(void) __attribute__((naked));
void booz_imu_hw_init(void) {
/* setup pins for SSP (SCK, MISO, MOSI) */
PINSEL1 |= 2 << 2 | 2 << 4 | 2 << 6;
/* setup SSP */
SSPCR0 = SSP_DDS | SSP_FRF | SSP_CPOL | SSP_CPHA | SSP_SCR;
SSPCR1 = SSP_LBM | SSP_MS | SSP_SOD;
SSPCPSR = 0x2;
/* initialize interrupt vector */
VICIntSelect &= ~VIC_BIT(VIC_SPI1); // SPI1 selected as IRQ
VICIntEnable = VIC_BIT(VIC_SPI1); // SPI1 interrupt enabled
VICVectCntl7 = VIC_ENABLE | VIC_SPI1;
VICVectAddr7 = (uint32_t)SPI1_ISR; // address of the ISR
}
static inline bool_t isr_try_mag(void) {
switch (micromag_status) {
case MM_IDLE :
MmSendReq();
return TRUE;
case MM_GOT_EOC:
MmReadRes();
return TRUE;
}
return FALSE;
}
static inline bool_t isr_try_baro(void) {
#ifndef SCP1000_NO_EINT
switch (scp1000_status) {
case SCP1000_STA_STOPPED:
Scp1000SendConfig();
return TRUE;
case SCP1000_STA_GOT_EOC:
Scp1000Read();
return TRUE;
}
#else
if (scp1000_status == SCP1000_STA_STOPPED) {
Scp1000SendConfig();
return TRUE;
}
else if (scp1000_status == SCP1000_STA_WAIT_EOC && Scp1000DataReady()) {
scp1000_status = SCP1000_STA_GOT_EOC;
Scp1000Read();
return TRUE;
}
#endif
return FALSE;
}
static void SPI1_ISR(void) {
ISR_ENTRY();
switch (booz_imu_status) {
case BOOZ_IMU_STA_MEASURING_GYRO:
Max1167OnSpiInt();
/* we now have a gyro reading */
if (isr_try_mag())
booz_imu_status = BOOZ_IMU_STA_MEASURING_MAG;
else if (isr_try_baro())
booz_imu_status = BOOZ_IMU_STA_MEASURING_BARO;
else
booz_imu_status = BOOZ_IMU_STA_IDLE;
break;
case BOOZ_IMU_STA_MEASURING_MAG:
MmOnSpiIt();
if (isr_try_baro())
booz_imu_status = BOOZ_IMU_STA_MEASURING_BARO;
else
booz_imu_status = BOOZ_IMU_STA_IDLE;
break;
case BOOZ_IMU_STA_MEASURING_BARO:
Scp1000OnSpiIt();
booz_imu_status = BOOZ_IMU_STA_IDLE;
break;
}
VICVectAddr = 0x00000000; /* clear this interrupt from the VIC */
ISR_EXIT();
}
sw/airborne/arm7/booz_imu_hw.h
-30
View File
@@ -1,30 +0,0 @@
/*
* $Id$
*
* Copyright (C) 2008 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.
*
*/
#ifndef BOOZ_IMU_HW_H
#define BOOZ_IMU_HW_H
#endif /* BOOZ_IMU_HW_H */
sw/airborne/arm7/booz_link_mcu_hw.c
-261
View File
@@ -1,261 +0,0 @@
/*
* $Id$
*
* Copyright (C) 2008 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.
*
*/
#include "booz_link_mcu.h"
uint8_t* link_mcu_tx_buf;
uint8_t* link_mcu_rx_buf;
#ifdef BOOZ_FILTER_MCU
volatile uint8_t link_mcu_status;
volatile uint8_t link_mcu_buf_idx;
uint32_t it_count;
uint16_t foo_debug;
/*
wiring on IMU_V3
P0_4 SCK0
P0_5 MISO0
P0_6 MOSI0
P0_7 SSEL0
P1_24 DRDY
*/
#include "armVIC.h"
void SPI0_ISR(void) __attribute__((naked));
/* */
#define S0SPCR_BitEnable 0 << 2 /* enable more than 8 bits transferts */
#define S0SPCR_CPHA 1 << 3 /* data sampled on first edge */
#define S0SPCR_CPOL 0 << 4 /* clock idles high */
#define S0SPCR_MSTR 1 << 5 /* master mode */
#define S0SPCR_LSBF 0 << 6 /* MSB first */
#define S0SPCR_SPIE 0 << 7 /* SPI interrupt disabled */
#define S0SPCR_BITS 0 << 8 /* 16 bits transferts */
#define S0SPCR_VAL ( S0SPCR_BitEnable | \
S0SPCR_CPHA | \
S0SPCR_CPOL | \
S0SPCR_MSTR | \
S0SPCR_LSBF | \
S0SPCR_SPIE | \
S0SPCR_BITS )
#define SPI0_EnableInterrupt() SetBit(S0SPCR, 7)
#define SPI0_ClearInterrupt() SetBit(S0SPINT, SPI0IF)
void booz_link_mcu_hw_init ( void ) {
/* SS pin is output */
SetBit(IO0DIR, SS_PIN);
SPI0_UnselectSlave();
/* init SPI0 */
/* setup pins for sck, miso, mosi */
PINSEL0 |= 1<<8 | 1<<10 | 1<<12;
/* configure SPI : see above */
S0SPCR = S0SPCR_VAL;
/* setup SPI clock rate ~ 450Khz with 15MHz VPB */
S0SPCCR = 0x40;
/* initialize interrupt vector */
VICIntSelect &= ~VIC_BIT(VIC_SPI0); // SPI0 selected as IRQ
VICIntEnable = VIC_BIT(VIC_SPI0); // SPI0 interrupt enabled
VICVectCntl3 = VIC_ENABLE | VIC_SPI0;
VICVectAddr3 = (uint32_t)SPI0_ISR; // address of the ISR
/* clear all potentially pending interrupts */
uint16_t foo1 __attribute__((unused)) = S0SPSR;
uint16_t foo2 __attribute__((unused)) = S0SPDR;
SPI0_ClearInterrupt();
SPI0_EnableInterrupt();
link_mcu_status = LINK_MCU_STATUS_IDLE;
link_mcu_buf_idx = 0;
link_mcu_tx_buf = (uint8_t*)&inter_mcu_state;
link_mcu_rx_buf = (uint8_t*)&booz_link_mcu_state_unused;
}
void SPI0_ISR(void) {
ISR_ENTRY();
/* read status register */
uint16_t foo __attribute__((unused)) = S0SPSR;
foo_debug = foo;
it_count++;
/* read_data_register */
link_mcu_rx_buf[link_mcu_buf_idx] = S0SPDR;
link_mcu_buf_idx++;
if (link_mcu_buf_idx < LINK_MCU_BUF_LEN) {
S0SPDR = link_mcu_tx_buf[link_mcu_buf_idx];
}
else {
SPI0_UnselectSlave();
link_mcu_status = LINK_MCU_STATUS_IDLE;
}
SPI0_ClearInterrupt();
/* clear this interrupt from the VIC */
VICVectAddr = 0x00000000;
ISR_EXIT();
}
#endif /* BOOZ_FILTER_MCU */
#ifdef BOOZ_CONTROLLER_MCU
/*
IMU connected to SSP ( aka SPI1 ) as master
Controller is slave
*/
volatile uint32_t it_cnt;
volatile uint32_t rx_it_cnt;
volatile uint32_t ti_it_cnt;
#include "LPC21xx.h"
#include "interrupt_hw.h"
volatile uint8_t link_mcu_rx_buf_idx;
volatile uint8_t link_mcu_tx_buf_idx;
#define LinkMcuReceive() { \
while (bit_is_set(SSPSR, RNE)) { \
if (link_mcu_rx_buf_idx < LINK_MCU_BUF_LEN) { \
link_mcu_rx_buf[link_mcu_rx_buf_idx] = SSPDR; \
link_mcu_rx_buf_idx++; \
if (link_mcu_rx_buf_idx == LINK_MCU_BUF_LEN) \
booz_link_mcu_status = BOOZ_LINK_MCU_DATA_AVAILABLE; \
} \
else { \
uint8_t foo __attribute__ ((unused)); \
foo = SSPDR; \
} \
} \
}
static void SSP_ISR(void) __attribute__((naked));
/* SSPCR0 settings */
#define SSP_DDS 0x07 << 0 /* data size : 8 bits */
#define SSP_FRF 0x00 << 4 /* frame format : SPI */
#define SSP_CPOL 0x00 << 6 /* clock polarity : first clock transition */
#define SSP_CPHA 0x01 << 7 /* clock phase : SCK idles high */
#define SSP_SCR 0x01 << 8 /* serial clock rate : divide by 2 */
/* SSPCR1 settings */
#define SSP_LBM 0x00 << 0 /* loopback mode : disabled */
#define SSP_SSE 0x00 << 1 /* SSP enable : disabled */
#define SSP_MS 0x01 << 2 /* master slave mode : slave */
#define SSP_SOD 0x00 << 3 /* slave output disable : no */
#define SSPCR0_VAL (SSP_DDS | SSP_FRF | SSP_CPOL | SSP_CPHA | SSP_SCR )
#define SSPCR1_VAL (SSP_LBM | SSP_SSE | SSP_MS | SSP_SOD )
#define SSP_PINSEL1_SCK (2<<2)
#define SSP_PINSEL1_MISO (2<<4)
#define SSP_PINSEL1_MOSI (2<<6)
#define SSP_PINSEL1_SSEL (2<<8)
void booz_link_mcu_hw_init ( void ) {
/* setup pins for SSP (SCK, MISO, MOSI, SSEL) */
PINSEL1 |= SSP_PINSEL1_SCK | SSP_PINSEL1_MISO |
SSP_PINSEL1_MOSI | SSP_PINSEL1_SSEL ;
/* setup SSP */
SSPCR0 = SSPCR0_VAL;;
SSPCR1 = SSPCR1_VAL;
SSPCPSR = 0x10;
/* initialize interrupt vector */
VICIntSelect &= ~VIC_BIT( VIC_SPI1 ); /* SPI1 selected as IRQ */
VICIntEnable = VIC_BIT( VIC_SPI1 ); /* enable it */
VICVectCntl9 = VIC_ENABLE | VIC_SPI1;
VICVectAddr9 = (uint32_t)SSP_ISR; /* address of the ISR */
link_mcu_rx_buf = (uint8_t*)&inter_mcu_state;
link_mcu_tx_buf = (uint8_t*)&booz_link_mcu_state_unused;
BoozLinkMcuHwRestart();
}
static void SSP_ISR(void) {
ISR_ENTRY();
it_cnt++;
/* Rx half full */
if (bit_is_set(SSPMIS, RXMIS)) {
rx_it_cnt++;
/* LinkMcuTransmit(); */
LinkMcuReceive();
SSP_EnableRti();
}
/* Rx timeout */
if ( bit_is_set(SSPMIS, RTMIS)) {
ti_it_cnt++;
LinkMcuReceive();
SSP_ClearRti();
SSP_DisableRti();
SSP_Disable();
booz_link_mcu_status = BOOZ_LINK_MCU_DATA_AVAILABLE;
}
VICVectAddr = 0x00000000; /* clear this interrupt from the VIC */
ISR_EXIT();
}
#endif /* BOOZ_CONTROLLER_MCU */
sw/airborne/arm7/booz_link_mcu_hw.h
-99
View File
@@ -1,99 +0,0 @@
/*
* $Id$
*
* Copyright (C) 2008 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.
*
*/
#ifndef BOOZ_LINK_MCU_HW_H
#define BOOZ_LINK_MCU_HW_H
#include "LPC21xx.h"
#include "booz_debug.h"
#define LINK_MCU_STATUS_IDLE 0
#define LINK_MCU_STATUS_BUSY 1
#define LINK_IMU_ERR_OVERRUN 0
extern volatile uint8_t link_mcu_status;
extern uint8_t* link_mcu_tx_buf;
extern uint8_t* link_mcu_rx_buf;
#define LINK_MCU_BUF_LEN (sizeof(struct booz_inter_mcu_state)/sizeof(uint8_t))
#ifdef BOOZ_FILTER_MCU
extern volatile uint8_t link_mcu_buf_idx;
#define SS_PIN 7
#define SPI0_SelectSlave() SetBit(IO0CLR, SS_PIN)
#define SPI0_UnselectSlave() SetBit(IO0SET, SS_PIN)
#define BoozLinkMcuHwSend() { \
ASSERT((link_mcu_status == LINK_MCU_STATUS_IDLE), \
DEBUG_LINK_MCU_IMU, LINK_IMU_ERR_OVERRUN); \
if (link_mcu_status != LINK_MCU_STATUS_IDLE) return; \
SPI0_SelectSlave(); \
link_mcu_status = LINK_MCU_STATUS_BUSY; \
link_mcu_buf_idx = 0; \
S0SPDR = link_mcu_tx_buf[0]; \
}
#endif /* BOOZ_FILTER_MCU */
#ifdef BOOZ_CONTROLLER_MCU
extern volatile uint8_t link_mcu_rx_buf_idx;
extern volatile uint8_t link_mcu_tx_buf_idx;
#define SSP_Enable() SetBit(SSPCR1, SSE);
#define SSP_Disable() ClearBit(SSPCR1, SSE);
#define SSP_EnableRxi() SetBit(SSPIMSC, RXIM)
#define SSP_DisableRxi() ClearBit(SSPIMSC, RXIM)
#define SSP_EnableTxi() SetBit(SSPIMSC, TXIM)
#define SSP_DisableTxi() ClearBit(SSPIMSC, TXIM)
#define SSP_EnableRti() SetBit(SSPIMSC, RTIM);
#define SSP_DisableRti() ClearBit(SSPIMSC, RTIM);
#define SSP_ClearRti() SetBit(SSPICR, RTIC);
#define LinkMcuTransmit() { \
while (link_mcu_tx_buf_idx < LINK_MCU_BUF_LEN \
&& bit_is_set(SSPSR, TNF)) { \
SSPDR = link_mcu_tx_buf[link_mcu_tx_buf_idx]; \
link_mcu_tx_buf_idx++; \
} \
if (link_mcu_tx_buf_idx == LINK_MCU_BUF_LEN) \
SSP_DisableRxi(); \
}
#define BoozLinkMcuHwRestart() { \
link_mcu_rx_buf_idx = 0; \
link_mcu_tx_buf_idx = 0; \
SSP_EnableRxi(); \
SSP_Enable(); \
/* LinkMcuTransmit(); */ \
}
#endif /* BOOZ_CONTROLLER_MCU */
#endif /* BOOZ_LINK_MCU_HW_H */
sw/airborne/arm7/mb_buss_twi_controller.c
-36
View File
@@ -1,36 +0,0 @@
#include "mb_buss_twi_controller.h"
#include <string.h>
#include "i2c.h"
uint8_t mb_buss_twi_command;
uint8_t mb_buss_twi_nb_overun;
uint8_t mb_buss_twi_i2c_done;
#define MB_BUSS_TWI_CONTROLLER_MAX_CMD 255
/*
Slave address
front = 0x52
back = 0x54
right = 0x56
left = 0x58
*/
#define MB_BUSS_TWI_CONTROLLER_ADDR 0x52
void mb_buss_twi_controller_init(void) {
mb_buss_twi_nb_overun = 0;
mb_buss_twi_i2c_done = TRUE;
}
void mb_buss_twi_controller_set( float throttle ) {
if (mb_buss_twi_i2c_done) {
mb_buss_twi_command = throttle * MB_BUSS_TWI_CONTROLLER_MAX_CMD;
i2c_buf[0] = mb_buss_twi_command;
i2c_transmit(MB_BUSS_TWI_CONTROLLER_ADDR, 1, &mb_buss_twi_i2c_done);
}
else
mb_buss_twi_nb_overun++;
}
sw/airborne/arm7/mb_buss_twi_controller.h
-12
View File
@@ -1,12 +0,0 @@
#ifndef MB_BUSS_TWI_CONTROLLER_H
#define MB_BUSS_TWI_CONTROLLER_H
#include "std.h"
extern void mb_buss_twi_controller_init(void);
extern void mb_buss_twi_controller_set( float throttle );
#endif /* MB_BUSS_TWI_CONTROLLER_H */
sw/airborne/arm7/mb_current.c
-17
View File
@@ -1,17 +0,0 @@
#include "mb_current.h"
#include "adc.h"
static struct adc_buf mb_current_buf;
float mb_current_amp;
void mb_current_init(void) {
adc_buf_channel(4, &mb_current_buf, 16);
}
void mb_current_periodic(void) {
mb_current_amp = (float)mb_current_buf.sum * 0.00113607 - 2.8202;
}
sw/airborne/arm7/mb_current.h
-11
View File
@@ -1,11 +0,0 @@
#ifndef MB_CURRENT_H
#define MB_CURRENT_H
#include "std.h"
extern void mb_current_init(void);
extern void mb_current_periodic(void);
extern float mb_current_amp;
#endif /* MB_CURRENT_H */
sw/airborne/arm7/mb_modes.c
-93
View File
@@ -1,93 +0,0 @@
#include "mb_modes.h"
#include "adc.h"
#include "sys_time.h"
uint8_t mb_modes_mode;
float mb_modes_throttle;
float mb_modes_last_change_time;
float mb_modes_ramp_duration;
float mb_modes_step_low_throttle;
float mb_modes_step_high_throttle;
float mb_modes_step_duration;
static void mb_modes_manual( void );
static void mb_modes_ramp( void );
static void mb_modes_step( void );
static void mb_modes_prbs( void );
static struct adc_buf mb_modes_adc_buf; /* manual mode */
void mb_mode_init(void) {
adc_buf_channel(1, &mb_modes_adc_buf, 16);
mb_modes_mode = MB_MODES_IDLE;
mb_modes_throttle = 0.;
mb_modes_ramp_duration = 40;
mb_modes_step_low_throttle = 0.6;
mb_modes_step_high_throttle = 0.7;
mb_modes_step_duration = 1.;
}
void mb_mode_event(void) {}
void mb_mode_periodic(void) {
switch (mb_modes_mode) {
case MB_MODES_IDLE :
mb_modes_throttle = 0.;
break;
case MB_MODES_MANUAL :
mb_modes_manual();
break;
case MB_MODES_RAMP :
mb_modes_ramp();
break;
case MB_MODES_STEP :
mb_modes_step();
break;
default:
mb_modes_throttle = 0.;
}
}
static void mb_modes_manual( void ) {
uint16_t poti = mb_modes_adc_buf.sum;
mb_modes_throttle = (float)poti/(16.*1024.);
}
static void mb_modes_ramp( void ) {
float now = GET_CUR_TIME_FLOAT();
float elapsed = now - mb_modes_last_change_time;
if ( elapsed < mb_modes_ramp_duration)
mb_modes_throttle = elapsed/mb_modes_ramp_duration;
else if ( elapsed < 2 * mb_modes_ramp_duration)
mb_modes_throttle = 2 - elapsed/mb_modes_ramp_duration;
else {
mb_modes_last_change_time = now;
mb_modes_throttle = 0.;
}
}
static void mb_modes_step( void ) {
float now = GET_CUR_TIME_FLOAT();
float elapsed = now - mb_modes_last_change_time;
if ( elapsed < mb_modes_step_duration)
mb_modes_throttle = mb_modes_step_low_throttle;
else if ( elapsed < 2 * mb_modes_step_duration)
mb_modes_throttle = mb_modes_step_high_throttle;
else {
mb_modes_last_change_time = now;
mb_modes_throttle = mb_modes_step_low_throttle;
}
}
sw/airborne/arm7/mb_modes.h
-34
View File
@@ -1,34 +0,0 @@
#ifndef MB_MODES_H
#define MB_MODES_H
#include "std.h"
#define MB_MODES_IDLE 0
#define MB_MODES_MANUAL 1
#define MB_MODES_RAMP 2
#define MB_MODES_STEP 3
#define MB_MODES_PRBS 4
extern uint8_t mb_modes_mode;
extern float mb_modes_throttle;
extern float mb_modes_last_change_time;
extern float mb_modes_ramp_duration;
extern float mb_modes_step_low_throttle;
extern float mb_modes_step_high_throttle;
extern float mb_modes_step_duration;
extern void mb_mode_init(void);
extern void mb_mode_event(void);
extern void mb_mode_periodic(void);
#define mb_modes_SetMode(_val) { \
mb_modes_mode = _val; \
mb_modes_last_change_time = GET_CUR_TIME_FLOAT(); \
}
#endif /* MB_MODES_H */
sw/airborne/arm7/mb_scale.c
-17
View File
@@ -1,17 +0,0 @@
#include "mb_scale.h"
volatile uint32_t pulse_len;
float mb_scale_thrust;
float mb_scale_torque;
void mb_scale_init ( void ) {
/* select pin for capture */
ICP_PINSEL |= ICP_PINSEL_VAL << ICP_PINSEL_BIT;
/* enable capture 0.3 on falling edge + trigger interrupt */
T0CCR |= TCCR_CR3_F | TCCR_CR3_I;
}
void mb_scale_periodic(void) {
mb_scale_thrust = 500. / 240500. * (pulse_len - 3073500);
}
sw/airborne/arm7/mb_scale.h
-30
View File
@@ -1,30 +0,0 @@
#ifndef MB_SCALE_H
#define MB_SCALE_H
/* P0.29 CAP0.3 */
#define ICP_PINSEL PINSEL1
#define ICP_PINSEL_VAL 0x02
#define ICP_PINSEL_BIT 26
#include "led.h"
extern volatile uint32_t pulse_len;
extern float mb_scale_thrust;
extern float mb_scale_torque;
void mb_scale_init ( void );
void mb_scale_periodic(void);
#define MB_SCALE_ICP_ISR() { \
static uint32_t last; \
uint32_t now = T0CR3; \
pulse_len = now - last; \
last = now; \
LED_TOGGLE(2); \
}
#endif /* MB_SCALE_H */
sw/airborne/arm7/mb_servo.c
-66
View File
@@ -1,66 +0,0 @@
#include "mb_servo.h"
#include "sys_time.h"
#define MY_NB_CLOCK_TIMER_PWM(time_us) SYS_TICS_OF_USEC(time_us)
uint32_t mb_servo_max_pulse_ns, mb_servo_min_pulse_ns;
void mb_servo_set_ns(uint32_t duration_ns);
void mb_servo_init( void ) {
/* set P0.21 as PWM5 output */
PINSEL1 |= (0X01 << 10);
/* enable and select the type of PWM channel */
PWMPCR |= PWMPCR_ENA5;
/* set Match0 value (refresh rate) */
PWMMR0 = MY_NB_CLOCK_TIMER_PWM(25000);
/* commit PWMMRx changes */
PWMLER = PWMLER_LATCH0;
/* enable PWM timer in PWM mode */
PWMTCR = PWMTCR_COUNTER_ENABLE | PWMTCR_PWM_ENABLE;
mb_servo_min_pulse_ns = MIN_SERVO_NS;
mb_servo_max_pulse_ns = MAX_SERVO_NS;
}
void mb_servo_set_range( uint32_t min_pulse_ns, uint32_t max_pulse_ns ) {
mb_servo_min_pulse_ns = min_pulse_ns;
mb_servo_max_pulse_ns = max_pulse_ns;
}
void mb_servo_set_us(uint32_t duration_us) {
/* set Match5 value (pulse duration )*/
PWMMR5 = MY_NB_CLOCK_TIMER_PWM(duration_us);
/* commit PWMMRx changes */
PWMLER = PWMLER_LATCH5;
}
void mb_servo_set_ns(uint32_t duration_ns) {
/* set Match5 value (pulse duration )*/
PWMMR5 = SYS_TICS_OF_NSEC(duration_ns);
/* commit PWMMRx changes */
PWMLER = PWMLER_LATCH5;
}
/* normalized throttle between 0. and 1. */
void mb_servo_set(float throttle) {
uint32_t range = mb_servo_max_pulse_ns - mb_servo_min_pulse_ns;
uint32_t pulse_ns = mb_servo_min_pulse_ns + throttle * (range);
mb_servo_set_ns(pulse_ns);
}
#define FOO_DELAY() { \
uint32_t foo = 0; \
while (foo<10000000) foo++; \
}
/* arm the brushless controller */
void mb_servo_arm (void) {
mb_servo_set(0.);
FOO_DELAY();
mb_servo_set(1.);
FOO_DELAY();
mb_servo_set(0.);
}
sw/airborne/arm7/mb_servo.h
-19
View File
@@ -1,19 +0,0 @@
#ifndef MB_SERVO_H
#define MB_SERVO_H
#include "std.h"
#define MIN_SERVO_US 1000
#define MAX_SERVO_US 2000
#define MIN_SERVO_NS 1000000
#define MAX_SERVO_NS 2000000
#define RANGE_SERVO_US (MAX_SERVO_US - MIN_SERVO_US)
void mb_servo_init( void );
void mb_servo_set_range( uint32_t min_pulse_ns, uint32_t max_pulse_ns );
void mb_servo_set_us(uint32_t duration_us);
//void mb_servo_set_ns(uint32_t duration_ns);
void mb_servo_set(float throttle);
void mb_servo_arm (void);
#endif /* MB_SERVO_H */
sw/airborne/arm7/mb_tacho.c
-55
View File
@@ -1,55 +0,0 @@
#include "mb_tacho.h"
#include "LPC21xx.h"
#include "interrupt_hw.h"
volatile uint32_t mb_tacho_duration;
volatile uint8_t got_one_pulse;
volatile float mb_tacho_averaged;
volatile uint8_t mb_tacho_nb_pulse;
/* INPUT CAPTURE CAP0.0 on P0.22*/
#define MB_TACHO_PINSEL PINSEL1
#define MB_TACHO_PINSEL_VAL 0x02
#define MB_TACHO_PINSEL_BIT 12
void mb_tacho_init ( void ) {
/* select pin for capture */
MB_TACHO_PINSEL |= MB_TACHO_PINSEL_VAL << MB_TACHO_PINSEL_BIT;
/* enable capture 0.2 on falling edge + trigger interrupt */
T0CCR |= TCCR_CR0_F | TCCR_CR0_I;
}
uint32_t mb_tacho_get_duration( void ) {
int_disable();
uint32_t my_duration = 0;
if (got_one_pulse) {
my_duration = mb_tacho_duration;
}
got_one_pulse = FALSE;
int_enable();
return my_duration;
}
float mb_tacho_get_averaged( void ) {
int_disable();
float ret;
float tacho;
const float tach_to_rpm = 15000000.*60./36.;
if (mb_tacho_nb_pulse)
tacho = mb_tacho_averaged / (float)mb_tacho_nb_pulse ;
else
tacho = 0.;
if (tacho ==0)
ret = 0;
else
ret = tach_to_rpm/tacho;
mb_tacho_averaged = 0.;
mb_tacho_nb_pulse = 0;
int_enable();
return ret;
}
sw/airborne/arm7/mb_tacho.h
-26
View File
@@ -1,26 +0,0 @@
#ifndef MB_TACHO_H
#define MB_TACHO_H
#include "std.h"
extern void mb_tacho_init ( void );
extern uint32_t mb_tacho_get_duration( void );
extern float mb_tacho_get_averaged( void );
extern volatile uint32_t mb_tacho_duration;
extern volatile uint8_t got_one_pulse;
extern volatile float mb_tacho_averaged;
extern volatile uint8_t mb_tacho_nb_pulse;
#define MB_TACHO_ISR() { \
static uint32_t tmb_last; \
uint32_t t_now = T0CR0; \
uint32_t diff = t_now - tmb_last; \
mb_tacho_duration = diff; \
mb_tacho_averaged += diff; \
mb_tacho_nb_pulse++; \
tmb_last = t_now; \
got_one_pulse = TRUE; \
}
#endif /* MB_TACHO_H */
sw/airborne/arm7/mb_twi_controller.c
-28
View File
@@ -1,28 +0,0 @@
#include "mb_twi_controller.h"
#include <string.h>
#include "i2c.h"
uint16_t mb_twi_command;
uint8_t mb_twi_nb_overun;
uint8_t mb_twi_i2c_done;
void mb_twi_controller_init(void) {
mb_twi_nb_overun = 0;
mb_twi_i2c_done = TRUE;
}
void mb_twi_controller_set( float throttle ) {
if (mb_twi_i2c_done) {
mb_twi_command = throttle * MB_TWI_CONTROLLER_MAX_CMD;
i2c_buf[0] = (uint8_t)(mb_twi_command&0xFF);
i2c_buf[1] = (uint8_t)(mb_twi_command>>8);
i2c_transmit(MB_TWI_CONTROLLER_ADDR, 2, &mb_twi_i2c_done);
}
else
mb_twi_nb_overun++;
}
sw/airborne/arm7/mb_twi_controller.h
-22
View File
@@ -1,22 +0,0 @@
#ifndef MB_TWI_CONTROLLER_H
#define MB_TWI_CONTROLLER_H
#include "std.h"
extern void mb_twi_controller_init(void);
extern void mb_twi_controller_set( float throttle );
#define MB_TWI_CONTROLLER_MAX_CMD 65535
/*
Slave address
front = 0x52
back = 0x54
right = 0x56
left = 0x58
*/
#define MB_TWI_CONTROLLER_ADDR 0x52
#endif /* MB_TWI_CONTROLLER_H */
sw/airborne/booz_ahrs.c
-19
View File
@@ -1,19 +0,0 @@
#include "booz_ahrs.h"
uint8_t booz_ahrs_status;
FLOAT_T booz_ahrs_phi;
FLOAT_T booz_ahrs_theta;
FLOAT_T booz_ahrs_psi;
FLOAT_T booz_ahrs_p;
FLOAT_T booz_ahrs_q;
FLOAT_T booz_ahrs_r;
FLOAT_T booz_ahrs_bp;
FLOAT_T booz_ahrs_bq;
FLOAT_T booz_ahrs_br;
FLOAT_T booz_ahrs_measure_phi;
FLOAT_T booz_ahrs_measure_theta;
FLOAT_T booz_ahrs_measure_psi;
sw/airborne/booz_ahrs.h
-114
View File
@@ -1,114 +0,0 @@
/*
* $Id$
*
* Copyright (C) 2008 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.
*
*/
#ifndef BOOZ_AHRS_H
#define BOOZ_AHRS_H
#include "std.h"
#define BOOZ_AHRS_STATUS_UNINIT 0
#define BOOZ_AHRS_STATUS_RUNNING 1
#define BOOZ_AHRS_STATUS_CRASHED 2
extern uint8_t booz_ahrs_status;
extern FLOAT_T booz_ahrs_phi;
extern FLOAT_T booz_ahrs_theta;
extern FLOAT_T booz_ahrs_psi;
extern FLOAT_T booz_ahrs_p;
extern FLOAT_T booz_ahrs_q;
extern FLOAT_T booz_ahrs_r;
extern FLOAT_T booz_ahrs_bp;
extern FLOAT_T booz_ahrs_bq;
extern FLOAT_T booz_ahrs_br;
extern FLOAT_T booz_ahrs_measure_phi;
extern FLOAT_T booz_ahrs_measure_theta;
extern FLOAT_T booz_ahrs_measure_psi;
extern void booz_ahrs_init(void);
extern void booz_ahrs_start( const float* accel, const float* gyro, const float* mag);
/*extern void booz_ahrs_run(const float* accel, const float* gyro, const float* mag);*/
extern void booz_ahrs_predict( const float* gyro );
extern void booz_ahrs_update_accel( const float* accel);
extern void booz_ahrs_update_mag( const float* mag );
//#define __AHRS_IMPL_HEADER(_typ, _ext) _t##_ext
//#define _AHRS_IMPL_HEADER(_typ, _ext) __AHRS_IMPL_HEADER(_typ, _ext)
//#define AHRS_IMPL_HEADER(_typ) _AHRS_IMPL_HEADER(_typ, ".h")
//#error BOOZ_AHRS_TYPE
//#include \"AHRS_IMPL_HEADER(BOOZ_AHRS_TYPE)\"
#define BOOZ_AHRS_MULTITILT 0
#define BOOZ_AHRS_QUATERNION 1
#define BOOZ_AHRS_EULER 2
#define BOOZ_AHRS_COMP_FILTER 3
#if defined BOOZ_AHRS_TYPE && BOOZ_AHRS_TYPE == BOOZ_AHRS_MULTITILT
#include "ahrs_multitilt.h"
#elif defined BOOZ_AHRS_TYPE && BOOZ_AHRS_TYPE == BOOZ_AHRS_QUATERNION
#include "ahrs_quat_fast_ekf.h"
#elif defined BOOZ_AHRS_TYPE && BOOZ_AHRS_TYPE == BOOZ_AHRS_EULER
#include "ahrs_euler_fast_ekf.h"
#elif defined BOOZ_AHRS_TYPE && BOOZ_AHRS_TYPE == BOOZ_AHRS_COMP_FILTER
#include "ahrs_comp_filter.h"
#endif
#define WRAP(x,a) { while (x > a) x -= 2 * a; while (x <= -a) x += 2 * a;}
#define PhiOfAccel(_phi, _accel) { \
_phi = atan2(-_accel[AXIS_Y], -_accel[AXIS_Z]); \
}
#define ThetaOfAccel(_theta, _accel) { \
const float g2 = \
_accel[AXIS_X]*_accel[AXIS_X] + \
_accel[AXIS_Y]*_accel[AXIS_Y] + \
_accel[AXIS_Z]*_accel[AXIS_Z]; \
_theta = asin( accel[AXIS_X] / sqrt( g2 ) ); \
}
#define PsiOfMag(_psi, _mag) { \
const float cphi = cos( booz_ahrs_phi ); \
const float sphi = sin( booz_ahrs_phi ); \
const float ctheta = cos( booz_ahrs_theta ); \
const float stheta = sin( booz_ahrs_theta ); \
\
const float mn = \
ctheta* _mag[AXIS_X]+ \
sphi*stheta* _mag[AXIS_Y]+ \
cphi*stheta* _mag[AXIS_Z]; \
const float me = \
/* 0* mag[AXIS_X]+ */ \
cphi* mag[AXIS_Y]+ \
-sphi* mag[AXIS_Z]; \
_psi = -atan2( me, mn ); \
}
#endif /* BOOZ_AHRS_H */
sw/airborne/booz_autopilot.c
-83
View File
@@ -1,83 +0,0 @@
/*
* $Id$
*
* Copyright (C) 2008 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.
*
*/
#include "booz_autopilot.h"
#include "radio_control.h"
#include "commands.h"
#include "booz_control.h"
#include "booz_nav.h"
uint8_t booz_autopilot_mode;
void booz_autopilot_init(void) {
booz_autopilot_mode = BOOZ_AP_MODE_FAILSAFE;
}
void booz_autopilot_periodic_task(void) {
switch (booz_autopilot_mode) {
case BOOZ_AP_MODE_FAILSAFE:
case BOOZ_AP_MODE_KILL:
SetCommands(commands_failsafe);
break;
case BOOZ_AP_MODE_RATE:
booz_control_rate_run();
SetCommands(booz_control_commands);
break;
case BOOZ_AP_MODE_ATTITUDE:
case BOOZ_AP_MODE_HEADING_HOLD:
booz_control_attitude_run();
SetCommands(booz_control_commands);
break;
case BOOZ_AP_MODE_NAV:
booz_nav_run();
booz_control_attitude_run();
SetCommands(booz_control_commands);
break;
}
}
void booz_autopilot_on_rc_event(void) {
/* I think this should be hidden in rc code */
/* the ap gets a mode everytime - the rc filters it */
if (rc_values_contains_avg_channels) {
booz_autopilot_mode = BOOZ_AP_MODE_OF_PPRZ(rc_values[RADIO_MODE]);
rc_values_contains_avg_channels = FALSE;
}
switch (booz_autopilot_mode) {
case BOOZ_AP_MODE_RATE:
booz_control_rate_read_setpoints_from_rc();
break;
case BOOZ_AP_MODE_ATTITUDE:
case BOOZ_AP_MODE_HEADING_HOLD:
booz_control_attitude_read_setpoints_from_rc();
break;
case BOOZ_AP_MODE_NAV:
booz_nav_read_setpoints_from_rc();
break;
}
}
sw/airborne/booz_autopilot.h
-50
View File
@@ -1,50 +0,0 @@
/*
* $Id$
*
* Copyright (C) 2008 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.
*
*/
#ifndef BOOZ_AUTOPILOT_H
#define BOOZ_AUTOPILOT_H
#include "std.h"
#define BOOZ_AP_MODE_FAILSAFE 0
#define BOOZ_AP_MODE_KILL 1
#define BOOZ_AP_MODE_RATE 2
#define BOOZ_AP_MODE_ATTITUDE 3
#define BOOZ_AP_MODE_HEADING_HOLD 4
#define BOOZ_AP_MODE_NAV 5
extern uint8_t booz_autopilot_mode;
extern void booz_autopilot_init(void);
extern void booz_autopilot_periodic_task(void);
extern void booz_autopilot_on_rc_event(void);
#define TRESHOLD_RATE_PPRZ (MIN_PPRZ / 2)
#define TRESHOLD_ATTITUDE_PPRZ (MAX_PPRZ/2)
#define BOOZ_AP_MODE_OF_PPRZ(rc) \
((rc) < TRESHOLD_RATE_PPRZ ? BOOZ_AP_MODE_ATTITUDE : \
(rc) < TRESHOLD_ATTITUDE_PPRZ ? BOOZ_AP_MODE_HEADING_HOLD : \
BOOZ_AP_MODE_NAV )
#endif /* BOOZ_AUTOPILOT_H */
sw/airborne/booz_control.c
-205
View File
@@ -1,205 +0,0 @@
/*
* $Id$
*
* Copyright (C) 2008 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.
*
*/
#include "booz_control.h"
#include "booz_estimator.h"
#include "booz_autopilot.h"
#include "radio_control.h"
#define BOOZ_CONTROL_MIN_THROTTLE 0.05
float booz_control_p_sp;
float booz_control_q_sp;
float booz_control_r_sp;
float booz_control_power_sp;
float booz_control_rate_pq_pgain;
float booz_control_rate_pq_dgain;
float booz_control_rate_r_pgain;
float booz_control_rate_r_dgain;
float booz_control_rate_last_err_p;
float booz_control_rate_last_err_q;
float booz_control_rate_last_err_r;
pprz_t booz_control_commands[COMMANDS_NB];
#define BOOZ_CONTROL_RATE_PQ_PGAIN -700.
#define BOOZ_CONTROL_RATE_PQ_DGAIN 15.
#define BOOZ_CONTROL_RATE_R_PGAIN -600.
#define BOOZ_CONTROL_RATE_R_DGAIN 5.
/* setpoints for max stick throw in degres per second */
#define BOOZ_CONTROL_RATE_PQ_MAX_SP 120.
#define BOOZ_CONTROL_RATE_R_MAX_SP 100.
float booz_control_attitude_phi_sp;
float booz_control_attitude_theta_sp;
float booz_control_attitude_psi_sp;
float booz_control_attitude_phi_theta_pgain;
float booz_control_attitude_phi_theta_dgain;
float booz_control_attitude_psi_pgain;
float booz_control_attitude_psi_dgain;
#define BOOZ_CONTROL_ATTITUDE_PHI_THETA_PGAIN -1250.
#define BOOZ_CONTROL_ATTITUDE_PHI_THETA_DGAIN -700.
#define BOOZ_CONTROL_ATTITUDE_PSI_PGAIN -1050.
#define BOOZ_CONTROL_ATTITUDE_PSI_DGAIN -850.
/* setpoints for max stick throw in degres */
#define BOOZ_CONTROL_ATTITUDE_PHI_THETA_MAX_SP 30.
#define BOOZ_CONTROL_ATTITUDE_PSI_MAX_SP 45.
#define BOOZ_CONTROL_ATTITUDE_DT_UPDATE_SP (1./50.)
void booz_control_init(void) {
booz_control_p_sp = 0.;
booz_control_q_sp = 0.;
booz_control_r_sp = 0.;
booz_control_power_sp = 0.;
booz_control_rate_last_err_p = 0.;
booz_control_rate_last_err_q = 0.;
booz_control_rate_last_err_r = 0.;
booz_control_rate_pq_pgain = BOOZ_CONTROL_RATE_PQ_PGAIN;
booz_control_rate_pq_dgain = BOOZ_CONTROL_RATE_PQ_DGAIN;
booz_control_rate_r_pgain = BOOZ_CONTROL_RATE_R_PGAIN;
booz_control_rate_r_dgain = BOOZ_CONTROL_RATE_R_DGAIN;
booz_control_attitude_phi_sp = 0.;
booz_control_attitude_theta_sp =0.;
booz_control_attitude_psi_sp =0.;
booz_control_attitude_phi_theta_pgain = BOOZ_CONTROL_ATTITUDE_PHI_THETA_PGAIN;
booz_control_attitude_phi_theta_dgain = BOOZ_CONTROL_ATTITUDE_PHI_THETA_DGAIN;
booz_control_attitude_psi_pgain = BOOZ_CONTROL_ATTITUDE_PSI_PGAIN;
booz_control_attitude_psi_dgain = BOOZ_CONTROL_ATTITUDE_PSI_DGAIN;
}
void booz_control_rate_read_setpoints_from_rc(void) {
booz_control_p_sp = -rc_values[RADIO_ROLL] * RadOfDeg(BOOZ_CONTROL_RATE_PQ_MAX_SP)/MAX_PPRZ;
booz_control_q_sp = rc_values[RADIO_PITCH] * RadOfDeg(BOOZ_CONTROL_RATE_PQ_MAX_SP)/MAX_PPRZ;
booz_control_r_sp = -rc_values[RADIO_YAW] * RadOfDeg(BOOZ_CONTROL_RATE_R_MAX_SP)/MAX_PPRZ;
booz_control_power_sp = rc_values[RADIO_THROTTLE] / (float)MAX_PPRZ;
}
void booz_control_rate_run(void) {
if (booz_control_power_sp < BOOZ_CONTROL_MIN_THROTTLE) {
booz_control_commands[COMMAND_P] = 0;
booz_control_commands[COMMAND_Q] = 0;
booz_control_commands[COMMAND_R] = 0;
booz_control_commands[COMMAND_THROTTLE] = 0;
}
else {
const float rate_err_p = booz_estimator_uf_p - booz_control_p_sp;
const float rate_d_err_p = rate_err_p - booz_control_rate_last_err_p;
booz_control_rate_last_err_p = rate_err_p;
const float cmd_p = booz_control_rate_pq_pgain * ( rate_err_p + booz_control_rate_pq_dgain * rate_d_err_p );
const float rate_err_q = booz_estimator_uf_q - booz_control_q_sp;
const float rate_d_err_q = rate_err_q - booz_control_rate_last_err_q;
booz_control_rate_last_err_q = rate_err_q;
const float cmd_q = booz_control_rate_pq_pgain * ( rate_err_q + booz_control_rate_pq_dgain * rate_d_err_q );
const float rate_err_r = booz_estimator_uf_r - booz_control_r_sp;
const float rate_d_err_r = rate_err_r - booz_control_rate_last_err_r;
booz_control_rate_last_err_r = rate_err_r;
const float cmd_r = booz_control_rate_r_pgain * ( rate_err_r + booz_control_rate_r_dgain * rate_d_err_r );
booz_control_commands[COMMAND_P] = TRIM_PPRZ((int16_t)cmd_p);
booz_control_commands[COMMAND_Q] = TRIM_PPRZ((int16_t)cmd_q);
booz_control_commands[COMMAND_R] = TRIM_PPRZ((int16_t)cmd_r);
booz_control_commands[COMMAND_THROTTLE] = TRIM_PPRZ((int16_t) (booz_control_power_sp * MAX_PPRZ));
}
}
void booz_control_attitude_read_setpoints_from_rc(void) {
booz_control_attitude_phi_sp = -rc_values[RADIO_ROLL] *
RadOfDeg(BOOZ_CONTROL_ATTITUDE_PHI_THETA_MAX_SP)/MAX_PPRZ;
booz_control_attitude_theta_sp = rc_values[RADIO_PITCH] *
RadOfDeg(BOOZ_CONTROL_ATTITUDE_PHI_THETA_MAX_SP)/MAX_PPRZ;
if (booz_autopilot_mode == BOOZ_AP_MODE_ATTITUDE) {
booz_control_r_sp = -rc_values[RADIO_YAW] * RadOfDeg(BOOZ_CONTROL_RATE_R_MAX_SP)/MAX_PPRZ;
}
else { /* BOOZ_AP_MODE_HEADING_HOLD */
if (booz_control_power_sp < BOOZ_CONTROL_MIN_THROTTLE)
booz_control_attitude_psi_sp = booz_estimator_psi;
else {
booz_control_attitude_psi_sp += -rc_values[RADIO_YAW] *
RadOfDeg(BOOZ_CONTROL_ATTITUDE_PSI_MAX_SP)*BOOZ_CONTROL_ATTITUDE_DT_UPDATE_SP/MAX_PPRZ;
}
}
booz_control_power_sp = rc_values[RADIO_THROTTLE] / (float)MAX_PPRZ;
}
void booz_control_attitude_run(void) {
if (booz_control_power_sp < BOOZ_CONTROL_MIN_THROTTLE) {
booz_control_commands[COMMAND_P] = 0;
booz_control_commands[COMMAND_Q] = 0;
booz_control_commands[COMMAND_R] = 0;
booz_control_commands[COMMAND_THROTTLE] = 0;
}
else {
const float att_err_phi = booz_estimator_phi - booz_control_attitude_phi_sp;
const float cmd_p = booz_control_attitude_phi_theta_pgain * att_err_phi +
booz_control_attitude_phi_theta_dgain * booz_estimator_p ;
const float att_err_theta = booz_estimator_theta - booz_control_attitude_theta_sp;
const float cmd_q = booz_control_attitude_phi_theta_pgain * att_err_theta +
booz_control_attitude_phi_theta_dgain * booz_estimator_q;
float cmd_r;
if (booz_autopilot_mode == BOOZ_AP_MODE_ATTITUDE) {
const float rate_err_r = booz_estimator_r - booz_control_r_sp;
const float rate_d_err_r = rate_err_r - booz_control_rate_last_err_r;
booz_control_rate_last_err_r = rate_err_r;
cmd_r = booz_control_rate_r_pgain * ( rate_err_r + booz_control_rate_r_dgain * rate_d_err_r );
}
else {
float att_err_psi = booz_estimator_psi - booz_control_attitude_psi_sp;
NormRadAngle(att_err_psi);
cmd_r = booz_control_attitude_psi_pgain * att_err_psi +
booz_control_attitude_psi_dgain * booz_estimator_r;
}
booz_control_commands[COMMAND_P] = TRIM_PPRZ((int16_t)cmd_p);
booz_control_commands[COMMAND_Q] = TRIM_PPRZ((int16_t)cmd_q);
booz_control_commands[COMMAND_R] = TRIM_PPRZ((int16_t)cmd_r);
booz_control_commands[COMMAND_THROTTLE] = TRIM_PPRZ((int16_t) (booz_control_power_sp * MAX_PPRZ));
}
}
sw/airborne/booz_control.h
-69
View File
@@ -1,69 +0,0 @@
/*
* $Id$
*
* Copyright (C) 2008 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.
*
*/
#ifndef BOOZ_CONTROL_H
#define BOOZ_CONTROL_H
#include "std.h"
#include "paparazzi.h"
extern void booz_control_init(void);
extern void booz_control_rate_read_setpoints_from_rc(void);
extern void booz_control_rate_run(void);
extern void booz_control_attitude_read_setpoints_from_rc(void);
extern void booz_control_attitude_run(void);
extern float booz_control_p_sp;
extern float booz_control_q_sp;
extern float booz_control_r_sp;
extern float booz_control_power_sp;
extern float booz_control_rate_pq_pgain;
extern float booz_control_rate_pq_dgain;
extern float booz_control_rate_r_pgain;
extern float booz_control_rate_r_dgain;
extern float booz_control_attitude_phi_sp;
extern float booz_control_attitude_theta_sp;
extern float booz_control_attitude_psi_sp;
extern float booz_control_attitude_phi_theta_pgain;
extern float booz_control_attitude_phi_theta_dgain;
extern float booz_control_attitude_psi_pgain;
extern float booz_control_attitude_psi_dgain;
#define BoozControlAttitudeSetSetPoints(_phi_sp, _theta_sp, _psi_sp, _power_sp) { \
booz_control_attitude_phi_sp = _phi_sp; \
booz_control_attitude_theta_sp = _theta_sp; \
booz_control_attitude_psi_sp = _psi_sp; \
booz_control_power_sp = _power_sp; \
}
#include "airframe.h"
extern pprz_t booz_control_commands[COMMANDS_NB];
#endif /* BOOZ_CONTROL_H */
sw/airborne/booz_controller_main.c
-172
View File
@@ -1,172 +0,0 @@
/*
* $Id$
*
* Copyright (C) 2008 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.
*
*/
#include "booz_controller_main.h"
#include "std.h"
#include "init_hw.h"
#include "interrupt_hw.h"
#include "sys_time.h"
#include "led.h"
#include "booz_energy.h"
#include "commands.h"
#include "i2c.h"
#include "actuators.h"
#include "radio_control.h"
#include "adc.h"
#include "booz_link_mcu.h"
#include "booz_estimator.h"
#include "booz_control.h"
#include "booz_nav.h"
#include "booz_autopilot.h"
#include "uart.h"
#include "messages.h"
#include "downlink.h"
#include "booz_controller_telemetry.h"
#include "datalink.h"
int16_t trim_p = 0;
int16_t trim_q = 0;
int16_t trim_r = 0;
uint8_t vbat = 0;
#ifndef SITL
int main( void ) {
booz_controller_main_init();
while(1) {
if (sys_time_periodic())
booz_controller_main_periodic_task();
booz_controller_main_event_task();
}
return 0;
}
#endif
STATIC_INLINE void booz_controller_main_init( void ) {
hw_init();
led_init();
sys_time_init();
adc_init();
booz_energy_init();
i2c_init();
actuators_init();
SetCommands(commands_failsafe);
ppm_init();
radio_control_init();
booz_link_mcu_init();
booz_estimator_init();
booz_control_init();
booz_nav_init();
booz_autopilot_init();
//FIXME
#ifndef SITL
uart1_init_tx();
#endif
int_enable();
DOWNLINK_SEND_BOOT(&cpu_time_sec);
}
#define PeriodicPrescaleBy5( _code_0, _code_1, _code_2, _code_3, _code_4) { \
static uint8_t _50hz = 0; \
_50hz++; \
if (_50hz >= 5) _50hz = 0; \
switch (_50hz) { \
case 0: \
_code_0; \
break; \
case 1: \
_code_1; \
break; \
case 2: \
_code_2; \
break; \
case 3: \
_code_3; \
break; \
case 4: \
_code_4; \
break; \
} \
}
STATIC_INLINE void booz_controller_main_periodic_task( void ) {
/* check for timeout */
booz_link_mcu_periodic();
/* run control loops */
booz_autopilot_periodic_task();
// commands[COMMAND_P] = 0;
// commands[COMMAND_Q] = 0;
// commands[COMMAND_R] = 0;
// commands[COMMAND_THROTTLE] = MAX_PPRZ/3;
SetActuatorsFromCommands(commands);
PeriodicPrescaleBy5( \
{ \
radio_control_periodic_task(); \
if (rc_status != RC_OK) \
booz_autopilot_mode = BOOZ_AP_MODE_FAILSAFE; \
}, \
{ \
booz_controller_telemetry_periodic_task(); \
}, \
{ \
booz_energy_periodic(); \
}, \
{}, \
{} \
); \
}
STATIC_INLINE void booz_controller_main_event_task( void ) {
// FIXME
#ifndef SITL
DlEventCheckAndHandle();
#endif
BoozLinkMcuEventCheckAndHandle(booz_estimator_read_inter_mcu_state);
RadioControlEventCheckAndHandle(booz_autopilot_on_rc_event);
}
sw/airborne/booz_controller_main.h
-14
View File
@@ -1,14 +0,0 @@
#ifndef BOOZ_CONTROLLER_MAIN_H
#define BOOZ_CONTROLLER_MAIN_H
#ifdef SITL
#define STATIC_INLINE
#else
#define STATIC_INLINE static inline
#endif
STATIC_INLINE void booz_controller_main_init( void );
STATIC_INLINE void booz_controller_main_periodic_task( void );
STATIC_INLINE void booz_controller_main_event_task( void );
#endif /* BOOZ_CONTROLLER_MAIN_H */
sw/airborne/booz_controller_telemetry.c
-5
View File
@@ -1,5 +0,0 @@
#include "booz_controller_telemetry.h"
uint8_t telemetry_mode_Controller;
sw/airborne/booz_controller_telemetry.h
-106
View File
@@ -1,106 +0,0 @@
#ifndef BOOZ_CONTROLLER_TELEMETRY_H
#define BOOZ_CONTROLLER_TELEMETRY_H
#include "std.h"
#include "messages.h"
#include "periodic.h"
#include "uart.h"
#include "booz_energy.h"
#include "radio_control.h"
#include "actuators.h"
#include "booz_link_mcu.h"
#include "booz_estimator.h"
#include "booz_autopilot.h"
#include "booz_control.h"
#include "booz_nav.h"
#include "actuators_buss_twi_blmc_hw.h"
#include "settings.h"
#include "downlink.h"
#define PERIODIC_SEND_ALIVE() DOWNLINK_SEND_ALIVE()
#define PERIODIC_SEND_BOOZ_STATUS() \
DOWNLINK_SEND_BOOZ_STATUS(&booz_link_mcu_nb_err, \
&booz_link_mcu_status, \
&rc_status, \
&booz_autopilot_mode, \
&booz_energy_bat, \
&cpu_time_sec, \
&buss_twi_blmc_nb_err)
#define PERIODIC_SEND_RC() DOWNLINK_SEND_RC(PPM_NB_PULSES, rc_values)
#define PERIODIC_SEND_BOOZ_FD() \
DOWNLINK_SEND_BOOZ_FD(&booz_estimator_p, \
&booz_estimator_q, \
&booz_estimator_r, \
&booz_estimator_phi, \
&booz_estimator_theta, \
&booz_estimator_psi);
#define PERIODIC_SEND_ACTUATORS() \
DOWNLINK_SEND_ACTUATORS(SERVOS_NB, actuators);
#define PERIODIC_SEND_BOOZ_CONTROL() { \
switch (booz_autopilot_mode) { \
case BOOZ_AP_MODE_RATE: \
DOWNLINK_SEND_BOOZ_RATE_LOOP(&booz_estimator_uf_p, &booz_control_p_sp, \
&booz_estimator_uf_q, &booz_control_q_sp, \
&booz_estimator_uf_r, &booz_control_r_sp, \
&booz_control_power_sp); \
break; \
case BOOZ_AP_MODE_ATTITUDE: \
case BOOZ_AP_MODE_HEADING_HOLD: \
case BOOZ_AP_MODE_NAV: \
DOWNLINK_SEND_BOOZ_ATT_LOOP(&booz_estimator_phi, &booz_control_attitude_phi_sp, \
&booz_estimator_theta, &booz_control_attitude_theta_sp, \
&booz_estimator_psi, &booz_control_attitude_psi_sp, \
&booz_control_power_sp); \
break; \
} \
}
#define PERIODIC_SEND_BOOZ_UF_RATES() \
DOWNLINK_SEND_BOOZ_UF_RATES(&booz_estimator_uf_p, \
&booz_estimator_uf_q, \
&booz_estimator_uf_r);
#define PERIODIC_SEND_BOOZ_VERT_LOOP() { \
DOWNLINK_SEND_BOOZ_VERT_LOOP(&booz_nav_hover_z_sp, \
&booz_estimator_w, \
&booz_estimator_z, \
&booz_nav_hover_power_command); \
}
#define PERIODIC_SEND_BOOZ_HOV_LOOP() { \
DOWNLINK_SEND_BOOZ_HOV_LOOP(&booz_nav_hover_x_sp, \
&booz_nav_hover_y_sp, \
&booz_estimator_u, \
&booz_estimator_x, \
&booz_estimator_v, \
&booz_estimator_y, \
&booz_nav_hover_phi_command, \
&booz_nav_hover_theta_command); \
}
#define PERIODIC_SEND_BOOZ_CMDS() DOWNLINK_SEND_BOOZ_CMDS(&buss_twi_blmc_motor_power[SERVO_MOTOR_FRONT],\
&buss_twi_blmc_motor_power[SERVO_MOTOR_BACK], \
&buss_twi_blmc_motor_power[SERVO_MOTOR_LEFT], \
&buss_twi_blmc_motor_power[SERVO_MOTOR_RIGHT]);
#define PERIODIC_SEND_DL_VALUE() PeriodicSendDlValue()
extern uint8_t telemetry_mode_Controller;
static inline void booz_controller_telemetry_periodic_task(void) {
PeriodicSendController()
}
#endif /* BOOZ_CONTROLLER_TELEMETRY_H */
sw/airborne/booz_datalink.c
-24
View File
@@ -1,24 +0,0 @@
#define DATALINK_C
#include "datalink.h"
#include "settings.h"
#include "downlink.h"
#include "messages.h"
#include "dl_protocol.h"
#include "uart.h"
#define IdOfMsg(x) (x[1])
void dl_parse_msg(void) {
uint8_t msg_id = IdOfMsg(dl_buffer);
switch (msg_id) {
case DL_SETTING : {
uint8_t i = DL_SETTING_index(dl_buffer);
float var = DL_SETTING_value(dl_buffer);
DlSetting(i, var);
DOWNLINK_SEND_DL_VALUE(&i, &var);
break;
}
}
}
sw/airborne/booz_debug.c
-4
View File
@@ -1,4 +0,0 @@
#include "booz_debug.h"
uint8_t booz_debug_mod;
uint8_t booz_debug_err;
sw/airborne/booz_debug.h
-31
View File
@@ -1,31 +0,0 @@
#ifndef BOOZ_DEBUG_H
#define BOOZ_DEBUG_H
#ifdef BOOZ_DEBUG
#include "uart.h"
#include "messages.h"
#include "downlink.h"
extern uint8_t booz_debug_mod;
extern uint8_t booz_debug_err;
#define DEBUG_IMU 0
#define DEBUG_MAX_1117 1
#define DEBUG_SCP1000 2
#define DEBUG_LINK_MCU_IMU 3
#define ASSERT(cond, mod, err) { \
if (!(cond)) { \
booz_debug_mod = mod; \
booz_debug_err = err; \
DOWNLINK_SEND_BOOZ_ERROR(&booz_debug_mod, &booz_debug_err); \
} \
}
#else
#define ASSERT(cond, mod, err) {}
#endif
#endif /* BOOZ_DEBUG_H */
sw/airborne/booz_energy.c
-16
View File
@@ -1,16 +0,0 @@
#include "booz_energy.h"
#include CONFIG
#include "adc.h"
uint8_t booz_energy_bat;
static struct adc_buf bat_adc_buf;
void booz_energy_init( void ) {
adc_buf_channel(ADC_BAT, &bat_adc_buf, DEFAULT_AV_NB_SAMPLE);
}
void booz_energy_periodic( void ) {
booz_energy_bat = bat_adc_buf.sum * 0.004295;
}
sw/airborne/booz_energy.h
-14
View File
@@ -1,14 +0,0 @@
#ifndef BOOZ_ENERGY_H
#define BOOZ_ENERGY_H
#include "std.h"
extern uint8_t booz_energy_bat;
extern void booz_energy_init( void );
extern void booz_energy_periodic( void );
#endif /* BOOZ_ENERGY_H */
sw/airborne/booz_estimator.c
-128
View File
@@ -1,128 +0,0 @@
#include "booz_estimator.h"
#include "booz_inter_mcu.h"
float booz_estimator_uf_p;
float booz_estimator_uf_q;
float booz_estimator_uf_r;
float booz_estimator_p;
float booz_estimator_q;
float booz_estimator_r;
float booz_estimator_phi;
float booz_estimator_theta;
float booz_estimator_psi;
float booz_estimator_dcm[AXIS_NB][AXIS_NB];
float booz_estimator_x;
float booz_estimator_y;
float booz_estimator_z;
float booz_estimator_vx;
float booz_estimator_vy;
float booz_estimator_vz;
float booz_estimator_u;
float booz_estimator_v;
float booz_estimator_w;
void booz_estimator_init( void ) {
booz_estimator_uf_p = 0.;
booz_estimator_uf_q = 0.;
booz_estimator_uf_r = 0.;
booz_estimator_p = 0.;
booz_estimator_q = 0.;
booz_estimator_r = 0.;
booz_estimator_phi = 0.;
booz_estimator_theta = 0.;
booz_estimator_psi = 0.;
booz_estimator_x = 0.;
booz_estimator_y = 0.;
booz_estimator_z = 0.;
booz_estimator_vx = 0.;
booz_estimator_vy = 0.;
booz_estimator_vz = 0.;
booz_estimator_u = 0.;
booz_estimator_v = 0.;
booz_estimator_w = 0.;
}
#define BoozEstimatorComputeDCM() { \
\
float sinPHI = sin( booz_estimator_phi ); \
float cosPHI = cos( booz_estimator_phi ); \
float sinTHETA = sin( booz_estimator_theta); \
float cosTHETA = cos( booz_estimator_theta); \
float sinPSI = sin( booz_estimator_psi); \
float cosPSI = cos( booz_estimator_psi); \
\
booz_estimator_dcm[0][0] = cosTHETA * cosPSI; \
booz_estimator_dcm[0][1] = cosTHETA * sinPSI; \
booz_estimator_dcm[0][2] = -sinTHETA; \
booz_estimator_dcm[1][0] = sinPHI * sinTHETA * cosPSI - cosPHI * sinPSI; \
booz_estimator_dcm[1][1] = sinPHI * sinTHETA * sinPSI + cosPHI * cosPSI; \
booz_estimator_dcm[1][2] = sinPHI * cosTHETA; \
booz_estimator_dcm[2][0] = cosPHI * sinTHETA * cosPSI + sinPHI * sinPSI; \
booz_estimator_dcm[2][1] = cosPHI * sinTHETA * sinPSI - sinPHI * cosPSI; \
booz_estimator_dcm[2][2] = cosPHI * cosTHETA; \
\
}
#define BoozEstimatorUpdateBodySpeed() { \
\
booz_estimator_u = \
booz_estimator_dcm[AXIS_U][AXIS_X] * booz_estimator_vx + \
booz_estimator_dcm[AXIS_U][AXIS_Y] * booz_estimator_vy + \
booz_estimator_dcm[AXIS_U][AXIS_Z] * booz_estimator_vz ; \
\
booz_estimator_v = \
booz_estimator_dcm[AXIS_V][AXIS_X] * booz_estimator_vx + \
booz_estimator_dcm[AXIS_V][AXIS_Y] * booz_estimator_vy + \
booz_estimator_dcm[AXIS_V][AXIS_Z] * booz_estimator_vz ; \
\
booz_estimator_w = \
booz_estimator_dcm[AXIS_W][AXIS_X] * booz_estimator_vx + \
booz_estimator_dcm[AXIS_W][AXIS_Y] * booz_estimator_vy + \
booz_estimator_dcm[AXIS_W][AXIS_Z] * booz_estimator_vz ; \
\
}
void booz_estimator_read_inter_mcu_state( void ) {
booz_estimator_uf_p = inter_mcu_state.r_rates[AXIS_P] * M_PI/RATE_PI_S;
booz_estimator_uf_q = inter_mcu_state.r_rates[AXIS_Q] * M_PI/RATE_PI_S;
booz_estimator_uf_r = inter_mcu_state.r_rates[AXIS_R] * M_PI/RATE_PI_S;
booz_estimator_p = inter_mcu_state.f_rates[AXIS_P] * M_PI/RATE_PI_S;
booz_estimator_q = inter_mcu_state.f_rates[AXIS_Q] * M_PI/RATE_PI_S;
booz_estimator_r = inter_mcu_state.f_rates[AXIS_R] * M_PI/RATE_PI_S;
booz_estimator_phi = inter_mcu_state.f_eulers[AXIS_X] * M_PI/ANGLE_PI;
booz_estimator_theta = inter_mcu_state.f_eulers[AXIS_Y] * M_PI/ANGLE_PI;
booz_estimator_psi = inter_mcu_state.f_eulers[AXIS_Z] * M_PI/ANGLE_PI;
booz_estimator_x = inter_mcu_state.pos[AXIS_X];
booz_estimator_y = inter_mcu_state.pos[AXIS_Y];
booz_estimator_z = inter_mcu_state.pos[AXIS_Z];
booz_estimator_vx = inter_mcu_state.speed[AXIS_X];
booz_estimator_vy = inter_mcu_state.speed[AXIS_Y];
booz_estimator_vz = inter_mcu_state.speed[AXIS_Z];
BoozEstimatorComputeDCM();
BoozEstimatorUpdateBodySpeed();
}
sw/airborne/booz_estimator.h
-41
View File
@@ -1,41 +0,0 @@
#ifndef BOOZ_ESTIMATOR_H
#define BOOZ_ESTIMATOR_H
#include "6dof.h"
/* unfiltered rates available when filter crashed or uninitialed */
extern float booz_estimator_uf_p;
extern float booz_estimator_uf_q;
extern float booz_estimator_uf_r;
extern float booz_estimator_p;
extern float booz_estimator_q;
extern float booz_estimator_r;
extern float booz_estimator_phi;
extern float booz_estimator_theta;
extern float booz_estimator_psi;
extern float booz_estimator_dcm[AXIS_NB][AXIS_NB];
/* position in earth frame : not yet available - sim only */
extern float booz_estimator_x;
extern float booz_estimator_y;
extern float booz_estimator_z;
/* speed in earth frame : not yet available - sim only */
extern float booz_estimator_vx;
extern float booz_estimator_vy;
extern float booz_estimator_vz;
/* speed in body frame : not yet available - sim only */
extern float booz_estimator_u;
extern float booz_estimator_v;
extern float booz_estimator_w;
extern void booz_estimator_init( void );
extern void booz_estimator_read_inter_mcu_state( void );
#endif /* BOOZ_ESTIMATOR_H */
sw/airborne/booz_filter_main.c
-159
View File
@@ -1,159 +0,0 @@
/*
* $Id$
*
* Copyright (C) 2008 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.
*
*/
#include "booz_filter_main.h"
#include "std.h"
#include "init_hw.h"
#include "interrupt_hw.h"
#include "sys_time.h"
#include "booz_imu.h"
#include "booz_still_detection.h"
#include "booz_ahrs.h"
#include "gps.h"
#include "booz_ins.h"
#include "uart.h"
#include "messages.h"
#include "downlink.h"
#include "booz_filter_telemetry.h"
#include "booz_link_mcu.h"
static inline void on_imu_accel( void );
static inline void on_imu_gyro( void );
static inline void on_imu_mag( void );
static inline void on_imu_baro( void );
static inline void on_gps( void );
#ifndef SITL
int main( void ) {
{ uint32_t foo = 0; while (foo < 1e5) foo++;}
booz_filter_main_init();
while (1) {
if (sys_time_periodic())
booz_filter_main_periodic_task();
booz_filter_main_event_task();
}
return 0;
}
#endif
STATIC_INLINE void booz_filter_main_init( void ) {
hw_init();
sys_time_init();
//FIXME
#ifndef SITL
uart0_init_tx();
uart1_init_tx();
#endif
booz_imu_init();
booz_still_detection_init();
booz_ahrs_init();
booz_ins_init();
booz_link_mcu_init();
int_enable();
}
// static uint32_t t0, t1, diff;
STATIC_INLINE void booz_filter_main_event_task( void ) {
/* check if measurements are available */
BoozImuEvent(on_imu_accel, on_imu_gyro, on_imu_mag, on_imu_baro);
GpsEventCheckAndHandle(on_gps, FALSE);
}
STATIC_INLINE void booz_filter_main_periodic_task( void ) {
booz_imu_periodic();
RunOnceEvery(4, {booz_filter_telemetry_periodic_task();})
}
static inline void on_imu_accel( void ) {
if (booz_ahrs_status == BOOZ_AHRS_STATUS_RUNNING) {
RunOnceEvery(4, {booz_ahrs_update_accel(imu_accel);});
}
if (booz_ins_status == BOOZ_INS_STATUS_RUNNING) {
booz_ins_predict(imu_accel);
}
}
static inline void on_imu_gyro( void ) {
switch (booz_ahrs_status) {
case BOOZ_AHRS_STATUS_UNINIT :
booz_still_detection_run();
if (booz_still_detection_status == BSD_STATUS_LOCKED) {
booz_ahrs_start(booz_still_detection_accel,
booz_still_detection_gyro,
booz_still_detection_mag);
booz_ins_start(booz_still_detection_accel,
booz_still_detection_pressure);
}
break;
case BOOZ_AHRS_STATUS_RUNNING :
// t0 = T0TC;
booz_ahrs_predict(imu_gyro);
// t1 = T0TC;
// diff = t1 - t0;
// DOWNLINK_SEND_TIME(&diff);
break;
}
// DOWNLINK_SEND_IMU_GYRO(&imu_gyro[AXIS_P], &imu_gyro[AXIS_Q], &imu_gyro[AXIS_R]);
booz_link_mcu_send();
}
static inline void on_imu_mag( void ) {
// DOWNLINK_SEND_IMU_MAG(&imu_mag[AXIS_X], &imu_mag[AXIS_Y], &imu_mag[AXIS_Z]);
if (booz_ahrs_status == BOOZ_AHRS_STATUS_RUNNING) {
booz_ahrs_update_mag(imu_mag);
}
}
static inline void on_imu_baro( void ) {
// DOWNLINK_SEND_IMU_PRESSURE(&imu_pressure);
booz_ins_update_pressure(imu_pressure);
}
static inline void on_gps( void ) {
}
sw/airborne/booz_filter_main.h
-14
View File
@@ -1,14 +0,0 @@
#ifndef BOOZ_FILTER_MAIN_H
#define BOOZ_FILTER_MAIN_H
#ifdef SITL
#define STATIC_INLINE
#else
#define STATIC_INLINE static inline
#endif
STATIC_INLINE void booz_filter_main_init( void );
STATIC_INLINE void booz_filter_main_periodic_task( void );
STATIC_INLINE void booz_filter_main_event_task( void );
#endif /* BOOZ_FILTER_MAIN_H */
sw/airborne/booz_filter_telemetry.c
-3
View File
@@ -1,3 +0,0 @@
#include "booz_filter_telemetry.h"
uint8_t telemetry_mode_Filter;
sw/airborne/booz_filter_telemetry.h
-180
View File
@@ -1,180 +0,0 @@
/*
* $Id$
*
* Copyright (C) 2008 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.
*
*/
#ifndef BOOZ_FILTER_TELEMETRY_H
#define BOOZ_FILTER_TELEMETRY_H
#include "std.h"
#include "messages.h"
#include "periodic.h"
#include "uart.h"
#include "booz_imu.h"
#include "booz_ahrs.h"
#include "booz_ins.h"
#include "settings.h"
#include "downlink.h"
/*
*
* IMU
*
*/
#define PERIODIC_SEND_IMU_GYRO() \
DOWNLINK_SEND_IMU_GYRO(&imu_gyro[AXIS_X], \
&imu_gyro[AXIS_Y], \
&imu_gyro[AXIS_Z]);
#define PERIODIC_SEND_IMU_GYRO_RAW() \
DOWNLINK_SEND_IMU_GYRO_RAW(&imu_gyro_raw[AXIS_X], \
&imu_gyro_raw[AXIS_Y], \
&imu_gyro_raw[AXIS_Z]);
#if 0
#define PERIODIC_SEND_IMU_GYRO_LP() \
DOWNLINK_SEND_IMU_GYRO_LP(&imu_gyro_lp[AXIS_X], \
&imu_gyro_lp[AXIS_Y], \
&imu_gyro_lp[AXIS_Z]);
#endif
#define PERIODIC_SEND_IMU_ACCEL() \
DOWNLINK_SEND_IMU_ACCEL(&imu_accel[AXIS_X], \
&imu_accel[AXIS_Y], \
&imu_accel[AXIS_Z]);
#define PERIODIC_SEND_IMU_ACCEL_RAW() \
DOWNLINK_SEND_IMU_ACCEL_RAW(&imu_accel_raw[AXIS_X], \
&imu_accel_raw[AXIS_Y], \
&imu_accel_raw[AXIS_Z]);
#define PERIODIC_SEND_IMU_GYRO_RAW_AVG() \
DOWNLINK_SEND_IMU_GYRO_RAW_AVG(&imu_vs_gyro_raw_avg[AXIS_X], \
&imu_vs_gyro_raw_avg[AXIS_Y], \
&imu_vs_gyro_raw_avg[AXIS_Z], \
&imu_vs_gyro_raw_var[AXIS_X], \
&imu_vs_gyro_raw_var[AXIS_Y], \
&imu_vs_gyro_raw_var[AXIS_Z]);
#define PERIODIC_SEND_IMU_ACCEL_RAW_AVG() \
DOWNLINK_SEND_IMU_ACCEL_RAW_AVG(&imu_vs_accel_raw_avg[AXIS_X], \
&imu_vs_accel_raw_avg[AXIS_Y], \
&imu_vs_accel_raw_avg[AXIS_Z], \
&imu_vs_accel_raw_var[AXIS_X], \
&imu_vs_accel_raw_var[AXIS_Y], \
&imu_vs_accel_raw_var[AXIS_Z]);
#define PERIODIC_SEND_IMU_MAG() \
DOWNLINK_SEND_IMU_MAG(&imu_mag[AXIS_X], \
&imu_mag[AXIS_Y], \
&imu_mag[AXIS_Z]);
#define PERIODIC_SEND_IMU_MAG_RAW() \
DOWNLINK_SEND_IMU_MAG_RAW(&imu_mag_raw[AXIS_X], \
&imu_mag_raw[AXIS_Y], \
&imu_mag_raw[AXIS_Z]);
#define PERIODIC_SEND_IMU_PRESSURE() \
DOWNLINK_SEND_IMU_PRESSURE(&imu_pressure);
/*
*
* AHRS
*
*/
#if defined BOOZ_AHRS_TYPE && BOOZ_AHRS_TYPE == BOOZ_AHRS_MULTITILT
#define PERIODIC_SEND_AHRS_COV() \
DOWNLINK_SEND_AHRS_EULER_COV(&mtt_P_phi[0][0], &mtt_P_phi[0][1], \
&mtt_P_phi[1][1], \
&mtt_P_theta[0][0], &mtt_P_theta[0][1], \
&mtt_P_theta[1][1], \
&mtt_P_psi[0][0], &mtt_P_psi[0][1], \
&mtt_P_psi[1][1]); \
#define PERIODIC_SEND_AHRS_STATE() \
DOWNLINK_SEND_AHRS_EULER_STATE(&booz_ahrs_phi, &booz_ahrs_theta, &booz_ahrs_psi, \
&booz_ahrs_bp, &booz_ahrs_bq, &booz_ahrs_br);
#elif defined BOOZ_AHRS_TYPE && BOOZ_AHRS_TYPE == BOOZ_AHRS_QUATERNION
#define PERIODIC_SEND_AHRS_COV() { \
DOWNLINK_SEND_AHRS_QUAT_COV(&afe_P[0][0], &afe_P[1][1], \
&afe_P[2][2], &afe_P[3][3], \
&afe_P[4][4], &afe_P[5][5], \
&afe_P[6][6]); \
}
#define PERIODIC_SEND_AHRS_STATE() \
DOWNLINK_SEND_AHRS_QUAT_STATE(&afe_q0, &afe_q1, &afe_q2, &afe_q3, \
&booz_ahrs_bp, &booz_ahrs_bq, &booz_ahrs_br);
#elif defined BOOZ_AHRS_TYPE && BOOZ_AHRS_TYPE == BOOZ_AHRS_COMP_FILTER
#define PERIODIC_SEND_AHRS_STATE() \
DOWNLINK_SEND_AHRS_EULER_STATE(&booz_ahrs_phi, &booz_ahrs_theta, &booz_ahrs_psi, \
&booz_ahrs_bp, &booz_ahrs_bq, &booz_ahrs_br);
#define PERIODIC_SEND_AHRS_COV() {}
#endif /* BOOZ_AHRS_TYPE */
#define PERIODIC_SEND_AHRS_MEASURE() \
DOWNLINK_SEND_AHRS_MEASURE(&booz_ahrs_measure_phi, \
&booz_ahrs_measure_theta, \
&booz_ahrs_measure_psi);
#define PERIODIC_SEND_INS_STATE() \
DOWNLINK_SEND_BOOZ_INS_STATE(&booz_ins_z, &booz_ins_z_meas, &booz_ins_zdot, & booz_ins_baz);
#if 0
#define PERIODIC_SEND_BOOZ_DEBUG() { \
float m_phi = atan2(imu_accel[AXIS_Y], imu_accel[AXIS_Z]); \
const float g2 = \
imu_accel[AXIS_X]*imu_accel[AXIS_X] + \
imu_accel[AXIS_Y]*imu_accel[AXIS_Y] + \
imu_accel[AXIS_Z]*imu_accel[AXIS_Z]; \
float m_theta = -asin( imu_accel[AXIS_X] / sqrt( g2 ) ); \
DOWNLINK_SEND_BOOZ_DEBUG(&m_phi, &m_theta); \
}
#endif
#define PERIODIC_SEND_DL_VALUE() PeriodicSendDlValue()
extern uint8_t telemetry_mode_Filter;
static inline void booz_filter_telemetry_periodic_task(void) {
PeriodicSendFilter()
}
#endif /* BOOZ_FILTER_TELEMETRY_H */
sw/airborne/booz_imu.c
-78
View File
@@ -1,78 +0,0 @@
/*
* $Id$
*
* Copyright (C) 2008 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.
*
*/
#include "booz_imu.h"
#include "booz_debug.h"
/* calibrated sensors */
float imu_accel[AXIS_NB]; /* accelerometers in m/s2 */
float imu_gyro[AXIS_NB]; /* gyros in rad/s */
float imu_mag[AXIS_NB]; /* magnetometer in arbitrary unit */
float imu_pressure; /* static pressure in pascals */
/* raw sensors */
uint16_t imu_accel_raw[AXIS_NB];
uint16_t imu_gyro_raw[AXIS_NB];
int16_t imu_mag_raw[AXIS_NB];
uint32_t imu_pressure_raw;
/* internal ADCs */
struct adc_buf buf_ax;
struct adc_buf buf_ay;
struct adc_buf buf_az;
uint8_t booz_imu_status;
#define IMU_ERR_OVERUN 0
void booz_imu_init(void) {
uint8_t i;
for (i=0; i<AXIS_NB; i++) {
imu_accel_raw[i] = 0;
imu_gyro_raw[i] = 0;
imu_mag_raw[i] = 0;
imu_accel[i] = 0.;
imu_gyro[i] = 0.;
imu_mag[i] = 0.;
}
imu_pressure_raw = 0;
imu_pressure = 0.;
booz_imu_status = BOOZ_IMU_STA_IDLE;
adc_buf_channel(IMU_ACCEL_X_CHAN, &buf_ax, DEFAULT_AV_NB_SAMPLE);
adc_buf_channel(IMU_ACCEL_Y_CHAN, &buf_ay, DEFAULT_AV_NB_SAMPLE);
adc_buf_channel(IMU_ACCEL_Z_CHAN, &buf_az, DEFAULT_AV_NB_SAMPLE);
adc_init();
max1167_init();
micromag_init();
scp1000_init();
booz_imu_hw_init();
}
void booz_imu_periodic(void) {
ASSERT((booz_imu_status == BOOZ_IMU_STA_IDLE), DEBUG_IMU, IMU_ERR_OVERUN);
booz_imu_status = BOOZ_IMU_STA_MEASURING_GYRO;
max1167_read();
}
sw/airborne/booz_imu.h
-116
View File
@@ -1,116 +0,0 @@
/*
* $Id$
*
* Copyright (C) 2008 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.
*
*/
#ifndef BOOZ_IMU_H
#define BOOZ_IMU_H
#include "std.h"
#include "6dof.h"
#include "adc.h"
#include "max1167.h"
#include "micromag.h"
#include "scp1000.h"
#include "airframe.h"
#define BOOZ_IMU_STA_IDLE 0
#define BOOZ_IMU_STA_MEASURING_GYRO 1
#define BOOZ_IMU_STA_MEASURING_MAG 2
#define BOOZ_IMU_STA_MEASURING_BARO 3
extern uint8_t booz_imu_status;
/* calibrated sensors */
extern float imu_accel[AXIS_NB]; /* accelerometers in m/s2 */
extern float imu_gyro[AXIS_NB]; /* gyros in rad/s */
extern float imu_mag[AXIS_NB]; /* magnetometer in arbitrary unit */
extern float imu_pressure; /* static pressure in pascals */
/* raw sensors */
extern uint16_t imu_accel_raw[AXIS_NB];
extern uint16_t imu_gyro_raw[AXIS_NB];
extern int16_t imu_mag_raw[AXIS_NB];
extern uint32_t imu_pressure_raw;
/* internal ADCs */
extern struct adc_buf buf_ax;
extern struct adc_buf buf_ay;
extern struct adc_buf buf_az;
extern void booz_imu_init(void);
extern void booz_imu_periodic(void);
#define BoozImuScaleSensor3(_s_cal, _s_name, _s_raw) { \
_s_cal[AXIS_X] = _s_name##_X_GAIN * \
(float)((int32_t)_s_raw[AXIS_X] - _s_name##_X_NEUTRAL); \
_s_cal[AXIS_Y] = _s_name##_Y_GAIN * \
(float)((int32_t)_s_raw[AXIS_Y] - _s_name##_Y_NEUTRAL); \
_s_cal[AXIS_Z] = _s_name##_Z_GAIN * \
(float)((int32_t)_s_raw[AXIS_Z] - _s_name##_Z_NEUTRAL); \
}
#define BoozImuScaleSensor1(_s_cal, _s_name, _s_raw) { \
_s_cal = _s_name##_GAIN * \
(float)((int32_t)_s_raw - _s_name##_NEUTRAL); \
}
#define BoozImuEvent(accel_handler, gyro_handler, mag_handler, baro_handler) { \
if (max1167_status == STA_MAX1167_DATA_AVAILABLE) { \
max1167_status = STA_MAX1167_IDLE; \
imu_gyro_raw[AXIS_P] = max1167_values[IMU_GYRO_X_CHAN]; \
imu_gyro_raw[AXIS_Q] = max1167_values[IMU_GYRO_Y_CHAN]; \
imu_gyro_raw[AXIS_R] = max1167_values[IMU_GYRO_Z_CHAN]; \
BoozImuScaleSensor3(imu_gyro, IMU_GYRO, imu_gyro_raw); \
\
imu_accel_raw[AXIS_X] = buf_ax.sum; \
imu_accel_raw[AXIS_Y] = buf_ay.sum; \
imu_accel_raw[AXIS_Z] = buf_az.sum; \
BoozImuScaleSensor3(imu_accel, IMU_ACCEL, imu_accel_raw); \
gyro_handler(); \
accel_handler(); \
} \
else if (micromag_status == MM_DATA_AVAILABLE) { \
micromag_status = MM_IDLE; \
imu_mag_raw[AXIS_X] = micromag_values[IMU_MAG_X_CHAN]; \
imu_mag_raw[AXIS_Y] = micromag_values[IMU_MAG_Y_CHAN]; \
imu_mag_raw[AXIS_Z] = micromag_values[IMU_MAG_Z_CHAN]; \
BoozImuScaleSensor3(imu_mag, IMU_MAG, imu_mag_raw); \
mag_handler(); \
} \
else if (scp1000_status == SCP1000_STA_DATA_AVAILABLE) { \
scp1000_status = SCP1000_STA_WAIT_EOC; \
imu_pressure_raw = scp1000_pressure; \
BoozImuScaleSensor1(imu_pressure, IMU_PRESSURE, imu_pressure_raw); \
baro_handler(); \
} \
}
extern void booz_imu_hw_init(void);
#include "booz_imu_hw.h"
#endif /* BOOZ_IMU_H */
sw/airborne/booz_ins.c
-69
View File
@@ -1,69 +0,0 @@
/*
* $Id$
*
* Copyright (C) 2008 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.
*
*/
#include "booz_ins.h"
#include "6dof.h"
#include "tl_vfilter.h"
FLOAT_T booz_ins_x;
FLOAT_T booz_ins_y;
FLOAT_T booz_ins_z;
FLOAT_T booz_ins_xdot;
FLOAT_T booz_ins_ydot;
FLOAT_T booz_ins_zdot;
FLOAT_T booz_ins_qnh;
FLOAT_T booz_ins_x_meas;
FLOAT_T booz_ins_y_meas;
FLOAT_T booz_ins_z_meas;
FLOAT_T booz_ins_bax;
FLOAT_T booz_ins_bay;
FLOAT_T booz_ins_baz;
uint8_t booz_ins_status;
void booz_ins_init( void ) {
booz_ins_status = BOOZ_INS_STATUS_UNINIT;
}
void booz_ins_start( const float* accel, const float pressure) {
booz_ins_qnh = pressure;
booz_ins_status = BOOZ_INS_STATUS_RUNNING;
tl_vf_init(0., 0., 0.);
}
void booz_ins_predict( const float* accel) {
tl_vf_predict(accel[AXIS_Z]);
booz_ins_z = tl_vf_z;
booz_ins_zdot = tl_vf_zdot;
booz_ins_baz = tl_vf_bias;
}
void booz_ins_update_pressure( const float pressure ) {
booz_ins_z_meas = (pressure - booz_ins_qnh)*0.084;
tl_vf_update(booz_ins_z_meas);
}

Some files were not shown because too many files have changed in this diff Show More