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

*** empty log message ***

Browse Source
This commit is contained in:
Antoine Drouin
2006-06-04 17:14:31 +00:00
parent 1087afb249
commit fbbf097b36
35 changed files with 379 additions and 75 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Makefile
+5 -4
View File
@@ -189,13 +189,14 @@ help:
test_all_example_airframes:
make AIRCRAFT=Tux clean_ac ac
make AIRCRAFT=TJ1 clean_ac ap
make AIRCRAFT=MJ4 clean_ac ap
make AIRCRAFT=Slayer clean_ac ac
make AIRCRAFT=Plaster clean_ac sim ac
make AIRCRAFT=Twin4 clean_ac ac
make AIRCRAFT=Tux clean_ac ac
make AIRCRAFT=Twin1 clean_ac sim ac
make AIRCRAFT=Twin2 clean_ac sim
make AIRCRAFT=MJ1 clean_ac ac
make AIRCRAFT=TJ1 clean_ac ap
make AIRCRAFT=MJ4 clean_ac ap
make AIRCRAFT=GRZE3 clean_ac ap
make AIRCRAFT=Twin4 clean_ac ac
make AIRCRAFT=TS5 clean_ac ac
conf/airframes/microjet4.xml
+5 -4
View File
@@ -39,11 +39,11 @@
</section>
<section name="adc" prefix="ADC_CHANNEL_">
<define name="IR1" value="3"/>
<define name="IR2" value="2"/>
<define name="IR_TOP" value="1"/>
<define name="IR1" value="ADC_0"/>
<define name="IR2" value="ADC_1"/>
<define name="IR_TOP" value="ADC_2"/>
<define name="IR_NB_SAMPLES" value="16"/>
<define name="VSUPPLY" value="7"/>
<define name="VSUPPLY" value="ADC_3"/>
</section>
<section name="INFRARED" prefix="IR_">
@@ -53,6 +53,7 @@
<define name="RAD_OF_IR_CONTRAST" value="0.75"/>
<linear name="RollOfIrs" arity="2" coeff1="-0.5" coeff2="0.5"/>
<linear name="PitchOfIrs" arity="2" coeff1="-0.5" coeff2="-0.5"/>
<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"/>
conf/airframes/plaster1.xml
+14
View File
@@ -118,11 +118,25 @@
<define name="DEVICE_ADDRESS" value=""/>
</section>
<section name="AGGRESSIVE" prefix="AGR_">
<define name="CLIMB_GAZ" value="1.0"/><!-- Gaz for Aggressive Climb -->
<define name="DESCENT_GAZ" value="0.1"/><!-- Gaz for Aggressive Decent -->
<define name="CLIMB_PITCH" value="0.52"/><!-- Pitch for Aggressive Climb -->
<define name="DESCENT_PITCH" value="-0.7"/><!-- Pitch for Aggressive Decent -->
<define name="BLEND_START" value="30"/><!-- Altitude Error to Initiate Aggressive Climb CANNOT BE ZERO!!-->
<define name="BLEND_END" value="15"/><!-- 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>
<makefile>
include $(PAPARAZZI_SRC)/conf/autopilot/v1_2_1.makefile
include $(PAPARAZZI_SRC)/conf/autopilot/twin_mcu_avr.makefile
fbw.CFLAGS += -DTELEMETRY_MODE_FBW=1
ap.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=XBeeTransport
ap.srcs += downlink.c
conf/airframes/slayer1.xml
+40 -16
View File
@@ -39,13 +39,13 @@
</section>
<section name="adc" prefix="ADC_CHANNEL_">
<define name="IR1" value="3"/>
<define name="IR2" value="2"/>
<define name="IR_TOP" value="1"/>
<define name="IR1" value="ADC_1"/>
<define name="IR2" value="ADC_0"/>
<define name="IR_TOP" value="ADC_2"/>
<define name="IR_NB_SAMPLES" value="16"/>
<define name="VSUPPLY" value="7"/>
<define name="GYRO_ROLL" value="5"/>
<define name="GYRO_PITCH" value="6"/>
<define name="VSUPPLY" value="ADC_7"/>
<define name="GYRO_ROLL" value="ADC_3"/>
<define name="GYRO_PITCH" value="ADC_4"/>
<define name="GYRO_NB_SAMPLES" value="16"/>
</section>
@@ -54,24 +54,25 @@
<define name="PITCH_NEUTRAL_DEFAULT" value="0" unit="deg"/>
<define name="DEFAULT_CONTRAST" value="200"/>
<define name="RAD_OF_IR_CONTRAST" value="0.75"/>
<linear name="RollOfIrs" arity="2" coeff1="-1" coeff2="1"/>
<linear name="PitchOfIrs" arity="2" coeff1="-1" coeff2="-1"/>
<linear name="RollOfIrs" arity="2" coeff1=".5" coeff2="0"/>
<linear name="PitchOfIrs" arity="2" coeff1="0" coeff2=".5"/>
<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="-1024"/>
<define name="ADC_ROLL_NEUTRAL" value="256"/>
<define name="ADC_PITCH_NEUTRAL" value="256"/>
<define name="ADC_TOP_NEUTRAL" value="512"/>
</section>
<section name="GYRO" prefix="GYRO_">
<define name="ADC_ROLL_NEUTRAL" value="0"/>
<define name="ADC_PITCH_NEUTRAL" value="0"/>
<define name="ADC_ROLL_NEUTRAL" value="503"/>
<define name="ADC_PITCH_NEUTRAL" value="469"/>
</section>
<section name="BAT">
<define name="MILLIAMP_PER_PERCENT" value="0.86"/>
<define name="VOLTAGE_ADC_A" value="0.0174"/>
<define name="VOLTAGE_ADC_B" value="0.313"/>
<define name="VOLTAGE_ADC_A" value="0.01818"/>
<define name="VOLTAGE_ADC_B" value="-0.09"/>
<define name="VoltageOfAdc(adc)" value="(VOLTAGE_ADC_A * adc + VOLTAGE_ADC_B)"/>
<define name="LOW_BATTERY" value="9.3" unit="V"/>
</section>
@@ -113,6 +114,28 @@
<define name="YAW_RESPONSE_FACTOR" value="1.35"/>
<define name="WEIGHT" value="1.3"/>
</section>
<section name="AGGRESSIVE" prefix="AGR_">
<define name="CLIMB_GAZ" value="1.0"/><!-- Gaz for Aggressive Climb -->
<define name="DESCENT_GAZ" value="0.1"/><!-- Gaz for Aggressive Decent -->
<define name="CLIMB_PITCH" value="0.52"/><!-- Pitch for Aggressive Climb -->
<define name="DESCENT_PITCH" value="-0.7"/><!-- Pitch for Aggressive Decent -->
<define name="BLEND_START" value="30"/><!-- Altitude Error to Initiate Aggressive Climb CANNOT BE ZERO!!-->
<define name="BLEND_END" value="15"/><!-- 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">
<define name="GYRO_MAX_RATE" value="150."/>
<define name="ROLLRATESUM_NB_SAMPLES" value="60"/>
<define name="ATTITUDE_PGAIN" value="100.0"/>
<define name="ROLL_RATE_PGAIN" value="85.0"/>
<define name="ROLL_RATE_IGAIN" value="0.0"/>
<define name="ROLL_RATE_DGAIN" value="0.0"/>
<define name="ROLL_ATTITUDE_PGAIN" value="120.0"/><!-- new gyro ,was 600-->
</section>
<!--
<section name="DATALINK" prefix="DATALINK_">
@@ -142,7 +165,7 @@ ap.srcs += downlink.c pprz_transport.c $(SRC_ARCH)/uart_hw.c datalink.c
ap.CFLAGS += -DINTER_MCU
ap.srcs += inter_mcu.c
ap.CFLAGS += -DADC -DUSE_AD0 -DUSE_AD0_1 -DUSE_AD0_2 -DUSE_AD0_3 -DUSE_AD1 -DUSE_AD1_7
ap.CFLAGS += -DADC -DUSE_AD0 -DUSE_AD0_1 -DUSE_AD0_2 -DUSE_AD0_3 -DUSE_AD1 -DUSE_AD1_2 -DUSE_AD1_7 -DUSE_AD1_3
ap.srcs += $(SRC_ARCH)/adc_hw.c
ap.CFLAGS += -DGPS -DUBX -DUSE_UART1 -DGPS_LINK=Uart1 -DUART1_BAUD=B19200
@@ -155,9 +178,10 @@ ap.srcs += infrared.c estimator.c
ap.srcs += nav.c pid.c
ap.CFLAGS += -DIDG300
ap.CFLAGS += -DGYRO -DIDC300 -DATTITUDE_RATE_MODE
ap.srcs += gyro.c
# Harware In The Loop
#ap.CFLAGS += -DHITL
conf/airframes/twinjet1.xml
+44 -4
View File
@@ -37,11 +37,14 @@
</section>
<section name="adc" prefix="ADC_CHANNEL_">
<define name="IR1" value="1"/>
<define name="IR2" value="2"/>
<define name="IR_TOP" value="3"/>
<define name="IR1" value="AdcBank0(1)"/>
<define name="IR2" value="AdcBank0(2)"/>
<define name="IR_TOP" value="AdcBank0(3)"/>
<define name="GYRO_ROLL" value="AdcBank1(2)"/>
<define name="GYRO_TEMP" value="AdcBank1(3)"/>
<define name="IR_NB_SAMPLES" value="16"/>
<define name="VSUPPLY" value="6"/>
<define name="GYRO_NB_SAMPLES" value="16"/>
<define name="VSUPPLY" value="AdcBank0(6)"/>
</section>
<section name="INFRARED" prefix="IR_">
@@ -51,12 +54,19 @@
<define name="RAD_OF_IR_CONTRAST" value="0.75"/>
<linear name="RollOfIrs" arity="2" coeff1="0" coeff2="-1"/>
<linear name="PitchOfIrs" arity="2" coeff1="-1" coeff2="0"/>
<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="-512"/>
<define name="ADC_PITCH_NEUTRAL" value="-512"/>
<define name="ADC_TOP_NEUTRAL" value="512"/>
</section>
<section name="GYRO" prefix="GYRO_">
<define name="ADC_ROLL_NEUTRAL" value="316"/>
<define name="ADC_TEMP_NEUTRAL" value="328"/>
<define name="ADC_TEMP_SLOPE" value="-0.36"/>
</section>
<section name="PID">
<define name="ROLL_PGAIN" value="5000."/>
@@ -68,6 +78,16 @@
<define name="AILERON_OF_GAZ" value="0.0"/>
</section>
<section name="GYRO">
<define name="GYRO_MAX_RATE" value="150."/>
<define name="ROLLRATESUM_NB_SAMPLES" value="60"/>
<define name="ATTITUDE_PGAIN" value="25.0"/>
<define name="ROLL_RATE_PGAIN" value="85.0"/>
<define name="ROLL_RATE_IGAIN" value="0.0"/>
<define name="ROLL_RATE_DGAIN" value="0.0"/>
<define name="ROLL_ATTITUDE_PGAIN" value="120.0"/><!-- new gyro ,was 600-->
</section>
<section name="ALT" prefix="CLIMB_">
<define name="PITCH_PGAIN" value="-0.1"/>
<define name="PITCH_IGAIN" value="0.025"/>
@@ -112,6 +132,19 @@
<define name="HOME_RADIUS" value="50" unit="m"/>
</section>
<section name="AGGRESSIVE" prefix="AGR_">
<define name="CLIMB_GAZ" value="0.65"/><!-- Gaz for Aggressive Climb -->
<define name="DESCENT_GAZ" value="0.1"/><!-- Gaz for Aggressive Decent -->
<define name="CLIMB_PITCH" value="0.2"/><!-- 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>
<makefile>
include $(PAPARAZZI_SRC)/conf/autopilot/classix.makefile
@@ -148,12 +181,19 @@ ap.srcs += inter_mcu.c link_mcu.c spi.c $(SRC_ARCH)/spi_hw.c
ap.CFLAGS += -DGPS -DUBX -DUSE_UART1 -DGPS_LINK=Uart1 -DUART1_BAUD=B38400
ap.srcs += gps_ubx.c gps.c
# ADCs for infrared
ap.CFLAGS += -DADC -DUSE_AD0 -DUSE_AD0_1 -DUSE_AD0_2 -DUSE_AD0_3
# ADCs for gyro
ap.CFLAGS += -DUSE_AD1 -DUSE_AD1_2 -DUSE_AD1_3
ap.srcs += $(SRC_ARCH)/adc_hw.c
ap.CFLAGS += -DINFRARED
ap.srcs += infrared.c estimator.c
ap.CFLAGS += -DGYRO -DATTITUDE_RATE_MODE -DSPARK_FUN
ap.srcs += gyro.c
ap.srcs += nav.c pid.c
test.CFLAGS += -DFBW -DCONFIG=\"classix.h\" -DLED -DTIME_LED=1 -DACTUATORS=\"servos_4017_hw.h\" -DSERVOS_4017 -DSERVOS_4017_CLOCK_FALLING -DUSE_UART0 -DDATALINK -DPPRZ_INPUT -DPPRZ_UART=Uart0
conf/airframes/twinstar1.xml
+1 -1
View File
@@ -61,7 +61,7 @@
<define name="ROLL_PGAIN" value="10000."/>
<define name="PITCH_OF_ROLL" value="0.0"/>
<define name="PITCH_PGAIN" value="15000."/>
<define name="MAX_ROLL" value="0.35"/>
<define name="MAX_ROLL" value="0.55"/>
<define name="MAX_PITCH" value="0.35"/>
<define name="MIN_PITCH" value="-0.35"/>
<define name="AILERON_OF_GAZ" value="0"/>
conf/airframes/twinstar2.xml
+1 -1
View File
@@ -64,7 +64,7 @@
<define name="ROLL_PGAIN" value="10000."/>
<define name="PITCH_OF_ROLL" value="0.0"/>
<define name="PITCH_PGAIN" value="15000."/>
<define name="MAX_ROLL" value="0.35"/>
<define name="MAX_ROLL" value="0.55"/>
<define name="MAX_PITCH" value="0.35"/>
<define name="MIN_PITCH" value="-0.35"/>
<define name="AILERON_OF_GAZ" value="0"/>
conf/autopilot/tiny.h
+10
View File
@@ -51,4 +51,14 @@
#define PPM_PINSEL_VAL 0x02
#define PPM_PINSEL_BIT 12
/* ADC */
#define ADC_0 AdcBank0(3)
#define ADC_1 AdcBank0(2)
#define ADC_2 AdcBank0(1)
#define ADC_3 AdcBank1(7)
#define ADC_4 AdcBank1(3)
#define ADC_5 AdcBank1(4)
#define ADC_6 AdcBank1(5)
#define ADC_7 AdcBank1(2)
#endif /* CONFIG_TINY_H */
conf/flight_plans/mav05_ccw.xml
+3 -1
View File
@@ -16,6 +16,8 @@
<dl_setting VAR="phi_c" MIN="-1." STEP="0.05" MAX="1."/>
<dl_setting VAR="theta_c" MIN="-1." STEP="0.05" MAX="1."/>
<dl_setting VAR="course_pgain" MIN="-1." STEP="0.05" MAX="-0.1"/>
<dl_setting VAR="nav_prebank" MIN="0." STEP="0.1" MAX="1."/>
<dl_setting VAR="nav_speed_depend" MIN="0." STEP="0.1" MAX="1."/>
</dl_settings>
<waypoints>
@@ -31,7 +33,7 @@
</waypoints>
<include Y="0" NAME="zz" PROCEDURE="zigzag.xml" X="0" ROTATE="0">
<arg NAME="alt" VALUE="GROUND_ALT+50"/>
<arg NAME="rad" VALUE="-40"/>
<arg NAME="rad" VALUE="40"/>
<with FROM="end" TO="follow"/>
</include>
conf/flight_plans/mav05_cw.xml
+9
View File
@@ -11,6 +11,15 @@
<setting VAR="ir_roll_neutral" RANGE="-0.1" RC="gain_1_down" TYPE="float"/>
</mode>
</rc_control>
<dl_settings>
<dl_setting VAR="ir_roll_neutral" MAX="0.2" STEP="0.01" MIN="-0.2"/>
<dl_setting VAR="roll_pgain" MAX="15000" STEP="100" MIN="5000"/>
<dl_setting VAR="phi_c" MIN="-1." STEP="0.05" MAX="1."/>
<dl_setting VAR="theta_c" MIN="-1." STEP="0.05" MAX="1."/>
<dl_setting VAR="course_pgain" MIN="-1." STEP="0.05" MAX="-0.1"/>
</dl_settings>
<waypoints>
<waypoint name="HOME" x="-30" y="30" alt="730."/>
</waypoints>
conf/flight_plans/sectors.xml
+9
View File
@@ -21,4 +21,13 @@
<point pos="WGS84 43.448759 1.264170"/>
</polygon>
</sector>
<sector name="latrape">
<polygon>
<point pos="WGS84 43.239970 1.333672"/>
<point pos="WGS84 43.241134 1.298982"/>
<point pos="WGS84 43.232172 1.299416"/>
<point pos="WGS84 43.232776 1.334940"/>
</polygon>
</sector>
</sectors>
conf/messages.xml
+5
View File
@@ -232,6 +232,11 @@
<field name="alt" type="float" unit="m"></field>
</message>
<message name="GYRO_RATES" id="36">
<field name="roll" type="float" unit="rad/s"></field>
<field name="pitch" type="float" unit="rad/s"></field>
</message>
<message name="PPM" ID="100">
<field name="values" type="uint16[]" unit="ticks"/>
conf/telemetry/default.xml
+1
View File
@@ -22,6 +22,7 @@
<message name="WP_MOVED" period="0.5"/>
<message name="RAD_OF_IR" period="1"/>
<message name="IR_SENSORS" period="1"/>
<message name="GYRO_RATES" period="1"/>
</mode>
<mode name="debug">
<message name="CALIB_START" period="0.5"/>
sw/airborne/adc.h
+1
View File
@@ -35,6 +35,7 @@
#define _ADC_H_
#include <inttypes.h>
#include "adc_hw.h"
#define NB_ADC 8
#define MAX_AV_NB_SAMPLE 0x20
sw/airborne/ap_downlink.h
+11 -2
View File
@@ -95,18 +95,27 @@
#ifdef INFRARED
#define PERIODIC_SEND_IR_SENSORS() DOWNLINK_SEND_IR_SENSORS(&ir_pitch, &ir_roll, &ir_top);
#define PERIODIC_SEND_ADC() {}
#define PERIODIC_SEND_RAD_OF_IR() Downlink({ int16_t rad = DeciDegOfRad(estimator_rad); DOWNLINK_SEND_RAD_OF_IR(&ir_roll, &rad, &estimator_rad_of_ir);})
#define PERIODIC_SEND_CALIB_START() if (!estimator_flight_time && calib_status == WAITING_CALIB_CONTRAST) { DOWNLINK_SEND_CALIB_START(); }
#define PERIODIC_SEND_CALIB_CONTRAST() if (!estimator_flight_time && calib_status == CALIB_DONE) { DOWNLINK_SEND_CALIB_CONTRAST(&ir_contrast); }
#else
#define PERIODIC_SEND_ADC() {}
#define SEND_RAD_OF_IR() {}
#define PERIODIC_SEND_CALIB_START() {}
#define PERIODIC_SEND_CALIB_CONTRAST() {}
#endif
#define PERIODIC_SEND_ADC() {}
#ifdef IDC300
#include "gyro.h"
#define PERIODIC_SEND_GYRO_RATES() DOWNLINK_SEND_GYRO_RATES(&roll_rate, &pitch_rate)
#elif defined SPARK_FUN
#define PERIODIC_SEND_GYRO_RATES() DOWNLINK_SEND_GYRO_RATES(&roll_rate, &temp_comp)
#else
#define PERIODIC_SEND_GYRO_RATES() {}
#endif
#define PERIODIC_SEND_CALIBRATION() DOWNLINK_SEND_CALIBRATION(&climb_sum_err, &climb_pgain, &course_pgain)
#define PERIODIC_SEND_CIRCLE() if (in_circle) { DOWNLINK_SEND_CIRCLE(&circle_x, &circle_y, &circle_radius); }
sw/airborne/arm7/adc_hw.c
+3 -2
View File
@@ -4,7 +4,8 @@
#include "armVIC.h"
#include CONFIG
static struct adc_buf* buffers[NB_ADC];
/** First NB_ADC for bank 0, others for bank 2 */
static struct adc_buf* buffers[NB_ADC*2];
volatile uint16_t adc0_val[NB_ADC] = {1, 2, 3, 4, 5, 6, 7, 8};
volatile uint16_t adc1_val[NB_ADC] = {9, 10, 11, 12, 13, 14, 15, 16};
@@ -211,7 +212,7 @@ void adcISR1 ( void ) {
uint16_t value = (uint16_t)(tmp >> 6) & 0x03FF;
adc1_val[channel] = value;
LED_TOGGLE(2);
struct adc_buf* buf = buffers[channel];
struct adc_buf* buf = buffers[channel+NB_ADC];
if (buf) {
uint8_t new_head = buf->head + 1;
if (new_head >= buf->av_nb_sample) new_head = 0;
sw/airborne/arm7/adc_hw.h
+2
View File
@@ -0,0 +1,2 @@
#define AdcBank0(x) (x)
#define AdcBank1(x) (x+NB_ADC)
sw/airborne/autopilot.h
-1
View File
@@ -88,7 +88,6 @@ extern uint8_t fatal_error_nb;
#define GAZ_THRESHOLD_TAKEOFF (pprz_t)(MAX_PPRZ * 0.9)
extern uint8_t vertical_mode;
extern bool_t auto_pitch;
extern uint8_t lateral_mode;
extern uint8_t vsupply;
sw/airborne/avr/adc_hw.h
View File
sw/airborne/infrared.c
+10 -4
View File
@@ -40,6 +40,8 @@ int16_t ir_roll;
int16_t ir_pitch;
int16_t ir_top;
float z_constrast_mode;
/** Initialized to \a IR_DEFAULT_CONTRAST. Changed with calibration */
int16_t ir_contrast = IR_DEFAULT_CONTRAST;
/** Initialized to \a IR_DEFAULT_CONTRAST.
@@ -92,7 +94,7 @@ void ir_update(void) {
ir_roll = IR_RollOfIrs(x1_mean, x2_mean);
ir_pitch = IR_PitchOfIrs(x1_mean, x2_mean);
#ifdef ADC_CHANNEL_IR_TOP
ir_top = buf_ir_top.sum/buf_ir_top.av_nb_sample - IR_ADC_TOP_NEUTRAL;
ir_top = IR_TopOfIr(buf_ir_top.sum/buf_ir_top.av_nb_sample - IR_ADC_TOP_NEUTRAL);
#endif
/** neutrals are not taken into account in SITL and HITL */
@@ -199,7 +201,11 @@ void estimator_update_state_infrared( void ) {
float rad_of_ir = (ir_estim_mode == IR_ESTIM_MODE_ON ?
estimator_rad_of_ir :
ir_rad_of_ir);
estimator_phi = rad_of_ir * ir_roll - ir_roll_neutral;
estimator_theta = rad_of_ir * ir_pitch - ir_pitch_neutral;
#ifndef ADC_CHANNEL_IR_TOP
z_constrast_mode = 0;
#endif
ir_top = Max(ir_top, 1);
float c = rad_of_ir*(1-z_constrast_mode)+z_constrast_mode*(IR_RAD_OF_IR_CONTRAST/ir_top);
estimator_phi = c * ir_roll - ir_roll_neutral;
estimator_theta = c * ir_pitch - ir_pitch_neutral;
}
sw/airborne/infrared.h
+1
View File
@@ -46,6 +46,7 @@ extern int16_t ir_top; /* averaged vertical ir adc */
extern float ir_rad_of_ir;
extern int16_t ir_contrast;
extern float z_constrast_mode;
/** Status of the calibration. Can be one of the \a calibration \a states */
extern uint8_t calib_status;
sw/airborne/main_ap.c
+8 -3
View File
@@ -39,6 +39,7 @@
#include "pid.h"
#include "gps.h"
#include "infrared.h"
#include "gyro.h"
#include "ap_downlink.h"
#include "nav.h"
#include "autopilot.h"
@@ -82,8 +83,6 @@ uint8_t lateral_mode = LATERAL_MODE_MANUAL;
uint8_t ir_estim_mode = IR_ESTIM_MODE_ON;
bool_t auto_pitch = FALSE;
bool_t rc_event_1, rc_event_2;
uint8_t vsupply;
@@ -119,7 +118,7 @@ static inline uint8_t pprz_mode_update( void ) {
/** \fn inline uint8_t ir_estim_mode_update( void )
* \brief update ir estimation if RADIO_LLS is true \n
*/
inline uint8_t ir_estim_mode_update( void ) {
static inline uint8_t ir_estim_mode_update( void ) {
return ModeUpdate(ir_estim_mode, IR_ESTIM_MODE_OF_PULSE(fbw_state->channels[RADIO_LLS]));
}
#endif
@@ -411,6 +410,9 @@ inline void periodic_task_ap( void ) {
}
switch (_20Hz) {
case 0:
#if defined GYRO
gyro_update();
#endif
break;
case 1: {
static uint8_t odd;
@@ -469,6 +471,9 @@ void init_ap( void ) {
#ifdef INFRARED
ir_init();
#endif
#ifdef GYRO
gyro_init();
#endif
#ifdef GPS
gps_init();
#endif
sw/airborne/main_ap_2.c
-1
View File
@@ -23,7 +23,6 @@
#include "airframe.h"
float pitch_of_vz;
float pitch_of_vz_pgain;
bool_t auto_pitch = FALSE;
uint8_t ac_ident = AC_ID;
float desired_roll = 0.;
float desired_pitch = 0.;
sw/airborne/nav.c
+107 -23
View File
@@ -40,6 +40,10 @@
#include "cam.h"
#include "traffic_info.h"
#define Distance2(p1_x, p1_y, p2_x, p2_y) ((p1_x-p2_x)*(p1_x-p2_x)+(p1_y-p2_y)*(p1_y-p2_y))
#define Square(_x) ((_x)*(_x))
#define G 9.81
uint8_t nav_stage, nav_block;
/** To save the current stage when an exception is raised */
uint8_t excpt_stage;
@@ -65,6 +69,9 @@ bool_t in_segment = FALSE;
int16_t circle_x, circle_y, circle_radius;
int16_t segment_x_1, segment_y_1, segment_x_2, segment_y_2;
uint8_t horizontal_mode;
float circle_bank = 0;
float nav_prebank, nav_speed_depend;
#define RcRoll(travel) (fbw_state->channels[RADIO_ROLL]* (float)travel /(float)MAX_PPRZ)
@@ -115,12 +122,27 @@ static float qdr;
sum_alpha += alpha_diff; \
} else \
new_circle = FALSE; \
float dist2_center = Distance2(estimator_x, estimator_y, x, y); \
float dist_carrot = CARROT*estimator_hspeed_mod; \
float abs_radius = fabs(radius); \
float sign_radius = radius > 0 ? 1 : -1; \
circle_bank = \
(dist2_center > Square(abs_radius + dist_carrot) \
|| dist2_center < Square(abs_radius - dist_carrot)) ? \
0 : \
sign_radius * atan2(estimator_hspeed_mod*estimator_hspeed_mod, \
G*abs_radius); \
circle_bank *= nav_prebank; \
circle_count = fabs(sum_alpha) / (2*M_PI); \
float alpha_carrot = alpha + CARROT / -radius * estimator_hspeed_mod; \
float carrot_angle = CARROT / abs_radius * estimator_hspeed_mod; \
carrot_angle = Min(carrot_angle, M_PI/4); \
carrot_angle = Max(carrot_angle, M_PI/16); \
float alpha_carrot = alpha - sign_radius * carrot_angle; \
horizontal_mode = HORIZONTAL_MODE_CIRCLE; \
fly_to_xy(x+cos(alpha_carrot)*fabs(radius), \
y+sin(alpha_carrot)*fabs(radius)); \
qdr = DegOfRad(M_PI/2 - alpha_carrot); \
float radius_carrot = abs_radius + nav_prebank * (abs_radius / cos(carrot_angle) - abs_radius); \
fly_to_xy(x+cos(alpha_carrot)*radius_carrot, \
y+sin(alpha_carrot)*radius_carrot); \
qdr = DegOfRad(M_PI/2 - alpha); \
NormCourse(qdr); \
in_circle = TRUE; \
circle_x = x; \
@@ -217,7 +239,7 @@ static inline void survey_init(float y_south, float y_north, float grid) {
#define MIN_DIST2_WP (15.*15.)
#define DISTANCE2(p1_x, p1_y, p2) ((p1_x-p2.x)*(p1_x-p2.x)+(p1_y-p2.y)*(p1_y-p2.y))
// #define DISTANCE2(p1_x, p1_y, p2) ((p1_x-p2.x)*(p1_x-p2.x)+(p1_y-p2.y)*(p1_y-p2.y))
int32_t nav_utm_east0 = NAV_UTM_EAST0;
int32_t nav_utm_north0 = NAV_UTM_NORTH0;
@@ -380,14 +402,12 @@ void nav_without_gps(void) {
#ifdef SECTION_FAILSAFE
lateral_mode = LATERAL_MODE_ROLL;
nav_desired_roll = FAILSAFE_DEFAULT_ROLL;
auto_pitch = FALSE;
nav_pitch = FAILSAFE_DEFAULT_PITCH;
vertical_mode = VERTICAL_MODE_AUTO_GAZ;
nav_desired_gaz = TRIM_UPPRZ((FAILSAFE_DEFAULT_GAZ)*MAX_PPRZ);
#else
lateral_mode = LATERAL_MODE_ROLL;
nav_desired_roll = 0;
auto_pitch = FALSE;
nav_pitch = 0;
vertical_mode = VERTICAL_MODE_AUTO_GAZ;
nav_desired_gaz = TRIM_UPPRZ((CLIMB_LEVEL_GAZ)*MAX_PPRZ);
@@ -397,6 +417,15 @@ void nav_without_gps(void) {
float course_pgain = COURSE_PGAIN;
float desired_course = 0.;
float max_roll = MAX_ROLL;
float altitude_error;
#define CLIMB_MODE_GAZ 0
#define CLIMB_MODE_PITCH 1
#define CLIMB_MODE_AGRESSIVE 2
#define CLIMB_MODE_BLENDED 3
uint8_t climb_mode = CLIMB_MODE_GAZ;
/** \brief Computes ::nav_desired_roll from course estimation and expected
course.
@@ -404,7 +433,20 @@ float max_roll = MAX_ROLL;
void course_pid_run( void ) {
float err = estimator_hspeed_dir - desired_course;
NormRadAngle(err);
nav_desired_roll = course_pgain * err;
float speed_depend_nav = estimator_hspeed_mod/NOMINAL_AIRSPEED;
speed_depend_nav = Max(speed_depend_nav,0.66);
speed_depend_nav = Min(speed_depend_nav,1.5);
speed_depend_nav = 1. + nav_speed_depend * (speed_depend_nav-1.);
float roll_from_err = course_pgain * speed_depend_nav * err;
#if defined AGR_CLIMB_GAZ
if (climb_mode == CLIMB_MODE_AGRESSIVE) {
if (altitude_error < 0)
roll_from_err *= AGR_CLIMB_NAV_RATIO;
else
roll_from_err *= AGR_DESCENT_NAV_RATIO;
}
#endif
nav_desired_roll = (in_circle ? circle_bank : 0) + roll_from_err;
if (nav_desired_roll > max_roll)
nav_desired_roll = max_roll;
else if (nav_desired_roll < -max_roll)
@@ -427,31 +469,73 @@ float climb_level_gaz = CLIMB_LEVEL_GAZ;
/** \brief Computes desired_gaz and updates nav_pitch from desired_climb */
void
climb_pid_run ( void ) {
float fgaz = 0;
float err = estimator_z_dot - desired_climb;
if (auto_pitch) { /* gaz constant */
desired_gaz = nav_desired_gaz;
float controlled_gaz = climb_pgain * (err + climb_igain * climb_sum_err) + climb_level_gaz + CLIMB_GAZ_OF_CLIMB*desired_climb;
/* pitch offset for climb */
pitch_of_vz = desired_climb * pitch_of_vz_pgain;
switch (climb_mode) {
case CLIMB_MODE_GAZ:
fgaz = controlled_gaz;
climb_sum_err += err;
if (climb_sum_err > MAX_CLIMB_SUM_ERR) climb_sum_err = MAX_CLIMB_SUM_ERR;
if (climb_sum_err < - MAX_CLIMB_SUM_ERR) climb_sum_err = - MAX_CLIMB_SUM_ERR;
nav_pitch += pitch_of_vz;
break;
case CLIMB_MODE_PITCH:
fgaz = nav_desired_gaz;
nav_pitch = climb_pitch_pgain * (err + climb_pitch_igain * climb_pitch_sum_err);
Bound(nav_pitch, MIN_PITCH, MAX_PITCH);
climb_pitch_sum_err += err;
BoundAbs(climb_pitch_sum_err, MAX_PITCH_CLIMB_SUM_ERR);
} else { /* pitch almost constant */
/* pitch offset for climb */
pitch_of_vz = (desired_climb > 0) ? desired_climb * pitch_of_vz_pgain : 0.;
float fgaz = climb_pgain * (err + climb_igain * climb_sum_err) + climb_level_gaz + CLIMB_GAZ_OF_CLIMB*desired_climb;
climb_sum_err += err;
if (climb_sum_err > MAX_CLIMB_SUM_ERR) climb_sum_err = MAX_CLIMB_SUM_ERR;
if (climb_sum_err < - MAX_CLIMB_SUM_ERR) climb_sum_err = - MAX_CLIMB_SUM_ERR;
desired_gaz = TRIM_UPPRZ(fgaz * MAX_PPRZ);
nav_pitch += pitch_of_vz;
break;
#if defined AGR_CLIMB_GAZ
case CLIMB_MODE_AGRESSIVE:
if (altitude_error < 0) {
fgaz = AGR_CLIMB_GAZ;
nav_pitch = AGR_CLIMB_PITCH;
} else {
fgaz = AGR_DESCENT_GAZ;
nav_pitch = AGR_DESCENT_PITCH;
}
break;
case CLIMB_MODE_BLENDED: {
float ratio = (fabs(altitude_error) - AGR_BLEND_END) / (AGR_BLEND_START-AGR_BLEND_END);
if (altitude_error < 0) {
fgaz = ratio*AGR_CLIMB_GAZ + (1-ratio)*controlled_gaz;
nav_pitch = ratio*AGR_CLIMB_PITCH + (1-ratio)*pitch_of_vz;
} else {
fgaz = ratio*AGR_DESCENT_GAZ + (1-ratio)*controlled_gaz;
nav_pitch = ratio*AGR_DESCENT_PITCH + (1-ratio)*pitch_of_vz;
}
break;
}
#endif
}
desired_gaz = TRIM_UPPRZ(fgaz * MAX_PPRZ);
}
float altitude_pgain = ALTITUDE_PGAIN;
void altitude_pid_run(void) {
float err = estimator_z - desired_altitude;
desired_climb = pre_climb + altitude_pgain * err;
if (desired_climb < -CLIMB_MAX) desired_climb = -CLIMB_MAX;
if (desired_climb > CLIMB_MAX) desired_climb = CLIMB_MAX;
altitude_error = estimator_z - desired_altitude;
#ifdef AGR_CLIMB_GAZ
float dist = fabs(altitude_error);
if (dist < AGR_BLEND_END) {
#endif
desired_climb = pre_climb + altitude_pgain * altitude_error;
if (desired_climb < -CLIMB_MAX) desired_climb = -CLIMB_MAX;
if (desired_climb > CLIMB_MAX) desired_climb = CLIMB_MAX;
climb_mode = CLIMB_MODE_GAZ;
#ifdef AGR_CLIMB_GAZ
} else if (dist > AGR_BLEND_START)
climb_mode = CLIMB_MODE_AGRESSIVE;
else
climb_mode = CLIMB_MODE_BLENDED;
#endif
}
sw/airborne/nav.h
+1
View File
@@ -104,6 +104,7 @@ extern float pitch_of_vz_pgain;
extern float pitch_of_vz;
extern float aileron_of_gaz;
extern float climb_level_gaz;
extern float nav_prebank, nav_speed_depend;
void climb_pid_run(void);
void altitude_pid_run(void);
sw/airborne/pid.c
+67 -2
View File
@@ -34,30 +34,95 @@
#include "autopilot.h"
#include "infrared.h"
#include "gyro.h"
#include "estimator.h"
#include "nav.h"
float desired_roll = 0.;
float desired_pitch = 0.;
int16_t desired_gaz, desired_aileron, desired_elevator;
float roll_pgain = ROLL_PGAIN;
float pitch_pgain = PITCH_PGAIN;
#ifdef PITCH_IGAIN
float pitch_igain = PITCH_IGAIN;
#endif
float pitch_of_roll = PITCH_OF_ROLL;
#ifdef ATTITUDE_RATE_MODE
float attitude_pgain = ATTITUDE_PGAIN;
float roll_rate_pgain = ROLL_RATE_PGAIN;
float roll_rate_dgain = ROLL_RATE_DGAIN;
float roll_rate_igain = ROLL_RATE_IGAIN;
#ifdef PITCH_RATE_PGAIN
float pitch_rate_pgain = PITCH_RATE_PGAIN;
float pitch_rate_dgain = PITCH_RATE_DGAIN;
float pitch_rate_igain = PITCH_RATE_IGAIN;
#endif
#endif
float pitch_of_vz_pgain = CLIMB_PITCH_OF_VZ_PGAIN;
float pitch_of_vz = 0.;
float aileron_of_gaz = AILERON_OF_GAZ;
float rate_mode = 0;
/** \brief Computes ::desired_aileron and ::desired_elevator from attitude
* estimation and expected attitude.
*/
void roll_pitch_pid_run( void ) {
/** Attitude PID */
float err = estimator_phi - desired_roll;
desired_aileron = TRIM_PPRZ(roll_pgain * err + desired_gaz * aileron_of_gaz);
Bound(err, -M_PI/2., M_PI/2);
float std_desired_aileron = TRIM_PPRZ(roll_pgain * err + desired_gaz * aileron_of_gaz);
#ifndef ATTITUDE_RATE_MODE
desired_aileron = std_desired_aileron;
#else
float desired_roll_rate = -attitude_pgain*err;
Bound(desired_roll_rate, -GYRO_MAX_RATE, GYRO_MAX_RATE);
/** Roll rate pid */
err = roll_rate - desired_roll_rate;
Bound(err, -GYRO_MAX_RATE, GYRO_MAX_RATE);
static float rollrate_sum_err = 0.;
static uint8_t rollratesum_idx = 0;
static float rollratesum_values[ROLLRATESUM_NB_SAMPLES];
rollrate_sum_err -= rollratesum_values[rollratesum_idx];
rollratesum_values[rollratesum_idx] = err;
rollrate_sum_err += err;
rollratesum_idx++;
if (rollratesum_idx >= ROLLRATESUM_NB_SAMPLES) rollratesum_idx = 0;
/* D term calculations */
static float last_roll_rate;
float d_err = roll_rate - last_roll_rate;
last_roll_rate = roll_rate;
float rate_desired_aileron = TRIM_PPRZ(roll_rate_pgain*(err + roll_rate_igain*(rollrate_sum_err/ROLLRATESUM_NB_SAMPLES) + roll_rate_dgain*d_err));
desired_aileron = rate_mode*rate_desired_aileron+(1-rate_mode)*std_desired_aileron;
#endif
/** Pitch pi */
if (pitch_of_roll <0.)
pitch_of_roll = 0.;
err = -(estimator_theta - desired_pitch - pitch_of_roll * fabs(estimator_phi));
err = -(estimator_theta - desired_pitch - pitch_of_roll*fabs(estimator_phi));
Bound(err, -M_PI/2, M_PI/2);
#ifdef PITCH_IGAIN
pitch_sum_err -= pitchsum_values[pitchsum_idx];
pitchsum_values[pitchsum_idx] = err;
pitch_sum_err += pitchsum_values[pitchsum_idx];
pitchsum_idx++;
if (pitchsum_idx >= PITCHSUM_NB_SAMPLES) pitchsum_idx = 0;
desired_elevator = TRIM_PPRZ(pitch_pgain * thr_pitch_multi * (err + (pitch_sum_err/(pitchsum_nb_samples))*pitch_igain));
#else
desired_elevator = TRIM_PPRZ(pitch_pgain * err);
#endif
}
sw/airborne/pid.h
+3
View File
@@ -33,8 +33,11 @@ extern float desired_roll;
extern float max_roll;
extern float desired_pitch;
extern float roll_pgain;
extern float roll_rate_pgain;
extern float attitude_pgain;
extern float pitch_pgain;
extern float pitch_of_roll;
extern float rate_mode;
void roll_pitch_pid_run( void );
sw/airborne/sim/adc_hw.h
View File
sw/ground_segment/cockpit/map2d.ml
+2
View File
@@ -906,6 +906,8 @@ let button_press = fun (geomap:G.widget) ev ->
let label = GMisc.label ~text:name ~packing:eb#add () in
eb#coerce#misc#modify_bg [`NORMAL, `NAME color;`ACTIVE, `NAME color];
(fp_notebook:GPack.notebook)#append_page ~tab_label:eb#coerce fp#window#coerce;
fp#connect_selection (fun _ -> prerr_endline "click");
fp#hide ();
ignore (reset_wp_menu#connect#activate (reset_waypoints fp));
Hashtbl.add live_aircrafts ac_id { track = track; color = color;
sw/lib/ocaml/mapCanvas.ml
+1 -1
View File
@@ -363,7 +363,7 @@ class basic_widget = fun ?(height=800) ?width ?(projection = Mercator) ?georef (
let scroll_event = GdkEvent.Scroll.cast ev in
let (x, y) = canvas#get_scroll_offsets in
let xr = GdkEvent.Scroll.x_root scroll_event in
let yr = GdkEvent.Scroll.y_root scroll_event -. 35. in
let yr = GdkEvent.Scroll.y_root scroll_event -. 50. in
match GdkEvent.Scroll.direction scroll_event with
`UP ->
canvas#scroll_to (x+truncate xr) (y+truncate yr);
sw/lib/ocaml/mapFP.ml
+2
View File
@@ -216,6 +216,8 @@ class flight_plan = fun ?edit geomap color fp_dtd xml ->
)
path
method connect_selection = fun cb -> XmlEdit.connect_selection xml_tree_view cb
initializer (
(** Create a graphic waypoint when it is created from the xml editor *)
XmlEdit.connect xml_wpts (function XmlEdit.New_child node -> ignore (create_wp node) | _ -> ())
sw/lib/ocaml/mapFP.mli
+1
View File
@@ -43,6 +43,7 @@ class flight_plan :
method xml : Xml.xml
method insert_path : (MapWaypoints.waypoint * float) list -> unit
method highlight_stage : int -> int -> unit
method connect_selection : (XmlEdit.node->unit) -> unit
end
(** Extracts [lat0] and [Lon0] attributes *)
sw/lib/ocaml/xmlEdit.ml
+8 -1
View File
@@ -379,6 +379,12 @@ let connect = fun ((model, path):node) cb ->
let current_cb = try model#get ~row ~column:event with _ -> fun _ -> () in
model#set ~row ~column:event (fun e -> cb e; current_cb e)
let selection_cbs = Hashtbl.create 3
let connect_selection = fun (model,_) cb ->
let l = try Hashtbl.find selection_cbs model with Not_found -> [] in
Hashtbl.replace selection_cbs model (cb::l)
let expand_node = fun ?all (_, (tree_view:GTree.view)) ((model, path):node) ->
tree_view#expand_row ?all path
@@ -477,7 +483,8 @@ let create = fun ?(edit=true) dtd xml ->
let attribs = tree_model#get ~row ~column:attributes in
attribs_model#clear ();
tag_of_last_selection := tree_model#get ~row ~column:tag_col;
set_attributes attribs_model attribs
set_attributes attribs_model attribs;
List.iter (fun cb -> cb (tree_model, path)) (Hashtbl.find selection_cbs tree_model)
| _ -> () in
let _c = tree_view#selection#connect#after#changed ~callback:selection_changed in
sw/lib/ocaml/xmlEdit.mli
+1
View File
@@ -65,6 +65,7 @@ val add_child : node -> tag -> attributes -> node
(** Modifications *)
val connect : node -> (event -> unit) -> unit
val connect_selection : t -> (node -> unit) -> unit
(** To be kept informed about modifications *)
val selection : t -> node
sw/tools/gen_flight_plan.ml
+3 -4
View File
@@ -155,10 +155,9 @@ let output_vmode x wp last_wp =
let pitch = try Xml.attrib x "pitch" with _ -> "0.0" in
if pitch = "auto"
then begin
lprintf "auto_pitch = TRUE;\n";
lprintf "nav_desired_gaz = TRIM_UPPRZ(%s*MAX_PPRZ);\n" (parsed_attrib x "gaz")
lprintf "climb_mode = CLIMB_MODE_PITCH;\n";
lprintf "nav_desired_gaz = %s;\n" (parsed_attrib x "gaz")
end else begin
lprintf "auto_pitch = FALSE;\n";
lprintf "nav_pitch = %s;\n" (parse pitch);
end;
let vmode = try ExtXml.attrib x "vmode" with _ -> "alt" in
@@ -194,7 +193,7 @@ let output_vmode x wp last_wp =
if (pitch = "auto") then
failwith "auto pich mode not compatible with vmode=gaz";
lprintf "vertical_mode = VERTICAL_MODE_AUTO_GAZ;\n";
lprintf "nav_desired_gaz = TRIM_UPPRZ(%s*MAX_PPRZ);\n" (parsed_attrib x "gaz")
lprintf "nav_desired_gaz = %s;\n" (parsed_attrib x "gaz")
| x -> failwith (sprintf "Unknown vmode '%s'" x)
end;
vmode