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

Merge pull request #471 from gautierhattenberger/master

Browse Source 
Remove HIGH_ACCEL_FLAG
This commit is contained in:
Gautier Hattenberger
2013-07-23 12:56:35 -07:00
parent 635a14dbb0 4b41dcd513
commit 4255275be3
9 changed files with 108 additions and 160 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/ENAC/fixed-wing/apogee.xml
+1 -3
View File
@@ -33,9 +33,7 @@
<!-- Actuators are automatically chosen according to board--> <!-- Actuators are automatically chosen according to board-->
<subsystem name="imu" type="apogee"/> <subsystem name="imu" type="apogee"/>
<subsystem name="ahrs" type="float_dcm"> <subsystem name="ahrs" type="float_dcm"/>
<define name="USE_HIGH_ACCEL_FLAG"/>
</subsystem>
<subsystem name="ins" type="alt_float"/> <subsystem name="ins" type="alt_float"/>
<subsystem name="control"/> <subsystem name="control"/>
<subsystem name="navigation"/> <subsystem name="navigation"/>
conf/airframes/ENAC/fixed-wing/eternity1.xml
+1 -3
View File
@@ -26,9 +26,7 @@
<subsystem name="radio_control" type="ppm"/> <subsystem name="radio_control" type="ppm"/>
<subsystem name="telemetry" type="transparent"/> <subsystem name="telemetry" type="transparent"/>
<subsystem name="imu" type="umarim"/> <subsystem name="imu" type="umarim"/>
<subsystem name="ahrs" type="float_dcm"> <subsystem name="ahrs" type="float_dcm"/>
<define name="USE_HIGH_ACCEL_FLAG"/>
</subsystem>
<subsystem name="ins" type="alt_float"/> <subsystem name="ins" type="alt_float"/>
<subsystem name="control"/> <subsystem name="control"/>
<subsystem name="gps" type="ublox"/> <subsystem name="gps" type="ublox"/>
conf/airframes/ENAC/fixed-wing/firestorm.xml
+1 -3
View File
@@ -32,9 +32,7 @@
<!-- Actuators are automatically chosen according to board--> <!-- Actuators are automatically chosen according to board-->
<subsystem name="imu" type="umarim"/> <subsystem name="imu" type="umarim"/>
<subsystem name="ahrs" type="float_dcm"> <subsystem name="ahrs" type="float_dcm"/>
<define name="USE_HIGH_ACCEL_FLAG"/>
</subsystem>
<subsystem name="ins" type="alt_float"> <subsystem name="ins" type="alt_float">
<!--define name="USE_BAROMETER"/--> <!--define name="USE_BAROMETER"/-->
</subsystem> </subsystem>
conf/airframes/ENAC/fixed-wing/funjet2.xml
+1 -3
View File
@@ -39,9 +39,7 @@
<!-- Actuators are automatically chosen according to board--> <!-- Actuators are automatically chosen according to board-->
<subsystem name="imu" type="umarim"/> <subsystem name="imu" type="umarim"/>
<subsystem name="ahrs" type="float_dcm"> <subsystem name="ahrs" type="float_dcm"/>
<define name="USE_HIGH_ACCEL_FLAG"/>
</subsystem>
<subsystem name="ins" type="alt_float"/> <subsystem name="ins" type="alt_float"/>
<subsystem name="control" type="new"> <subsystem name="control" type="new">
<define name="USE_GYRO_PITCH_RATE"/> <define name="USE_GYRO_PITCH_RATE"/>
conf/airframes/ENAC/fixed-wing/mythe.xml
+1 -3
View File
@@ -66,9 +66,7 @@
<!-- Actuators are automatically chosen according to board--> <!-- Actuators are automatically chosen according to board-->
<subsystem name="imu" type="umarim"/> <subsystem name="imu" type="umarim"/>
<subsystem name="ahrs" type="float_dcm"> <subsystem name="ahrs" type="float_dcm"/>
<define name="USE_HIGH_ACCEL_FLAG"/>
</subsystem>
<subsystem name="ins" type="alt_float"/> <subsystem name="ins" type="alt_float"/>
<subsystem name="control" type="new"/> <subsystem name="control" type="new"/>
<subsystem name="navigation"/> <subsystem name="navigation"/>
conf/airframes/ENAC/fixed-wing/weasel.xml
+25 -30
View File
@@ -20,26 +20,27 @@
<define name="USE_I2C1"/> <define name="USE_I2C1"/>
<!--define name="AGR_CLIMB"/--> <!--define name="AGR_CLIMB"/-->
<define name="ALT_KALMAN"/> <define name="ALT_KALMAN"/>
<define name="USE_AIRSPEED"/> <!--define name="USE_AIRSPEED"/-->
<!--define name="USE_BAROMETER"/--> <!--define name="USE_BAROMETER"/-->
<!--define name="LOITER_TRIM"/--> <!--define name="LOITER_TRIM"/-->
<!--define name="PITCH_TRIM"/--> <!--define name="PITCH_TRIM"/-->
<target name="sim" board="pc"/> <target name="sim" board="pc"/>
<target name="ap" board="umarim_1.0"> <target name="ap" board="umarim_1.0"/>
<configure name="FLASH_MODE" value="IAP"/>
</target>
<subsystem name="radio_control" type="ppm"/> <subsystem name="radio_control" type="ppm">
<define name="USE_PPM_RSSI_GPIO"/>
<define name="PPM_RSSI_IOPIN" value="IO0PIN"/>
<define name="PPM_RSSI_PIN" value="17"/>
<define name="PPM_RSSI_VALID_LEVEL" value="0"/>
</subsystem>
<!-- Communication --> <!-- Communication -->
<subsystem name="telemetry" type="xbee_api"/> <subsystem name="telemetry" type="xbee_api"/>
<!-- Actuators are automatically chosen according to board--> <!-- Actuators are automatically chosen according to board-->
<subsystem name="imu" type="umarim"/> <subsystem name="imu" type="umarim"/>
<subsystem name="ahrs" type="float_dcm"> <subsystem name="ahrs" type="float_dcm"/>
<define name="USE_HIGH_ACCEL_FLAG"/>
</subsystem>
<subsystem name="ins" type="alt_float"/> <subsystem name="ins" type="alt_float"/>
<subsystem name="control" type="new"/> <subsystem name="control" type="new"/>
<subsystem name="navigation"/> <subsystem name="navigation"/>
@@ -49,17 +50,11 @@
<!--subsystem name="spi"/--> <!--subsystem name="spi"/-->
</firmware> </firmware>
<firmware name="setup">
<target name="setup_actuators" board="umarim_1.0"/>
<target name="tunnel" board="umarim_1.0"/>
</firmware>
<!-- commands section --> <!-- commands section -->
<servos> <servos>
<servo name="MOTOR" no="7" min="1040" neutral="1040" max="2000"/> <servo name="MOTOR" no="7" min="1040" neutral="1040" max="2000"/>
<servo name="AILEVON_LEFT" no="6" min="1900" neutral="1543" max="1100"/> <servo name="AILEVON_LEFT" no="6" min="1750" neutral="1543" max="1250"/>
<servo name="AILEVON_RIGHT" no="3" min="1100" neutral="1561" max="1900"/> <servo name="AILEVON_RIGHT" no="3" min="1250" neutral="1561" max="1750"/>
</servos> </servos>
<commands> <commands>
@@ -83,13 +78,13 @@
<let var="aileron" value="@ROLL * AILEVON_AILERON_RATE"/> <let var="aileron" value="@ROLL * AILEVON_AILERON_RATE"/>
<let var="elevator" value="@PITCH * AILEVON_ELEVATOR_RATE"/> <let var="elevator" value="@PITCH * AILEVON_ELEVATOR_RATE"/>
<set servo="MOTOR" value="@THROTTLE"/> <set servo="MOTOR" value="@THROTTLE"/>
<set servo="AILEVON_LEFT" value="$elevator + $aileron"/> <set servo="AILEVON_LEFT" value="$elevator - $aileron"/>
<set servo="AILEVON_RIGHT" value="$elevator - $aileron"/> <set servo="AILEVON_RIGHT" value="$elevator + $aileron"/>
</command_laws> </command_laws>
<section name="AUTO1" prefix="AUTO1_"> <section name="AUTO1" prefix="AUTO1_">
<define name="MAX_ROLL" value="0.85"/> <define name="MAX_ROLL" value="45" unit="deg"/>
<define name="MAX_PITCH" value="0.6"/> <define name="MAX_PITCH" value="30" unit="deg"/>
</section> </section>
<section name="IMU" prefix="IMU_"> <section name="IMU" prefix="IMU_">
@@ -123,8 +118,8 @@
</section> </section>
<section name="INS" prefix="INS_"> <section name="INS" prefix="INS_">
<define name="ROLL_NEUTRAL_DEFAULT" value="0.048" unit="rad"/> <define name="ROLL_NEUTRAL_DEFAULT" value="3." unit="deg"/>
<define name="PITCH_NEUTRAL_DEFAULT" value="0." unit="rad"/> <define name="PITCH_NEUTRAL_DEFAULT" value="0." unit="deg"/>
<define name="BARO_SENS" value="1.0"/> <!-- TODO calibration --> <define name="BARO_SENS" value="1.0"/> <!-- TODO calibration -->
</section> </section>
@@ -161,15 +156,15 @@
<!-- Climb loop (throttle) --> <!-- Climb loop (throttle) -->
<define name="AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT" value="0.1" unit="%/(m/s)"/> <define name="AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT" value="0.1" unit="%/(m/s)"/>
<define name="AUTO_THROTTLE_PGAIN" value="0.004"/> <!-- -0.005 --> <define name="AUTO_THROTTLE_PGAIN" value="0.003"/> <!-- -0.005 -->
<define name="AUTO_THROTTLE_DGAIN" value="0.01"/> <!-- 0.005 --> <define name="AUTO_THROTTLE_DGAIN" value="0.007"/> <!-- 0.005 -->
<define name="AUTO_THROTTLE_IGAIN" value="0.004"/> <!-- 0.005 --> <define name="AUTO_THROTTLE_IGAIN" value="0.002"/> <!-- 0.005 -->
<!-- Climb loop (pitch) --> <!-- Climb loop (pitch) -->
<define name="AUTO_THROTTLE_PITCH_OF_VZ_PGAIN" value="0.02"/> <define name="AUTO_THROTTLE_PITCH_OF_VZ_PGAIN" value="0.02"/>
<define name="AUTO_PITCH_PGAIN" value="0.038"/> <!-- -0.03 --> <define name="AUTO_PITCH_PGAIN" value="0.006"/> <!-- -0.03 -->
<define name="AUTO_PITCH_DGAIN" value="0.036"/> <!-- -0.03 --> <define name="AUTO_PITCH_DGAIN" value="0.0"/> <!-- -0.03 -->
<define name="AUTO_PITCH_IGAIN" value="0.0"/> <define name="AUTO_PITCH_IGAIN" value="0.002"/>
<!-- airspeed control --> <!-- airspeed control -->
<define name="AUTO_AIRSPEED_SETPOINT" value="16."/> <define name="AUTO_AIRSPEED_SETPOINT" value="16."/>
@@ -206,8 +201,8 @@
<define name="ROLL_KFFA" value="0"/> <define name="ROLL_KFFA" value="0"/>
<define name="ROLL_KFFD" value="0"/> <define name="ROLL_KFFD" value="0"/>
<define name="PITCH_PGAIN" value="10000."/> <define name="PITCH_PGAIN" value="8000"/>
<define name="PITCH_DGAIN" value="0."/> <define name="PITCH_DGAIN" value="500"/>
<define name="PITCH_IGAIN" value="250."/> <define name="PITCH_IGAIN" value="250."/>
<define name="PITCH_KFFA" value="0."/> <define name="PITCH_KFFA" value="0."/>
<define name="PITCH_KFFD" value="0."/> <define name="PITCH_KFFD" value="0."/>
conf/airframes/ENAC/quadrotor/booz2_g1.xml
+1 -1
View File
@@ -213,7 +213,7 @@
<define name="IGAIN" value="15"/> <define name="IGAIN" value="15"/>
<define name="NGAIN" value="0"/> <define name="NGAIN" value="0"/>
<!-- feedforward --> <!-- feedforward -->
<define name="AGAIN" value="100"/> <define name="AGAIN" value="0"/>
</section> </section>
<section name="BAT"> <section name="BAT">
conf/airframes/ENAC/quadrotor/hen1.xml
+19 -19
View File
@@ -3,11 +3,11 @@
<airframe name="Hummingbird (Asctec) Enac-Navgo 1"> <airframe name="Hummingbird (Asctec) Enac-Navgo 1">
<modules main_freq="512"> <modules main_freq="512">
<load name="mavlink_decoder.xml"> <!--load name="mavlink_decoder.xml">
<configure name="MAVLINK_PORT" value="UART0"/> <configure name="MAVLINK_PORT" value="UART0"/>
<configure name="MAVLINK_BAUD" value="B115200"/> <configure name="MAVLINK_BAUD" value="B115200"/>
</load> </load>
<load name="px4flow.xml"/> <load name="px4flow.xml"/-->
<!--load name="adc_generic.xml"> <!--load name="adc_generic.xml">
<configure name="ADC_CHANNEL_GENERIC1" value="ADC_0"/> <configure name="ADC_CHANNEL_GENERIC1" value="ADC_0"/>
</load--> </load-->
@@ -27,19 +27,19 @@
</target> </target>
<subsystem name="radio_control" type="ppm"/> <subsystem name="radio_control" type="ppm"/>
<subsystem name="telemetry" type="transparent"/> <subsystem name="telemetry" type="xbee_api"/>
<!--subsystem name="actuators" type="skiron"> <!--subsystem name="actuators" type="skiron">
<define name="SKIRON_I2C_SCL_TIME" value="25"/> <define name="SKIRON_I2C_SCL_TIME" value="25"/>
</subsystem--> </subsystem-->
<subsystem name="actuators" type="asctec_v2"/> <subsystem name="actuators" type="asctec_v2"/>
<subsystem name="motor_mixing"/> <subsystem name="motor_mixing"/>
<subsystem name="imu" type="navgo"/> <subsystem name="imu" type="navgo"/>
<!--subsystem name="gps" type="ublox"> <subsystem name="gps" type="ublox">
<configure name="GPS_BAUD" value="B57600"/> <configure name="GPS_BAUD" value="B57600"/>
</subsystem--> </subsystem>
<subsystem name="stabilization" type="euler"/> <subsystem name="stabilization" type="int_euler"/>
<subsystem name="ahrs" type="int_cmpl_quat"/> <subsystem name="ahrs" type="int_cmpl_quat"/>
<subsystem name="ins" type="hff"/> <subsystem name="ins"/>
</firmware> </firmware>
<section name="MIXING" prefix="MOTOR_MIXING_"> <section name="MIXING" prefix="MOTOR_MIXING_">
@@ -168,21 +168,21 @@
<define name="REF_MAX_RDOT" value="RadOfDeg(900.)"/> <define name="REF_MAX_RDOT" value="RadOfDeg(900.)"/>
<!-- feedback --> <!-- feedback -->
<define name="PHI_PGAIN" value="1500"/> <define name="PHI_PGAIN" value="1272"/>
<define name="PHI_DGAIN" value="200"/> <define name="PHI_DGAIN" value="132"/>
<define name="PHI_IGAIN" value="200"/> <define name="PHI_IGAIN" value="146"/>
<define name="THETA_PGAIN" value="1500"/> <define name="THETA_PGAIN" value="1272"/>
<define name="THETA_DGAIN" value="200"/> <define name="THETA_DGAIN" value="132"/>
<define name="THETA_IGAIN" value="200"/> <define name="THETA_IGAIN" value="157"/>
<define name="PSI_PGAIN" value="1000"/> <define name="PSI_PGAIN" value="1000"/>
<define name="PSI_DGAIN" value="350"/> <define name="PSI_DGAIN" value="244"/>
<define name="PSI_IGAIN" value="20"/> <define name="PSI_IGAIN" value="20"/>
<!-- feedforward --> <!-- feedforward -->
<define name="PHI_DDGAIN" value="170"/> <define name="PHI_DDGAIN" value="170"/>
<define name="THETA_DDGAIN" value="170"/> <define name="THETA_DDGAIN" value="150"/>
<define name="PSI_DDGAIN" value="170"/> <define name="PSI_DDGAIN" value="170"/>
<define name="PHI_AGAIN" value="0"/> <define name="PHI_AGAIN" value="0"/>
@@ -212,10 +212,11 @@
</section> </section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_"> <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="USE_REF" value="FALSE"/>
<define name="MAX_BANK" value="30" unit="deg"/> <define name="MAX_BANK" value="30" unit="deg"/>
<define name="PGAIN" value="50"/> <define name="PGAIN" value="40"/>
<define name="DGAIN" value="80"/> <define name="DGAIN" value="70"/>
<define name="IGAIN" value="20"/> <define name="IGAIN" value="15"/>
<define name="NGAIN" value="0"/> <define name="NGAIN" value="0"/>
<!-- feedforward --> <!-- feedforward -->
<define name="AGAIN" value="0"/> <define name="AGAIN" value="0"/>
@@ -228,7 +229,6 @@
<section name="AUTOPILOT"> <section name="AUTOPILOT">
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/> <define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_Z_HOLD"/> <define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_Z_HOLD"/>
<!--define name="MODE_AUTO2" value="AP_MODE_ATTITUDE_Z_HOLD"/-->
<define name="MODE_AUTO2" value="AP_MODE_NAV"/> <define name="MODE_AUTO2" value="AP_MODE_NAV"/>
</section> </section>
sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
-37
View File
@@ -20,7 +20,6 @@
* Theory: http://code.google.com/p/gentlenav/downloads/list file DCMDraft2.pdf * Theory: http://code.google.com/p/gentlenav/downloads/list file DCMDraft2.pdf
* *
* Options: * Options:
* - USE_HIGH_ACCEL_FLAG: no compensation when high accelerations present
* - USE_MAGNETOMETER_ONGROUND: use magnetic compensation before takeoff only while GPS course not good * - USE_MAGNETOMETER_ONGROUND: use magnetic compensation before takeoff only while GPS course not good
* - USE_AHRS_GPS_ACCELERATIONS: forward acceleration compensation from GPS speed * - USE_AHRS_GPS_ACCELERATIONS: forward acceleration compensation from GPS speed
* *
@@ -114,18 +113,6 @@ int renorm_blowup_count = 0;
float imu_health = 0.; float imu_health = 0.;
#endif #endif
#if USE_HIGH_ACCEL_FLAG
// High Accel Flag
#define HIGH_ACCEL_LOW_SPEED 15.0
#define HIGH_ACCEL_LOW_SPEED_RESUME 4.0 // Hysteresis
#define HIGH_ACCEL_HIGH_THRUST (0.8*MAX_PPRZ)
#define HIGH_ACCEL_HIGH_THRUST_RESUME (0.1*MAX_PPRZ) // Hysteresis
bool_t high_accel_done;
bool_t high_accel_flag;
// Command vector for thrust (fixed_wing)
#include "inter_mcu.h"
#endif
static inline void set_dcm_matrix_from_rmat(struct FloatRMat *rmat) static inline void set_dcm_matrix_from_rmat(struct FloatRMat *rmat)
{ {
@@ -155,11 +142,6 @@ void ahrs_init(void) {
/* set inital filter dcm */ /* set inital filter dcm */
set_dcm_matrix_from_rmat(&ahrs_impl.body_to_imu_rmat); set_dcm_matrix_from_rmat(&ahrs_impl.body_to_imu_rmat);
#if USE_HIGH_ACCEL_FLAG
high_accel_done = FALSE;
high_accel_flag = FALSE;
#endif
ahrs_impl.gps_speed = 0; ahrs_impl.gps_speed = 0;
ahrs_impl.gps_acceleration = 0; ahrs_impl.gps_acceleration = 0;
ahrs_impl.gps_course = 0; ahrs_impl.gps_course = 0;
@@ -437,25 +419,6 @@ void Drift_correction(void)
// Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0) // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
Accel_weight = Chop(1 - 2*fabs(1 - Accel_magnitude),0,1); // Accel_weight = Chop(1 - 2*fabs(1 - Accel_magnitude),0,1); //
#if USE_HIGH_ACCEL_FLAG
// Test for high acceleration:
// - low speed
// - high thrust
float speed = *stateGetHorizontalSpeedNorm_f();
if (speed < HIGH_ACCEL_LOW_SPEED && ap_state->commands[COMMAND_THROTTLE] > HIGH_ACCEL_HIGH_THRUST && !high_accel_done) {
high_accel_flag = TRUE;
} else {
high_accel_flag = FALSE;
if (speed > HIGH_ACCEL_LOW_SPEED && !high_accel_done) {
high_accel_done = TRUE; // After takeoff, don't use high accel before landing (GS small, Throttle small)
}
if (speed < HIGH_ACCEL_HIGH_THRUST_RESUME && ap_state->commands[COMMAND_THROTTLE] < HIGH_ACCEL_HIGH_THRUST_RESUME) {
high_accel_done = FALSE; // Activate high accel after landing
}
}
if (high_accel_flag) { Accel_weight = 0.; }
#endif
#if PERFORMANCE_REPORTING == 1 #if PERFORMANCE_REPORTING == 1
{ {