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

[rotorcraft] gps_sim_hitl: set gps alt and hmsl

Browse Source 
now the simple HITL should work with ins gps_passthrough
This commit is contained in:
Felix Ruess
2014-04-05 18:38:33 +02:00
parent 2446fc0c9d
commit 774595890e
1 changed files with 13 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/subsystems/gps/gps_sim_hitl.h
+13 -12
View File
@@ -28,7 +28,7 @@
#define GPS_SIM_HITL_H
#include "std.h"
#include "subsystems/ins.h"
#include "state.h"
#include "guidance/guidance_h.h"
#include "guidance/guidance_v.h"
@@ -40,9 +40,7 @@ extern uint32_t gps_sim_hitl_timer;
SysTimeTimerStart(gps_sim_hitl_timer); \
gps.last_msg_ticks = sys_time.nb_sec_rem; \
gps.last_msg_time = sys_time.nb_sec; \
if (ins_impl.ltp_initialized) { \
struct NedCoor_i ned_coor; \
struct NedCoor_i ned_vel; \
if (state.ned_initialized_i) { \
if (!autopilot_in_flight) { \
struct Int32Vect2 zero_vector; \
INT_VECT2_ZERO(zero_vector); \
@@ -55,14 +53,17 @@ extern uint32_t gps_sim_hitl_timer;
guidance_v_zd_ref = 0; \
guidance_v_zdd_ref = 0; \
} \
VECT3_ASSIGN(ned_coor, guidance_h_pos_ref.x * INT32_POS_OF_CM_DEN / INT32_POS_OF_CM_NUM, \
guidance_h_pos_ref.y * INT32_POS_OF_CM_DEN / INT32_POS_OF_CM_NUM, \
guidance_v_z_ref * INT32_POS_OF_CM_DEN / INT32_POS_OF_CM_NUM); \
VECT3_ASSIGN(ned_vel, guidance_h_speed_ref.x * INT32_SPEED_OF_CM_S_DEN / INT32_SPEED_OF_CM_S_NUM, \
guidance_h_speed_ref.y * INT32_SPEED_OF_CM_S_DEN / INT32_SPEED_OF_CM_S_NUM, \
guidance_v_zd_ref * INT32_SPEED_OF_CM_S_DEN / INT32_SPEED_OF_CM_S_NUM); \
ecef_of_ned_point_i(&gps.ecef_pos, &ins_impl.ltp_def, &ned_coor); \
ecef_of_ned_vect_i(&gps.ecef_vel, &ins_impl.ltp_def, &ned_vel); \
struct NedCoor_i ned_c; \
ned_c.x = guidance_h_pos_ref.x * INT32_POS_OF_CM_DEN / INT32_POS_OF_CM_NUM; \
ned_c.y = guidance_h_pos_ref.y * INT32_POS_OF_CM_DEN / INT32_POS_OF_CM_NUM; \
ned_c.z = guidance_v_z_ref * INT32_POS_OF_CM_DEN / INT32_POS_OF_CM_NUM; \
ecef_of_ned_point_i(&gps.ecef_pos, &state.ned_origin_i, &ned_c); \
gps.lla_pos.alt = state.ned_origin_i.lla.alt - ned_c.z; \
gps.hmsl = state.ned_origin_i.hmsl - ned_c.z; \
ned_c.x = guidance_h_speed_ref.x * INT32_SPEED_OF_CM_S_DEN / INT32_SPEED_OF_CM_S_NUM; \
ned_c.y = guidance_h_speed_ref.y * INT32_SPEED_OF_CM_S_DEN / INT32_SPEED_OF_CM_S_NUM; \
ned_c.z = guidance_v_zd_ref * INT32_SPEED_OF_CM_S_DEN / INT32_SPEED_OF_CM_S_NUM; \
ecef_of_ned_vect_i(&gps.ecef_vel, &state.ned_origin_i, &ned_c); \
gps.fix = GPS_FIX_3D; \
gps.last_3dfix_ticks = sys_time.nb_sec_rem; \
gps.last_3dfix_time = sys_time.nb_sec; \