diff --git a/sw/airborne/booz/ahrs_comp_filter.c b/sw/airborne/booz/ahrs_comp_filter.c new file mode 100644 index 0000000000..8d7adb44d0 --- /dev/null +++ b/sw/airborne/booz/ahrs_comp_filter.c @@ -0,0 +1,117 @@ +/* + * $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 + +#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; +} + diff --git a/sw/airborne/booz/ahrs_comp_filter.h b/sw/airborne/booz/ahrs_comp_filter.h new file mode 100644 index 0000000000..7f3f66a7ef --- /dev/null +++ b/sw/airborne/booz/ahrs_comp_filter.h @@ -0,0 +1,32 @@ +/* + * $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 */ diff --git a/sw/airborne/booz/ahrs_multitilt.c b/sw/airborne/booz/ahrs_multitilt.c new file mode 100644 index 0000000000..53c8e07a5e --- /dev/null +++ b/sw/airborne/booz/ahrs_multitilt.c @@ -0,0 +1,162 @@ +#include "booz_ahrs.h" + +#include +#include + +#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 + +} + + + diff --git a/sw/airborne/booz/ahrs_multitilt.h b/sw/airborne/booz/ahrs_multitilt.h new file mode 100644 index 0000000000..d8f69ef527 --- /dev/null +++ b/sw/airborne/booz/ahrs_multitilt.h @@ -0,0 +1,38 @@ +/* + * $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 */ diff --git a/sw/airborne/booz/ahrs_quat_fast_ekf.h b/sw/airborne/booz/ahrs_quat_fast_ekf.h new file mode 100644 index 0000000000..ec26797719 --- /dev/null +++ b/sw/airborne/booz/ahrs_quat_fast_ekf.h @@ -0,0 +1,23 @@ +#ifndef AHRS_QUAT_FAST_EKF_H +#define AHRS_QUAT_FAST_EKF_H + +#include +#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 */ diff --git a/sw/airborne/booz/ahrs_quat_fast_ekf_utils.h b/sw/airborne/booz/ahrs_quat_fast_ekf_utils.h new file mode 100644 index 0000000000..c9dd352707 --- /dev/null +++ b/sw/airborne/booz/ahrs_quat_fast_ekf_utils.h @@ -0,0 +1,172 @@ +#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 */ diff --git a/sw/airborne/booz/arm7/actuators_buss_twi_blmc_hw.c b/sw/airborne/booz/arm7/actuators_buss_twi_blmc_hw.c new file mode 100644 index 0000000000..341f61f247 --- /dev/null +++ b/sw/airborne/booz/arm7/actuators_buss_twi_blmc_hw.c @@ -0,0 +1,18 @@ +#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 +#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 */ diff --git a/sw/airborne/booz/arm7/booz_imu_hw.c b/sw/airborne/booz/arm7/booz_imu_hw.c new file mode 100644 index 0000000000..2047f853c6 --- /dev/null +++ b/sw/airborne/booz/arm7/booz_imu_hw.c @@ -0,0 +1,105 @@ +#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(); +} diff --git a/sw/airborne/booz/arm7/booz_imu_hw.h b/sw/airborne/booz/arm7/booz_imu_hw.h new file mode 100644 index 0000000000..e9f424d7e9 --- /dev/null +++ b/sw/airborne/booz/arm7/booz_imu_hw.h @@ -0,0 +1,30 @@ +/* + * $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 */ diff --git a/sw/airborne/booz/arm7/booz_link_mcu_hw.c b/sw/airborne/booz/arm7/booz_link_mcu_hw.c new file mode 100644 index 0000000000..8457ccd537 --- /dev/null +++ b/sw/airborne/booz/arm7/booz_link_mcu_hw.c @@ -0,0 +1,261 @@ +/* + * $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 */ diff --git a/sw/airborne/booz/arm7/booz_link_mcu_hw.h b/sw/airborne/booz/arm7/booz_link_mcu_hw.h new file mode 100644 index 0000000000..6eca5395f8 --- /dev/null +++ b/sw/airborne/booz/arm7/booz_link_mcu_hw.h @@ -0,0 +1,99 @@ +/* + * $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 */ diff --git a/sw/airborne/booz/booz_ahrs.c b/sw/airborne/booz/booz_ahrs.c new file mode 100644 index 0000000000..a8b4e7b91b --- /dev/null +++ b/sw/airborne/booz/booz_ahrs.c @@ -0,0 +1,19 @@ +#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; diff --git a/sw/airborne/booz/booz_ahrs.h b/sw/airborne/booz/booz_ahrs.h new file mode 100644 index 0000000000..4198496f0e --- /dev/null +++ b/sw/airborne/booz/booz_ahrs.h @@ -0,0 +1,114 @@ +/* + * $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 */ diff --git a/sw/airborne/booz/booz_autopilot.c b/sw/airborne/booz/booz_autopilot.c new file mode 100644 index 0000000000..f91ea24c0c --- /dev/null +++ b/sw/airborne/booz/booz_autopilot.c @@ -0,0 +1,83 @@ +/* + * $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; + } +} diff --git a/sw/airborne/booz/booz_autopilot.h b/sw/airborne/booz/booz_autopilot.h new file mode 100644 index 0000000000..6d438ce0f0 --- /dev/null +++ b/sw/airborne/booz/booz_autopilot.h @@ -0,0 +1,50 @@ +/* + * $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 */ diff --git a/sw/airborne/booz/booz_control.c b/sw/airborne/booz/booz_control.c new file mode 100644 index 0000000000..e9575e5714 --- /dev/null +++ b/sw/airborne/booz/booz_control.c @@ -0,0 +1,205 @@ +/* + * $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)); + } +} diff --git a/sw/airborne/booz/booz_control.h b/sw/airborne/booz/booz_control.h new file mode 100644 index 0000000000..6cb4ed36f4 --- /dev/null +++ b/sw/airborne/booz/booz_control.h @@ -0,0 +1,69 @@ +/* + * $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 */ diff --git a/sw/airborne/booz/booz_controller_main.c b/sw/airborne/booz/booz_controller_main.c new file mode 100644 index 0000000000..1f6f7ff98f --- /dev/null +++ b/sw/airborne/booz/booz_controller_main.c @@ -0,0 +1,172 @@ +/* + * $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); + +} diff --git a/sw/airborne/booz/booz_controller_main.h b/sw/airborne/booz/booz_controller_main.h new file mode 100644 index 0000000000..60551e2bba --- /dev/null +++ b/sw/airborne/booz/booz_controller_main.h @@ -0,0 +1,14 @@ +#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 */ diff --git a/sw/airborne/booz/booz_controller_telemetry.c b/sw/airborne/booz/booz_controller_telemetry.c new file mode 100644 index 0000000000..80e85f3b25 --- /dev/null +++ b/sw/airborne/booz/booz_controller_telemetry.c @@ -0,0 +1,5 @@ +#include "booz_controller_telemetry.h" + + +uint8_t telemetry_mode_Controller; + diff --git a/sw/airborne/booz/booz_controller_telemetry.h b/sw/airborne/booz/booz_controller_telemetry.h new file mode 100644 index 0000000000..81d8039d4a --- /dev/null +++ b/sw/airborne/booz/booz_controller_telemetry.h @@ -0,0 +1,106 @@ +#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(16, MD5SUM) + +#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 */ diff --git a/sw/airborne/booz/booz_datalink.c b/sw/airborne/booz/booz_datalink.c new file mode 100644 index 0000000000..627358c4ad --- /dev/null +++ b/sw/airborne/booz/booz_datalink.c @@ -0,0 +1,24 @@ +#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; + } + } +} diff --git a/sw/airborne/booz/booz_debug.c b/sw/airborne/booz/booz_debug.c new file mode 100644 index 0000000000..f5366eb160 --- /dev/null +++ b/sw/airborne/booz/booz_debug.c @@ -0,0 +1,4 @@ +#include "booz_debug.h" + +uint8_t booz_debug_mod; +uint8_t booz_debug_err; diff --git a/sw/airborne/booz/booz_debug.h b/sw/airborne/booz/booz_debug.h new file mode 100644 index 0000000000..7c1a4e9313 --- /dev/null +++ b/sw/airborne/booz/booz_debug.h @@ -0,0 +1,31 @@ +#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 */ diff --git a/sw/airborne/booz/booz_energy.c b/sw/airborne/booz/booz_energy.c new file mode 100644 index 0000000000..bd0328c40b --- /dev/null +++ b/sw/airborne/booz/booz_energy.c @@ -0,0 +1,16 @@ +#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; +} diff --git a/sw/airborne/booz/booz_energy.h b/sw/airborne/booz/booz_energy.h new file mode 100644 index 0000000000..8cd4f51d04 --- /dev/null +++ b/sw/airborne/booz/booz_energy.h @@ -0,0 +1,14 @@ +#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 */ diff --git a/sw/airborne/booz/booz_estimator.c b/sw/airborne/booz/booz_estimator.c new file mode 100644 index 0000000000..3983280964 --- /dev/null +++ b/sw/airborne/booz/booz_estimator.c @@ -0,0 +1,128 @@ +#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(); + +} + + diff --git a/sw/airborne/booz/booz_estimator.h b/sw/airborne/booz/booz_estimator.h new file mode 100644 index 0000000000..07e330c512 --- /dev/null +++ b/sw/airborne/booz/booz_estimator.h @@ -0,0 +1,41 @@ +#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 */ diff --git a/sw/airborne/booz/booz_filter_main.c b/sw/airborne/booz/booz_filter_main.c new file mode 100644 index 0000000000..b317f95538 --- /dev/null +++ b/sw/airborne/booz/booz_filter_main.c @@ -0,0 +1,159 @@ +/* + * $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 ) { + + +} diff --git a/sw/airborne/booz/booz_filter_main.h b/sw/airborne/booz/booz_filter_main.h new file mode 100644 index 0000000000..9b30afd1f9 --- /dev/null +++ b/sw/airborne/booz/booz_filter_main.h @@ -0,0 +1,14 @@ +#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 */ diff --git a/sw/airborne/booz/booz_filter_telemetry.c b/sw/airborne/booz/booz_filter_telemetry.c new file mode 100644 index 0000000000..e5b51e4360 --- /dev/null +++ b/sw/airborne/booz/booz_filter_telemetry.c @@ -0,0 +1,3 @@ +#include "booz_filter_telemetry.h" + +uint8_t telemetry_mode_Filter; diff --git a/sw/airborne/booz/booz_filter_telemetry.h b/sw/airborne/booz/booz_filter_telemetry.h new file mode 100644 index 0000000000..0fe339fc3d --- /dev/null +++ b/sw/airborne/booz/booz_filter_telemetry.h @@ -0,0 +1,180 @@ +/* + * $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 */ diff --git a/sw/airborne/booz/booz_imu.c b/sw/airborne/booz/booz_imu.c new file mode 100644 index 0000000000..1985c88640 --- /dev/null +++ b/sw/airborne/booz/booz_imu.c @@ -0,0 +1,78 @@ +/* + * $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 16384 */ +/* rates are transmitted as int16 : 1 PI s-1 -> 16384 */ +#define ANGLE_PI 0X3FFF +#define RATE_PI_S 0X3FFF + +struct booz_inter_mcu_state { + int16_t r_rates [AXIS_NB]; + int16_t f_rates [AXIS_NB]; + int16_t f_eulers[AXIS_NB]; + int16_t pad0; + float pos[AXIS_NB]; + float speed[AXIS_NB]; + uint8_t status; + uint8_t pad1; + uint16_t crc; +}; + +extern struct booz_inter_mcu_state inter_mcu_state; + +#ifdef BOOZ_FILTER_MCU +extern void inter_mcu_fill_state(void); +#endif + +#endif /* BOOZ_INTER_MCU_H */ diff --git a/sw/airborne/booz/booz_link_mcu.c b/sw/airborne/booz/booz_link_mcu.c new file mode 100644 index 0000000000..ed8df43761 --- /dev/null +++ b/sw/airborne/booz/booz_link_mcu.c @@ -0,0 +1,84 @@ +/* + * $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" + +#include "std.h" + +struct booz_inter_mcu_state booz_link_mcu_state_unused; +uint16_t booz_link_mcu_crc; + + + +#ifdef BOOZ_FILTER_MCU /* FILTER LPC code */ + +/* FIXME!!!! two function with same name in single MCU configuration */ +/* by chance booz_link_mcu_hw_init does nothing in sim */ +#ifndef SITL +void booz_link_mcu_init ( void ) { + booz_link_mcu_hw_init(); +} +#endif + +void booz_link_mcu_send ( void ) { + inter_mcu_fill_state(); + BoozLinkMcuComputeCRC(); + inter_mcu_state.crc = booz_link_mcu_crc; + BoozLinkMcuHwSend(); +} + + + +#endif /* BOOZ_FILTER_MCU */ + + + + + +#ifdef BOOZ_CONTROLLER_MCU /* lpc controller board */ + +#include "spi.h" + +#include "booz_estimator.h" + +volatile uint8_t booz_link_mcu_status; +uint32_t booz_link_mcu_nb_err; +uint32_t booz_link_mcu_timeout; +#define BOOZ_LINK_MCU_TIMEOUT 100 + +void booz_link_mcu_init ( void ) { + + booz_link_mcu_hw_init(); + booz_link_mcu_status = BOOZ_LINK_MCU_IDLE; + booz_link_mcu_nb_err = 0; + booz_link_mcu_timeout = BOOZ_LINK_MCU_TIMEOUT; + +} + +extern void booz_link_mcu_periodic( void ) { + if (booz_link_mcu_timeout < BOOZ_LINK_MCU_TIMEOUT) + booz_link_mcu_timeout++; +} + +#endif /* BOOZ_CONTROLLER_MCU */ diff --git a/sw/airborne/booz/booz_link_mcu.h b/sw/airborne/booz/booz_link_mcu.h new file mode 100644 index 0000000000..858b3505bf --- /dev/null +++ b/sw/airborne/booz/booz_link_mcu.h @@ -0,0 +1,95 @@ +/* + * $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_LINK_MCU_H +#define BOOZ_LINK_LINK_MCU_H + +#include + +#include "6dof.h" + +#include "booz_link_mcu_hw.h" + +#include "booz_inter_mcu.h" + +extern void booz_link_mcu_init ( void ); +extern void booz_link_mcu_hw_init ( void ); +extern struct booz_inter_mcu_state booz_link_mcu_state_unused; /* single dir only */ +extern uint16_t booz_link_mcu_crc; + +#define BOOZ_LINK_MCU_CRC_INIT 0x0 +#define BOOZ_LINK_MCU_PAYLOAD_LENGTH (sizeof(struct booz_inter_mcu_state) - 2) +#define BoozLinkMcuCrcLow(x) (((x)&0xff)) +#define BoozLinkMcuCrcHigh(x) (((x)>>8)) +#define BoozLinkMcuComputeCRC() { \ + uint8_t i; \ + booz_link_mcu_crc = BOOZ_LINK_MCU_CRC_INIT; \ + for(i = 0; i < BOOZ_LINK_MCU_PAYLOAD_LENGTH; i++) { \ + uint8_t _byte = ((uint8_t*)&inter_mcu_state)[i]; \ + uint8_t a = ((uint8_t)BoozLinkMcuCrcHigh(booz_link_mcu_crc)) + _byte; \ + uint8_t b = ((uint8_t)BoozLinkMcuCrcLow(booz_link_mcu_crc)) + a; \ + booz_link_mcu_crc = b | a << 8; \ + } \ + } + +#ifdef BOOZ_FILTER_MCU + +extern void booz_link_mcu_send( void ); + +#endif /* BOOZ_FILTER_MCU */ + + + + +#ifdef BOOZ_CONTROLLER_MCU + +#define BOOZ_LINK_MCU_IDLE 0 +#define BOOZ_LINK_MCU_DATA_AVAILABLE 1 +#define BOOZ_LINK_MCU_SHORT_READ 2 +#define BOOZ_LINK_MCU_OVER_READ 3 + +extern volatile uint8_t booz_link_mcu_status; +extern uint32_t booz_link_mcu_nb_err; +extern uint32_t booz_link_mcu_timeout; +extern void booz_link_mcu_periodic( void ); + +#define BoozLinkMcuEventCheckAndHandle(handler) { \ + if (booz_link_mcu_status == BOOZ_LINK_MCU_DATA_AVAILABLE) { \ + BoozLinkMcuComputeCRC(); \ + if (booz_link_mcu_crc == inter_mcu_state.crc) { \ + booz_link_mcu_timeout = 0; \ + handler(); \ + } \ + else { \ + booz_link_mcu_nb_err++; \ + } \ + BoozLinkMcuHwRestart(); \ + booz_link_mcu_status = BOOZ_LINK_MCU_IDLE; \ + } \ + } \ + +#endif /* BOOZ_CONTROLLER_MCU */ + + +#endif /* BOOZ_LINK_LINK_MCU_H */ diff --git a/sw/airborne/booz/booz_nav.c b/sw/airborne/booz/booz_nav.c new file mode 100644 index 0000000000..42be721b00 --- /dev/null +++ b/sw/airborne/booz/booz_nav.c @@ -0,0 +1,57 @@ +#include "booz_nav.h" + +#include + +#include "booz_estimator.h" +#include "booz_control.h" +#include "radio_control.h" + + +#define Block(x) case x: nav_block=x; +#define InitBlock() { nav_stage = 0; block_time = 0; InitStage(); } +#define NextBlock() { nav_block++; InitBlock(); } +#define GotoBlock(b) { nav_block=b; InitBlock(); } + +#define Stage(s) case s: nav_stage=s; +#define NextStage() { nav_stage++; InitStage() } +#define NextStageFrom(wp) { last_wp = wp; NextStage() } +#define GotoStage(s) { nav_stage = s; InitStage() } + +#define NavGotoWaypoint(_wp) {} +#define NavVerticalAutoThrottleMode(_foo) {} +#define NavVerticalAltitudeMode(_foo, _bar) {} + +#define NavCircleWaypoint(_foo, _bar) {} + +extern void nav_home(void); +#define NAV_C +#include "flight_plan.h" + +const uint8_t nb_waypoint; +struct point waypoints[NB_WAYPOINT] = WAYPOINTS; + +uint8_t nav_stage, nav_block; +uint16_t stage_time, block_time; + + +#include "booz_nav_hover.h" + +void booz_nav_init(void) { + nav_block = 0; + nav_stage = 0; + booz_nav_hover_init(); +} + + +void booz_nav_run(void) { + booz_nav_hover_run(); + BoozControlAttitudeSetSetPoints(booz_nav_hover_phi_command, booz_nav_hover_theta_command, + booz_nav_hover_psi_sp, booz_nav_hover_power_command); +} + +void booz_nav_read_setpoints_from_rc(void) { + booz_nav_hover_read_setpoints_from_rc(); +} + + + diff --git a/sw/airborne/booz/booz_nav.h b/sw/airborne/booz/booz_nav.h new file mode 100644 index 0000000000..872ad075ee --- /dev/null +++ b/sw/airborne/booz/booz_nav.h @@ -0,0 +1,35 @@ +#ifndef BOOZ_NAV_H +#define BOOZ_NAV_H + +#include "std.h" + +#include "booz_nav_hover.h" + +extern void booz_nav_init(void); +extern void booz_nav_run(void); +extern void booz_nav_read_setpoints_from_rc(void); + + +struct point { + float x; + float y; + float a; +}; + +#define WaypointX(_wp) (waypoints[_wp].x) +#define WaypointY(_wp) (waypoints[_wp].y) +#define WaypointAlt(_wp) (waypoints[_wp].a) + +extern const uint8_t nb_waypoint; +extern struct point waypoints[]; + +extern uint16_t stage_time, block_time; +extern uint8_t nav_stage, nav_block; + +extern void nav_init_stage( void ); +#define InitStage() { nav_init_stage(); return; } + + + + +#endif /* BOOZ_NAV_H */ diff --git a/sw/airborne/booz/booz_nav_hover.c b/sw/airborne/booz/booz_nav_hover.c new file mode 100644 index 0000000000..e56603eee1 --- /dev/null +++ b/sw/airborne/booz/booz_nav_hover.c @@ -0,0 +1,185 @@ +#include "booz_nav.h" + +#include + +#include "booz_estimator.h" +#include "booz_control.h" +#include "radio_control.h" + +float booz_nav_hover_x_sp; +float booz_nav_hover_y_sp; +float booz_nav_hover_z_sp; +float booz_nav_hover_psi_sp; + +float booz_nav_hover_h_max_err; +float booz_nav_hover_h_pgain; +float booz_nav_hover_h_dgain; +float booz_nav_hover_v_max_err; +float booz_nav_hover_v_pgain; +float booz_nav_hover_v_dgain; + +float booz_nav_hover_phi_command; +float booz_nav_hover_theta_command; +float booz_nav_hover_power_command; + +#define BOOZ_NAV_HOVER_H_MAX_ERR 10. +/* rad / m */ +#define BOOZ_NAV_HOVER_H_PGAIN (RadOfDeg(20.) / BOOZ_NAV_HOVER_H_MAX_ERR) +/* rad / (ms-1) */ +#define BOOZ_NAV_HOVER_H_DGAIN (RadOfDeg(5.)) + +#define BOOZ_NAV_HOVER_MAX_PHI_COMMAND RadOfDeg(30.) +#define BOOZ_NAV_HOVER_MAX_THETA_COMMAND RadOfDeg(30.) + +#define BOOZ_NAV_HOVER_V_MAX_ERR 10. +#define BOOZ_NAV_HOVER_V_PGAIN 0.01 +#define BOOZ_NAV_HOVER_V_DGAIN 0.015 +#define BOOZ_NAV_HOVER_V_HOVER_POWER 0.318 + +#ifdef NAV_VERTICAL +static void booz_nav_hover_vertical_loop_run(void); +#endif +#ifdef NAV_HORIZONTAL +static void booz_nav_hover_horizontal_loop_run(void); +#endif + +void booz_nav_hover_init(void) { + + booz_nav_hover_x_sp = 0.; + booz_nav_hover_y_sp = 0.; + booz_nav_hover_z_sp = 0.; + booz_nav_hover_psi_sp = 0.; + + booz_nav_hover_h_max_err = BOOZ_NAV_HOVER_H_MAX_ERR; + booz_nav_hover_h_pgain = BOOZ_NAV_HOVER_H_PGAIN; + booz_nav_hover_h_dgain = BOOZ_NAV_HOVER_H_DGAIN; + booz_nav_hover_v_max_err = BOOZ_NAV_HOVER_V_MAX_ERR; + booz_nav_hover_v_pgain = BOOZ_NAV_HOVER_V_PGAIN; + booz_nav_hover_v_dgain = BOOZ_NAV_HOVER_V_DGAIN; + + booz_nav_hover_phi_command = 0.; + booz_nav_hover_theta_command = 0.; + booz_nav_hover_power_command = 0.; + +} + +void booz_nav_hover_run(void) { +#ifdef NAV_VERTICAL + booz_nav_hover_vertical_loop_run(); +#endif +#ifdef NAV_HORIZONTAL + booz_nav_hover_horizontal_loop_run(); +#endif +} + +void booz_nav_hover_read_setpoints_from_rc(void) { +#ifdef NAV_VERTICAL + /* direct altitude between 1 and -9 meters */ + booz_nav_hover_z_sp = 1. - 10. / MAX_PPRZ * (float)rc_values[RADIO_THROTTLE]; +#else + booz_nav_hover_power_command = (float)rc_values[RADIO_THROTTLE] / MAX_PPRZ; +#endif + +#ifdef NAV_HORIZONTAL + booz_nav_hover_x_sp = 0.; + booz_nav_hover_y_sp = 0.; + booz_nav_hover_psi_sp = 0.; +#else + booz_nav_hover_phi_command = (float)-rc_values[RADIO_ROLL] / MAX_PPRZ; + booz_nav_hover_theta_command = (float)rc_values[RADIO_PITCH] / MAX_PPRZ;; + booz_nav_hover_psi_sp = 0.; +#endif +} + + + + + +#ifdef NAV_VERTICAL +static void booz_nav_hover_vertical_loop_run(void) { + float vertical_err = booz_estimator_z - booz_nav_hover_z_sp; + Bound(vertical_err, -booz_nav_hover_v_max_err, booz_nav_hover_v_max_err); + float bank_coef = cos(booz_estimator_phi)*cos(booz_estimator_theta); + if (bank_coef < 1e-1) bank_coef = 1e-1; + float adjusted_hover_power = BOOZ_NAV_HOVER_V_HOVER_POWER / bank_coef; + booz_nav_hover_power_command = adjusted_hover_power + + booz_nav_hover_v_pgain * vertical_err + + booz_nav_hover_v_dgain * booz_estimator_vz; + + Bound(booz_nav_hover_power_command, 0., 1.); + +} +#endif + + + + + +#ifdef NAV_HORIZONTAL +static void booz_nav_hover_horizontal_loop_run(void) { + + /* get a position error vector */ + float x_error = booz_estimator_x - booz_nav_hover_x_sp; + float y_error = booz_estimator_y - booz_nav_hover_y_sp; + // printf("earth pos error %f %f\n", x_error, y_error); + /* get norm and unit position error vector */ + float norm_error = sqrt(x_error*x_error+ y_error*y_error); + float unit_x_error = x_error / norm_error; + float unit_y_error = y_error / norm_error; + // printf("earth pos error %f %f %f\n", unit_x_error, unit_y_error, norm_error); + /* bound the error in amplitude */ + Bound(norm_error, 0., booz_nav_hover_h_max_err); + /* convert unit error vector to body frame */ + float x_unit_err_body = unit_x_error * booz_estimator_dcm[AXIS_X][AXIS_X] + + unit_y_error * booz_estimator_dcm[AXIS_X][AXIS_Y]; + float y_unit_err_body = unit_x_error * booz_estimator_dcm[AXIS_Y][AXIS_X] + + unit_y_error * booz_estimator_dcm[AXIS_Y][AXIS_Y]; + // printf("body pos error %f %f\n", x_unit_err_body, y_unit_err_body); + /* */ + + booz_nav_hover_phi_command = - booz_nav_hover_h_pgain * y_unit_err_body * norm_error + + - booz_nav_hover_h_dgain * booz_estimator_v; + booz_nav_hover_theta_command = booz_nav_hover_h_pgain * x_unit_err_body * norm_error + + booz_nav_hover_h_dgain * booz_estimator_u; + + Bound(booz_nav_hover_phi_command, -BOOZ_NAV_HOVER_MAX_PHI_COMMAND, BOOZ_NAV_HOVER_MAX_PHI_COMMAND); + Bound(booz_nav_hover_theta_command, -BOOZ_NAV_HOVER_MAX_THETA_COMMAND, BOOZ_NAV_HOVER_MAX_THETA_COMMAND); + +} +#endif + + + + +# if 0 +#define STICK_ABSOLUTE_POS 0 +#define STICK_ABSOLUTE_SPEED 1 +#define STICK_RELATIVE_SPEED 2 +#define STICK_MODE STICK_RELATIVE_SPEED +#define DT_READ_SETPOINTS 0.004 +void booz_nav_hover_read_setpoints_from_rc(void) { +#ifndef DISABLE_NAV + booz_nav_hover_z_sp = -1. - 10. / MAX_PPRZ * (float)rc_values[RADIO_THROTTLE]; +#if defined STICK_MODE && STICK_MODE == STICK_ABSOLUTE_POS + booz_nav_horizontal_x_sp = -20. / MAX_PPRZ * (float)rc_values[RADIO_PITCH]; /* +/- 20m */ + booz_nav_horizontal_y_sp = -20. / MAX_PPRZ * (float)rc_values[RADIO_ROLL]; /* warning RC roll negativ to the right !! +/- 20m */ +#elif defined STICK_MODE && STICK_MODE == STICK_ABSOLUTE_SPEED + booz_nav_horizontal_x_sp += -5. * DT_READ_SETPOINTS / MAX_PPRZ * (float)rc_values[RADIO_PITCH]; /* +/-5 m/s */ + booz_nav_horizontal_y_sp += 5. * DT_READ_SETPOINTS / MAX_PPRZ * (float)rc_values[RADIO_ROLL]; /* +/-5 m/s */ +#elif defined STICK_MODE && STICK_MODE == STICK_RELATIVE_SPEED + float booz_nav_hover_dx_bod = -5. / MAX_PPRZ * (float)rc_values[RADIO_PITCH]; + float booz_nav_hover_dy_bod = -5. / MAX_PPRZ * (float)rc_values[RADIO_ROLL]; /* warning RC roll negativ to the right !! +/- 5m/s */ + /* convert vector to earth frame ( use transpose of DCM ) */ + float booz_nav_hover_dx_earth = booz_nav_hover_dx_bod * booz_estimator_dcm[AXIS_X][AXIS_X] + + booz_nav_hover_dy_bod * booz_estimator_dcm[AXIS_Y][AXIS_X]; + float booz_nav_hover_dy_earth = booz_nav_hover_dx_bod * booz_estimator_dcm[AXIS_X][AXIS_Y] + + booz_nav_hover_dy_bod * booz_estimator_dcm[AXIS_Y][AXIS_Y]; + booz_nav_hover_x_sp += (booz_nav_hover_dx_earth * DT_READ_SETPOINTS); + booz_nav_hover_y_sp += (booz_nav_hover_dy_earth * DT_READ_SETPOINTS); + booz_nav_hover_psi_sp += (-RadOfDeg(60.) * DT_READ_SETPOINTS / MAX_PPRZ * (float)rc_values[RADIO_YAW]); + booz_nav_hover_psi_sp = RadOfDeg(0.); + NormRadAngle(booz_nav_hover_psi_sp); +#endif /* STICK_MODE */ +#endif /* DISABLE_NAV */ +} +#endif /* 0 */ diff --git a/sw/airborne/booz/booz_nav_hover.h b/sw/airborne/booz/booz_nav_hover.h new file mode 100644 index 0000000000..83fefae5e6 --- /dev/null +++ b/sw/airborne/booz/booz_nav_hover.h @@ -0,0 +1,28 @@ +#ifndef BOOZ_NAV_HOVER_H +#define BOOZ_NAV_HOVER_H + + +#ifndef DISABLE_NAV +extern float booz_nav_hover_x_sp; +extern float booz_nav_hover_y_sp; +extern float booz_nav_hover_z_sp; +extern float booz_nav_hover_psi_sp; + +extern float booz_nav_hover_h_max_err; +extern float booz_nav_hover_h_pgain; +extern float booz_nav_hover_h_dgain; +extern float booz_nav_hover_v_max_err; +extern float booz_nav_hover_v_pgain; +extern float booz_nav_hover_v_dgain; + +extern float booz_nav_hover_phi_command; +extern float booz_nav_hover_theta_command; +extern float booz_nav_hover_power_command; +#endif + +extern void booz_nav_hover_init(void); +extern void booz_nav_hover_run(void); +extern void booz_nav_hover_read_setpoints_from_rc(void); + + +#endif /* BOOZ_NAV_HOVER_H */ diff --git a/sw/airborne/booz/booz_still_detection.c b/sw/airborne/booz/booz_still_detection.c new file mode 100644 index 0000000000..21facac6eb --- /dev/null +++ b/sw/airborne/booz/booz_still_detection.c @@ -0,0 +1,136 @@ +/* + * $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_still_detection.h" +#include "booz_imu.h" +#include "sys_time.h" + +uint8_t booz_still_detection_status; +float booz_still_detection_accel[AXIS_NB]; +float booz_still_detection_gyro[AXIS_NB]; +float booz_still_detection_mag[AXIS_NB]; +float booz_still_detection_pressure; + +#define BSD_BUF_LEN 128 + +static uint16_t bsd_accel_raw[AXIS_NB][BSD_BUF_LEN]; +static uint32_t bsd_accel_raw_sum[AXIS_NB]; +uint32_t bsd_accel_raw_avg[AXIS_NB]; +float bsd_accel_raw_var[AXIS_NB]; + +static uint16_t bsd_gyro_raw[AXIS_NB][BSD_BUF_LEN]; +static uint32_t bsd_gyro_raw_sum[AXIS_NB]; +static uint32_t bsd_gyro_raw_avg[AXIS_NB]; +static float bsd_gyro_raw_var[AXIS_NB]; + +static int16_t bsd_mag_raw[AXIS_NB][BSD_BUF_LEN]; +static int32_t bsd_mag_raw_sum[AXIS_NB]; +static int32_t bsd_mag_raw_avg[AXIS_NB]; +static float bsd_mag_raw_var[AXIS_NB]; + +static uint32_t bsd_pressure_raw[BSD_BUF_LEN]; +static uint32_t bsd_pressure_raw_sum; +static uint32_t bsd_pressure_raw_avg; +//static float bsd_pressure_raw_var; + +static int16_t bsd_buf_head; +static bool_t bsd_buf_filled; + +static inline void bsd_set_initial_values(void); + +#define BsdUpdateAverageArray(_sensor) { \ + bsd_##_sensor##_raw_sum[axis] -= bsd_##_sensor##_raw[axis][bsd_buf_head]; \ + bsd_##_sensor##_raw[axis][bsd_buf_head] = imu_##_sensor##_raw[axis]; \ + bsd_##_sensor##_raw_sum[axis] += imu_##_sensor##_raw[axis]; \ + bsd_##_sensor##_raw_avg[axis] = bsd_##_sensor##_raw_sum[axis] / BSD_BUF_LEN; \ + } + +#define BsdUpdateAverageScal(_sensor) { \ + bsd_##_sensor##_raw_sum -= bsd_##_sensor##_raw[bsd_buf_head]; \ + bsd_##_sensor##_raw[bsd_buf_head] = imu_##_sensor##_raw; \ + bsd_##_sensor##_raw_sum += imu_##_sensor##_raw; \ + bsd_##_sensor##_raw_avg = bsd_##_sensor##_raw_sum / BSD_BUF_LEN; \ + } + +void booz_still_detection_init(void) { + bsd_buf_filled = FALSE; + bsd_buf_head = -1; + booz_still_detection_status = BSD_STATUS_UNINIT; +} + +void booz_still_detection_run(void) { + if (booz_still_detection_status == BSD_STATUS_LOCKED) + return; + /* update sliding average of sensors */ + bsd_buf_head++; + if (bsd_buf_head >= BSD_BUF_LEN) { + bsd_buf_filled = TRUE; + bsd_buf_head = 0; + } + + uint8_t axis, j; + for (axis=0; axis 2 && + bsd_accel_raw_var[AXIS_X] < BSD_ACCEL_RAW_MAX_VAR && + bsd_accel_raw_var[AXIS_Y] < BSD_ACCEL_RAW_MAX_VAR && + bsd_accel_raw_var[AXIS_Z] < BSD_ACCEL_RAW_MAX_VAR ) + bsd_set_initial_values(); + } +} + +static inline void bsd_set_initial_values(void) { + booz_still_detection_status = BSD_STATUS_LOCKED; + BoozImuScaleSensor3(booz_still_detection_accel, IMU_ACCEL, bsd_accel_raw_avg); + BoozImuScaleSensor3(booz_still_detection_gyro, IMU_GYRO, bsd_gyro_raw_avg); + BoozImuScaleSensor3(booz_still_detection_mag, IMU_MAG, bsd_mag_raw_avg); + BoozImuScaleSensor1(booz_still_detection_pressure, IMU_PRESSURE, bsd_pressure_raw_avg); +} diff --git a/sw/airborne/booz/booz_still_detection.h b/sw/airborne/booz/booz_still_detection.h new file mode 100644 index 0000000000..2bf560b4a2 --- /dev/null +++ b/sw/airborne/booz/booz_still_detection.h @@ -0,0 +1,53 @@ +/* + * $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. + * + */ + +/* + detect a still vehicle by monitoring variance of imu sensors +*/ + + +#ifndef BOOZ_STILL_DETECTION_H +#define BOOZ_STILL_DETECTION_H + +#include "std.h" +#include "6dof.h" + +extern float booz_still_detection_accel[AXIS_NB]; +extern float booz_still_detection_gyro[AXIS_NB]; +extern float booz_still_detection_mag[AXIS_NB]; +extern float booz_still_detection_pressure; + +extern uint32_t bsd_accel_raw_avg[AXIS_NB]; +extern float bsd_accel_raw_var[AXIS_NB]; + +extern uint8_t booz_still_detection_status; +#define BSD_STATUS_UNINIT 0 +#define BSD_STATUS_LOCKED 1 + +extern void booz_still_detection_init(void); +extern void booz_still_detection_run(void); + +#define BSD_ACCEL_RAW_MAX_VAR 7000. + +#endif /* BOOZ_STILL_DETECTION_H */ diff --git a/sw/airborne/booz/test/booz_test_imu.c b/sw/airborne/booz/test/booz_test_imu.c new file mode 100644 index 0000000000..7deecc3d3d --- /dev/null +++ b/sw/airborne/booz/test/booz_test_imu.c @@ -0,0 +1,90 @@ +#include "std.h" +#include "init_hw.h" +#include "interrupt_hw.h" +#include "sys_time.h" + +#include "uart.h" +#include "messages.h" +#include "downlink.h" + +#include "booz_imu.h" + +static inline void main_init(void); +static inline void main_periodic(void); +static inline void main_event(void); + +uint32_t t0, t1, diff; + +int main( void ) { + main_init(); + while (1) { + if (sys_time_periodic()) + main_periodic(); + main_event(); + } + return 0; +} + + +static inline void main_init(void) { + hw_init(); + + sys_time_init(); + + uart1_init_tx(); + + booz_imu_init(); + + int_enable(); +} + + +static inline void main_periodic(void) { + // t0 = T0TC; + booz_imu_periodic(); + RunOnceEvery(250,DOWNLINK_SEND_BOOT(&cpu_time_sec)); +} + + +static inline void on_imu_accel_available(void) { + RunOnceEvery(4, { \ + DOWNLINK_SEND_IMU_ACCEL_RAW(&imu_accel_raw[AXIS_X], &imu_accel_raw[AXIS_Y], &imu_accel_raw[AXIS_Z]); \ + DOWNLINK_SEND_IMU_ACCEL(&imu_accel[AXIS_X], &imu_accel[AXIS_Y], &imu_accel[AXIS_Z]); \ + }); +} + +static inline void on_imu_gyro_available(void) { + //DOWNLINK_SEND_IMU_GYRO_RAW(&imu_gyro_raw[AXIS_P], &imu_gyro_raw[AXIS_Q], &imu_gyro_raw[AXIS_R]); + //DOWNLINK_SEND_IMU_GYRO(&imu_gyro[AXIS_P], &imu_gyro[AXIS_Q], &imu_gyro[AXIS_R]); + // t1 = T0TC; + // diff = t1 - t0; + // DOWNLINK_SEND_TIME(&diff); +} + +static inline void on_imu_mag_available(void) { + DOWNLINK_SEND_IMU_MAG_RAW(&imu_mag_raw[AXIS_X], &imu_mag_raw[AXIS_Y], &imu_mag_raw[AXIS_Z]); + DOWNLINK_SEND_IMU_MAG(&imu_mag[AXIS_X], &imu_mag[AXIS_Y], &imu_mag[AXIS_Z]); + // t1 = T0TC; + // diff = t1 - t0; + // DOWNLINK_SEND_TIME(&diff); + // t0 = t1; +} + +static inline void on_imu_baro_available(void) { + float pressure = 0.25 * imu_pressure_raw; + static float ground_pressure = 0.; + if (ground_pressure == 0) ground_pressure = pressure; + float z_baro = (ground_pressure - pressure)*0.084; + DOWNLINK_SEND_TL_IMU_PRESSURE(&pressure); + DOWNLINK_SEND_TL_IMU_RANGEMETER(&z_baro); + t1 = T0TC; + diff = t1 - t0; + DOWNLINK_SEND_TIME(&diff); + t0 = t1; +} + +static inline void main_event(void) { + BoozImuEvent(on_imu_accel_available, on_imu_gyro_available, on_imu_mag_available, on_imu_baro_available); + +} + diff --git a/sw/airborne/booz/test/booz_test_link_mcu_ctl.c b/sw/airborne/booz/test/booz_test_link_mcu_ctl.c new file mode 100644 index 0000000000..dcb82b5ee1 --- /dev/null +++ b/sw/airborne/booz/test/booz_test_link_mcu_ctl.c @@ -0,0 +1,77 @@ +#include "std.h" +#include "init_hw.h" +#include "interrupt_hw.h" +#include "sys_time.h" + +#include "uart.h" +#include "messages.h" +#include "downlink.h" + +#include "booz_link_mcu.h" + +static inline void main_init(void); +static inline void main_periodic(void); +static inline void main_event(void); + +int main( void ) { + main_init(); + while (1) { + if (sys_time_periodic()) + main_periodic(); + main_event(); + } + return 0; +} + + +static inline void main_init(void) { + hw_init(); + + sys_time_init(); + + uart1_init_tx(); + + booz_link_mcu_init(); + + int_enable(); +} + +extern volatile uint32_t it_cnt; +extern volatile uint32_t rx_it_cnt; +extern volatile uint32_t ti_it_cnt; + +static inline void main_periodic(void) { + RunOnceEvery(250, { \ + static uint32_t it_last; \ + uint16_t it_nb = it_cnt - it_last; \ + it_last = it_cnt; \ + DOWNLINK_SEND_TAKEOFF(&it_nb); \ + static float rx_it_last; \ + float diff_rx = rx_it_cnt - rx_it_last; \ + rx_it_last = rx_it_cnt; \ + static float ti_it_last; \ + float diff_ti = ti_it_cnt - ti_it_last; \ + ti_it_last = ti_it_cnt; \ + DOWNLINK_SEND_BOOZ_DEBUG_BAR(&diff_rx, &diff_ti); \ + }); +} + + +static inline void on_link_event(void) { + RunOnceEvery(250, { \ + + uint8_t f0 = link_mcu_rx_buf[0]; \ + uint8_t f1 = link_mcu_rx_buf[1]; \ + uint8_t f2 = link_mcu_rx_buf[2]; \ + uint8_t f3 = link_mcu_rx_buf[3]; \ + DOWNLINK_SEND_BOOZ_CMDS(&f0, &f1, &f2, &f3); \ + }); + + + +} + +static inline void main_event(void) { + BoozLinkMcuEventCheckAndHandle(on_link_event); +} + diff --git a/sw/airborne/booz/test/booz_test_link_mcu_imu.c b/sw/airborne/booz/test/booz_test_link_mcu_imu.c new file mode 100644 index 0000000000..d8d2174e85 --- /dev/null +++ b/sw/airborne/booz/test/booz_test_link_mcu_imu.c @@ -0,0 +1,57 @@ +#include "std.h" +#include "init_hw.h" +#include "interrupt_hw.h" +#include "sys_time.h" + +#include "uart.h" +#include "messages.h" +#include "downlink.h" + +#include "booz_link_mcu.h" + +static inline void main_init(void); +static inline void main_periodic(void); +static inline void main_event(void); + +int main( void ) { + main_init(); + while (1) { + if (sys_time_periodic()) + main_periodic(); + main_event(); + } + return 0; +} + + +static inline void main_init(void) { + hw_init(); + + sys_time_init(); + + uart1_init_tx(); + + booz_link_mcu_init(); + + int_enable(); +} + +extern uint8_t foo_debug; +extern uint32_t it_count; + +static inline void main_periodic(void) { + RunOnceEvery(250, { \ + static uint32_t it_last; \ + uint16_t it_nb = it_count - it_last; \ + it_last = it_count; \ + DOWNLINK_SEND_BOOT(&it_nb) \ + }); + // RunOnceEvery(250,DOWNLINK_SEND_TAKEOFF(&foo_debug)); + booz_link_mcu_send(); +} + + +static inline void main_event(void) { + +} + diff --git a/sw/airborne/booz/test/booz_test_still_detection.c b/sw/airborne/booz/test/booz_test_still_detection.c new file mode 100644 index 0000000000..4278074e73 --- /dev/null +++ b/sw/airborne/booz/test/booz_test_still_detection.c @@ -0,0 +1,95 @@ +/* + * $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 "std.h" +#include "init_hw.h" +#include "interrupt_hw.h" +#include "sys_time.h" + +#include "uart.h" +#include "messages.h" +#include "downlink.h" + +#include "booz_imu.h" +#include "booz_still_detection.h" + +static inline void main_init(void); +static inline void main_periodic(void); +static inline void main_event(void); + +uint32_t t0, t1, diff; + +int main( void ) { + main_init(); + while (1) { + if (sys_time_periodic()) + main_periodic(); + main_event(); + } + return 0; +} + + +static inline void main_init(void) { + hw_init(); + + sys_time_init(); + + uart1_init_tx(); + + booz_imu_init(); + + booz_still_detection_init(); + + int_enable(); +} + + +static inline void main_periodic(void) { + booz_imu_periodic(); +} + + +static inline void on_imu_accel(void) { + booz_still_detection_run(); + uint16_t sd_ax = bsd_accel_raw_avg[AXIS_X]; + uint16_t sd_ay = bsd_accel_raw_avg[AXIS_Y]; + uint16_t sd_az = bsd_accel_raw_avg[AXIS_Z]; + DOWNLINK_SEND_IMU_ACCEL_RAW(&sd_ax, &sd_ay, &sd_az); + DOWNLINK_SEND_IMU_ACCEL(&bsd_accel_raw_var[AXIS_X], &bsd_accel_raw_var[AXIS_Y], &bsd_accel_raw_var[AXIS_Z]); +} + +static inline void on_imu_gyro(void) { +} + +static inline void on_imu_mag(void) { +} + +static inline void on_imu_baro(void) { +} + +static inline void main_event(void) { + BoozImuEvent(on_imu_accel, on_imu_gyro, on_imu_mag, on_imu_baro); +} + diff --git a/sw/airborne/booz/test/booz_test_vfilter.c b/sw/airborne/booz/test/booz_test_vfilter.c new file mode 100644 index 0000000000..39d2cfc299 --- /dev/null +++ b/sw/airborne/booz/test/booz_test_vfilter.c @@ -0,0 +1,85 @@ +#include "std.h" +#include "init_hw.h" +#include "interrupt_hw.h" +#include "sys_time.h" + +#include "uart.h" +#include "messages.h" +#include "downlink.h" + +#include "booz_imu.h" +#include "tl_vfilter.h" + +static inline void main_init(void); +static inline void main_periodic(void); +static inline void main_event(void); + +uint32_t t0, t1, diff; + +int main( void ) { + main_init(); + while (1) { + if (sys_time_periodic()) + main_periodic(); + main_event(); + } + return 0; +} + + +static inline void main_init(void) { + hw_init(); + + sys_time_init(); + + uart1_init_tx(); + + booz_imu_init(); + + tl_vf_init(0., 0., 0.); + + int_enable(); +} + + +static inline void main_periodic(void) { + booz_imu_periodic(); + RunOnceEvery(250, DOWNLINK_SEND_BOOT(&cpu_time_sec)); +} + + +static inline void on_imu_gyro_available(void) { + RunOnceEvery(10, { \ + DOWNLINK_SEND_IMU_ACCEL(&imu_accel[AXIS_X], &imu_accel[AXIS_Y], &imu_accel[AXIS_Z]); \ + DOWNLINK_SEND_TL_KALM_V_COV(&tl_vf_P[0][0], &tl_vf_P[1][1], &tl_vf_P[2][2]); \ + }); + // t0 = T0TC; + tl_vf_predict(imu_accel[AXIS_Z]); + // t1 = T0TC; + // diff = t1 - t0; + // DOWNLINK_SEND_TIME(&diff); + DOWNLINK_SEND_TL_KALM_V_STATE(&tl_vf_z, &tl_vf_zdot, &tl_vf_bias, &tl_vf_z_meas); +} + +static inline void on_imu_mag_available(void) { +} + +static inline void on_imu_baro_available(void) { + float pressure = 0.25 * imu_pressure_raw; + static float ground_pressure = 0.; + if (ground_pressure == 0) ground_pressure = pressure; + float z_baro = (ground_pressure - pressure)*0.084; + DOWNLINK_SEND_TL_IMU_PRESSURE(&pressure); + DOWNLINK_SEND_TL_IMU_RANGEMETER(&z_baro); + // t0 = T0TC; + tl_vf_update(-z_baro); + // t1 = T0TC; + // diff = t1 - t0; + // DOWNLINK_SEND_TIME(&diff); + // t0 = t1; +} + +static inline void main_event(void) { + BoozImuEvent(on_imu_gyro_available, on_imu_mag_available, on_imu_baro_available); +} +