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

[ins_ext] fix ins_ext init to set local ned frame

Browse Source
This commit is contained in:
Gautier Hattenberger
2012-12-04 18:04:09 +01:00
parent 0f36a8123c
commit b8d87a10f9
3 changed files with 18 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/ENAC/quadrotor/booz2_g1.xml
+9 -7
View File
@@ -3,8 +3,10 @@
<modules main_freq="512">
<load name="servo_switch.xml"/>
<load name="rotorcraft_cam.xml"/>
<!--load name="sonar_maxbotix_booz.xml"/-->
<!--load name="adc_generic_booz.xml"/-->
<load name="sonar_maxbotix_booz.xml"/>
<!--load name="adc_generic.xml">
<configure name="ADC_CHANNEL_GENERIC1" value="ADC_0"/>
</load-->
<!--load name="sys_mon.xml"/-->
</modules>
@@ -34,8 +36,7 @@
</subsystem>
<subsystem name="stabilization" type="euler"/>
<subsystem name="ahrs" type="int_cmpl_euler"/>
<subsystem name="ins" type="hff"/>
<!--subsystem name="ins" type="extended"/-->
<subsystem name="ins" type="extended"/>
</firmware>
<servos driver="Asctec">
@@ -164,9 +165,10 @@
<section name="INS" prefix="INS_">
<define name="BARO_SENS" value="15." integer="16"/>
<!--define name="SONAR_SENS" value="0.0128633" integer="16"/--> <!-- 3.3V supply -->
<define name="SONAR_SENS" value="0.00833333" integer="16"/> <!-- 5V supply -->
<define name="SONAR_MAX_RANGE" value="4.0"/>
<define name="SONAR_SENS" value="0.00650498" integer="16"/> <!-- XL-MaxSonar-EZ4 5V supply -->
<define name="SONAR_MAX_RANGE" value="5.0"/>
<define name="SONAR_MIN_RANGE" value="0.25"/>
<!--define name="SONAR_VARIANCE_THRESHOLD" value="0.01"/-->
</section>
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
conf/boards/booz_1.0.makefile
+3 -3
View File
@@ -21,11 +21,11 @@ endif
#
# default LED configuration
#
RADIO_CONTROL_LED ?= 2
BARO_LED ?= none
RADIO_CONTROL_LED ?= 1
BARO_LED ?= 2
AHRS_ALIGNER_LED ?= 3
GPS_LED ?= 4
SYS_TIME_LED ?= 1
SYS_TIME_LED ?= none
RADIO_CONTROL_LINK = UART0
sw/airborne/subsystems/ins/ins_int_extended.c
+6
View File
@@ -102,6 +102,7 @@ void ins_init() {
ltp_def_from_ecef_i(&ins_ltp_def, &ecef_nav0);
ins_ltp_def.hmsl = NAV_ALT0;
stateSetLocalOrigin_i(&ins_ltp_def);
#else
ins_ltp_initialised = FALSE;
#endif
@@ -165,6 +166,7 @@ void ins_propagate() {
ins_ltp_accel.y = accel_meas_ltp.y;
#endif /* USE_HFF */
INS_NED_TO_STATE();
}
void ins_update_baro() {
@@ -199,6 +201,7 @@ void ins_update_gps(void) {
ins_ltp_def.lla.alt = gps.lla_pos.alt;
ins_ltp_def.hmsl = gps.hmsl;
ins_ltp_initialised = TRUE;
stateSetLocalOrigin_i(&ins_ltp_def);
}
ned_of_ecef_point_i(&ins_gps_pos_cm_ned, &ins_ltp_def, &gps.ecef_pos);
ned_of_ecef_vect_i(&ins_gps_speed_cm_s_ned, &ins_ltp_def, &gps.ecef_vel);
@@ -263,6 +266,9 @@ void ins_update_sonar() {
/* update filter assuming a flat ground */
if (sonar < INS_SONAR_MAX_RANGE
#ifdef INS_SONAR_MIN_RANGE
&& sonar > INS_SONAR_MIN_RANGE
#endif
#ifdef INS_SONAR_THROTTLE_THRESHOLD
&& stabilization_cmd[COMMAND_THRUST] < INS_SONAR_THROTTLE_THRESHOLD
#endif