diff --git a/sw/airborne/ahrs_comp_filter.c b/sw/airborne/ahrs_comp_filter.c deleted file mode 100644 index 8d7adb44d0..0000000000 --- a/sw/airborne/ahrs_comp_filter.c +++ /dev/null @@ -1,117 +0,0 @@ -/* - * $Id$ - * - * Copyright (C) 2008 Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ - -#include "booz_ahrs.h" - -#include - -#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/ahrs_comp_filter.h b/sw/airborne/ahrs_comp_filter.h deleted file mode 100644 index 7f3f66a7ef..0000000000 --- a/sw/airborne/ahrs_comp_filter.h +++ /dev/null @@ -1,32 +0,0 @@ -/* - * $Id$ - * - * Copyright (C) 2008 Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ - -#ifndef AHRS_COMP_FLT_H -#define AHRS_COMP_FLT_H - -extern FLOAT_T comp_filter_int_phi; -extern FLOAT_T comp_filter_int_theta; -extern FLOAT_T comp_filter_int_psi; - -#endif /* AHRS_COMP_FLT_H */ diff --git a/sw/airborne/ahrs_multitilt.c b/sw/airborne/ahrs_multitilt.c deleted file mode 100644 index 53c8e07a5e..0000000000 --- a/sw/airborne/ahrs_multitilt.c +++ /dev/null @@ -1,162 +0,0 @@ -#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/ahrs_multitilt.h b/sw/airborne/ahrs_multitilt.h deleted file mode 100644 index d8f69ef527..0000000000 --- a/sw/airborne/ahrs_multitilt.h +++ /dev/null @@ -1,38 +0,0 @@ -/* - * $Id$ - * - * Copyright (C) 2008 Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ - -#ifndef AHRS_MULTITILT_H -#define AHRS_MULTITILT_H - -#include "std.h" - - -extern float mtt_P_phi[2][2]; - -extern float mtt_P_theta[2][2]; - -extern float mtt_P_psi[2][2]; - - -#endif /* AHRS_MULTITILT_H */ diff --git a/sw/airborne/ahrs_quat_fast_ekf.h b/sw/airborne/ahrs_quat_fast_ekf.h deleted file mode 100644 index ec26797719..0000000000 --- a/sw/airborne/ahrs_quat_fast_ekf.h +++ /dev/null @@ -1,23 +0,0 @@ -#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/ahrs_quat_fast_ekf_utils.h b/sw/airborne/ahrs_quat_fast_ekf_utils.h deleted file mode 100644 index c9dd352707..0000000000 --- a/sw/airborne/ahrs_quat_fast_ekf_utils.h +++ /dev/null @@ -1,172 +0,0 @@ -#ifndef AHRS_QUAT_FAST_EKF_UTILS_H -#define AHRS_QUAT__FAST_EKF_UTILS_H - -#include "ahrs_quat_fast_ekf.h" - -/* - * Compute the five elements of the DCM that we use for our - * rotations and Jacobians. This is used by several other functions - * to speedup their computations. - */ -#define AFE_DCM_OF_QUAT() { \ - afe_dcm00 = 1.0-2*(afe_q2*afe_q2 + afe_q3*afe_q3); \ - afe_dcm01 = 2*(afe_q1*afe_q2 + afe_q0*afe_q3); \ - afe_dcm02 = 2*(afe_q1*afe_q3 - afe_q0*afe_q2); \ - afe_dcm12 = 2*(afe_q2*afe_q3 + afe_q0*afe_q1); \ - afe_dcm22 = 1.0-2*(afe_q1*afe_q1 + afe_q2*afe_q2); \ - } -/* - * Compute Euler angles from our DCM. - */ -#define AFE_PHI_OF_DCM() { booz_ahrs_phi = atan2( afe_dcm12, afe_dcm22 );} -#define AFE_THETA_OF_DCM() { booz_ahrs_theta = -asin( afe_dcm02 );} -#define AFE_PSI_OF_DCM() { booz_ahrs_psi = atan2( afe_dcm01, afe_dcm00 );} -#define AFE_EULER_OF_DCM() { AFE_PHI_OF_DCM(); AFE_THETA_OF_DCM(); AFE_PSI_OF_DCM()} - -/* - * initialise the quaternion from the set of eulers - */ -#define AFE_QUAT_OF_EULER() { \ - const FLOAT_T phi2 = booz_ahrs_phi / 2.0; \ - const FLOAT_T theta2 = booz_ahrs_theta / 2.0; \ - const FLOAT_T psi2 = booz_ahrs_psi / 2.0; \ - \ - const FLOAT_T sinphi2 = sin( phi2 ); \ - const FLOAT_T cosphi2 = cos( phi2 ); \ - \ - const FLOAT_T sintheta2 = sin( theta2 ); \ - const FLOAT_T costheta2 = cos( theta2 ); \ - \ - const FLOAT_T sinpsi2 = sin( psi2 ); \ - const FLOAT_T cospsi2 = cos( psi2 ); \ - \ - afe_q0 = cosphi2 * costheta2 * cospsi2 + sinphi2 * sintheta2 * sinpsi2; \ - afe_q1 = -cosphi2 * sintheta2 * sinpsi2 + sinphi2 * costheta2 * cospsi2; \ - afe_q2 = cosphi2 * sintheta2 * cospsi2 + sinphi2 * costheta2 * sinpsi2; \ - afe_q3 = cosphi2 * costheta2 * sinpsi2 - sinphi2 * sintheta2 * cospsi2; \ - } - -/* - * normalize quaternion - */ -#define AFE_NORM_QUAT() { \ - FLOAT_T mag = afe_q0*afe_q0 + afe_q1*afe_q1 \ - + afe_q2*afe_q2 + afe_q3*afe_q3; \ - mag = sqrt( mag ); \ - afe_q0 /= mag; \ - afe_q1 /= mag; \ - afe_q2 /= mag; \ - afe_q3 /= mag; \ - } - - - - -/* - * Compute the Jacobian of the measurements to the system states. - */ -#define AFE_COMPUTE_H_PHI() { \ - const FLOAT_T phi_err = 2 / (afe_dcm22*afe_dcm22 + afe_dcm12*afe_dcm12); \ - afe_H[0] = (afe_q1 * afe_dcm22) * phi_err; \ - afe_H[1] = (afe_q0 * afe_dcm22 + 2 * afe_q1 * afe_dcm12) * phi_err; \ - afe_H[2] = (afe_q3 * afe_dcm22 + 2 * afe_q2 * afe_dcm12) * phi_err; \ - afe_H[3] = (afe_q2 * afe_dcm22) * phi_err; \ -} - -#define AFE_COMPUTE_H_THETA() { \ - const FLOAT_T theta_err = -2 / sqrt( 1 - afe_dcm02*afe_dcm02 ); \ - afe_H[0] = -afe_q2 * theta_err; \ - afe_H[1] = afe_q3 * theta_err; \ - afe_H[2] = -afe_q0 * theta_err; \ - afe_H[3] = afe_q1 * theta_err; \ - } - -#define AFE_COMPUTE_H_PSI() { \ - const FLOAT_T psi_err = 2 / (afe_dcm00*afe_dcm00 + afe_dcm01*afe_dcm01); \ - afe_H[0] = (afe_q3 * afe_dcm00) * psi_err; \ - afe_H[1] = (afe_q2 * afe_dcm00) * psi_err; \ - afe_H[2] = (afe_q1 * afe_dcm00 + 2 * afe_q2 * afe_dcm01) * psi_err; \ - afe_H[3] = (afe_q0 * afe_dcm00 + 2 * afe_q3 * afe_dcm01) * psi_err; \ - } - - -/* convert sensor reading into euler angle measurement */ -static inline FLOAT_T afe_phi_of_accel( const FLOAT_T* accel ) { - return atan2(accel[AXIS_Y], accel[AXIS_Z]); -} - -static inline FLOAT_T afe_theta_of_accel( const FLOAT_T* accel) { - FLOAT_T g2 = - accel[AXIS_X]*accel[AXIS_X] + - accel[AXIS_Y]*accel[AXIS_Y] + - accel[AXIS_Z]*accel[AXIS_Z]; - return -asin( accel[AXIS_X] / sqrt( g2 ) ); -} -/* - * The rotation matrix to rotate from NED frame to body frame without - * rotating in the yaw axis is: - * - * [ 1 0 0 ] [ cos(Theta) 0 -sin(Theta) ] - * [ 0 cos(Phi) sin(Phi) ] [ 0 1 0 ] - * [ 0 -sin(Phi) cos(Phi) ] [ sin(Theta) 0 cos(Theta) ] - * - * This expands to: - * - * [ cos(Theta) 0 -sin(Theta) ] - * [ sin(Phi)*sin(Theta) cos(Phi) sin(Phi)*cos(Theta)] - * [ cos(Phi)*sin(Theta) -sin(Phi) cos(Phi)*cos(Theta)] - * - * However, to untilt the compass reading, we need to use the - * transpose of this matrix. - * - * [ cos(Theta) sin(Phi)*sin(Theta) cos(Phi)*sin(Theta) ] - * [ 0 cos(Phi) -sin(Phi) ] - * [ -sin(Theta) sin(Phi)*cos(Theta) cos(Phi)*cos(Theta) ] - * - * Additionally, - * since we already have the DCM computed for our current attitude, - * we can short cut all of the trig. substituting - * in from the definition of euler2quat and quat2euler, we have: - * - * [ cos(Theta) -dcm12*dcm02 -dcm22*dcm02 ] - * [ 0 dcm22 -dcm12 ] - * [ dcm02 dcm12 dcm22 ] - * - */ -static inline FLOAT_T afe_psi_of_mag( const int16_t* mag ) { - const FLOAT_T ctheta = cos( booz_ahrs_theta ); -#if 0 - const FLOAT_T mn = ctheta * mag[0] - - (afe_dcm12 * mag[1] + afe_dcm22 * mag[2]) * afe_dcm02 / ctheta; - - const FLOAT_T me = - (afe_dcm22 * mag[1] - afe_dcm12 * mag[2]) / ctheta; -#else - const FLOAT_T stheta = sin( booz_ahrs_theta ); - const FLOAT_T cphi = cos( booz_ahrs_phi ); - const FLOAT_T sphi = sin( booz_ahrs_phi ); - - const FLOAT_T mn = - ctheta* mag[0]+ - sphi*stheta* mag[1]+ - cphi*stheta* mag[2]; - const FLOAT_T me = - 0* mag[0]+ - cphi* mag[1]+ - -sphi* mag[2]; -#endif - - const FLOAT_T psi = -atan2( me, mn ); - return psi; -} - -#define AFE_WARP(x, b) { \ - while( x < -b ) \ - x += 2 * b; \ - while( x > b ) \ - x -= 2 * b; \ - } - - - -#endif /* AHRS_QUAT_FAST_EKF_UTILS_H */ diff --git a/sw/airborne/arm7/actuators_buss_twi_blmc_hw.c b/sw/airborne/arm7/actuators_buss_twi_blmc_hw.c deleted file mode 100644 index 341f61f247..0000000000 --- a/sw/airborne/arm7/actuators_buss_twi_blmc_hw.c +++ /dev/null @@ -1,18 +0,0 @@ -#include "actuators.h" - -uint8_t buss_twi_blmc_motor_power[BUSS_TWI_BLMC_NB]; -volatile bool_t buss_twi_blmc_status; -volatile uint8_t buss_twi_blmc_nb_err; -volatile bool_t buss_twi_blmc_i2c_done; -volatile uint8_t buss_twi_blmc_idx; - -const uint8_t buss_twi_blmc_addr[BUSS_TWI_BLMC_NB] = { 0x52, 0x54, 0x56, 0x58 }; - -void actuators_init ( void ) { - uint8_t i; - for (i=0; i -#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/arm7/booz_imu_hw.c b/sw/airborne/arm7/booz_imu_hw.c deleted file mode 100644 index 2047f853c6..0000000000 --- a/sw/airborne/arm7/booz_imu_hw.c +++ /dev/null @@ -1,105 +0,0 @@ -#include "booz_imu.h" - -/* SSPCR0 settings */ -#define SSP_DDS 0x07 << 0 /* data size : 8 bits */ -#define SSP_FRF 0x00 << 4 /* frame format : SPI */ -#define SSP_CPOL 0x00 << 6 /* clock polarity : data captured on first clock transition */ -#define SSP_CPHA 0x00 << 7 /* clock phase : SCK idles low */ -#define SSP_SCR 0x0F << 8 /* serial clock rate : divide by 16 */ - -/* SSPCR1 settings */ -#define SSP_LBM 0x00 << 0 /* loopback mode : disabled */ -#define SSP_SSE 0x00 << 1 /* SSP enable : disabled */ -#define SSP_MS 0x00 << 2 /* master slave mode : master */ -#define SSP_SOD 0x00 << 3 /* slave output disable : don't care when master */ - -static void SPI1_ISR(void) __attribute__((naked)); - -void booz_imu_hw_init(void) { - - /* setup pins for SSP (SCK, MISO, MOSI) */ - PINSEL1 |= 2 << 2 | 2 << 4 | 2 << 6; - - /* setup SSP */ - SSPCR0 = SSP_DDS | SSP_FRF | SSP_CPOL | SSP_CPHA | SSP_SCR; - SSPCR1 = SSP_LBM | SSP_MS | SSP_SOD; - SSPCPSR = 0x2; - - /* initialize interrupt vector */ - VICIntSelect &= ~VIC_BIT(VIC_SPI1); // SPI1 selected as IRQ - VICIntEnable = VIC_BIT(VIC_SPI1); // SPI1 interrupt enabled - VICVectCntl7 = VIC_ENABLE | VIC_SPI1; - VICVectAddr7 = (uint32_t)SPI1_ISR; // address of the ISR - -} - - -static inline bool_t isr_try_mag(void) { - switch (micromag_status) { - case MM_IDLE : - MmSendReq(); - return TRUE; - case MM_GOT_EOC: - MmReadRes(); - return TRUE; - } - return FALSE; -} - -static inline bool_t isr_try_baro(void) { -#ifndef SCP1000_NO_EINT - switch (scp1000_status) { - case SCP1000_STA_STOPPED: - Scp1000SendConfig(); - return TRUE; - case SCP1000_STA_GOT_EOC: - Scp1000Read(); - return TRUE; - } -#else - if (scp1000_status == SCP1000_STA_STOPPED) { - Scp1000SendConfig(); - return TRUE; - } - else if (scp1000_status == SCP1000_STA_WAIT_EOC && Scp1000DataReady()) { - scp1000_status = SCP1000_STA_GOT_EOC; - Scp1000Read(); - return TRUE; - } -#endif - return FALSE; -} - -static void SPI1_ISR(void) { - ISR_ENTRY(); - - switch (booz_imu_status) { - - case BOOZ_IMU_STA_MEASURING_GYRO: - Max1167OnSpiInt(); - /* we now have a gyro reading */ - if (isr_try_mag()) - booz_imu_status = BOOZ_IMU_STA_MEASURING_MAG; - else if (isr_try_baro()) - booz_imu_status = BOOZ_IMU_STA_MEASURING_BARO; - else - booz_imu_status = BOOZ_IMU_STA_IDLE; - break; - - case BOOZ_IMU_STA_MEASURING_MAG: - MmOnSpiIt(); - if (isr_try_baro()) - booz_imu_status = BOOZ_IMU_STA_MEASURING_BARO; - else - booz_imu_status = BOOZ_IMU_STA_IDLE; - break; - - case BOOZ_IMU_STA_MEASURING_BARO: - Scp1000OnSpiIt(); - booz_imu_status = BOOZ_IMU_STA_IDLE; - break; - } - - VICVectAddr = 0x00000000; /* clear this interrupt from the VIC */ - ISR_EXIT(); -} diff --git a/sw/airborne/arm7/booz_imu_hw.h b/sw/airborne/arm7/booz_imu_hw.h deleted file mode 100644 index e9f424d7e9..0000000000 --- a/sw/airborne/arm7/booz_imu_hw.h +++ /dev/null @@ -1,30 +0,0 @@ -/* - * $Id$ - * - * Copyright (C) 2008 Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ - -#ifndef BOOZ_IMU_HW_H -#define BOOZ_IMU_HW_H - - - -#endif /* BOOZ_IMU_HW_H */ diff --git a/sw/airborne/arm7/booz_link_mcu_hw.c b/sw/airborne/arm7/booz_link_mcu_hw.c deleted file mode 100644 index 8457ccd537..0000000000 --- a/sw/airborne/arm7/booz_link_mcu_hw.c +++ /dev/null @@ -1,261 +0,0 @@ -/* - * $Id$ - * - * Copyright (C) 2008 Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ - -#include "booz_link_mcu.h" - - -uint8_t* link_mcu_tx_buf; -uint8_t* link_mcu_rx_buf; - - - - - -#ifdef BOOZ_FILTER_MCU - -volatile uint8_t link_mcu_status; -volatile uint8_t link_mcu_buf_idx; - -uint32_t it_count; -uint16_t foo_debug; - - -/* - wiring on IMU_V3 - P0_4 SCK0 - P0_5 MISO0 - P0_6 MOSI0 - P0_7 SSEL0 - P1_24 DRDY -*/ - -#include "armVIC.h" - - -void SPI0_ISR(void) __attribute__((naked)); - - -/* */ -#define S0SPCR_BitEnable 0 << 2 /* enable more than 8 bits transferts */ -#define S0SPCR_CPHA 1 << 3 /* data sampled on first edge */ -#define S0SPCR_CPOL 0 << 4 /* clock idles high */ -#define S0SPCR_MSTR 1 << 5 /* master mode */ -#define S0SPCR_LSBF 0 << 6 /* MSB first */ -#define S0SPCR_SPIE 0 << 7 /* SPI interrupt disabled */ -#define S0SPCR_BITS 0 << 8 /* 16 bits transferts */ - -#define S0SPCR_VAL ( S0SPCR_BitEnable | \ - S0SPCR_CPHA | \ - S0SPCR_CPOL | \ - S0SPCR_MSTR | \ - S0SPCR_LSBF | \ - S0SPCR_SPIE | \ - S0SPCR_BITS ) - - - -#define SPI0_EnableInterrupt() SetBit(S0SPCR, 7) -#define SPI0_ClearInterrupt() SetBit(S0SPINT, SPI0IF) - -void booz_link_mcu_hw_init ( void ) { - - /* SS pin is output */ - SetBit(IO0DIR, SS_PIN); - SPI0_UnselectSlave(); - - /* init SPI0 */ - /* setup pins for sck, miso, mosi */ - PINSEL0 |= 1<<8 | 1<<10 | 1<<12; - /* configure SPI : see above */ - S0SPCR = S0SPCR_VAL; - /* setup SPI clock rate ~ 450Khz with 15MHz VPB */ - S0SPCCR = 0x40; - - /* initialize interrupt vector */ - VICIntSelect &= ~VIC_BIT(VIC_SPI0); // SPI0 selected as IRQ - VICIntEnable = VIC_BIT(VIC_SPI0); // SPI0 interrupt enabled - VICVectCntl3 = VIC_ENABLE | VIC_SPI0; - VICVectAddr3 = (uint32_t)SPI0_ISR; // address of the ISR - - /* clear all potentially pending interrupts */ - uint16_t foo1 __attribute__((unused)) = S0SPSR; - uint16_t foo2 __attribute__((unused)) = S0SPDR; - SPI0_ClearInterrupt(); - SPI0_EnableInterrupt(); - - link_mcu_status = LINK_MCU_STATUS_IDLE; - link_mcu_buf_idx = 0; - - link_mcu_tx_buf = (uint8_t*)&inter_mcu_state; - link_mcu_rx_buf = (uint8_t*)&booz_link_mcu_state_unused; - -} - - -void SPI0_ISR(void) { - ISR_ENTRY(); - /* read status register */ - uint16_t foo __attribute__((unused)) = S0SPSR; - foo_debug = foo; - it_count++; - - /* read_data_register */ - link_mcu_rx_buf[link_mcu_buf_idx] = S0SPDR; - link_mcu_buf_idx++; - if (link_mcu_buf_idx < LINK_MCU_BUF_LEN) { - S0SPDR = link_mcu_tx_buf[link_mcu_buf_idx]; - } - else { - SPI0_UnselectSlave(); - link_mcu_status = LINK_MCU_STATUS_IDLE; - } - - SPI0_ClearInterrupt(); - /* clear this interrupt from the VIC */ - VICVectAddr = 0x00000000; - ISR_EXIT(); -} - -#endif /* BOOZ_FILTER_MCU */ - - - - - - - - - - - - - - -#ifdef BOOZ_CONTROLLER_MCU - -/* - IMU connected to SSP ( aka SPI1 ) as master - Controller is slave -*/ - -volatile uint32_t it_cnt; -volatile uint32_t rx_it_cnt; -volatile uint32_t ti_it_cnt; - -#include "LPC21xx.h" -#include "interrupt_hw.h" - -volatile uint8_t link_mcu_rx_buf_idx; -volatile uint8_t link_mcu_tx_buf_idx; - -#define LinkMcuReceive() { \ - while (bit_is_set(SSPSR, RNE)) { \ - if (link_mcu_rx_buf_idx < LINK_MCU_BUF_LEN) { \ - link_mcu_rx_buf[link_mcu_rx_buf_idx] = SSPDR; \ - link_mcu_rx_buf_idx++; \ - if (link_mcu_rx_buf_idx == LINK_MCU_BUF_LEN) \ - booz_link_mcu_status = BOOZ_LINK_MCU_DATA_AVAILABLE; \ - } \ - else { \ - uint8_t foo __attribute__ ((unused)); \ - foo = SSPDR; \ - } \ - } \ - } - - -static void SSP_ISR(void) __attribute__((naked)); - -/* SSPCR0 settings */ -#define SSP_DDS 0x07 << 0 /* data size : 8 bits */ -#define SSP_FRF 0x00 << 4 /* frame format : SPI */ -#define SSP_CPOL 0x00 << 6 /* clock polarity : first clock transition */ -#define SSP_CPHA 0x01 << 7 /* clock phase : SCK idles high */ -#define SSP_SCR 0x01 << 8 /* serial clock rate : divide by 2 */ - -/* SSPCR1 settings */ -#define SSP_LBM 0x00 << 0 /* loopback mode : disabled */ -#define SSP_SSE 0x00 << 1 /* SSP enable : disabled */ -#define SSP_MS 0x01 << 2 /* master slave mode : slave */ -#define SSP_SOD 0x00 << 3 /* slave output disable : no */ - -#define SSPCR0_VAL (SSP_DDS | SSP_FRF | SSP_CPOL | SSP_CPHA | SSP_SCR ) -#define SSPCR1_VAL (SSP_LBM | SSP_SSE | SSP_MS | SSP_SOD ) - -#define SSP_PINSEL1_SCK (2<<2) -#define SSP_PINSEL1_MISO (2<<4) -#define SSP_PINSEL1_MOSI (2<<6) -#define SSP_PINSEL1_SSEL (2<<8) - -void booz_link_mcu_hw_init ( void ) { - - /* setup pins for SSP (SCK, MISO, MOSI, SSEL) */ - PINSEL1 |= SSP_PINSEL1_SCK | SSP_PINSEL1_MISO | - SSP_PINSEL1_MOSI | SSP_PINSEL1_SSEL ; - - /* setup SSP */ - SSPCR0 = SSPCR0_VAL;; - SSPCR1 = SSPCR1_VAL; - SSPCPSR = 0x10; - - /* initialize interrupt vector */ - VICIntSelect &= ~VIC_BIT( VIC_SPI1 ); /* SPI1 selected as IRQ */ - VICIntEnable = VIC_BIT( VIC_SPI1 ); /* enable it */ - VICVectCntl9 = VIC_ENABLE | VIC_SPI1; - VICVectAddr9 = (uint32_t)SSP_ISR; /* address of the ISR */ - - link_mcu_rx_buf = (uint8_t*)&inter_mcu_state; - link_mcu_tx_buf = (uint8_t*)&booz_link_mcu_state_unused; - - BoozLinkMcuHwRestart(); -} - -static void SSP_ISR(void) { - ISR_ENTRY(); - - it_cnt++; - - /* Rx half full */ - if (bit_is_set(SSPMIS, RXMIS)) { - rx_it_cnt++; - /* LinkMcuTransmit(); */ - LinkMcuReceive(); - SSP_EnableRti(); - } - - /* Rx timeout */ - if ( bit_is_set(SSPMIS, RTMIS)) { - ti_it_cnt++; - LinkMcuReceive(); - SSP_ClearRti(); - SSP_DisableRti(); - SSP_Disable(); - booz_link_mcu_status = BOOZ_LINK_MCU_DATA_AVAILABLE; - } - - VICVectAddr = 0x00000000; /* clear this interrupt from the VIC */ - ISR_EXIT(); -} - -#endif /* BOOZ_CONTROLLER_MCU */ diff --git a/sw/airborne/arm7/booz_link_mcu_hw.h b/sw/airborne/arm7/booz_link_mcu_hw.h deleted file mode 100644 index 6eca5395f8..0000000000 --- a/sw/airborne/arm7/booz_link_mcu_hw.h +++ /dev/null @@ -1,99 +0,0 @@ -/* - * $Id$ - * - * Copyright (C) 2008 Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ - -#ifndef BOOZ_LINK_MCU_HW_H -#define BOOZ_LINK_MCU_HW_H - -#include "LPC21xx.h" -#include "booz_debug.h" - -#define LINK_MCU_STATUS_IDLE 0 -#define LINK_MCU_STATUS_BUSY 1 - -#define LINK_IMU_ERR_OVERRUN 0 - -extern volatile uint8_t link_mcu_status; -extern uint8_t* link_mcu_tx_buf; -extern uint8_t* link_mcu_rx_buf; -#define LINK_MCU_BUF_LEN (sizeof(struct booz_inter_mcu_state)/sizeof(uint8_t)) - -#ifdef BOOZ_FILTER_MCU - -extern volatile uint8_t link_mcu_buf_idx; - -#define SS_PIN 7 -#define SPI0_SelectSlave() SetBit(IO0CLR, SS_PIN) -#define SPI0_UnselectSlave() SetBit(IO0SET, SS_PIN) - -#define BoozLinkMcuHwSend() { \ - ASSERT((link_mcu_status == LINK_MCU_STATUS_IDLE), \ - DEBUG_LINK_MCU_IMU, LINK_IMU_ERR_OVERRUN); \ - if (link_mcu_status != LINK_MCU_STATUS_IDLE) return; \ - SPI0_SelectSlave(); \ - link_mcu_status = LINK_MCU_STATUS_BUSY; \ - link_mcu_buf_idx = 0; \ - S0SPDR = link_mcu_tx_buf[0]; \ - } - -#endif /* BOOZ_FILTER_MCU */ - - -#ifdef BOOZ_CONTROLLER_MCU - -extern volatile uint8_t link_mcu_rx_buf_idx; -extern volatile uint8_t link_mcu_tx_buf_idx; - -#define SSP_Enable() SetBit(SSPCR1, SSE); -#define SSP_Disable() ClearBit(SSPCR1, SSE); -#define SSP_EnableRxi() SetBit(SSPIMSC, RXIM) -#define SSP_DisableRxi() ClearBit(SSPIMSC, RXIM) -#define SSP_EnableTxi() SetBit(SSPIMSC, TXIM) -#define SSP_DisableTxi() ClearBit(SSPIMSC, TXIM) -#define SSP_EnableRti() SetBit(SSPIMSC, RTIM); -#define SSP_DisableRti() ClearBit(SSPIMSC, RTIM); -#define SSP_ClearRti() SetBit(SSPICR, RTIC); - -#define LinkMcuTransmit() { \ - while (link_mcu_tx_buf_idx < LINK_MCU_BUF_LEN \ - && bit_is_set(SSPSR, TNF)) { \ - SSPDR = link_mcu_tx_buf[link_mcu_tx_buf_idx]; \ - link_mcu_tx_buf_idx++; \ - } \ - if (link_mcu_tx_buf_idx == LINK_MCU_BUF_LEN) \ - SSP_DisableRxi(); \ - } - -#define BoozLinkMcuHwRestart() { \ - link_mcu_rx_buf_idx = 0; \ - link_mcu_tx_buf_idx = 0; \ - SSP_EnableRxi(); \ - SSP_Enable(); \ - /* LinkMcuTransmit(); */ \ - } - - - -#endif /* BOOZ_CONTROLLER_MCU */ - -#endif /* BOOZ_LINK_MCU_HW_H */ diff --git a/sw/airborne/arm7/mb_buss_twi_controller.c b/sw/airborne/arm7/mb_buss_twi_controller.c deleted file mode 100644 index b9f6aac7ea..0000000000 --- a/sw/airborne/arm7/mb_buss_twi_controller.c +++ /dev/null @@ -1,36 +0,0 @@ -#include "mb_buss_twi_controller.h" - -#include - -#include "i2c.h" - -uint8_t mb_buss_twi_command; - -uint8_t mb_buss_twi_nb_overun; -uint8_t mb_buss_twi_i2c_done; - - -#define MB_BUSS_TWI_CONTROLLER_MAX_CMD 255 -/* - Slave address - front = 0x52 - back = 0x54 - right = 0x56 - left = 0x58 -*/ -#define MB_BUSS_TWI_CONTROLLER_ADDR 0x52 - -void mb_buss_twi_controller_init(void) { - mb_buss_twi_nb_overun = 0; - mb_buss_twi_i2c_done = TRUE; -} - -void mb_buss_twi_controller_set( float throttle ) { - if (mb_buss_twi_i2c_done) { - mb_buss_twi_command = throttle * MB_BUSS_TWI_CONTROLLER_MAX_CMD; - i2c_buf[0] = mb_buss_twi_command; - i2c_transmit(MB_BUSS_TWI_CONTROLLER_ADDR, 1, &mb_buss_twi_i2c_done); - } - else - mb_buss_twi_nb_overun++; -} diff --git a/sw/airborne/arm7/mb_buss_twi_controller.h b/sw/airborne/arm7/mb_buss_twi_controller.h deleted file mode 100644 index c14cfcb69e..0000000000 --- a/sw/airborne/arm7/mb_buss_twi_controller.h +++ /dev/null @@ -1,12 +0,0 @@ -#ifndef MB_BUSS_TWI_CONTROLLER_H -#define MB_BUSS_TWI_CONTROLLER_H - -#include "std.h" - -extern void mb_buss_twi_controller_init(void); - -extern void mb_buss_twi_controller_set( float throttle ); - -#endif /* MB_BUSS_TWI_CONTROLLER_H */ - - diff --git a/sw/airborne/arm7/mb_current.c b/sw/airborne/arm7/mb_current.c deleted file mode 100644 index 5e3046cde4..0000000000 --- a/sw/airborne/arm7/mb_current.c +++ /dev/null @@ -1,17 +0,0 @@ -#include "mb_current.h" - -#include "adc.h" - -static struct adc_buf mb_current_buf; - -float mb_current_amp; - -void mb_current_init(void) { - adc_buf_channel(4, &mb_current_buf, 16); - -} - - -void mb_current_periodic(void) { - mb_current_amp = (float)mb_current_buf.sum * 0.00113607 - 2.8202; -} diff --git a/sw/airborne/arm7/mb_current.h b/sw/airborne/arm7/mb_current.h deleted file mode 100644 index cd911455ea..0000000000 --- a/sw/airborne/arm7/mb_current.h +++ /dev/null @@ -1,11 +0,0 @@ -#ifndef MB_CURRENT_H -#define MB_CURRENT_H - -#include "std.h" - -extern void mb_current_init(void); -extern void mb_current_periodic(void); - -extern float mb_current_amp; - -#endif /* MB_CURRENT_H */ diff --git a/sw/airborne/arm7/mb_modes.c b/sw/airborne/arm7/mb_modes.c deleted file mode 100644 index 29defc90c6..0000000000 --- a/sw/airborne/arm7/mb_modes.c +++ /dev/null @@ -1,93 +0,0 @@ -#include "mb_modes.h" - -#include "adc.h" -#include "sys_time.h" - - -uint8_t mb_modes_mode; -float mb_modes_throttle; - -float mb_modes_last_change_time; - -float mb_modes_ramp_duration; - -float mb_modes_step_low_throttle; -float mb_modes_step_high_throttle; -float mb_modes_step_duration; - -static void mb_modes_manual( void ); -static void mb_modes_ramp( void ); -static void mb_modes_step( void ); -static void mb_modes_prbs( void ); - - -static struct adc_buf mb_modes_adc_buf; /* manual mode */ - -void mb_mode_init(void) { - adc_buf_channel(1, &mb_modes_adc_buf, 16); - mb_modes_mode = MB_MODES_IDLE; - mb_modes_throttle = 0.; - - mb_modes_ramp_duration = 40; - - mb_modes_step_low_throttle = 0.6; - mb_modes_step_high_throttle = 0.7; - mb_modes_step_duration = 1.; - -} - -void mb_mode_event(void) {} - -void mb_mode_periodic(void) { - switch (mb_modes_mode) { - case MB_MODES_IDLE : - mb_modes_throttle = 0.; - break; - case MB_MODES_MANUAL : - mb_modes_manual(); - break; - case MB_MODES_RAMP : - mb_modes_ramp(); - break; - case MB_MODES_STEP : - mb_modes_step(); - break; - default: - mb_modes_throttle = 0.; - } -} - - -static void mb_modes_manual( void ) { - uint16_t poti = mb_modes_adc_buf.sum; - mb_modes_throttle = (float)poti/(16.*1024.); -} - -static void mb_modes_ramp( void ) { - float now = GET_CUR_TIME_FLOAT(); - float elapsed = now - mb_modes_last_change_time; - if ( elapsed < mb_modes_ramp_duration) - mb_modes_throttle = elapsed/mb_modes_ramp_duration; - else if ( elapsed < 2 * mb_modes_ramp_duration) - mb_modes_throttle = 2 - elapsed/mb_modes_ramp_duration; - else { - mb_modes_last_change_time = now; - mb_modes_throttle = 0.; - } -} - - -static void mb_modes_step( void ) { - float now = GET_CUR_TIME_FLOAT(); - float elapsed = now - mb_modes_last_change_time; - if ( elapsed < mb_modes_step_duration) - mb_modes_throttle = mb_modes_step_low_throttle; - else if ( elapsed < 2 * mb_modes_step_duration) - mb_modes_throttle = mb_modes_step_high_throttle; - else { - mb_modes_last_change_time = now; - mb_modes_throttle = mb_modes_step_low_throttle; - } -} - - diff --git a/sw/airborne/arm7/mb_modes.h b/sw/airborne/arm7/mb_modes.h deleted file mode 100644 index 9f73263817..0000000000 --- a/sw/airborne/arm7/mb_modes.h +++ /dev/null @@ -1,34 +0,0 @@ -#ifndef MB_MODES_H -#define MB_MODES_H - -#include "std.h" - - -#define MB_MODES_IDLE 0 -#define MB_MODES_MANUAL 1 -#define MB_MODES_RAMP 2 -#define MB_MODES_STEP 3 -#define MB_MODES_PRBS 4 - -extern uint8_t mb_modes_mode; -extern float mb_modes_throttle; - -extern float mb_modes_last_change_time; - -extern float mb_modes_ramp_duration; - -extern float mb_modes_step_low_throttle; -extern float mb_modes_step_high_throttle; -extern float mb_modes_step_duration; - - -extern void mb_mode_init(void); -extern void mb_mode_event(void); -extern void mb_mode_periodic(void); - -#define mb_modes_SetMode(_val) { \ - mb_modes_mode = _val; \ - mb_modes_last_change_time = GET_CUR_TIME_FLOAT(); \ - } - -#endif /* MB_MODES_H */ diff --git a/sw/airborne/arm7/mb_scale.c b/sw/airborne/arm7/mb_scale.c deleted file mode 100644 index f5945b520b..0000000000 --- a/sw/airborne/arm7/mb_scale.c +++ /dev/null @@ -1,17 +0,0 @@ -#include "mb_scale.h" - -volatile uint32_t pulse_len; -float mb_scale_thrust; -float mb_scale_torque; - -void mb_scale_init ( void ) { - /* select pin for capture */ - ICP_PINSEL |= ICP_PINSEL_VAL << ICP_PINSEL_BIT; - /* enable capture 0.3 on falling edge + trigger interrupt */ - T0CCR |= TCCR_CR3_F | TCCR_CR3_I; -} - - -void mb_scale_periodic(void) { - mb_scale_thrust = 500. / 240500. * (pulse_len - 3073500); -} diff --git a/sw/airborne/arm7/mb_scale.h b/sw/airborne/arm7/mb_scale.h deleted file mode 100644 index 794291fbf4..0000000000 --- a/sw/airborne/arm7/mb_scale.h +++ /dev/null @@ -1,30 +0,0 @@ -#ifndef MB_SCALE_H -#define MB_SCALE_H - -/* P0.29 CAP0.3 */ -#define ICP_PINSEL PINSEL1 -#define ICP_PINSEL_VAL 0x02 -#define ICP_PINSEL_BIT 26 - -#include "led.h" - -extern volatile uint32_t pulse_len; -extern float mb_scale_thrust; -extern float mb_scale_torque; - -void mb_scale_init ( void ); -void mb_scale_periodic(void); - - - -#define MB_SCALE_ICP_ISR() { \ - static uint32_t last; \ - uint32_t now = T0CR3; \ - pulse_len = now - last; \ - last = now; \ - LED_TOGGLE(2); \ - } - - - -#endif /* MB_SCALE_H */ diff --git a/sw/airborne/arm7/mb_servo.c b/sw/airborne/arm7/mb_servo.c deleted file mode 100644 index 3dd0a67393..0000000000 --- a/sw/airborne/arm7/mb_servo.c +++ /dev/null @@ -1,66 +0,0 @@ -#include "mb_servo.h" - -#include "sys_time.h" -#define MY_NB_CLOCK_TIMER_PWM(time_us) SYS_TICS_OF_USEC(time_us) - -uint32_t mb_servo_max_pulse_ns, mb_servo_min_pulse_ns; - -void mb_servo_set_ns(uint32_t duration_ns); - -void mb_servo_init( void ) { - /* set P0.21 as PWM5 output */ - PINSEL1 |= (0X01 << 10); - /* enable and select the type of PWM channel */ - PWMPCR |= PWMPCR_ENA5; - /* set Match0 value (refresh rate) */ - PWMMR0 = MY_NB_CLOCK_TIMER_PWM(25000); - /* commit PWMMRx changes */ - PWMLER = PWMLER_LATCH0; - /* enable PWM timer in PWM mode */ - PWMTCR = PWMTCR_COUNTER_ENABLE | PWMTCR_PWM_ENABLE; - mb_servo_min_pulse_ns = MIN_SERVO_NS; - mb_servo_max_pulse_ns = MAX_SERVO_NS; -} - -void mb_servo_set_range( uint32_t min_pulse_ns, uint32_t max_pulse_ns ) { - mb_servo_min_pulse_ns = min_pulse_ns; - mb_servo_max_pulse_ns = max_pulse_ns; -} - -void mb_servo_set_us(uint32_t duration_us) { - /* set Match5 value (pulse duration )*/ - PWMMR5 = MY_NB_CLOCK_TIMER_PWM(duration_us); - /* commit PWMMRx changes */ - PWMLER = PWMLER_LATCH5; -} - -void mb_servo_set_ns(uint32_t duration_ns) { - /* set Match5 value (pulse duration )*/ - PWMMR5 = SYS_TICS_OF_NSEC(duration_ns); - /* commit PWMMRx changes */ - PWMLER = PWMLER_LATCH5; -} - -/* normalized throttle between 0. and 1. */ -void mb_servo_set(float throttle) { - uint32_t range = mb_servo_max_pulse_ns - mb_servo_min_pulse_ns; - uint32_t pulse_ns = mb_servo_min_pulse_ns + throttle * (range); - mb_servo_set_ns(pulse_ns); -} - -#define FOO_DELAY() { \ - uint32_t foo = 0; \ - while (foo<10000000) foo++; \ - } - - -/* arm the brushless controller */ - -void mb_servo_arm (void) { - mb_servo_set(0.); - FOO_DELAY(); - mb_servo_set(1.); - FOO_DELAY(); - mb_servo_set(0.); -} - diff --git a/sw/airborne/arm7/mb_servo.h b/sw/airborne/arm7/mb_servo.h deleted file mode 100644 index 26a8f2ef2b..0000000000 --- a/sw/airborne/arm7/mb_servo.h +++ /dev/null @@ -1,19 +0,0 @@ - -#ifndef MB_SERVO_H -#define MB_SERVO_H - -#include "std.h" - -#define MIN_SERVO_US 1000 -#define MAX_SERVO_US 2000 -#define MIN_SERVO_NS 1000000 -#define MAX_SERVO_NS 2000000 -#define RANGE_SERVO_US (MAX_SERVO_US - MIN_SERVO_US) - -void mb_servo_init( void ); -void mb_servo_set_range( uint32_t min_pulse_ns, uint32_t max_pulse_ns ); -void mb_servo_set_us(uint32_t duration_us); -//void mb_servo_set_ns(uint32_t duration_ns); -void mb_servo_set(float throttle); -void mb_servo_arm (void); -#endif /* MB_SERVO_H */ diff --git a/sw/airborne/arm7/mb_tacho.c b/sw/airborne/arm7/mb_tacho.c deleted file mode 100644 index 3e153ac2c2..0000000000 --- a/sw/airborne/arm7/mb_tacho.c +++ /dev/null @@ -1,55 +0,0 @@ -#include "mb_tacho.h" - -#include "LPC21xx.h" - -#include "interrupt_hw.h" - -volatile uint32_t mb_tacho_duration; -volatile uint8_t got_one_pulse; -volatile float mb_tacho_averaged; -volatile uint8_t mb_tacho_nb_pulse; - -/* INPUT CAPTURE CAP0.0 on P0.22*/ -#define MB_TACHO_PINSEL PINSEL1 -#define MB_TACHO_PINSEL_VAL 0x02 -#define MB_TACHO_PINSEL_BIT 12 - -void mb_tacho_init ( void ) { - /* select pin for capture */ - MB_TACHO_PINSEL |= MB_TACHO_PINSEL_VAL << MB_TACHO_PINSEL_BIT; - /* enable capture 0.2 on falling edge + trigger interrupt */ - T0CCR |= TCCR_CR0_F | TCCR_CR0_I; -} - -uint32_t mb_tacho_get_duration( void ) { - int_disable(); - uint32_t my_duration = 0; - if (got_one_pulse) { - my_duration = mb_tacho_duration; - } - got_one_pulse = FALSE; - int_enable(); - return my_duration; -} - -float mb_tacho_get_averaged( void ) { - int_disable(); - float ret; - float tacho; - const float tach_to_rpm = 15000000.*60./36.; - if (mb_tacho_nb_pulse) - tacho = mb_tacho_averaged / (float)mb_tacho_nb_pulse ; - else - tacho = 0.; - - if (tacho ==0) - ret = 0; - else - ret = tach_to_rpm/tacho; - - mb_tacho_averaged = 0.; - mb_tacho_nb_pulse = 0; - int_enable(); - - return ret; -} diff --git a/sw/airborne/arm7/mb_tacho.h b/sw/airborne/arm7/mb_tacho.h deleted file mode 100644 index 25f12860ed..0000000000 --- a/sw/airborne/arm7/mb_tacho.h +++ /dev/null @@ -1,26 +0,0 @@ -#ifndef MB_TACHO_H -#define MB_TACHO_H - -#include "std.h" - -extern void mb_tacho_init ( void ); -extern uint32_t mb_tacho_get_duration( void ); -extern float mb_tacho_get_averaged( void ); - -extern volatile uint32_t mb_tacho_duration; -extern volatile uint8_t got_one_pulse; -extern volatile float mb_tacho_averaged; -extern volatile uint8_t mb_tacho_nb_pulse; - -#define MB_TACHO_ISR() { \ - static uint32_t tmb_last; \ - uint32_t t_now = T0CR0; \ - uint32_t diff = t_now - tmb_last; \ - mb_tacho_duration = diff; \ - mb_tacho_averaged += diff; \ - mb_tacho_nb_pulse++; \ - tmb_last = t_now; \ - got_one_pulse = TRUE; \ - } - -#endif /* MB_TACHO_H */ diff --git a/sw/airborne/arm7/mb_twi_controller.c b/sw/airborne/arm7/mb_twi_controller.c deleted file mode 100644 index 58b32c095d..0000000000 --- a/sw/airborne/arm7/mb_twi_controller.c +++ /dev/null @@ -1,28 +0,0 @@ -#include "mb_twi_controller.h" - -#include - -#include "i2c.h" - -uint16_t mb_twi_command; - -uint8_t mb_twi_nb_overun; -uint8_t mb_twi_i2c_done; - - -void mb_twi_controller_init(void) { - mb_twi_nb_overun = 0; - mb_twi_i2c_done = TRUE; -} - -void mb_twi_controller_set( float throttle ) { - if (mb_twi_i2c_done) { - mb_twi_command = throttle * MB_TWI_CONTROLLER_MAX_CMD; - i2c_buf[0] = (uint8_t)(mb_twi_command&0xFF); - i2c_buf[1] = (uint8_t)(mb_twi_command>>8); - i2c_transmit(MB_TWI_CONTROLLER_ADDR, 2, &mb_twi_i2c_done); - } - else - mb_twi_nb_overun++; - -} diff --git a/sw/airborne/arm7/mb_twi_controller.h b/sw/airborne/arm7/mb_twi_controller.h deleted file mode 100644 index 7a55e373ff..0000000000 --- a/sw/airborne/arm7/mb_twi_controller.h +++ /dev/null @@ -1,22 +0,0 @@ -#ifndef MB_TWI_CONTROLLER_H -#define MB_TWI_CONTROLLER_H - -#include "std.h" - -extern void mb_twi_controller_init(void); - -extern void mb_twi_controller_set( float throttle ); - -#define MB_TWI_CONTROLLER_MAX_CMD 65535 -/* - Slave address - front = 0x52 - back = 0x54 - right = 0x56 - left = 0x58 -*/ -#define MB_TWI_CONTROLLER_ADDR 0x52 - -#endif /* MB_TWI_CONTROLLER_H */ - - diff --git a/sw/airborne/booz_ahrs.c b/sw/airborne/booz_ahrs.c deleted file mode 100644 index a8b4e7b91b..0000000000 --- a/sw/airborne/booz_ahrs.c +++ /dev/null @@ -1,19 +0,0 @@ -#include "booz_ahrs.h" - -uint8_t booz_ahrs_status; - -FLOAT_T booz_ahrs_phi; -FLOAT_T booz_ahrs_theta; -FLOAT_T booz_ahrs_psi; - -FLOAT_T booz_ahrs_p; -FLOAT_T booz_ahrs_q; -FLOAT_T booz_ahrs_r; - -FLOAT_T booz_ahrs_bp; -FLOAT_T booz_ahrs_bq; -FLOAT_T booz_ahrs_br; - -FLOAT_T booz_ahrs_measure_phi; -FLOAT_T booz_ahrs_measure_theta; -FLOAT_T booz_ahrs_measure_psi; diff --git a/sw/airborne/booz_ahrs.h b/sw/airborne/booz_ahrs.h deleted file mode 100644 index 4198496f0e..0000000000 --- a/sw/airborne/booz_ahrs.h +++ /dev/null @@ -1,114 +0,0 @@ -/* - * $Id$ - * - * Copyright (C) 2008 Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ - -#ifndef BOOZ_AHRS_H -#define BOOZ_AHRS_H - -#include "std.h" - -#define BOOZ_AHRS_STATUS_UNINIT 0 -#define BOOZ_AHRS_STATUS_RUNNING 1 -#define BOOZ_AHRS_STATUS_CRASHED 2 - -extern uint8_t booz_ahrs_status; - -extern FLOAT_T booz_ahrs_phi; -extern FLOAT_T booz_ahrs_theta; -extern FLOAT_T booz_ahrs_psi; - -extern FLOAT_T booz_ahrs_p; -extern FLOAT_T booz_ahrs_q; -extern FLOAT_T booz_ahrs_r; - -extern FLOAT_T booz_ahrs_bp; -extern FLOAT_T booz_ahrs_bq; -extern FLOAT_T booz_ahrs_br; - -extern FLOAT_T booz_ahrs_measure_phi; -extern FLOAT_T booz_ahrs_measure_theta; -extern FLOAT_T booz_ahrs_measure_psi; - -extern void booz_ahrs_init(void); -extern void booz_ahrs_start( const float* accel, const float* gyro, const float* mag); -/*extern void booz_ahrs_run(const float* accel, const float* gyro, const float* mag);*/ - -extern void booz_ahrs_predict( const float* gyro ); -extern void booz_ahrs_update_accel( const float* accel); -extern void booz_ahrs_update_mag( const float* mag ); - - -//#define __AHRS_IMPL_HEADER(_typ, _ext) _t##_ext -//#define _AHRS_IMPL_HEADER(_typ, _ext) __AHRS_IMPL_HEADER(_typ, _ext) -//#define AHRS_IMPL_HEADER(_typ) _AHRS_IMPL_HEADER(_typ, ".h") -//#error BOOZ_AHRS_TYPE -//#include \"AHRS_IMPL_HEADER(BOOZ_AHRS_TYPE)\" - -#define BOOZ_AHRS_MULTITILT 0 -#define BOOZ_AHRS_QUATERNION 1 -#define BOOZ_AHRS_EULER 2 -#define BOOZ_AHRS_COMP_FILTER 3 - -#if defined BOOZ_AHRS_TYPE && BOOZ_AHRS_TYPE == BOOZ_AHRS_MULTITILT -#include "ahrs_multitilt.h" -#elif defined BOOZ_AHRS_TYPE && BOOZ_AHRS_TYPE == BOOZ_AHRS_QUATERNION -#include "ahrs_quat_fast_ekf.h" -#elif defined BOOZ_AHRS_TYPE && BOOZ_AHRS_TYPE == BOOZ_AHRS_EULER -#include "ahrs_euler_fast_ekf.h" -#elif defined BOOZ_AHRS_TYPE && BOOZ_AHRS_TYPE == BOOZ_AHRS_COMP_FILTER -#include "ahrs_comp_filter.h" -#endif - - -#define WRAP(x,a) { while (x > a) x -= 2 * a; while (x <= -a) x += 2 * a;} - -#define PhiOfAccel(_phi, _accel) { \ - _phi = atan2(-_accel[AXIS_Y], -_accel[AXIS_Z]); \ - } - -#define ThetaOfAccel(_theta, _accel) { \ - const float g2 = \ - _accel[AXIS_X]*_accel[AXIS_X] + \ - _accel[AXIS_Y]*_accel[AXIS_Y] + \ - _accel[AXIS_Z]*_accel[AXIS_Z]; \ - _theta = asin( accel[AXIS_X] / sqrt( g2 ) ); \ - } - -#define PsiOfMag(_psi, _mag) { \ - const float cphi = cos( booz_ahrs_phi ); \ - const float sphi = sin( booz_ahrs_phi ); \ - const float ctheta = cos( booz_ahrs_theta ); \ - const float stheta = sin( booz_ahrs_theta ); \ - \ - const float mn = \ - ctheta* _mag[AXIS_X]+ \ - sphi*stheta* _mag[AXIS_Y]+ \ - cphi*stheta* _mag[AXIS_Z]; \ - const float me = \ - /* 0* mag[AXIS_X]+ */ \ - cphi* mag[AXIS_Y]+ \ - -sphi* mag[AXIS_Z]; \ - _psi = -atan2( me, mn ); \ - } - -#endif /* BOOZ_AHRS_H */ diff --git a/sw/airborne/booz_autopilot.c b/sw/airborne/booz_autopilot.c deleted file mode 100644 index f91ea24c0c..0000000000 --- a/sw/airborne/booz_autopilot.c +++ /dev/null @@ -1,83 +0,0 @@ -/* - * $Id$ - * - * Copyright (C) 2008 Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ - -#include "booz_autopilot.h" - -#include "radio_control.h" -#include "commands.h" -#include "booz_control.h" -#include "booz_nav.h" - -uint8_t booz_autopilot_mode; - -void booz_autopilot_init(void) { - booz_autopilot_mode = BOOZ_AP_MODE_FAILSAFE; -} - -void booz_autopilot_periodic_task(void) { - - switch (booz_autopilot_mode) { - case BOOZ_AP_MODE_FAILSAFE: - case BOOZ_AP_MODE_KILL: - SetCommands(commands_failsafe); - break; - case BOOZ_AP_MODE_RATE: - booz_control_rate_run(); - SetCommands(booz_control_commands); - break; - case BOOZ_AP_MODE_ATTITUDE: - case BOOZ_AP_MODE_HEADING_HOLD: - booz_control_attitude_run(); - SetCommands(booz_control_commands); - break; - case BOOZ_AP_MODE_NAV: - booz_nav_run(); - booz_control_attitude_run(); - SetCommands(booz_control_commands); - break; - } - -} - - -void booz_autopilot_on_rc_event(void) { - /* I think this should be hidden in rc code */ - /* the ap gets a mode everytime - the rc filters it */ - if (rc_values_contains_avg_channels) { - booz_autopilot_mode = BOOZ_AP_MODE_OF_PPRZ(rc_values[RADIO_MODE]); - rc_values_contains_avg_channels = FALSE; - } - switch (booz_autopilot_mode) { - case BOOZ_AP_MODE_RATE: - booz_control_rate_read_setpoints_from_rc(); - break; - case BOOZ_AP_MODE_ATTITUDE: - case BOOZ_AP_MODE_HEADING_HOLD: - booz_control_attitude_read_setpoints_from_rc(); - break; - case BOOZ_AP_MODE_NAV: - booz_nav_read_setpoints_from_rc(); - break; - } -} diff --git a/sw/airborne/booz_autopilot.h b/sw/airborne/booz_autopilot.h deleted file mode 100644 index 6d438ce0f0..0000000000 --- a/sw/airborne/booz_autopilot.h +++ /dev/null @@ -1,50 +0,0 @@ -/* - * $Id$ - * - * Copyright (C) 2008 Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ - -#ifndef BOOZ_AUTOPILOT_H -#define BOOZ_AUTOPILOT_H - -#include "std.h" - -#define BOOZ_AP_MODE_FAILSAFE 0 -#define BOOZ_AP_MODE_KILL 1 -#define BOOZ_AP_MODE_RATE 2 -#define BOOZ_AP_MODE_ATTITUDE 3 -#define BOOZ_AP_MODE_HEADING_HOLD 4 -#define BOOZ_AP_MODE_NAV 5 - -extern uint8_t booz_autopilot_mode; - -extern void booz_autopilot_init(void); -extern void booz_autopilot_periodic_task(void); -extern void booz_autopilot_on_rc_event(void); - -#define TRESHOLD_RATE_PPRZ (MIN_PPRZ / 2) -#define TRESHOLD_ATTITUDE_PPRZ (MAX_PPRZ/2) -#define BOOZ_AP_MODE_OF_PPRZ(rc) \ - ((rc) < TRESHOLD_RATE_PPRZ ? BOOZ_AP_MODE_ATTITUDE : \ - (rc) < TRESHOLD_ATTITUDE_PPRZ ? BOOZ_AP_MODE_HEADING_HOLD : \ - BOOZ_AP_MODE_NAV ) - -#endif /* BOOZ_AUTOPILOT_H */ diff --git a/sw/airborne/booz_control.c b/sw/airborne/booz_control.c deleted file mode 100644 index e9575e5714..0000000000 --- a/sw/airborne/booz_control.c +++ /dev/null @@ -1,205 +0,0 @@ -/* - * $Id$ - * - * Copyright (C) 2008 Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ - -#include "booz_control.h" - -#include "booz_estimator.h" -#include "booz_autopilot.h" -#include "radio_control.h" - -#define BOOZ_CONTROL_MIN_THROTTLE 0.05 - -float booz_control_p_sp; -float booz_control_q_sp; -float booz_control_r_sp; -float booz_control_power_sp; - -float booz_control_rate_pq_pgain; -float booz_control_rate_pq_dgain; -float booz_control_rate_r_pgain; -float booz_control_rate_r_dgain; -float booz_control_rate_last_err_p; -float booz_control_rate_last_err_q; -float booz_control_rate_last_err_r; - -pprz_t booz_control_commands[COMMANDS_NB]; - -#define BOOZ_CONTROL_RATE_PQ_PGAIN -700. -#define BOOZ_CONTROL_RATE_PQ_DGAIN 15. - -#define BOOZ_CONTROL_RATE_R_PGAIN -600. -#define BOOZ_CONTROL_RATE_R_DGAIN 5. - -/* setpoints for max stick throw in degres per second */ -#define BOOZ_CONTROL_RATE_PQ_MAX_SP 120. -#define BOOZ_CONTROL_RATE_R_MAX_SP 100. - - -float booz_control_attitude_phi_sp; -float booz_control_attitude_theta_sp; -float booz_control_attitude_psi_sp; -float booz_control_attitude_phi_theta_pgain; -float booz_control_attitude_phi_theta_dgain; -float booz_control_attitude_psi_pgain; -float booz_control_attitude_psi_dgain; - -#define BOOZ_CONTROL_ATTITUDE_PHI_THETA_PGAIN -1250. -#define BOOZ_CONTROL_ATTITUDE_PHI_THETA_DGAIN -700. - -#define BOOZ_CONTROL_ATTITUDE_PSI_PGAIN -1050. -#define BOOZ_CONTROL_ATTITUDE_PSI_DGAIN -850. - -/* setpoints for max stick throw in degres */ -#define BOOZ_CONTROL_ATTITUDE_PHI_THETA_MAX_SP 30. -#define BOOZ_CONTROL_ATTITUDE_PSI_MAX_SP 45. - -#define BOOZ_CONTROL_ATTITUDE_DT_UPDATE_SP (1./50.) - -void booz_control_init(void) { - - booz_control_p_sp = 0.; - booz_control_q_sp = 0.; - booz_control_r_sp = 0.; - booz_control_power_sp = 0.; - - booz_control_rate_last_err_p = 0.; - booz_control_rate_last_err_q = 0.; - booz_control_rate_last_err_r = 0.; - - booz_control_rate_pq_pgain = BOOZ_CONTROL_RATE_PQ_PGAIN; - booz_control_rate_pq_dgain = BOOZ_CONTROL_RATE_PQ_DGAIN; - booz_control_rate_r_pgain = BOOZ_CONTROL_RATE_R_PGAIN; - booz_control_rate_r_dgain = BOOZ_CONTROL_RATE_R_DGAIN; - - - booz_control_attitude_phi_sp = 0.; - booz_control_attitude_theta_sp =0.; - booz_control_attitude_psi_sp =0.; - booz_control_attitude_phi_theta_pgain = BOOZ_CONTROL_ATTITUDE_PHI_THETA_PGAIN; - booz_control_attitude_phi_theta_dgain = BOOZ_CONTROL_ATTITUDE_PHI_THETA_DGAIN; - booz_control_attitude_psi_pgain = BOOZ_CONTROL_ATTITUDE_PSI_PGAIN; - booz_control_attitude_psi_dgain = BOOZ_CONTROL_ATTITUDE_PSI_DGAIN; - -} - - -void booz_control_rate_read_setpoints_from_rc(void) { - - booz_control_p_sp = -rc_values[RADIO_ROLL] * RadOfDeg(BOOZ_CONTROL_RATE_PQ_MAX_SP)/MAX_PPRZ; - booz_control_q_sp = rc_values[RADIO_PITCH] * RadOfDeg(BOOZ_CONTROL_RATE_PQ_MAX_SP)/MAX_PPRZ; - booz_control_r_sp = -rc_values[RADIO_YAW] * RadOfDeg(BOOZ_CONTROL_RATE_R_MAX_SP)/MAX_PPRZ; - booz_control_power_sp = rc_values[RADIO_THROTTLE] / (float)MAX_PPRZ; - -} - - -void booz_control_rate_run(void) { - - if (booz_control_power_sp < BOOZ_CONTROL_MIN_THROTTLE) { - booz_control_commands[COMMAND_P] = 0; - booz_control_commands[COMMAND_Q] = 0; - booz_control_commands[COMMAND_R] = 0; - booz_control_commands[COMMAND_THROTTLE] = 0; - } - else { - const float rate_err_p = booz_estimator_uf_p - booz_control_p_sp; - const float rate_d_err_p = rate_err_p - booz_control_rate_last_err_p; - booz_control_rate_last_err_p = rate_err_p; - const float cmd_p = booz_control_rate_pq_pgain * ( rate_err_p + booz_control_rate_pq_dgain * rate_d_err_p ); - - const float rate_err_q = booz_estimator_uf_q - booz_control_q_sp; - const float rate_d_err_q = rate_err_q - booz_control_rate_last_err_q; - booz_control_rate_last_err_q = rate_err_q; - const float cmd_q = booz_control_rate_pq_pgain * ( rate_err_q + booz_control_rate_pq_dgain * rate_d_err_q ); - - const float rate_err_r = booz_estimator_uf_r - booz_control_r_sp; - const float rate_d_err_r = rate_err_r - booz_control_rate_last_err_r; - booz_control_rate_last_err_r = rate_err_r; - const float cmd_r = booz_control_rate_r_pgain * ( rate_err_r + booz_control_rate_r_dgain * rate_d_err_r ); - - booz_control_commands[COMMAND_P] = TRIM_PPRZ((int16_t)cmd_p); - booz_control_commands[COMMAND_Q] = TRIM_PPRZ((int16_t)cmd_q); - booz_control_commands[COMMAND_R] = TRIM_PPRZ((int16_t)cmd_r); - booz_control_commands[COMMAND_THROTTLE] = TRIM_PPRZ((int16_t) (booz_control_power_sp * MAX_PPRZ)); - } - -} - -void booz_control_attitude_read_setpoints_from_rc(void) { - - booz_control_attitude_phi_sp = -rc_values[RADIO_ROLL] * - RadOfDeg(BOOZ_CONTROL_ATTITUDE_PHI_THETA_MAX_SP)/MAX_PPRZ; - booz_control_attitude_theta_sp = rc_values[RADIO_PITCH] * - RadOfDeg(BOOZ_CONTROL_ATTITUDE_PHI_THETA_MAX_SP)/MAX_PPRZ; - if (booz_autopilot_mode == BOOZ_AP_MODE_ATTITUDE) { - booz_control_r_sp = -rc_values[RADIO_YAW] * RadOfDeg(BOOZ_CONTROL_RATE_R_MAX_SP)/MAX_PPRZ; - } - else { /* BOOZ_AP_MODE_HEADING_HOLD */ - if (booz_control_power_sp < BOOZ_CONTROL_MIN_THROTTLE) - booz_control_attitude_psi_sp = booz_estimator_psi; - else { - booz_control_attitude_psi_sp += -rc_values[RADIO_YAW] * - RadOfDeg(BOOZ_CONTROL_ATTITUDE_PSI_MAX_SP)*BOOZ_CONTROL_ATTITUDE_DT_UPDATE_SP/MAX_PPRZ; - } - } - booz_control_power_sp = rc_values[RADIO_THROTTLE] / (float)MAX_PPRZ; -} - -void booz_control_attitude_run(void) { - - if (booz_control_power_sp < BOOZ_CONTROL_MIN_THROTTLE) { - booz_control_commands[COMMAND_P] = 0; - booz_control_commands[COMMAND_Q] = 0; - booz_control_commands[COMMAND_R] = 0; - booz_control_commands[COMMAND_THROTTLE] = 0; - } - else { - const float att_err_phi = booz_estimator_phi - booz_control_attitude_phi_sp; - const float cmd_p = booz_control_attitude_phi_theta_pgain * att_err_phi + - booz_control_attitude_phi_theta_dgain * booz_estimator_p ; - - const float att_err_theta = booz_estimator_theta - booz_control_attitude_theta_sp; - const float cmd_q = booz_control_attitude_phi_theta_pgain * att_err_theta + - booz_control_attitude_phi_theta_dgain * booz_estimator_q; - - float cmd_r; - if (booz_autopilot_mode == BOOZ_AP_MODE_ATTITUDE) { - const float rate_err_r = booz_estimator_r - booz_control_r_sp; - const float rate_d_err_r = rate_err_r - booz_control_rate_last_err_r; - booz_control_rate_last_err_r = rate_err_r; - cmd_r = booz_control_rate_r_pgain * ( rate_err_r + booz_control_rate_r_dgain * rate_d_err_r ); - } - else { - float att_err_psi = booz_estimator_psi - booz_control_attitude_psi_sp; - NormRadAngle(att_err_psi); - cmd_r = booz_control_attitude_psi_pgain * att_err_psi + - booz_control_attitude_psi_dgain * booz_estimator_r; - } - - booz_control_commands[COMMAND_P] = TRIM_PPRZ((int16_t)cmd_p); - booz_control_commands[COMMAND_Q] = TRIM_PPRZ((int16_t)cmd_q); - booz_control_commands[COMMAND_R] = TRIM_PPRZ((int16_t)cmd_r); - booz_control_commands[COMMAND_THROTTLE] = TRIM_PPRZ((int16_t) (booz_control_power_sp * MAX_PPRZ)); - } -} diff --git a/sw/airborne/booz_control.h b/sw/airborne/booz_control.h deleted file mode 100644 index 6cb4ed36f4..0000000000 --- a/sw/airborne/booz_control.h +++ /dev/null @@ -1,69 +0,0 @@ -/* - * $Id$ - * - * Copyright (C) 2008 Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ - -#ifndef BOOZ_CONTROL_H -#define BOOZ_CONTROL_H - -#include "std.h" -#include "paparazzi.h" - -extern void booz_control_init(void); - -extern void booz_control_rate_read_setpoints_from_rc(void); -extern void booz_control_rate_run(void); - -extern void booz_control_attitude_read_setpoints_from_rc(void); -extern void booz_control_attitude_run(void); - -extern float booz_control_p_sp; -extern float booz_control_q_sp; -extern float booz_control_r_sp; -extern float booz_control_power_sp; - -extern float booz_control_rate_pq_pgain; -extern float booz_control_rate_pq_dgain; -extern float booz_control_rate_r_pgain; -extern float booz_control_rate_r_dgain; - -extern float booz_control_attitude_phi_sp; -extern float booz_control_attitude_theta_sp; -extern float booz_control_attitude_psi_sp; - -extern float booz_control_attitude_phi_theta_pgain; -extern float booz_control_attitude_phi_theta_dgain; -extern float booz_control_attitude_psi_pgain; -extern float booz_control_attitude_psi_dgain; - - -#define BoozControlAttitudeSetSetPoints(_phi_sp, _theta_sp, _psi_sp, _power_sp) { \ - booz_control_attitude_phi_sp = _phi_sp; \ - booz_control_attitude_theta_sp = _theta_sp; \ - booz_control_attitude_psi_sp = _psi_sp; \ - booz_control_power_sp = _power_sp; \ - } - -#include "airframe.h" -extern pprz_t booz_control_commands[COMMANDS_NB]; - -#endif /* BOOZ_CONTROL_H */ diff --git a/sw/airborne/booz_controller_main.c b/sw/airborne/booz_controller_main.c deleted file mode 100644 index 1f6f7ff98f..0000000000 --- a/sw/airborne/booz_controller_main.c +++ /dev/null @@ -1,172 +0,0 @@ -/* - * $Id$ - * - * Copyright (C) 2008 Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ - -#include "booz_controller_main.h" - -#include "std.h" -#include "init_hw.h" -#include "interrupt_hw.h" -#include "sys_time.h" -#include "led.h" - -#include "booz_energy.h" - -#include "commands.h" -#include "i2c.h" -#include "actuators.h" -#include "radio_control.h" - -#include "adc.h" -#include "booz_link_mcu.h" - -#include "booz_estimator.h" -#include "booz_control.h" -#include "booz_nav.h" -#include "booz_autopilot.h" - -#include "uart.h" -#include "messages.h" -#include "downlink.h" -#include "booz_controller_telemetry.h" -#include "datalink.h" - - - -int16_t trim_p = 0; -int16_t trim_q = 0; -int16_t trim_r = 0; -uint8_t vbat = 0; - -#ifndef SITL -int main( void ) { - booz_controller_main_init(); - while(1) { - if (sys_time_periodic()) - booz_controller_main_periodic_task(); - booz_controller_main_event_task(); - } - return 0; -} -#endif - -STATIC_INLINE void booz_controller_main_init( void ) { - - hw_init(); - led_init(); - sys_time_init(); - - adc_init(); - booz_energy_init(); - - i2c_init(); - actuators_init(); - SetCommands(commands_failsafe); - - ppm_init(); - radio_control_init(); - - booz_link_mcu_init(); - - booz_estimator_init(); - booz_control_init(); - booz_nav_init(); - booz_autopilot_init(); - - //FIXME -#ifndef SITL - uart1_init_tx(); -#endif - - int_enable(); - - DOWNLINK_SEND_BOOT(&cpu_time_sec); -} - - -#define PeriodicPrescaleBy5( _code_0, _code_1, _code_2, _code_3, _code_4) { \ - static uint8_t _50hz = 0; \ - _50hz++; \ - if (_50hz >= 5) _50hz = 0; \ - switch (_50hz) { \ - case 0: \ - _code_0; \ - break; \ - case 1: \ - _code_1; \ - break; \ - case 2: \ - _code_2; \ - break; \ - case 3: \ - _code_3; \ - break; \ - case 4: \ - _code_4; \ - break; \ - } \ - } - -STATIC_INLINE void booz_controller_main_periodic_task( void ) { - - /* check for timeout */ - booz_link_mcu_periodic(); - /* run control loops */ - booz_autopilot_periodic_task(); - - // commands[COMMAND_P] = 0; - // commands[COMMAND_Q] = 0; - // commands[COMMAND_R] = 0; - // commands[COMMAND_THROTTLE] = MAX_PPRZ/3; - - SetActuatorsFromCommands(commands); - - PeriodicPrescaleBy5( \ - { \ - radio_control_periodic_task(); \ - if (rc_status != RC_OK) \ - booz_autopilot_mode = BOOZ_AP_MODE_FAILSAFE; \ - }, \ - { \ - booz_controller_telemetry_periodic_task(); \ - }, \ - { \ - booz_energy_periodic(); \ - }, \ - {}, \ - {} \ - ); \ -} - -STATIC_INLINE void booz_controller_main_event_task( void ) { - - // FIXME -#ifndef SITL - DlEventCheckAndHandle(); -#endif - - BoozLinkMcuEventCheckAndHandle(booz_estimator_read_inter_mcu_state); - - RadioControlEventCheckAndHandle(booz_autopilot_on_rc_event); - -} diff --git a/sw/airborne/booz_controller_main.h b/sw/airborne/booz_controller_main.h deleted file mode 100644 index 60551e2bba..0000000000 --- a/sw/airborne/booz_controller_main.h +++ /dev/null @@ -1,14 +0,0 @@ -#ifndef BOOZ_CONTROLLER_MAIN_H -#define BOOZ_CONTROLLER_MAIN_H - -#ifdef SITL -#define STATIC_INLINE -#else -#define STATIC_INLINE static inline -#endif - -STATIC_INLINE void booz_controller_main_init( void ); -STATIC_INLINE void booz_controller_main_periodic_task( void ); -STATIC_INLINE void booz_controller_main_event_task( void ); - -#endif /* BOOZ_CONTROLLER_MAIN_H */ diff --git a/sw/airborne/booz_controller_telemetry.c b/sw/airborne/booz_controller_telemetry.c deleted file mode 100644 index 80e85f3b25..0000000000 --- a/sw/airborne/booz_controller_telemetry.c +++ /dev/null @@ -1,5 +0,0 @@ -#include "booz_controller_telemetry.h" - - -uint8_t telemetry_mode_Controller; - diff --git a/sw/airborne/booz_controller_telemetry.h b/sw/airborne/booz_controller_telemetry.h deleted file mode 100644 index 1229c90752..0000000000 --- a/sw/airborne/booz_controller_telemetry.h +++ /dev/null @@ -1,106 +0,0 @@ -#ifndef BOOZ_CONTROLLER_TELEMETRY_H -#define BOOZ_CONTROLLER_TELEMETRY_H - -#include "std.h" -#include "messages.h" -#include "periodic.h" -#include "uart.h" - -#include "booz_energy.h" -#include "radio_control.h" -#include "actuators.h" -#include "booz_link_mcu.h" -#include "booz_estimator.h" -#include "booz_autopilot.h" -#include "booz_control.h" -#include "booz_nav.h" - -#include "actuators_buss_twi_blmc_hw.h" - -#include "settings.h" - -#include "downlink.h" - -#define PERIODIC_SEND_ALIVE() DOWNLINK_SEND_ALIVE() - -#define PERIODIC_SEND_BOOZ_STATUS() \ - DOWNLINK_SEND_BOOZ_STATUS(&booz_link_mcu_nb_err, \ - &booz_link_mcu_status, \ - &rc_status, \ - &booz_autopilot_mode, \ - &booz_energy_bat, \ - &cpu_time_sec, \ - &buss_twi_blmc_nb_err) - - -#define PERIODIC_SEND_RC() DOWNLINK_SEND_RC(PPM_NB_PULSES, rc_values) - -#define PERIODIC_SEND_BOOZ_FD() \ - DOWNLINK_SEND_BOOZ_FD(&booz_estimator_p, \ - &booz_estimator_q, \ - &booz_estimator_r, \ - &booz_estimator_phi, \ - &booz_estimator_theta, \ - &booz_estimator_psi); - -#define PERIODIC_SEND_ACTUATORS() \ - DOWNLINK_SEND_ACTUATORS(SERVOS_NB, actuators); - -#define PERIODIC_SEND_BOOZ_CONTROL() { \ - switch (booz_autopilot_mode) { \ - case BOOZ_AP_MODE_RATE: \ - DOWNLINK_SEND_BOOZ_RATE_LOOP(&booz_estimator_uf_p, &booz_control_p_sp, \ - &booz_estimator_uf_q, &booz_control_q_sp, \ - &booz_estimator_uf_r, &booz_control_r_sp, \ - &booz_control_power_sp); \ - break; \ - case BOOZ_AP_MODE_ATTITUDE: \ - case BOOZ_AP_MODE_HEADING_HOLD: \ - case BOOZ_AP_MODE_NAV: \ - DOWNLINK_SEND_BOOZ_ATT_LOOP(&booz_estimator_phi, &booz_control_attitude_phi_sp, \ - &booz_estimator_theta, &booz_control_attitude_theta_sp, \ - &booz_estimator_psi, &booz_control_attitude_psi_sp, \ - &booz_control_power_sp); \ - break; \ - } \ - } - -#define PERIODIC_SEND_BOOZ_UF_RATES() \ - DOWNLINK_SEND_BOOZ_UF_RATES(&booz_estimator_uf_p, \ - &booz_estimator_uf_q, \ - &booz_estimator_uf_r); - -#define PERIODIC_SEND_BOOZ_VERT_LOOP() { \ - DOWNLINK_SEND_BOOZ_VERT_LOOP(&booz_nav_hover_z_sp, \ - &booz_estimator_w, \ - &booz_estimator_z, \ - &booz_nav_hover_power_command); \ - } -#define PERIODIC_SEND_BOOZ_HOV_LOOP() { \ - DOWNLINK_SEND_BOOZ_HOV_LOOP(&booz_nav_hover_x_sp, \ - &booz_nav_hover_y_sp, \ - &booz_estimator_u, \ - &booz_estimator_x, \ - &booz_estimator_v, \ - &booz_estimator_y, \ - &booz_nav_hover_phi_command, \ - &booz_nav_hover_theta_command); \ - } - -#define PERIODIC_SEND_BOOZ_CMDS() DOWNLINK_SEND_BOOZ_CMDS(&buss_twi_blmc_motor_power[SERVO_MOTOR_FRONT],\ - &buss_twi_blmc_motor_power[SERVO_MOTOR_BACK], \ - &buss_twi_blmc_motor_power[SERVO_MOTOR_LEFT], \ - &buss_twi_blmc_motor_power[SERVO_MOTOR_RIGHT]); - - - -#define PERIODIC_SEND_DL_VALUE() PeriodicSendDlValue() - -extern uint8_t telemetry_mode_Controller; - -static inline void booz_controller_telemetry_periodic_task(void) { - PeriodicSendController() -} - - -#endif /* BOOZ_CONTROLLER_TELEMETRY_H */ diff --git a/sw/airborne/booz_datalink.c b/sw/airborne/booz_datalink.c deleted file mode 100644 index 627358c4ad..0000000000 --- a/sw/airborne/booz_datalink.c +++ /dev/null @@ -1,24 +0,0 @@ -#define DATALINK_C -#include "datalink.h" - -#include "settings.h" -#include "downlink.h" -#include "messages.h" -#include "dl_protocol.h" -#include "uart.h" - -#define IdOfMsg(x) (x[1]) - -void dl_parse_msg(void) { - uint8_t msg_id = IdOfMsg(dl_buffer); - switch (msg_id) { - - case DL_SETTING : { - uint8_t i = DL_SETTING_index(dl_buffer); - float var = DL_SETTING_value(dl_buffer); - DlSetting(i, var); - DOWNLINK_SEND_DL_VALUE(&i, &var); - break; - } - } -} diff --git a/sw/airborne/booz_debug.c b/sw/airborne/booz_debug.c deleted file mode 100644 index f5366eb160..0000000000 --- a/sw/airborne/booz_debug.c +++ /dev/null @@ -1,4 +0,0 @@ -#include "booz_debug.h" - -uint8_t booz_debug_mod; -uint8_t booz_debug_err; diff --git a/sw/airborne/booz_debug.h b/sw/airborne/booz_debug.h deleted file mode 100644 index 7c1a4e9313..0000000000 --- a/sw/airborne/booz_debug.h +++ /dev/null @@ -1,31 +0,0 @@ -#ifndef BOOZ_DEBUG_H -#define BOOZ_DEBUG_H - -#ifdef BOOZ_DEBUG - -#include "uart.h" -#include "messages.h" -#include "downlink.h" - -extern uint8_t booz_debug_mod; -extern uint8_t booz_debug_err; - -#define DEBUG_IMU 0 -#define DEBUG_MAX_1117 1 -#define DEBUG_SCP1000 2 -#define DEBUG_LINK_MCU_IMU 3 - - -#define ASSERT(cond, mod, err) { \ - if (!(cond)) { \ - booz_debug_mod = mod; \ - booz_debug_err = err; \ - DOWNLINK_SEND_BOOZ_ERROR(&booz_debug_mod, &booz_debug_err); \ - } \ - } -#else -#define ASSERT(cond, mod, err) {} -#endif - - -#endif /* BOOZ_DEBUG_H */ diff --git a/sw/airborne/booz_energy.c b/sw/airborne/booz_energy.c deleted file mode 100644 index bd0328c40b..0000000000 --- a/sw/airborne/booz_energy.c +++ /dev/null @@ -1,16 +0,0 @@ -#include "booz_energy.h" -#include CONFIG -#include "adc.h" - - - -uint8_t booz_energy_bat; -static struct adc_buf bat_adc_buf; - -void booz_energy_init( void ) { - adc_buf_channel(ADC_BAT, &bat_adc_buf, DEFAULT_AV_NB_SAMPLE); -} - -void booz_energy_periodic( void ) { - booz_energy_bat = bat_adc_buf.sum * 0.004295; -} diff --git a/sw/airborne/booz_energy.h b/sw/airborne/booz_energy.h deleted file mode 100644 index 8cd4f51d04..0000000000 --- a/sw/airborne/booz_energy.h +++ /dev/null @@ -1,14 +0,0 @@ -#ifndef BOOZ_ENERGY_H -#define BOOZ_ENERGY_H - -#include "std.h" - -extern uint8_t booz_energy_bat; - -extern void booz_energy_init( void ); -extern void booz_energy_periodic( void ); - - - - -#endif /* BOOZ_ENERGY_H */ diff --git a/sw/airborne/booz_estimator.c b/sw/airborne/booz_estimator.c deleted file mode 100644 index 3983280964..0000000000 --- a/sw/airborne/booz_estimator.c +++ /dev/null @@ -1,128 +0,0 @@ -#include "booz_estimator.h" - -#include "booz_inter_mcu.h" - - -float booz_estimator_uf_p; -float booz_estimator_uf_q; -float booz_estimator_uf_r; - -float booz_estimator_p; -float booz_estimator_q; -float booz_estimator_r; - -float booz_estimator_phi; -float booz_estimator_theta; -float booz_estimator_psi; - -float booz_estimator_dcm[AXIS_NB][AXIS_NB]; - -float booz_estimator_x; -float booz_estimator_y; -float booz_estimator_z; - -float booz_estimator_vx; -float booz_estimator_vy; -float booz_estimator_vz; - -float booz_estimator_u; -float booz_estimator_v; -float booz_estimator_w; - -void booz_estimator_init( void ) { - - booz_estimator_uf_p = 0.; - booz_estimator_uf_q = 0.; - booz_estimator_uf_r = 0.; - - booz_estimator_p = 0.; - booz_estimator_q = 0.; - booz_estimator_r = 0.; - - booz_estimator_phi = 0.; - booz_estimator_theta = 0.; - booz_estimator_psi = 0.; - - booz_estimator_x = 0.; - booz_estimator_y = 0.; - booz_estimator_z = 0.; - - booz_estimator_vx = 0.; - booz_estimator_vy = 0.; - booz_estimator_vz = 0.; - - booz_estimator_u = 0.; - booz_estimator_v = 0.; - booz_estimator_w = 0.; -} - - -#define BoozEstimatorComputeDCM() { \ - \ - float sinPHI = sin( booz_estimator_phi ); \ - float cosPHI = cos( booz_estimator_phi ); \ - float sinTHETA = sin( booz_estimator_theta); \ - float cosTHETA = cos( booz_estimator_theta); \ - float sinPSI = sin( booz_estimator_psi); \ - float cosPSI = cos( booz_estimator_psi); \ - \ - booz_estimator_dcm[0][0] = cosTHETA * cosPSI; \ - booz_estimator_dcm[0][1] = cosTHETA * sinPSI; \ - booz_estimator_dcm[0][2] = -sinTHETA; \ - booz_estimator_dcm[1][0] = sinPHI * sinTHETA * cosPSI - cosPHI * sinPSI; \ - booz_estimator_dcm[1][1] = sinPHI * sinTHETA * sinPSI + cosPHI * cosPSI; \ - booz_estimator_dcm[1][2] = sinPHI * cosTHETA; \ - booz_estimator_dcm[2][0] = cosPHI * sinTHETA * cosPSI + sinPHI * sinPSI; \ - booz_estimator_dcm[2][1] = cosPHI * sinTHETA * sinPSI - sinPHI * cosPSI; \ - booz_estimator_dcm[2][2] = cosPHI * cosTHETA; \ - \ -} - -#define BoozEstimatorUpdateBodySpeed() { \ - \ - booz_estimator_u = \ - booz_estimator_dcm[AXIS_U][AXIS_X] * booz_estimator_vx + \ - booz_estimator_dcm[AXIS_U][AXIS_Y] * booz_estimator_vy + \ - booz_estimator_dcm[AXIS_U][AXIS_Z] * booz_estimator_vz ; \ - \ - booz_estimator_v = \ - booz_estimator_dcm[AXIS_V][AXIS_X] * booz_estimator_vx + \ - booz_estimator_dcm[AXIS_V][AXIS_Y] * booz_estimator_vy + \ - booz_estimator_dcm[AXIS_V][AXIS_Z] * booz_estimator_vz ; \ - \ - booz_estimator_w = \ - booz_estimator_dcm[AXIS_W][AXIS_X] * booz_estimator_vx + \ - booz_estimator_dcm[AXIS_W][AXIS_Y] * booz_estimator_vy + \ - booz_estimator_dcm[AXIS_W][AXIS_Z] * booz_estimator_vz ; \ - \ - } - -void booz_estimator_read_inter_mcu_state( void ) { - - booz_estimator_uf_p = inter_mcu_state.r_rates[AXIS_P] * M_PI/RATE_PI_S; - booz_estimator_uf_q = inter_mcu_state.r_rates[AXIS_Q] * M_PI/RATE_PI_S; - booz_estimator_uf_r = inter_mcu_state.r_rates[AXIS_R] * M_PI/RATE_PI_S; - - booz_estimator_p = inter_mcu_state.f_rates[AXIS_P] * M_PI/RATE_PI_S; - booz_estimator_q = inter_mcu_state.f_rates[AXIS_Q] * M_PI/RATE_PI_S; - booz_estimator_r = inter_mcu_state.f_rates[AXIS_R] * M_PI/RATE_PI_S; - - booz_estimator_phi = inter_mcu_state.f_eulers[AXIS_X] * M_PI/ANGLE_PI; - booz_estimator_theta = inter_mcu_state.f_eulers[AXIS_Y] * M_PI/ANGLE_PI; - booz_estimator_psi = inter_mcu_state.f_eulers[AXIS_Z] * M_PI/ANGLE_PI; - - booz_estimator_x = inter_mcu_state.pos[AXIS_X]; - booz_estimator_y = inter_mcu_state.pos[AXIS_Y]; - booz_estimator_z = inter_mcu_state.pos[AXIS_Z]; - - booz_estimator_vx = inter_mcu_state.speed[AXIS_X]; - booz_estimator_vy = inter_mcu_state.speed[AXIS_Y]; - booz_estimator_vz = inter_mcu_state.speed[AXIS_Z]; - - BoozEstimatorComputeDCM(); - - BoozEstimatorUpdateBodySpeed(); - -} - - diff --git a/sw/airborne/booz_estimator.h b/sw/airborne/booz_estimator.h deleted file mode 100644 index 07e330c512..0000000000 --- a/sw/airborne/booz_estimator.h +++ /dev/null @@ -1,41 +0,0 @@ -#ifndef BOOZ_ESTIMATOR_H -#define BOOZ_ESTIMATOR_H - -#include "6dof.h" - -/* unfiltered rates available when filter crashed or uninitialed */ -extern float booz_estimator_uf_p; -extern float booz_estimator_uf_q; -extern float booz_estimator_uf_r; - -extern float booz_estimator_p; -extern float booz_estimator_q; -extern float booz_estimator_r; - -extern float booz_estimator_phi; -extern float booz_estimator_theta; -extern float booz_estimator_psi; - -extern float booz_estimator_dcm[AXIS_NB][AXIS_NB]; - -/* position in earth frame : not yet available - sim only */ -extern float booz_estimator_x; -extern float booz_estimator_y; -extern float booz_estimator_z; - -/* speed in earth frame : not yet available - sim only */ -extern float booz_estimator_vx; -extern float booz_estimator_vy; -extern float booz_estimator_vz; - -/* speed in body frame : not yet available - sim only */ -extern float booz_estimator_u; -extern float booz_estimator_v; -extern float booz_estimator_w; - - -extern void booz_estimator_init( void ); -extern void booz_estimator_read_inter_mcu_state( void ); - - -#endif /* BOOZ_ESTIMATOR_H */ diff --git a/sw/airborne/booz_filter_main.c b/sw/airborne/booz_filter_main.c deleted file mode 100644 index b317f95538..0000000000 --- a/sw/airborne/booz_filter_main.c +++ /dev/null @@ -1,159 +0,0 @@ -/* - * $Id$ - * - * Copyright (C) 2008 Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ - -#include "booz_filter_main.h" - -#include "std.h" -#include "init_hw.h" -#include "interrupt_hw.h" -#include "sys_time.h" - -#include "booz_imu.h" -#include "booz_still_detection.h" -#include "booz_ahrs.h" - -#include "gps.h" -#include "booz_ins.h" - -#include "uart.h" -#include "messages.h" -#include "downlink.h" -#include "booz_filter_telemetry.h" - -#include "booz_link_mcu.h" - -static inline void on_imu_accel( void ); -static inline void on_imu_gyro( void ); -static inline void on_imu_mag( void ); -static inline void on_imu_baro( void ); -static inline void on_gps( void ); - -#ifndef SITL -int main( void ) { - { uint32_t foo = 0; while (foo < 1e5) foo++;} - - booz_filter_main_init(); - - while (1) { - if (sys_time_periodic()) - booz_filter_main_periodic_task(); - booz_filter_main_event_task(); - } - return 0; -} -#endif - -STATIC_INLINE void booz_filter_main_init( void ) { - - hw_init(); - - sys_time_init(); - //FIXME -#ifndef SITL - uart0_init_tx(); - uart1_init_tx(); -#endif - booz_imu_init(); - - booz_still_detection_init(); - - booz_ahrs_init(); - - booz_ins_init(); - - booz_link_mcu_init(); - - int_enable(); - -} - -// static uint32_t t0, t1, diff; - -STATIC_INLINE void booz_filter_main_event_task( void ) { - /* check if measurements are available */ - BoozImuEvent(on_imu_accel, on_imu_gyro, on_imu_mag, on_imu_baro); - - GpsEventCheckAndHandle(on_gps, FALSE); -} - -STATIC_INLINE void booz_filter_main_periodic_task( void ) { - - booz_imu_periodic(); - RunOnceEvery(4, {booz_filter_telemetry_periodic_task();}) - -} - -static inline void on_imu_accel( void ) { - if (booz_ahrs_status == BOOZ_AHRS_STATUS_RUNNING) { - RunOnceEvery(4, {booz_ahrs_update_accel(imu_accel);}); - } - if (booz_ins_status == BOOZ_INS_STATUS_RUNNING) { - booz_ins_predict(imu_accel); - } -} - -static inline void on_imu_gyro( void ) { - - switch (booz_ahrs_status) { - - case BOOZ_AHRS_STATUS_UNINIT : - booz_still_detection_run(); - if (booz_still_detection_status == BSD_STATUS_LOCKED) { - booz_ahrs_start(booz_still_detection_accel, - booz_still_detection_gyro, - booz_still_detection_mag); - booz_ins_start(booz_still_detection_accel, - booz_still_detection_pressure); - } - break; - - case BOOZ_AHRS_STATUS_RUNNING : - // t0 = T0TC; - booz_ahrs_predict(imu_gyro); - // t1 = T0TC; - // diff = t1 - t0; - // DOWNLINK_SEND_TIME(&diff); - break; - - } - // DOWNLINK_SEND_IMU_GYRO(&imu_gyro[AXIS_P], &imu_gyro[AXIS_Q], &imu_gyro[AXIS_R]); - booz_link_mcu_send(); -} - -static inline void on_imu_mag( void ) { - // DOWNLINK_SEND_IMU_MAG(&imu_mag[AXIS_X], &imu_mag[AXIS_Y], &imu_mag[AXIS_Z]); - if (booz_ahrs_status == BOOZ_AHRS_STATUS_RUNNING) { - booz_ahrs_update_mag(imu_mag); - } -} - -static inline void on_imu_baro( void ) { - // DOWNLINK_SEND_IMU_PRESSURE(&imu_pressure); - booz_ins_update_pressure(imu_pressure); -} - -static inline void on_gps( void ) { - - -} diff --git a/sw/airborne/booz_filter_main.h b/sw/airborne/booz_filter_main.h deleted file mode 100644 index 9b30afd1f9..0000000000 --- a/sw/airborne/booz_filter_main.h +++ /dev/null @@ -1,14 +0,0 @@ -#ifndef BOOZ_FILTER_MAIN_H -#define BOOZ_FILTER_MAIN_H - -#ifdef SITL -#define STATIC_INLINE -#else -#define STATIC_INLINE static inline -#endif - -STATIC_INLINE void booz_filter_main_init( void ); -STATIC_INLINE void booz_filter_main_periodic_task( void ); -STATIC_INLINE void booz_filter_main_event_task( void ); - -#endif /* BOOZ_FILTER_MAIN_H */ diff --git a/sw/airborne/booz_filter_telemetry.c b/sw/airborne/booz_filter_telemetry.c deleted file mode 100644 index e5b51e4360..0000000000 --- a/sw/airborne/booz_filter_telemetry.c +++ /dev/null @@ -1,3 +0,0 @@ -#include "booz_filter_telemetry.h" - -uint8_t telemetry_mode_Filter; diff --git a/sw/airborne/booz_filter_telemetry.h b/sw/airborne/booz_filter_telemetry.h deleted file mode 100644 index 0fe339fc3d..0000000000 --- a/sw/airborne/booz_filter_telemetry.h +++ /dev/null @@ -1,180 +0,0 @@ -/* - * $Id$ - * - * Copyright (C) 2008 Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ - -#ifndef BOOZ_FILTER_TELEMETRY_H -#define BOOZ_FILTER_TELEMETRY_H -#include "std.h" -#include "messages.h" -#include "periodic.h" -#include "uart.h" - -#include "booz_imu.h" -#include "booz_ahrs.h" -#include "booz_ins.h" - -#include "settings.h" - -#include "downlink.h" - - - -/* - * - * IMU - * - */ -#define PERIODIC_SEND_IMU_GYRO() \ - DOWNLINK_SEND_IMU_GYRO(&imu_gyro[AXIS_X], \ - &imu_gyro[AXIS_Y], \ - &imu_gyro[AXIS_Z]); - -#define PERIODIC_SEND_IMU_GYRO_RAW() \ - DOWNLINK_SEND_IMU_GYRO_RAW(&imu_gyro_raw[AXIS_X], \ - &imu_gyro_raw[AXIS_Y], \ - &imu_gyro_raw[AXIS_Z]); - -#if 0 -#define PERIODIC_SEND_IMU_GYRO_LP() \ - DOWNLINK_SEND_IMU_GYRO_LP(&imu_gyro_lp[AXIS_X], \ - &imu_gyro_lp[AXIS_Y], \ - &imu_gyro_lp[AXIS_Z]); -#endif - -#define PERIODIC_SEND_IMU_ACCEL() \ - DOWNLINK_SEND_IMU_ACCEL(&imu_accel[AXIS_X], \ - &imu_accel[AXIS_Y], \ - &imu_accel[AXIS_Z]); - -#define PERIODIC_SEND_IMU_ACCEL_RAW() \ - DOWNLINK_SEND_IMU_ACCEL_RAW(&imu_accel_raw[AXIS_X], \ - &imu_accel_raw[AXIS_Y], \ - &imu_accel_raw[AXIS_Z]); - - -#define PERIODIC_SEND_IMU_GYRO_RAW_AVG() \ - DOWNLINK_SEND_IMU_GYRO_RAW_AVG(&imu_vs_gyro_raw_avg[AXIS_X], \ - &imu_vs_gyro_raw_avg[AXIS_Y], \ - &imu_vs_gyro_raw_avg[AXIS_Z], \ - &imu_vs_gyro_raw_var[AXIS_X], \ - &imu_vs_gyro_raw_var[AXIS_Y], \ - &imu_vs_gyro_raw_var[AXIS_Z]); - -#define PERIODIC_SEND_IMU_ACCEL_RAW_AVG() \ - DOWNLINK_SEND_IMU_ACCEL_RAW_AVG(&imu_vs_accel_raw_avg[AXIS_X], \ - &imu_vs_accel_raw_avg[AXIS_Y], \ - &imu_vs_accel_raw_avg[AXIS_Z], \ - &imu_vs_accel_raw_var[AXIS_X], \ - &imu_vs_accel_raw_var[AXIS_Y], \ - &imu_vs_accel_raw_var[AXIS_Z]); - -#define PERIODIC_SEND_IMU_MAG() \ - DOWNLINK_SEND_IMU_MAG(&imu_mag[AXIS_X], \ - &imu_mag[AXIS_Y], \ - &imu_mag[AXIS_Z]); - -#define PERIODIC_SEND_IMU_MAG_RAW() \ - DOWNLINK_SEND_IMU_MAG_RAW(&imu_mag_raw[AXIS_X], \ - &imu_mag_raw[AXIS_Y], \ - &imu_mag_raw[AXIS_Z]); - -#define PERIODIC_SEND_IMU_PRESSURE() \ - DOWNLINK_SEND_IMU_PRESSURE(&imu_pressure); - -/* - * - * AHRS - * - */ -#if defined BOOZ_AHRS_TYPE && BOOZ_AHRS_TYPE == BOOZ_AHRS_MULTITILT - -#define PERIODIC_SEND_AHRS_COV() \ - DOWNLINK_SEND_AHRS_EULER_COV(&mtt_P_phi[0][0], &mtt_P_phi[0][1], \ - &mtt_P_phi[1][1], \ - &mtt_P_theta[0][0], &mtt_P_theta[0][1], \ - &mtt_P_theta[1][1], \ - &mtt_P_psi[0][0], &mtt_P_psi[0][1], \ - &mtt_P_psi[1][1]); \ - -#define PERIODIC_SEND_AHRS_STATE() \ - DOWNLINK_SEND_AHRS_EULER_STATE(&booz_ahrs_phi, &booz_ahrs_theta, &booz_ahrs_psi, \ - &booz_ahrs_bp, &booz_ahrs_bq, &booz_ahrs_br); - - -#elif defined BOOZ_AHRS_TYPE && BOOZ_AHRS_TYPE == BOOZ_AHRS_QUATERNION - -#define PERIODIC_SEND_AHRS_COV() { \ - DOWNLINK_SEND_AHRS_QUAT_COV(&afe_P[0][0], &afe_P[1][1], \ - &afe_P[2][2], &afe_P[3][3], \ - &afe_P[4][4], &afe_P[5][5], \ - &afe_P[6][6]); \ - } - -#define PERIODIC_SEND_AHRS_STATE() \ - DOWNLINK_SEND_AHRS_QUAT_STATE(&afe_q0, &afe_q1, &afe_q2, &afe_q3, \ - &booz_ahrs_bp, &booz_ahrs_bq, &booz_ahrs_br); - -#elif defined BOOZ_AHRS_TYPE && BOOZ_AHRS_TYPE == BOOZ_AHRS_COMP_FILTER - -#define PERIODIC_SEND_AHRS_STATE() \ - DOWNLINK_SEND_AHRS_EULER_STATE(&booz_ahrs_phi, &booz_ahrs_theta, &booz_ahrs_psi, \ - &booz_ahrs_bp, &booz_ahrs_bq, &booz_ahrs_br); - -#define PERIODIC_SEND_AHRS_COV() {} - - - -#endif /* BOOZ_AHRS_TYPE */ - -#define PERIODIC_SEND_AHRS_MEASURE() \ - DOWNLINK_SEND_AHRS_MEASURE(&booz_ahrs_measure_phi, \ - &booz_ahrs_measure_theta, \ - &booz_ahrs_measure_psi); - - - -#define PERIODIC_SEND_INS_STATE() \ - DOWNLINK_SEND_BOOZ_INS_STATE(&booz_ins_z, &booz_ins_z_meas, &booz_ins_zdot, & booz_ins_baz); - - -#if 0 -#define PERIODIC_SEND_BOOZ_DEBUG() { \ - float m_phi = atan2(imu_accel[AXIS_Y], imu_accel[AXIS_Z]); \ - const float g2 = \ - imu_accel[AXIS_X]*imu_accel[AXIS_X] + \ - imu_accel[AXIS_Y]*imu_accel[AXIS_Y] + \ - imu_accel[AXIS_Z]*imu_accel[AXIS_Z]; \ - float m_theta = -asin( imu_accel[AXIS_X] / sqrt( g2 ) ); \ - DOWNLINK_SEND_BOOZ_DEBUG(&m_phi, &m_theta); \ -} -#endif - -#define PERIODIC_SEND_DL_VALUE() PeriodicSendDlValue() - -extern uint8_t telemetry_mode_Filter; - -static inline void booz_filter_telemetry_periodic_task(void) { - PeriodicSendFilter() -} - -#endif /* BOOZ_FILTER_TELEMETRY_H */ diff --git a/sw/airborne/booz_imu.c b/sw/airborne/booz_imu.c deleted file mode 100644 index 1985c88640..0000000000 --- a/sw/airborne/booz_imu.c +++ /dev/null @@ -1,78 +0,0 @@ -/* - * $Id$ - * - * Copyright (C) 2008 Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ - -#include "booz_imu.h" - -#include "booz_debug.h" - -/* calibrated sensors */ -float imu_accel[AXIS_NB]; /* accelerometers in m/s2 */ -float imu_gyro[AXIS_NB]; /* gyros in rad/s */ -float imu_mag[AXIS_NB]; /* magnetometer in arbitrary unit */ -float imu_pressure; /* static pressure in pascals */ - -/* raw sensors */ -uint16_t imu_accel_raw[AXIS_NB]; -uint16_t imu_gyro_raw[AXIS_NB]; -int16_t imu_mag_raw[AXIS_NB]; -uint32_t imu_pressure_raw; - -/* internal ADCs */ -struct adc_buf buf_ax; -struct adc_buf buf_ay; -struct adc_buf buf_az; - -uint8_t booz_imu_status; - -#define IMU_ERR_OVERUN 0 - -void booz_imu_init(void) { - uint8_t i; - for (i=0; i 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_link_mcu.c b/sw/airborne/booz_link_mcu.c deleted file mode 100644 index ed8df43761..0000000000 --- a/sw/airborne/booz_link_mcu.c +++ /dev/null @@ -1,84 +0,0 @@ -/* - * $Id$ - * - * Copyright (C) 2008 Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ - -#include "booz_link_mcu.h" - -#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_link_mcu.h b/sw/airborne/booz_link_mcu.h deleted file mode 100644 index 858b3505bf..0000000000 --- a/sw/airborne/booz_link_mcu.h +++ /dev/null @@ -1,95 +0,0 @@ -/* - * $Id$ - * - * Copyright (C) 2008 Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ - -#ifndef BOOZ_LINK_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_nav.c b/sw/airborne/booz_nav.c deleted file mode 100644 index 42be721b00..0000000000 --- a/sw/airborne/booz_nav.c +++ /dev/null @@ -1,57 +0,0 @@ -#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_nav.h b/sw/airborne/booz_nav.h deleted file mode 100644 index 872ad075ee..0000000000 --- a/sw/airborne/booz_nav.h +++ /dev/null @@ -1,35 +0,0 @@ -#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_nav_hover.c b/sw/airborne/booz_nav_hover.c deleted file mode 100644 index e56603eee1..0000000000 --- a/sw/airborne/booz_nav_hover.c +++ /dev/null @@ -1,185 +0,0 @@ -#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_nav_hover.h b/sw/airborne/booz_nav_hover.h deleted file mode 100644 index 83fefae5e6..0000000000 --- a/sw/airborne/booz_nav_hover.h +++ /dev/null @@ -1,28 +0,0 @@ -#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_still_detection.c b/sw/airborne/booz_still_detection.c deleted file mode 100644 index 21facac6eb..0000000000 --- a/sw/airborne/booz_still_detection.c +++ /dev/null @@ -1,136 +0,0 @@ -/* - * $Id$ - * - * Copyright (C) 2008 Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ - -#include "booz_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_still_detection.h b/sw/airborne/booz_still_detection.h deleted file mode 100644 index 2bf560b4a2..0000000000 --- a/sw/airborne/booz_still_detection.h +++ /dev/null @@ -1,53 +0,0 @@ -/* - * $Id$ - * - * Copyright (C) 2008 Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ - -/* - 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_imu.c b/sw/airborne/booz_test_imu.c deleted file mode 100644 index 7deecc3d3d..0000000000 --- a/sw/airborne/booz_test_imu.c +++ /dev/null @@ -1,90 +0,0 @@ -#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_link_mcu_ctl.c b/sw/airborne/booz_test_link_mcu_ctl.c deleted file mode 100644 index dcb82b5ee1..0000000000 --- a/sw/airborne/booz_test_link_mcu_ctl.c +++ /dev/null @@ -1,77 +0,0 @@ -#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_link_mcu_imu.c b/sw/airborne/booz_test_link_mcu_imu.c deleted file mode 100644 index d8d2174e85..0000000000 --- a/sw/airborne/booz_test_link_mcu_imu.c +++ /dev/null @@ -1,57 +0,0 @@ -#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_still_detection.c b/sw/airborne/booz_test_still_detection.c deleted file mode 100644 index 4278074e73..0000000000 --- a/sw/airborne/booz_test_still_detection.c +++ /dev/null @@ -1,95 +0,0 @@ -/* - * $Id$ - * - * Copyright (C) 2008 Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ - -#include "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_vfilter.c b/sw/airborne/booz_test_vfilter.c deleted file mode 100644 index 39d2cfc299..0000000000 --- a/sw/airborne/booz_test_vfilter.c +++ /dev/null @@ -1,85 +0,0 @@ -#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); -} - diff --git a/sw/airborne/main_motor_bench.c b/sw/airborne/main_motor_bench.c deleted file mode 100644 index 659d80dd44..0000000000 --- a/sw/airborne/main_motor_bench.c +++ /dev/null @@ -1,128 +0,0 @@ - -#include "std.h" -#include "init_hw.h" -#include "sys_time.h" -#include "led.h" -#include "interrupt_hw.h" -#include "mb_tacho.h" -#include "mb_servo.h" -#include "i2c.h" -#include "mb_twi_controller.h" -#include "mb_buss_twi_controller.h" -#include "mb_current.h" -#include "mb_scale.h" - - -#include "uart.h" -#include "messages.h" -#include "downlink.h" - -#include "adc.h" - -#include "mb_modes.h" - -#define CONTROLLER_TYPE BUSS - - -static inline void main_init( void ); -static inline void main_periodic_task( void ); -static inline void main_event_task( void ); - -static inline void main_dl_parse_msg( void ); - -int main( void ) { - main_init(); - while(1) { - if (sys_time_periodic()) - main_periodic_task(); - main_event_task(); - } - return 0; -} - -static inline void main_init( void ) { - //initialisation - hw_init(); - led_init(); - sys_time_init(); - mb_tacho_init(); - -#if defined CONTROLLER_TYPE && CONTROLLER_TYPE == PPRZ - i2c_init(); - mb_twi_controller_init(); -#elif defined CONTROLLER_TYPE && CONTROLLER_TYPE == BUSS - i2c_init(); - mb_buss_twi_controller_init(); -#endif - mb_servo_init(); - mb_servo_set_range( 1275000, 1825000 ); - - adc_init(); - mb_current_init(); - mb_scale_init(); - - uart0_init_tx(); - mb_mode_init(); - - int_enable(); -} - -static inline void main_periodic_task( void ) { - mb_mode_periodic(); - float throttle = mb_modes_throttle; - -#if defined CONTROLLER_TYPE && CONTROLLER_TYPE == PPRZ - mb_twi_controller_set(throttle); -#elif defined CONTROLLER_TYPE && CONTROLLER_TYPE == BUSS - mb_buss_twi_controller_set(throttle); -#endif - mb_servo_set(throttle); - - float rpm = mb_tacho_get_averaged(); - mb_current_periodic(); - float amps = mb_current_amp; - mb_scale_periodic(); - float thrust = mb_scale_thrust; - float torque = 0.; - - static uint8_t my_cnt = 0; - my_cnt++; - if (!(my_cnt%10)) { - LED_TOGGLE(1); - } - DOWNLINK_SEND_MOTOR_BENCH_STATUS(&cpu_time_ticks, &throttle, &rpm, &s , &thrust, &torque, &cpu_time_sec, &mb_modes_mode); -} - -static inline void main_event_task( void ) { - if (PprzBuffer()) { - ReadPprzBuffer(); - if (pprz_msg_received) { - pprz_parse_payload(); - pprz_msg_received = FALSE; - } - } - if (dl_msg_available) { - main_dl_parse_msg(); - dl_msg_available = FALSE; - LED_TOGGLE(1); - } -} - -bool_t dl_msg_available; - -#define MSG_SIZE 128 -uint8_t dl_buffer[MSG_SIZE] __attribute__ ((aligned)); - -#include "settings.h" - -#define IdOfMsg(x) (x[1]) - -static inline void main_dl_parse_msg(void) { - uint8_t msg_id = IdOfMsg(dl_buffer); - if (msg_id == 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); - } -} diff --git a/sw/airborne/main_pan_tilt_antenna.c b/sw/airborne/main_pan_tilt_antenna.c deleted file mode 100644 index 75e68e996e..0000000000 --- a/sw/airborne/main_pan_tilt_antenna.c +++ /dev/null @@ -1,86 +0,0 @@ -#include "std.h" -#include "init_hw.h" -#include "sys_time.h" -#include "led.h" -#include "interrupt_hw.h" -#include "uart.h" - -#include "messages.h" -#include "downlink.h" - -#include "pt_ant_motors.h" -#include "pt_ant_sensors.h" -#include "pt_ant_estimator.h" -#include "gps.h" - -#include "datalink.h" -//#include "traffic_info.h" - -#include "i2c.h" -#include "AMI601.h" - -static inline void main_init( void ); -static inline void main_periodic_task( void ); -static inline void main_event_task( void ); - -int main( void ) { - main_init(); - while(1) { - if (sys_time_periodic()) - main_periodic_task(); - main_event_task(); - } - return 0; -} - -static inline void main_init( void ) { - hw_init(); - sys_time_init(); - led_init(); - Uart0Init(); - Uart1Init(); - // gps_init(); - pt_ant_motors_init(); - // pt_ant_sensors_init_spi(); - // pt_ant_sensors_init(); - - i2c_init(); - ami601_init(); - - int_enable(); - - // gps_configure_uart(); - -} - -static inline void main_periodic_task( void ) { - // PtAntSensorsPeriodic(); - -#ifdef USE_GPS - GpsPeriodic(); -#endif - - - DOWNLINK_SEND_ESTIMATOR(&pt_ant_motors_y_power, &pt_ant_motors_z_power); - // LED_TOGGLE(1); - - ami601_periodic(); - DOWNLINK_SEND_IMU_ACCEL_RAW(&ami601_val[3], &ami601_val[5], &ami601_val[1]); // accel ?? - DOWNLINK_SEND_IMU_GYRO_RAW(&ami601_val[0], &ami601_val[4], &ami601_val[2]); // mag ?? - // ami601_scale_measures(); - // DOWNLINK_SEND_IMU_ACCEL(&ami601_ax, &ami601_ay, &ami601_az); - // DOWNLINK_SEND_IMU_GYRO(&ami601_mx, &ami601_my, &ami601_mz); - -} - -static inline void main_event_task( void ) { - - //PtAntSensorsEventCheckAndHandle(); - - DlEventCheckAndHandle(); - -#ifdef USE_GPS - if (GpsEventCheckAndHandle()) - return; -#endif -} diff --git a/sw/airborne/pt_ant_datalink.c b/sw/airborne/pt_ant_datalink.c deleted file mode 100644 index 0f3491b93d..0000000000 --- a/sw/airborne/pt_ant_datalink.c +++ /dev/null @@ -1,42 +0,0 @@ -#define DATALINK_C -#include "datalink.h" - -#include "settings.h" -#include "downlink.h" -#include "messages.h" -#include "dl_protocol.h" -#include "uart.h" - -#include "traffic_info.h" -#include "pt_ant_estimator.h" - -#define IdOfMsg(x) (x[1]) -#define MOfCm(_x) (((float)_x)/100.) - -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; - } - - case DL_ACINFO: { - LED_TOGGLE(1); - uint8_t id = DL_ACINFO_ac_id(dl_buffer); - struct ac_info_ ac; - ac.east = MOfCm(DL_ACINFO_utm_east(dl_buffer)); - ac.north = MOfCm(DL_ACINFO_utm_north(dl_buffer)); - ac.course = MOfCm(DL_ACINFO_alt(dl_buffer)); - ac.alt = RadOfDeg(((float)DL_ACINFO_course(dl_buffer))/ 10.); - ac.gspeed = MOfCm(DL_ACINFO_speed(dl_buffer)); - pt_ant_estimator_update_target(id, &ac); - break; - } - - } -} diff --git a/sw/airborne/pt_ant_motors.c b/sw/airborne/pt_ant_motors.c deleted file mode 100644 index 65b7d59157..0000000000 --- a/sw/airborne/pt_ant_motors.c +++ /dev/null @@ -1,25 +0,0 @@ -#include "pt_ant_motors.h" - -float pt_ant_motors_y_power; -float pt_ant_motors_z_power; - -void pt_ant_motors_init(void) { - /* configure direction pin as output */ - MOT_Y_DIR_DIR_REG |= 1 << MOT_Y_DIR_PIN; - MOT_Z_DIR_DIR_REG |= 1 << MOT_Z_DIR_PIN; - - /* configure pins for PWM */ - MOT_Y_PWM_PINSEL |= MOT_Y_PWM_PINSEL_VAL << MOT_Y_PWM_PINSEL_BIT; - MOT_Z_PWM_PINSEL |= MOT_Z_PWM_PINSEL_VAL << MOT_Z_PWM_PINSEL_BIT; - /* set chopping frequency */ - PWMMR0 = MOT_CHOP_PERIOD; - /* enable PWM outputs in single edge mode*/ - PWMPCR = MOT_Y_PWM_ENA | MOT_Z_PWM_ENA; - /* commit PWMMRx changes */ - PWMLER = PWMLER_LATCH0; - /* enable PWM timer in PWM mode */ - PWMTCR = PWMTCR_COUNTER_ENABLE | PWMTCR_PWM_ENABLE; - - pt_ant_motors_SetYPower(0.); - pt_ant_motors_SetZPower(0.); -} diff --git a/sw/airborne/pt_ant_motors.h b/sw/airborne/pt_ant_motors.h deleted file mode 100644 index 9905758f02..0000000000 --- a/sw/airborne/pt_ant_motors.h +++ /dev/null @@ -1,66 +0,0 @@ -#ifndef PT_ANT_MOTORS_H -#define PT_ANT_MOTORS_H - -#include "std.h" -#include "LPC21xx.h" -#include "sys_time.h" - -extern void pt_ant_motors_init(void); - -extern float pt_ant_motors_y_power; -extern float pt_ant_motors_z_power; - -#define pt_ant_motors_SetYPower(p) { \ - pt_ant_motors_y_power = p; \ - int32_t len = p * MOT_CHOP_PERIOD; \ - if (p>=0) \ - SetBit(MOT_Y_DIR_CLR_REG, MOT_Y_DIR_PIN); \ - else { \ - SetBit(MOT_Y_DIR_SET_REG, MOT_Y_DIR_PIN); \ - len = -len; \ - } \ - MOT_Y_PWM_REG = len; \ - PWMLER = MOT_Y_PWM_LATCH; \ - } - - -#define pt_ant_motors_SetZPower(p) { \ - pt_ant_motors_z_power = p; \ - int32_t len = p * MOT_CHOP_PERIOD; \ - if (p>=0) \ - SetBit(MOT_Z_DIR_CLR_REG, MOT_Z_DIR_PIN); \ - else { \ - SetBit(MOT_Z_DIR_SET_REG, MOT_Z_DIR_PIN); \ - len = -len; \ - } \ - MOT_Z_PWM_REG = len; \ - PWMLER = MOT_Z_PWM_LATCH; \ - } - -#define MOT_Z_PWM_PINSEL PINSEL0 -#define MOT_Z_PWM_PINSEL_VAL 2 -#define MOT_Z_PWM_PINSEL_BIT 14 -#define MOT_Z_PWM_ENA PWMPCR_ENA2 -#define MOT_Z_PWM_LATCH PWMLER_LATCH2 -#define MOT_Z_PWM_REG PWMMR2 -#define MOT_Z_DIR_DIR_REG IO0DIR -#define MOT_Z_DIR_CLR_REG IO0CLR -#define MOT_Z_DIR_SET_REG IO0SET -#define MOT_Z_DIR_PIN 15 - - -#define MOT_Y_PWM_PINSEL PINSEL1 -#define MOT_Y_PWM_PINSEL_VAL 1 -#define MOT_Y_PWM_PINSEL_BIT 10 -#define MOT_Y_PWM_ENA PWMPCR_ENA5 -#define MOT_Y_PWM_LATCH PWMLER_LATCH5 -#define MOT_Y_PWM_REG PWMMR5 -#define MOT_Y_DIR_DIR_REG IO0DIR -#define MOT_Y_DIR_CLR_REG IO0CLR -#define MOT_Y_DIR_SET_REG IO0SET -#define MOT_Y_DIR_PIN 19 - -/* 5KHz */ -#define MOT_CHOP_PERIOD SYS_TICS_OF_USEC(1000) - -#endif /* PT_ANT_MOTORS_H */