Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-30 11:37:06 +08:00
Code Issues Actions 25 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"> <modules main_freq="512">
<load name="servo_switch.xml"/> <load name="servo_switch.xml"/>
<load name="rotorcraft_cam.xml"/> <load name="rotorcraft_cam.xml"/>
<!--load name="sonar_maxbotix_booz.xml"/--> <load name="sonar_maxbotix_booz.xml"/>
<!--load name="adc_generic_booz.xml"/--> <!--load name="adc_generic.xml">
<configure name="ADC_CHANNEL_GENERIC1" value="ADC_0"/>
</load-->
<!--load name="sys_mon.xml"/--> <!--load name="sys_mon.xml"/-->
</modules> </modules>
@@ -34,8 +36,7 @@
</subsystem> </subsystem>
<subsystem name="stabilization" type="euler"/> <subsystem name="stabilization" type="euler"/>
<subsystem name="ahrs" type="int_cmpl_euler"/> <subsystem name="ahrs" type="int_cmpl_euler"/>
<subsystem name="ins" type="hff"/> <subsystem name="ins" type="extended"/>
<!--subsystem name="ins" type="extended"/-->
</firmware> </firmware>
<servos driver="Asctec"> <servos driver="Asctec">
@@ -164,9 +165,10 @@
<section name="INS" prefix="INS_"> <section name="INS" prefix="INS_">
<define name="BARO_SENS" value="15." integer="16"/> <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.00650498" integer="16"/> <!-- XL-MaxSonar-EZ4 5V supply -->
<define name="SONAR_SENS" value="0.00833333" integer="16"/> <!-- 5V supply --> <define name="SONAR_MAX_RANGE" value="5.0"/>
<define name="SONAR_MAX_RANGE" value="4.0"/> <define name="SONAR_MIN_RANGE" value="0.25"/>
<!--define name="SONAR_VARIANCE_THRESHOLD" value="0.01"/-->
</section> </section>
<section name="GUIDANCE_V" prefix="GUIDANCE_V_"> <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 # default LED configuration
# #
RADIO_CONTROL_LED ?= 2 RADIO_CONTROL_LED ?= 1
BARO_LED ?= none BARO_LED ?= 2
AHRS_ALIGNER_LED ?= 3 AHRS_ALIGNER_LED ?= 3
GPS_LED ?= 4 GPS_LED ?= 4
SYS_TIME_LED ?= 1 SYS_TIME_LED ?= none
RADIO_CONTROL_LINK = UART0 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); ltp_def_from_ecef_i(&ins_ltp_def, &ecef_nav0);
ins_ltp_def.hmsl = NAV_ALT0; ins_ltp_def.hmsl = NAV_ALT0;
stateSetLocalOrigin_i(&ins_ltp_def);
#else #else
ins_ltp_initialised = FALSE; ins_ltp_initialised = FALSE;
#endif #endif
@@ -165,6 +166,7 @@ void ins_propagate() {
ins_ltp_accel.y = accel_meas_ltp.y; ins_ltp_accel.y = accel_meas_ltp.y;
#endif /* USE_HFF */ #endif /* USE_HFF */
INS_NED_TO_STATE();
} }
void ins_update_baro() { 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.lla.alt = gps.lla_pos.alt;
ins_ltp_def.hmsl = gps.hmsl; ins_ltp_def.hmsl = gps.hmsl;
ins_ltp_initialised = TRUE; 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_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); 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 */ /* update filter assuming a flat ground */
if (sonar < INS_SONAR_MAX_RANGE if (sonar < INS_SONAR_MAX_RANGE
#ifdef INS_SONAR_MIN_RANGE
&& sonar > INS_SONAR_MIN_RANGE
#endif
#ifdef INS_SONAR_THROTTLE_THRESHOLD #ifdef INS_SONAR_THROTTLE_THRESHOLD
&& stabilization_cmd[COMMAND_THRUST] < INS_SONAR_THROTTLE_THRESHOLD && stabilization_cmd[COMMAND_THRUST] < INS_SONAR_THROTTLE_THRESHOLD
#endif #endif