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

gps-timeout fixed

Browse Source
This commit is contained in:
Christophe De Wagter
2011-05-12 19:39:40 +02:00
parent e346e98319
commit 9b02346532
2 changed files with 58 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/TU_Delft/skywalker.xml
+38 -35
View File
@@ -8,8 +8,8 @@
<servos>
<servo name="THROTTLE" no="0" min="1100" neutral="1100" max="1900"/>
<servo name="AILERON_LEFT" no="2" min="1100" neutral="1500" max="1900"/>
<servo name="AILERON_RIGHT" no="6" min="1100" neutral="1500" max="1900"/>
<servo name="AILERON_LEFT" no="2" min="900" neutral="1500" max="2100"/>
<servo name="AILERON_RIGHT" no="6" min="900" neutral="1500" max="2100"/>
<servo name="ELEVATOR" no="7" min="1900" neutral="1500" max="1100"/>
<servo name="RUDDER" no="3" min="1100" neutral="1500" max="1900"/>
</servos>
@@ -31,7 +31,7 @@
</rc_commands>
<section name="SERVO_MIXER_GAINS">
<define name="AILERON_NEUTRAL" value="0.45f"/>
<define name="AILERON_NEUTRAL" value="0.3f"/>
<define name="AILERON_RATE_UP" value="1.0f"/>
<define name="AILERON_RATE_DOWN" value="0.5f"/>
@@ -67,7 +67,7 @@
<set servo="AILERON_LEFT" value="($aileron_up * $leftturn) + ($aileron_down * $rightturn) - $brake_value*(BRAKE_AILEVON) - (MAX_PPRZ * AILERON_NEUTRAL)"/>
<set servo="AILERON_RIGHT" value="($aileron_up * $rightturn) + ($aileron_down * $leftturn) + $brake_value*(BRAKE_AILEVON) + (MAX_PPRZ *AILERON_NEUTRAL)"/>
<set servo="RUDDER" value="@YAW"/>
<set servo="RUDDER" value="@YAW + @ROLL * 0.3"/>
<set servo="THROTTLE" value="@THROTTLE"/>
<!-- Pitch with Brake-Trim Function -->
@@ -75,12 +75,12 @@
</command_laws>
<section name="AUTO1" prefix="AUTO1_">
<define name="MAX_ROLL" value="0.7"/>
<define name="MAX_PITCH" value="0.7"/>
<define name="MAX_ROLL" value="RadOfDeg(75)"/>
<define name="MAX_PITCH" value="RadOfDeg(45)"/>
</section>
<section name="BAT">
<define name="MilliAmpereOfAdc(adc)" value="((adc) - 505) * 124.0f"/>
<!-- <define name="MilliAmpereOfAdc(adc)" value="((adc) - 505) * 124.0f"/> -->
<define name="LOW_BAT_LEVEL" value="10.5" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="10" unit="V"/>
@@ -110,54 +110,56 @@
<section name="VERTICAL CONTROL" prefix="V_CTL_">
<define name="POWER_CTL_BAT_NOMINAL" value="11.1" unit="volt"/>
<!-- outer loop proportional gain -->
<define name="ALTITUDE_PGAIN" value="-0.03"/>
<define name="ALTITUDE_PGAIN" value="-0.108000002801"/>
<!-- outer loop saturation -->
<define name="ALTITUDE_MAX_CLIMB" value="2."/>
<!-- auto throttle inner loop -->
<define name="AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" value="0.32"/>
<define name="AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" value="0.57800000906"/>
<define name="AUTO_THROTTLE_MIN_CRUISE_THROTTLE" value="0.25"/>
<define name="AUTO_THROTTLE_MAX_CRUISE_THROTTLE" value="0.65"/>
<define name="AUTO_THROTTLE_LOITER_TRIM" value="1500"/>
<define name="AUTO_THROTTLE_DASH_TRIM" value="-4000"/>
<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.1"/>
<define name="AUTO_THROTTLE_PITCH_OF_VZ_PGAIN" value="0.05"/>
<define name="AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT" value="0.108999997377" unit="%/(m/s)"/>
<define name="AUTO_THROTTLE_PGAIN" value="0."/>
<define name="AUTO_THROTTLE_IGAIN" value="0."/>
<define name="AUTO_THROTTLE_PITCH_OF_VZ_PGAIN" value="0.324999988079"/>
<define name="THROTTLE_SLEW_LIMITER" value="2" unit="s"/>
</section>
<section name="HORIZONTAL CONTROL" prefix="H_CTL_">
<define name="COURSE_PGAIN" value="-1.20000004768"/>
<define name="COURSE_DGAIN" value="0.3"/>
<define name="COURSE_PRE_BANK_CORRECTION" value="0.2"/>
<define name="COURSE_DGAIN" value="0.300000011921"/>
<define name="COURSE_PRE_BANK_CORRECTION" value="1.01600003242"/>
<define name="ROLL_MAX_SETPOINT" value="0.75" unit="radians"/>
<define name="ROLL_MAX_SETPOINT" value="0.851999998093" unit="radians"/>
<define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
<define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
<define name="PITCH_PGAIN" value="-12000."/>
<define name="PITCH_DGAIN" value="1.5"/>
<define name="PITCH_PGAIN" value="-13476.5615234"/>
<define name="PITCH_DGAIN" value="7.73400020599"/>
<define name="ELEVATOR_OF_ROLL" value="1000."/>
<define name="ELEVATOR_OF_ROLL" value="3007.81298828"/>
<define name="ROLL_SLEW" value="1."/>
<define name="ROLL_ATTITUDE_GAIN" value="-7500"/>
<define name="ROLL_RATE_GAIN" value="0."/>
<define name="ROLL_ATTITUDE_GAIN" value="-11718.75"/>
<define name="ROLL_RATE_GAIN" value="-820.312011719"/>
</section>
<!--
<section name="AGGRESSIVE" prefix="AGR_">
<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_THROTTLE" value="1.00"/><!-- Gaz for Aggressive Climb -->
<define name="CLIMB_PITCH" value="0.3"/><!-- Pitch for Aggressive Climb -->
<define name="DESCENT_THROTTLE" value="0.1"/><!-- Gaz for Aggressive Decent -->
<define name="DESCENT_PITCH" value="-0.25"/><!-- Pitch for Aggressive Decent -->
<define name="CLIMB_NAV_RATIO" value="0.8"/><!-- Percent Navigation for Altitude Error Equal to Start Altitude -->
<define name="BLEND_START" value="20"/>
<define name="BLEND_END" value="10"/>
<define name="CLIMB_THROTTLE" value="1.00"/>
<define name="CLIMB_PITCH" value="0.3"/>
<define name="DESCENT_THROTTLE" value="0.1"/>
<define name="DESCENT_PITCH" value="-0.25"/>
<define name="CLIMB_NAV_RATIO" value="0.8"/>
<define name="DESCENT_NAV_RATIO" value="1.0"/>
</section>
-->
<section name="FAILSAFE" prefix="FAILSAFE_">
<define name="DEFAULT_THROTTLE" value="0.35" unit="%"/>
@@ -180,19 +182,19 @@
<configure name="XSENS_UART_NR" value="0"/>
</load>
<!-- <load name="light.xml">
<load name="light.xml">
<define name="LIGHT_LED_STROBE" value="3"/>
<define name="LIGHT_LED_NAV" value="2"/>
<define name="STROBE_LIGHT_MODE_DEFAULT" value="6"/>
<define name="NAV_LIGHT_MODE_DEFAULT" value="4"/>
</load>
<load name="digital_cam_i2c.xml"/> -->
<!-- <load name="ins_ppzuavimu.xml"/>
<load name="digital_cam.xml" >
<!-- <load name="digital_cam_i2c.xml"/> -->
<!-- <load name="ins_ppzuavimu.xml"/> -->
<load name="digital_cam.xml">
<define name="DC_SHUTTER_LED" value="2"/>
</load>
-->
</modules>
<firmware name="fixedwing">
@@ -202,7 +204,10 @@
<define name="WIND_INFO"/>
<define name="WIND_INFO_RET"/>
<define name="LOITER_TRIM"/>
<!--
<define name="ALT_KALMAN"/>
<define name="ALT_KALMAN_ENABLED" value="TRUE"/>
-->
</target>
<target name="sim" board="pc"/>
@@ -219,9 +224,7 @@
<subsystem name="navigation"/>
<subsystem name="gps" type="xsens"/>
<!--
<subsystem name="i2c"/>
-->
</firmware>
sw/airborne/modules/ins/ins_xsens.c
+20 -9
View File
@@ -32,6 +32,7 @@
#include "generated/airframe.h"
#include "sys_time.h"
#include "downlink.h"
#include "messages.h"
@@ -225,7 +226,7 @@ void handle_ins_msg( void) {
// Send to Estimator (Control)
EstimatorSetAtt(ins_phi, ins_psi, ins_theta);
EstimatorSetAtt(ins_phi+ins_roll_neutral, ins_psi, ins_theta+ins_pitch_neutral);
EstimatorSetRate(ins_p,ins_q);
// Position
@@ -236,13 +237,17 @@ void handle_ins_msg( void) {
EstimatorSetPosXY(gps_east, gps_north);
// Altitude and vertical speed
EstimatorSetAlt(-ins_z);
float hmsl = gps.hmsl;
hmsl /= 1000.0f;
EstimatorSetAlt(hmsl);
// Horizontal speed
float fspeed = sqrt(ins_vx*ins_vx + ins_vy*ins_vy);
if (gps.fix != GPS_FIX_3D)
{
fspeed = 0;
float fclimb = -ins_vz;
}
float fclimb = -ins_vz * ;
float fcourse = atan2f((float)ins_vy, (float)ins_vx);
EstimatorSetSpeedPol(fspeed, fcourse, fclimb);
@@ -251,7 +256,6 @@ void handle_ins_msg( void) {
gps.gspeed = fspeed * 100.;
gps.speed_3d = (uint16_t)(sqrt(ins_vx*ins_vx + ins_vy*ins_vy + ins_vz*ins_vz) * 100);
gps.course = fcourse * 1e7;
// reset_gps_watchdog();
}
void parse_ins_msg( void ) {
@@ -270,6 +274,8 @@ void parse_ins_msg( void ) {
gps.nb_channels = XSENS_GPSStatus_nch(xsens_msg_buf);
gps.num_sv = 0;
gps.last_fix_time = cpu_time_sec;
uint8_t i;
// Do not write outside buffer
for(i = 0; i < Min(gps.nb_channels, GPS_NB_CHANNELS); i++) {
@@ -297,7 +303,10 @@ void parse_ins_msg( void ) {
}
if (XSENS_MASK_RAWGPS(xsens_output_mode)) {
#if defined(USE_GPS_XSENS_RAW_DATA) && defined(USE_GPS_XSENS)
LED_TOGGLE(3);
#ifdef GPS_LED
LED_TOGGLE(GPS_LED);
#endif
gps.last_fix_time = cpu_time_sec;
gps.week = 0; // FIXME
gps.tow = XSENS_DATA_RAWGPS_itow(xsens_msg_buf,offset) * 10;
gps.lla_pos.lat = RadOfDeg(XSENS_DATA_RAWGPS_lat(xsens_msg_buf,offset));
@@ -326,11 +335,11 @@ void parse_ins_msg( void ) {
// Compute geoid (MSL) height
float hmsl;
WGS84_ELLIPSOID_TO_GEOID(lla_f.lat,lla_f.lon,hmsl);
gps.hmsl = (hmsl * 1000.0f);
gps.hmsl = XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset) - (hmsl * 1000.0f);
ins_vx = (INS_FORMAT)XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf,offset) / 100.;
ins_vy = (INS_FORMAT)XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf,offset) / 100.;
ins_vz = (INS_FORMAT)XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset) / 100.;
ins_vx = ((INS_FORMAT)XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf,offset)) / 100.;
ins_vy = ((INS_FORMAT)XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf,offset)) / 100.;
ins_vz = ((INS_FORMAT)XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset)) / 100.;
gps.ned_vel.x = XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf,offset);
gps.ned_vel.y = XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf,offset);
gps.ned_vel.z = XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset);
@@ -405,6 +414,8 @@ void parse_ins_msg( void ) {
}
if (XSENS_MASK_Position(xsens_output_mode)) {
#if (!defined(USE_GPS_XSENS_RAW_DATA)) && defined(USE_GPS_XSENS)
gps.last_fix_time = cpu_time_sec;
lla_f.lat = RadOfDeg(XSENS_DATA_Position_lat(xsens_msg_buf,offset));
lla_f.lon = RadOfDeg(XSENS_DATA_Position_lon(xsens_msg_buf,offset));
gps.lla_pos.lat = (int32_t)(lla_f.lat * 1e7);