Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-10 06:59:54 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

AirSpeed

Browse Source
This commit is contained in:
Christophe De Wagter
2009-09-07 14:54:03 +00:00
parent 46040ec396
commit 12ec3872a8
14 changed files with 160 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/holiday50.xml
+5 -5
View File
@@ -25,7 +25,7 @@
</rc_commands>
<ap_only_commands>
<copy command="CAM_TILT" />
<copy command="CAM_TILT"/>
</ap_only_commands>
<command_laws>
@@ -148,7 +148,7 @@
float v_ctl_pitch_of_vz = (v_ctl_climb_setpoint + d_err * v_ctl_auto_throttle_pitch_of_vz_dgain)
* v_ctl_auto_throttle_pitch_of_vz_pgain;
-->
<define name="AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" value="0.368"/>
<define name="AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" value="0.319999992847"/>
<define name="AUTO_THROTTLE_MIN_CRUISE_THROTTLE" value="0.20"/>
<define name="AUTO_THROTTLE_MAX_CRUISE_THROTTLE" value="0.45"/>
<define name="AUTO_THROTTLE_LOITER_TRIM" value="1500"/>
@@ -173,7 +173,7 @@
<define name="PITCH_MAX_SETPOINT" value="0.4" unit="radians"/>
<define name="PITCH_MIN_SETPOINT" value="-0.4" unit="radians"/>
<define name="PITCH_PGAIN" value="-8000."/> <!-- Pitch Angle PD control -->
<define name="PITCH_PGAIN" value="-4925.65087891"/> <!-- Pitch Angle PD control -->
<define name="PITCH_DGAIN" value="2.0"/>
<define name="ELEVATOR_OF_ROLL" value="2050"/> <!-- Feed forward ABS(roll) to elevator -->
@@ -298,8 +298,8 @@ include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
sim.CFLAGS += -DBOARD_CONFIG=\"tiny_1_1.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN -DWIND_INFO -DSTRONG_WIND
sim.srcs += nav_survey_rectangle.c nav_line.c
# traffic_info.c
#sim.CFLAGS += -DUSE_AIRSPEED
#sim.srcs += airspeed.c
sim.CFLAGS += -DUSE_AIRSPEED
sim.srcs += airspeed.c
sim.CFLAGS += -DCAM -DMOBILE_CAM -DPOINT_CAM -DPOINT_CAM_PITCH_ROLL
# -DTEST_CAM
conf/autopilot/tiny.makefile
+5
View File
@@ -7,6 +7,11 @@ ap.ARCH = arm7tdmi
ap.TARGET = autopilot
ap.TARGETDIR = autopilot
fbw.ARCHDIR = $(ARCHI)
fbw.ARCH = arm7tdmi
fbw.TARGET = autopilot
fbw.TARGETDIR = autopilot
LPC21ISP_BAUD = 38400
LPC21ISP_XTAL = 12000
conf/messages.xml
+11
View File
@@ -222,6 +222,11 @@
<field name="long" type="float" unit="deg"/>
</message>
<message name="MOTOR" id="34">
<field name="rpm" type="uint16" unit="Hz"/>
<field name="current" type="uint16" unit="mA"/>
</message>
<message name="WP_MOVED" id="35">
<field name="wp_id" type="uint8"/>
<field name="utm_east" type="float" unit="m"></field>
@@ -301,6 +306,12 @@
<field name="time" type="uint32" unit="us"></field>
</message>
<message name="ENERGY" id="37">
<field name="bat" type="float" unit="V"/>
<field name="amp" type="float" unit="A"/>
<field name="energy" type="uint16" unit="Wh"/>
</message>
<message name="WINDTURBINE_STATUS_" id="50">
<field name="ac_id" type="uint8"/>
<field name="tb_id" type="uint8"/>
conf/settings/light.xml
+4 -1
View File
@@ -3,7 +3,10 @@
<settings>
<dl_settings NAME="lights control">
<dl_settings NAME="lights">
<dl_setting module="light" var="light_duration_1" min="0" max="61" step="1"/>
<dl_setting module="light" var="light_duration_1" min="0" max="61" step="1">
<strip_button name="CamOn" icon="on.png" value="15"/>
<strip_button name="CamOff" icon="off.png" value="0"/>
</dl_setting>
<dl_setting module="light" var="light_duration_2" min="0" max="61" step="1"/>
</dl_settings>
</dl_settings>
sw/airborne/airspeed.c
+41
View File
@@ -0,0 +1,41 @@
#include "airspeed.h"
#include "adc.h"
#include "airframe.h"
#include "estimator.h"
#ifdef USE_AIRSPEED
uint16_t adc_airspeed_val;
#else
#error "You compiled the airspeed.c file but did not USE_AIRSPEED, which is needed in other *.c files"
#endif
#ifdef ADC_CHANNEL_AIRSPEED
#ifndef SITL
static struct adc_buf buf_airspeed;
#endif
#else
#error "You compiled the airspeed.c file but did not ADC_CHANNEL_AIRSPEED, which is needed in other *.c files"
#endif
void airspeed_init( void ) {
#ifdef ADC_CHANNEL_AIRSPEED
# ifndef ADC_CHANNEL_AIRSPEED_NB_SAMPLES
# error "You defined USE_AIRSPEED but did not assign a ADC_CHANNEL_AIRSPEED_NB_SAMPLES"
# endif
# ifndef SITL
adc_buf_channel(ADC_CHANNEL_AIRSPEED, &buf_airspeed, ADC_CHANNEL_AIRSPEED_NB_SAMPLES);
# endif
#else
# error "You defined USE_AIRSPEED but did not assign a ADC_CHANNEL_AIRSPEED"
#endif
}
void airspeed_update( void ) {
#ifdef ADC_CHANNEL_AIRSPEED
#ifndef SITL
adc_airspeed_val = (buf_airspeed.sum / buf_airspeed.av_nb_sample) - AIRSPEED_ZERO;
float airspeed = AIRSPEED_SCALE * adc_airspeed_val;
EstimatorSetAirspeed(airspeed);
#endif
#endif
}
sw/airborne/airspeed.h
+13
View File
@@ -0,0 +1,13 @@
#ifndef AIRSPEED_H
#define AIRSPEED_H
#include <inttypes.h>
#ifdef USE_AIRSPEED
extern uint16_t adc_airspeed_val;
#endif
void airspeed_init( void );
void airspeed_update( void );
#endif /* ADC_GENERIC_H */
sw/airborne/ap_downlink.h
+9
View File
@@ -173,6 +173,15 @@
#define PERIODIC_SEND_BARO_MS5534A(_chan) {}
#endif
#ifdef USE_AIRSPEED
#define PERIODIC_SEND_AIRSPEED(_chan) DOWNLINK_SEND_AIRSPEED (_chan, &adc_airspeed_val,&estimator_airspeed)
#else
#define PERIODIC_SEND_AIRSPEED(_chan) {}
#endif
#define PERIODIC_SEND_ENERGY(_chan) Downlink({ int16_t e = energy; DOWNLINK_SEND_ENERGY( &vsup, &curs, &e); })
#include "fw_h_ctl_a.h"
#define PERIODIC_SEND_H_CTL_A(_chan) DOWNLINK_SEND_H_CTL_A(_chan, &h_ctl_roll_sum_err, &h_ctl_ref_roll_angle, &h_ctl_pitch_sum_err, &h_ctl_ref_pitch_angle)
sw/airborne/datalink.c
+2
View File
@@ -114,7 +114,9 @@ void dl_parse_msg(void) {
if (msg_id == DL_WIND_INFO && DL_WIND_INFO_ac_id(dl_buffer) == AC_ID) {
wind_east = DL_WIND_INFO_east(dl_buffer);
wind_north = DL_WIND_INFO_north(dl_buffer);
#ifndef USE_AIRSPEED
estimator_airspeed = DL_WIND_INFO_airspeed(dl_buffer);
#endif
#ifdef WIND_INFO_RET
DOWNLINK_SEND_WIND_INFO_RET(DefaultChannel, &wind_east, &wind_north, &estimator_airspeed);
#endif
sw/airborne/estimator.c
+4
View File
@@ -95,6 +95,10 @@ void estimator_init( void ) {
EstimatorSetRate(0., 0.);
#ifdef USE_AIRSPEED
EstimatorSetAirspeed( 0. );
#endif
estimator_flight_time = 0;
estimator_airspeed = NOMINAL_AIRSPEED;
sw/airborne/estimator.h
+3
View File
@@ -110,6 +110,9 @@ extern void alt_kalman( float );
#endif
#ifdef USE_AIRSPEED
#define EstimatorSetAirspeed(airspeed) { estimator_airspeed = airspeed; }
#endif
#define EstimatorSetAtt(phi, psi, theta) { estimator_phi = phi; estimator_psi = psi; estimator_theta = theta; }
#define EstimatorSetPhiPsi(phi, psi) { estimator_phi = phi; estimator_psi = psi; }
sw/airborne/fw_v_ctl.c
+39
View File
@@ -78,6 +78,19 @@ inline static void v_ctl_climb_auto_throttle_loop( void );
inline static void v_ctl_climb_auto_pitch_loop( void );
#endif
#ifdef USE_AIRSPEED
float v_ctl_auto_airspeed_setpoint;
float v_ctl_auto_airspeed_pitch_pgain;
float v_ctl_auto_airspeed_throttle_pgain;
float v_ctl_auto_airspeed_throttle_igain;
float v_ctl_auto_airspeed_throttle_sum_err;
#define V_CTL_AUTO_AIRSPEED_THROTTLE_MAX_SUM_ERR 100
inline void v_ctl_airspeed_loop( void );
#endif
void v_ctl_init( void ) {
/* mode */
v_ctl_mode = V_CTL_MODE_MANUAL;
@@ -114,6 +127,15 @@ void v_ctl_init( void ) {
v_ctl_auto_pitch_sum_err = 0.;
#endif
#ifdef USE_AIRSPEED
v_ctl_auto_airspeed_setpoint = V_CTL_AUTO_AIRSPEED_SETPOINT;
v_ctl_auto_airspeed_pitch_pgain = V_CTL_AUTO_AIRSPEED_PITCH_PGAIN;
v_ctl_auto_airspeed_throttle_pgain = V_CTL_AUTO_AIRSPEED_THROTTLE_PGAIN;
v_ctl_auto_airspeed_throttle_igain = V_CTL_AUTO_AIRSPEED_THROTTLE_IGAIN;
v_ctl_auto_airspeed_throttle_sum_err = 0.;
#endif
v_ctl_throttle_setpoint = 0;
}
@@ -176,6 +198,18 @@ inline static void v_ctl_climb_auto_throttle_loop(void) {
/* pitch pre-command */
float v_ctl_pitch_of_vz = (v_ctl_climb_setpoint + d_err * v_ctl_auto_throttle_pitch_of_vz_dgain) * v_ctl_auto_throttle_pitch_of_vz_pgain;
#ifdef USE_AIRSPEED
float err_airspeed = (v_ctl_auto_airspeed_setpoint - estimator_airspeed);
v_ctl_auto_airspeed_throttle_sum_err += err_airspeed;
BoundAbs(v_ctl_auto_airspeed_throttle_sum_err, V_CTL_AUTO_AIRSPEED_THROTTLE_MAX_SUM_ERR);
float v_ctl_auto_airspeed_pitch_of_airspeed = (err_airspeed) * v_ctl_auto_airspeed_pitch_pgain;
float v_ctl_auto_airspeed_throttle_of_airspeed = (err_airspeed + v_ctl_auto_airspeed_throttle_sum_err * v_ctl_auto_airspeed_throttle_igain) * v_ctl_auto_airspeed_throttle_pgain;
controlled_throttle += v_ctl_auto_airspeed_throttle_of_airspeed;
#endif
#if defined AGR_CLIMB
switch (v_ctl_auto_throttle_submode) {
case V_CTL_AUTO_THROTTLE_AGRESSIVE:
@@ -216,6 +250,11 @@ inline static void v_ctl_climb_auto_throttle_loop(void) {
break;
} /* switch submode */
#endif
#ifdef USE_AIRSPEED
nav_pitch += v_ctl_auto_airspeed_pitch_of_airspeed;
#endif
v_ctl_throttle_setpoint = TRIM_UPPRZ(f_throttle * MAX_PPRZ);
}
sw/airborne/fw_v_ctl.h
+8
View File
@@ -87,6 +87,14 @@ extern void v_ctl_init( void );
extern void v_ctl_altitude_loop( void );
extern void v_ctl_climb_loop ( void );
#ifdef USE_AIRSPEED
/* "airspeed" inner loop parameters */
extern float v_ctl_auto_airspeed_setpoint;
extern float v_ctl_auto_airspeed_pitch_pgain;
extern float v_ctl_auto_airspeed_throttle_pgain;
extern float v_ctl_auto_airspeed_throttle_igain;
#endif
/** Computes throttle_slewed from throttle_setpoint */
extern void v_ctl_throttle_slew( void );
sw/airborne/light.h
+5 -2
View File
@@ -36,10 +36,13 @@ extern uint8_t light_duration_2;
#define LightPeriodicTask2(_1Hz) {}
#endif /* LIGHT_LED_1 */
#ifndef LIGHT_DURATION_INITIAL
#define LIGHT_DURATION_INITIAL 0
#endif
#define LightInit() { \
light_duration_1 = 0; \
light_duration_2 = 0; \
light_duration_1 = LIGHT_DURATION_INITIAL; \
light_duration_2 = LIGHT_DURATION_INITIAL; \
}
sw/airborne/main_ap.c
+11 -1
View File
@@ -69,6 +69,10 @@
#include "adc_generic.h"
#endif
#ifdef USE_AIRSPEED
#include "airspeed.h"
#endif
#ifdef TELEMETER
#include "srf08.h"
#endif
@@ -362,7 +366,7 @@ static void navigation_task( void ) {
SEND_NAVIGATION(DefaultChannel);
#endif
SEND_CAM();
SEND_CAM(DefaultChannel);
/* The nav task computes only nav_altitude. However, we are interested
by desired_altitude (= nav_alt+alt_shift) in any case.
@@ -580,6 +584,9 @@ void periodic_task_ap( void ) {
#if defined GYRO
gyro_update();
#endif
#if defined USE_AIRSPEED
airspeed_update();
#endif
#ifdef INFRARED
ir_update();
estimator_update_state_infrared();
@@ -629,6 +636,9 @@ void init_ap( void ) {
#ifdef GYRO
gyro_init();
#endif
#ifdef USE_AIRSPEED
airspeed_init();
#endif
#ifdef GPS
gps_init();
#endif