diff --git a/conf/airframes/microjet_example.xml b/conf/airframes/microjet_example.xml index 405a1356c0..833482d4ba 100644 --- a/conf/airframes/microjet_example.xml +++ b/conf/airframes/microjet_example.xml @@ -8,6 +8,16 @@ + + + + + + + + + + diff --git a/conf/messages.xml b/conf/messages.xml index 1c8d807e64..6d37a6050a 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -516,7 +516,10 @@ - + + + + diff --git a/conf/modules/AOA_adc.xml b/conf/modules/AOA_adc.xml new file mode 100644 index 0000000000..553ace5c0f --- /dev/null +++ b/conf/modules/AOA_adc.xml @@ -0,0 +1,24 @@ + + + + + + +
+ +
+ + + + + + + + + + + + + diff --git a/conf/settings/AOA_adc.xml b/conf/settings/AOA_adc.xml new file mode 100755 index 0000000000..72fa08af3a --- /dev/null +++ b/conf/settings/AOA_adc.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/sw/airborne/estimator.c b/sw/airborne/estimator.c index 72b1ea4c8a..7753689238 100644 --- a/sw/airborne/estimator.c +++ b/sw/airborne/estimator.c @@ -66,6 +66,7 @@ float estimator_hspeed_dir; /* wind */ float wind_east, wind_north; float estimator_airspeed; +float estimator_AOA; #define NORM_RAD_ANGLE2(x) { \ while (x > 2 * M_PI) x -= 2 * M_PI; \ @@ -100,7 +101,11 @@ void estimator_init( void ) { #ifdef USE_AIRSPEED EstimatorSetAirspeed( 0. ); #endif - + +#ifdef USE_AOA + EstimatorSetAOA( 0. ); +#endif + estimator_flight_time = 0; estimator_airspeed = NOMINAL_AIRSPEED; diff --git a/sw/airborne/estimator.h b/sw/airborne/estimator.h index 54d25d9a4d..9924f7f7e3 100644 --- a/sw/airborne/estimator.h +++ b/sw/airborne/estimator.h @@ -69,6 +69,8 @@ extern float estimator_hspeed_dir; extern float wind_east, wind_north; /* m/s */ extern float estimator_airspeed; /* m/s */ +/* Angle of Attack estimation */ +extern float estimator_AOA; /* radians */ void estimator_init( void ); void estimator_propagate_state( void ); @@ -121,6 +123,7 @@ extern void alt_kalman( float ); #endif #define EstimatorSetAirspeed(airspeed) { estimator_airspeed = airspeed; } +#define EstimatorSetAOA(AOA) { estimator_AOA = AOA; } #define EstimatorSetAtt(phi, psi, theta) { estimator_phi = phi; estimator_psi = psi; estimator_theta = theta; } #define EstimatorSetPhiPsi(phi, psi) { estimator_phi = phi; estimator_psi = psi; } diff --git a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c index d779ed7cf5..cff5f79434 100644 --- a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c +++ b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c @@ -65,6 +65,7 @@ float h_ctl_pitch_loop_setpoint; float h_ctl_pitch_pgain; float h_ctl_pitch_dgain; pprz_t h_ctl_elevator_setpoint; +uint8_t h_ctl_pitch_mode; /* inner loop pre-command */ float h_ctl_aileron_of_throttle; @@ -417,7 +418,18 @@ inline static void h_ctl_pitch_loop( void ) { h_ctl_pitch_setpoint - h_ctl_elevator_of_roll / h_ctl_pitch_pgain * fabs(estimator_phi); - float err = estimator_theta - h_ctl_pitch_loop_setpoint; + float err = 0; + switch (h_ctl_pitch_mode){ + case H_CTL_PITCH_MODE_THETA: + err = estimator_theta - h_ctl_pitch_loop_setpoint; + break; + case H_CTL_PITCH_MODE_AOA: + err = estimator_AOA - h_ctl_pitch_loop_setpoint; + break; + default: + err = estimator_theta - h_ctl_pitch_loop_setpoint; + break; + } float d_err = err - last_err; last_err = err; float cmd = h_ctl_pitch_pgain * (err + h_ctl_pitch_dgain * d_err); diff --git a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.h b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.h index 0c68e7bd64..4ba21c5a11 100644 --- a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.h +++ b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.h @@ -56,6 +56,11 @@ extern float h_ctl_roll_pgain; extern pprz_t h_ctl_aileron_setpoint; extern float h_ctl_roll_slew; +/* Pitch mode */ +#define H_CTL_PITCH_MODE_THETA 0 +#define H_CTL_PITCH_MODE_AOA 1 +extern uint8_t h_ctl_pitch_mode; + /* inner pitch loop parameters */ extern float h_ctl_pitch_setpoint; extern float h_ctl_pitch_loop_setpoint; diff --git a/sw/airborne/modules/sensors/AOA_adc.c b/sw/airborne/modules/sensors/AOA_adc.c new file mode 100644 index 0000000000..c2f713685a --- /dev/null +++ b/sw/airborne/modules/sensors/AOA_adc.c @@ -0,0 +1,83 @@ +/* + * Copyright (C) 2010 The Paparazzi Team + * + * Autor: Bruzzlee + * Angle of Attack ADC Sensor + * US DIGITAL MA3-A10-236-N + * + * 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 "modules/sensors/AOA_adc.h" +#include "mcu_periph/adc.h" +#include BOARD_CONFIG +#include "generated/airframe.h" +#include "estimator.h" +#include "std.h" +//Messages +#include "mcu_periph/uart.h" +#include "messages.h" +#include "downlink.h" + +uint16_t adc_AOA_val; + +//Downlink +#ifndef DOWNLINK_DEVICE +#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE +#endif + +#ifndef SITL // Use ADC if not in simulation + +#ifndef ADC_CHANNEL_AOA +#error "ADC_CHANNEL_AOA needs to be defined to use AOA_adc module" +#endif + +#ifndef ADC_CHANNEL_AOA_NB_SAMPLES +#define ADC_CHANNEL_AOA_NB_SAMPLES DEFAULT_AV_NB_SAMPLE +#endif + +struct adc_buf buf_AOA; +float AOA_offset, AOA_filter; +float AOA, AOA_old; +#endif + + +void AOA_adc_init( void ) { + AOA_offset = AOA_OFFSET; + AOA_filter = AOA_FILTER; + AOA_old = 0; +#ifndef SITL + adc_buf_channel(ADC_CHANNEL_AOA, &buf_AOA, ADC_CHANNEL_AOA_NB_SAMPLES); +#endif +} + +void AOA_adc_update( void ) { +#ifndef SITL + adc_AOA_val = buf_AOA.sum / buf_AOA.av_nb_sample; + +// PT1 filter and convert to rad + AOA = AOA_filter * AOA_old + (1 - AOA_filter) * (adc_AOA_val*(2*M_PI)/1024-M_PI+AOA_offset); + AOA_old = AOA; +#endif + RunOnceEvery(30, DOWNLINK_SEND_AOA_adc(DefaultChannel, &adc_AOA_val, &AOA)); + +#ifdef USE_AOA + EstimatorSetAOA(AOA); +#endif +} diff --git a/sw/airborne/modules/sensors/AOA_adc.h b/sw/airborne/modules/sensors/AOA_adc.h new file mode 100644 index 0000000000..c18e88aa3c --- /dev/null +++ b/sw/airborne/modules/sensors/AOA_adc.h @@ -0,0 +1,38 @@ +/* + * Copyright (C) 2010 The Paparazzi Team + * + * Autor: Bruzzlee + * Angle of Attack ADC Sensor + * US DIGITAL MA3-A10-236-N + * + * 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 AOA_ADC_H +#define AOA_ADC_H + +#include + +extern uint16_t adc_AOA_val; +extern float AOA_offset, AOA_filter; + +void AOA_adc_init( void ); +void AOA_adc_update( void ); + +#endif /* AOA_ADC_H */