mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-23 13:24:03 +08:00
*** empty log message ***
This commit is contained in:
Antoine Drouin
@@ -14,8 +14,6 @@
|
||||
<axis name="THROTTLE" failsafe_value="0"/>
|
||||
<axis name="ROLL" failsafe_value="0"/>
|
||||
<axis name="PITCH" failsafe_value="0"/>
|
||||
<axis name="CAM_ROLL" failsafe_value="0"/>
|
||||
<axis name="CAM_PITCH" failsafe_value="0"/>
|
||||
</commands>
|
||||
|
||||
<rc_commands>
|
||||
@@ -47,7 +45,6 @@
|
||||
<define name="IR2" value="ADC_1"/>
|
||||
<define name="IR_TOP" value="ADC_6"/>
|
||||
<define name="IR_NB_SAMPLES" value="16"/>
|
||||
<define name="VSUPPLY" value="ADC_3"/>
|
||||
<define name="GYRO_ROLL" value="ADC_4"/>
|
||||
<define name="GYRO_TEMP" value="ADC_5"/>
|
||||
<define name="GYRO_NB_SAMPLES" value="16"/>
|
||||
@@ -57,15 +54,13 @@
|
||||
<section name="INFRARED" prefix="IR_">
|
||||
<define name="ROLL_NEUTRAL_DEFAULT" value="6" unit="deg"/>
|
||||
<define name="PITCH_NEUTRAL_DEFAULT" value="9" unit="deg"/>
|
||||
<define name="DEFAULT_CONTRAST" value="200"/>
|
||||
<define name="RAD_OF_IR_CONTRAST" value="0.75"/>
|
||||
<linear name="RollOfIrs" arity="2" coeff1="-0.7" coeff2="0.7"/>
|
||||
<linear name="PitchOfIrs" arity="2" coeff1="-0.7" coeff2="-0.7"/>
|
||||
<linear name="TopOfIr" arity="1" coeff1="1"/>
|
||||
<define name="RAD_OF_IR_MAX_VALUE" value="0.0045"/>
|
||||
<define name="RAD_OF_IR_MIN_VALUE" value="0.00075"/>
|
||||
<define name="ADC_ROLL_NEUTRAL" value="0"/>
|
||||
<define name="ADC_PITCH_NEUTRAL" value="-716"/>
|
||||
<define name="ADC_IR1_NEUTRAL" value="512"/>
|
||||
<define name="ADC_IR2_NEUTRAL" value="512"/>
|
||||
<define name="ADC_TOP_NEUTRAL" value="512"/>
|
||||
</section>
|
||||
|
||||
@@ -80,10 +75,7 @@
|
||||
|
||||
<section name="BAT">
|
||||
<define name="MILLIAMP_PER_PERCENT" value="0.86"/>
|
||||
<define name="VOLTAGE_ADC_A" value="0.0177531"/>
|
||||
<define name="VOLTAGE_ADC_B" value="0.173626"/>
|
||||
<define name="VoltageOfAdc(adc)" value="(VOLTAGE_ADC_A * adc + VOLTAGE_ADC_B)"/>
|
||||
<define name="LOW_BATTERY" value="9.3" unit="V"/>
|
||||
<define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
|
||||
</section>
|
||||
|
||||
<section name="MISC">
|
||||
@@ -110,12 +102,11 @@
|
||||
<define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
|
||||
|
||||
|
||||
<define name="ROLL_PGAIN" value="5000."/>
|
||||
<define name="ROLL_DGAIN" value="0."/>
|
||||
<define name="AILERON_OF_THROTTLE" value="0.0"/>
|
||||
<define name="ROLL_ATTITUDE_GAIN" value="-1500"/>
|
||||
<define name="ROLL_RATE_GAIN" value="-10"/>
|
||||
<define name="PITCH_PGAIN" value="-9000."/>
|
||||
<define name="PITCH_DGAIN" value="1.5"/>
|
||||
<define name="ELEVATOR_OF_ROLL" value="1250"/>
|
||||
<define name="ELEVATOR_OF_ROLL" value="0"/>
|
||||
|
||||
|
||||
</section>
|
||||
@@ -136,7 +127,7 @@
|
||||
<define name="AUTO_THROTTLE_DASH_TRIM" value="-1000"/>
|
||||
<define name="AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT" value="0.15" unit="%/(m/s)"/>
|
||||
<define name="AUTO_THROTTLE_PGAIN" value="-0.01"/>
|
||||
<define name="AUTO_THROTTLE_IGAIN" value="-0.001"/>
|
||||
<define name="AUTO_THROTTLE_IGAIN" value="0.01"/>
|
||||
<define name="AUTO_THROTTLE_PITCH_OF_VZ_PGAIN" value="0.05"/>
|
||||
|
||||
<!-- auto pitch inner loop -->
|
||||
@@ -156,17 +147,6 @@
|
||||
<define name="DEFAULT_CIRCLE_RADIUS" value="100" unit="m"/>
|
||||
</section>
|
||||
|
||||
<section name="AGGRESSIVE" prefix="AGR_">
|
||||
<define name="CLIMB_GAZ" value="0.95"/><!-- Gaz for Aggressive Climb -->
|
||||
<define name="DESCENT_GAZ" value="0.1"/><!-- Gaz for Aggressive Decent -->
|
||||
<define name="CLIMB_PITCH" value="0.3"/><!-- Pitch for Aggressive Climb -->
|
||||
<define name="DESCENT_PITCH" value="-0.25"/><!-- Pitch for Aggressive Decent -->
|
||||
<define name="BLEND_START" value="20"/><!-- Altitude Error to Initiate Aggressive Climb CANNOT BE ZERO!!-->
|
||||
<define name="BLEND_END" value="10"/><!-- Altitude Error to Blend Aggressive to Regular Climb Modes CANNOT BE ZERO!!-->
|
||||
<define name="CLIMB_NAV_RATIO" value="0.8"/><!-- Percent Navigation for Altitude Error Equal to Start Altitude -->
|
||||
<define name="DESCENT_NAV_RATIO" value="1.0"/>
|
||||
</section>
|
||||
|
||||
|
||||
<section name="GYRO_GAINS">
|
||||
<define name="GYRO_MAX_RATE" value="200."/>
|
||||
@@ -242,9 +222,10 @@ ap.srcs += gyro.c
|
||||
|
||||
# Config for SITL simulation
|
||||
include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
|
||||
sim.CFLAGS += -DCONFIG=\"tiny.h\" -DLOITER_TRIM -DMOBILE_CAM -DCAM
|
||||
sim.srcs += nav_line.c traffic_info.c cam.c
|
||||
|
||||
sim.CFLAGS += -DCONFIG=\"tiny.h\"
|
||||
sim.srcs += nav_survey_rectangle.c traffic_info.c nav_line.c
|
||||
sim.CFLAGS += -DGYRO -DADXRS150
|
||||
sim.srcs += gyro.c
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -6,78 +6,73 @@
|
||||
<dl_settings>
|
||||
<dl_settings NAME="flight params">
|
||||
<dl_setting MAX="1000" MIN="0" STEP="10" VAR="flight_altitude" shortname="altitude"/>
|
||||
<dl_setting MAX="1" MIN="0" STEP="1" VAR="light_mode"/>
|
||||
<dl_setting MAX="1" MIN="0" STEP="1" VAR="gpio1_status"/>
|
||||
<dl_setting MAX="10" MIN="-10" STEP="0.5" VAR="wind_east"/>
|
||||
<dl_setting MAX="10" MIN="-10" STEP="0.5" VAR="wind_north"/>
|
||||
</dl_settings>
|
||||
|
||||
<dl_settings NAME="mode">
|
||||
<dl_setting MAX="2" MIN="0" STEP="1" VAR="pprz_mode"/>
|
||||
<dl_setting MAX="1" MIN="0" STEP="1" VAR="alt_kalman_enabled" shortname="alt_kalman"/>
|
||||
<dl_setting MAX="2" MIN="0" STEP="1" VAR="pprz_mode" module="autopilot"/>
|
||||
<dl_setting MAX="1" MIN="0" STEP="1" VAR="alt_kalman_enabled" shortname="alt_kalman" module="estimator"/>
|
||||
<dl_setting MAX="0" MIN="0" STEP="1" VAR="estimator_flight_time" shortname="flight time"/>
|
||||
<dl_setting MAX="1" MIN="0" STEP="1" VAR="launch"/>
|
||||
<dl_setting MAX="1" MIN="0" STEP="1" VAR="kill_throttle">
|
||||
<strip_button name="KILL" value="1"/>
|
||||
</dl_setting>
|
||||
<dl_setting MAX="1" MIN="0" STEP="1" VAR="telemetry_mode_Ap" shortname="tele_AP"/>
|
||||
<dl_setting MAX="2" MIN="0" STEP="1" VAR="gps_reset" module="gps_ubx" handler="Reset" shortname="GPS reset"/>
|
||||
</dl_settings>
|
||||
|
||||
<dl_settings NAME="control">
|
||||
<dl_settings NAME="ir">
|
||||
<dl_setting MAX="0.3" MIN="-0.3" STEP="0.01" VAR="ir_roll_neutral" shortname="roll_neutral"/>
|
||||
<dl_setting MAX="0.3" MIN="-0.3" STEP="0.01" VAR="ir_roll_neutral" shortname="roll_neutral" module="infrared"/>
|
||||
<dl_setting MAX="0.5" MIN="-0.3" STEP="0.01" VAR="ir_pitch_neutral" shortname="pitch_neutral"/>
|
||||
|
||||
<!--
|
||||
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="ir_correction_left" shortname="corr_left"/>
|
||||
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="ir_correction_right" shortname="corr_right"/>
|
||||
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="ir_correction_up" shortname="corr_up"/>
|
||||
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="ir_correction_down" shortname="corr_down"/>
|
||||
-->
|
||||
|
||||
<dl_setting MAX="1.5" MIN="0.4" STEP="0.05" VAR="ir_estimated_phi_pi_4" shortname="est. right"/>
|
||||
<dl_setting MAX="1.5" MIN="0.4" STEP="0.05" VAR="ir_estimated_phi_minus_pi_4" shortname="est. left"/>
|
||||
<dl_setting MAX="1.5" MIN="0.4" STEP="0.05" VAR="ir_estimated_theta_pi_4" shortname="est. up"/>
|
||||
<dl_setting MAX="1.5" MIN="0.4" STEP="0.05" VAR="ir_estimated_phi_minus_pi_4" shortname="est. down"/>
|
||||
<dl_setting MAX="1" MIN="0" STEP="1" VAR="ir_360" shortname="ir_360"/>
|
||||
</dl_settings>
|
||||
|
||||
|
||||
<dl_settings NAME="attitude">
|
||||
<dl_setting MAX="25000" MIN="000" STEP="250" VAR="h_ctl_roll_pgain" shortname="roll_pgain"/>
|
||||
<dl_setting MAX="500000" MIN="000" STEP="250" VAR="h_ctl_roll_dgain" shortname="roll_dgain"/>
|
||||
<dl_setting MAX="0.6" MIN="0" STEP="0.05" VAR="h_ctl_roll_max_setpoint" shortname="max_roll"/>
|
||||
<dl_setting MAX="0" MIN="-15000" STEP="250" VAR="h_ctl_roll_attitude_gain" shortname="roll pgain" module="fw_h_ctl"/>
|
||||
<dl_setting MAX="0" MIN="-25000" STEP="250" VAR="h_ctl_roll_rate_gain" shortname="roll dgain"/>
|
||||
<dl_setting MAX="1" MIN="0" STEP="0.05" VAR="h_ctl_roll_max_setpoint" shortname="max_roll"/>
|
||||
<dl_setting MAX="000" MIN="-25000" STEP="250" VAR="h_ctl_pitch_pgain" shortname="pitch_pgain"/>
|
||||
<dl_setting MAX="2" MIN="0." STEP="0.1" VAR="h_ctl_pitch_dgain" shortname="pitch_dgain"/>
|
||||
<dl_setting MAX="5000" MIN="0" STEP="100" VAR="h_ctl_elevator_of_roll" shortname="elevator_of_roll"/>
|
||||
<dl_setting MAX="5000" MIN="0" STEP="100" VAR="h_ctl_aileron_of_throttle" shortname="aileron_of_throttle"/>
|
||||
|
||||
|
||||
|
||||
</dl_settings>
|
||||
|
||||
<dl_settings name="alt">
|
||||
<dl_setting MAX="0" MIN="-0.2" STEP="0.01" VAR="v_ctl_altitude_pgain" shortname="alt_pgain"/>
|
||||
</dl_settings>
|
||||
|
||||
<dl_settings name="vertical">
|
||||
<dl_settings name="auto_throttle">
|
||||
<dl_setting MAX="1" MIN="0.0" STEP="0.05" VAR="v_ctl_auto_throttle_cruise_throttle" shortname="cruise throttle" module="fw_v_ctl" handler="SetCruiseThrottle">
|
||||
<strip_button name="Dash" value="1"/>
|
||||
<strip_button name="Loiter" value="0.1"/>
|
||||
<strip_button name="Cruise" value="0"/>
|
||||
</dl_setting>
|
||||
<dl_setting MAX="0.3" MIN="0" STEP="0.01" VAR="v_ctl_auto_throttle_climb_throttle_increment" shortname="throttle_incr"/>
|
||||
<dl_setting MAX="0.00" MIN="-0.05" STEP="0.005" VAR="v_ctl_auto_throttle_pgain" shortname="throttle_pgain"/>
|
||||
<dl_setting MAX="0.0" MIN="-0.1" STEP="0.001" VAR="v_ctl_auto_throttle_igain" shortname="throttle_igain"/>
|
||||
<dl_setting MAX="0" MIN="-4000" STEP="100" VAR="v_ctl_auto_throttle_dash_trim" shortname="dash trim"/>
|
||||
<dl_setting MIN="0" MAX="3000" STEP="100" VAR="v_ctl_auto_throttle_loiter_trim" shortname="loiter trim"/>
|
||||
|
||||
|
||||
<dl_setting MAX="0.00" MIN="-0.05" STEP="0.005" VAR="v_ctl_auto_throttle_pgain" shortname="throttle_pgain"/>
|
||||
<dl_setting MAX="1" MIN="0.0" STEP="0.05" VAR="v_ctl_auto_throttle_igain" shortname="throttle_igain" module="fw_v_ctl" handler="SetAutoThrottleIgain"/>
|
||||
<dl_setting MAX="2" MIN="0.0" STEP="0.1" VAR="v_ctl_auto_throttle_dgain" shortname="throttle_dgain"/>
|
||||
<dl_setting MAX="1" MIN="0" STEP="0.01" VAR="v_ctl_auto_throttle_climb_throttle_increment" shortname="throttle_incr"/>
|
||||
<dl_setting MAX="1" MIN="0" STEP="0.01" VAR="v_ctl_auto_throttle_pitch_of_vz_pgain" shortname="pitch_of_vz"/>
|
||||
<dl_setting MAX="10" MIN="-10" STEP="0.1" VAR="v_ctl_auto_throttle_pitch_of_vz_dgain" shortname="pitch_of_vz (d)"/>
|
||||
</dl_settings>
|
||||
|
||||
<dl_settings name="nav">
|
||||
<dl_setting MAX="-0.1" MIN="-2" STEP="0.05" VAR="h_ctl_course_pgain" shortname="course pgain"/>
|
||||
<dl_setting MAX="-0.1" MIN="-3" STEP="0.05" VAR="h_ctl_course_pgain" shortname="course pgain"/>
|
||||
<dl_setting MAX="2" MIN="0" STEP="0.1" VAR="h_ctl_course_dgain" shortname="course dgain"/>
|
||||
<dl_setting MAX="2" MIN="0.1" STEP="0.05" VAR="h_ctl_course_pre_bank_correction" shortname="pre bank cor"/>
|
||||
<dl_setting MAX="1" MIN="0.0" STEP="0.05" VAR="nav_glide_pitch_trim" shortname="glide pitch trim"/>
|
||||
<dl_setting MAX="1" MIN="0.02" STEP="0.01" VAR="h_ctl_roll_slew" shortname="roll slew"/>
|
||||
<dl_setting MAX="500" MIN="-500" STEP="5" VAR="nav_radius"/>
|
||||
<dl_setting MAX="359" MIN="0" STEP="5" VAR="nav_course"/>
|
||||
<dl_setting MAX="5" MIN="-5" STEP="0.5" VAR="nav_climb"/>
|
||||
<dl_setting MAX="50" MIN="-50" STEP="5" VAR="nav_shift" module="nav" handler="IncreaseShift" shortname="inc. shift">
|
||||
</dl_setting>
|
||||
<dl_setting MAX="20" MIN="10" STEP="0.5" VAR="nav_ground_speed_setpoint" shortname="ground speed"/>
|
||||
<dl_setting MAX="0." MIN="-0.05" STEP="0.01" VAR="nav_ground_speed_pgain" shortname="ground speed pgain"/>
|
||||
<dl_setting MAX="500" MIN="50" STEP="5" VAR="nav_survey_shift"/>
|
||||
|
||||
@@ -248,6 +248,11 @@ void h_ctl_attitude_loop ( void ) {
|
||||
#ifdef H_CTL_ROLL_ATTITUDE_GAIN
|
||||
inline static void h_ctl_roll_loop( void ) {
|
||||
float err = estimator_phi - h_ctl_roll_setpoint;
|
||||
#ifdef SITL
|
||||
static float last_err = 0;
|
||||
estimator_p = (err - last_err)/(1/60.);
|
||||
last_err = err;
|
||||
#endif
|
||||
float cmd = - h_ctl_roll_attitude_gain * err
|
||||
- h_ctl_roll_rate_gain * estimator_p
|
||||
+ v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle;
|
||||
|
||||
@@ -97,4 +97,9 @@ extern void v_ctl_throttle_slew( void );
|
||||
Bound(v_ctl_auto_throttle_cruise_throttle, V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE, V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE); \
|
||||
}
|
||||
|
||||
#define fw_v_ctl_SetAutoThrottleIgain(_v) { \
|
||||
v_ctl_auto_throttle_igain = _v; \
|
||||
v_ctl_auto_throttle_sum_err = 0; \
|
||||
}
|
||||
|
||||
#endif /* FW_V_CTL_H */
|
||||
|
||||
@@ -50,6 +50,7 @@ static struct adc_buf buf_pitch;
|
||||
#endif
|
||||
|
||||
void gyro_init( void) {
|
||||
#ifndef SITL
|
||||
adc_buf_channel(ADC_CHANNEL_GYRO_ROLL, &buf_roll, ADC_CHANNEL_GYRO_NB_SAMPLES);
|
||||
#if defined ADC_CHANNEL_GYRO_TEMP
|
||||
adc_buf_channel(ADC_CHANNEL_GYRO_TEMP, &buf_temp, ADC_CHANNEL_GYRO_NB_SAMPLES);
|
||||
@@ -57,11 +58,13 @@ void gyro_init( void) {
|
||||
#if defined ADC_CHANNEL_GYRO_PITCH
|
||||
adc_buf_channel(ADC_CHANNEL_GYRO_PITCH, &buf_pitch, ADC_CHANNEL_GYRO_NB_SAMPLES);
|
||||
#endif
|
||||
#endif /* SITL */
|
||||
}
|
||||
|
||||
|
||||
|
||||
void gyro_update( void ) {
|
||||
#ifndef SITL
|
||||
float pitch_rate = 0.;
|
||||
roll_rate_adc = (buf_roll.sum/buf_roll.av_nb_sample) - GYRO_ADC_ROLL_NEUTRAL;
|
||||
#if defined ADC_CHANNEL_GYRO_TEMP
|
||||
@@ -74,4 +77,5 @@ void gyro_update( void ) {
|
||||
#endif
|
||||
float roll_rate = GYRO_ROLL_DIRECTION * RadiansOfADC(roll_rate_adc, GYRO_ROLL_SCALE);
|
||||
EstimatorSetRate(roll_rate, pitch_rate);
|
||||
#endif /* SITL */
|
||||
}
|
||||
|
||||
@@ -191,7 +191,7 @@ module Make(A:Data.MISSION) = struct
|
||||
and dy = state.air_speed *. sin state.psi *. dt +. wy *. dt in
|
||||
state.x <- state.x +.dx ;
|
||||
state.y <- state.y +. dy;
|
||||
let gamma = (state.thrust -. drag) /. weight +. state.theta in
|
||||
let gamma = (state.thrust -. drag) /. weight +. state.theta -. 0.1 *. abs_float (sin state.phi)) in
|
||||
let dz = sin gamma *. state.air_speed *. dt in
|
||||
state.z <- state.z +. dz;
|
||||
|
||||
|
||||
Reference in New Issue
Block a user