Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
gps_sim_hitl.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2014 Sergey Krukowski <softsr@yahoo.de>
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
28 #include "modules/gps/gps.h"
29 #include "modules/core/abi.h"
30 
31 #include "state.h"
32 #include "guidance/guidance_h.h"
33 #include "guidance/guidance_v.h"
34 #include "autopilot.h"
35 
38 
40 {
42 }
43 
45 {
46  if (SysTimeTimer(gps_sim_hitl_timer) > 100000) {
51  if (!autopilot_in_flight()) {
52  struct Int32Vect2 zero_vector_i;
53  struct FloatVect2 zero_vector_f;
54  INT_VECT2_ZERO(zero_vector_i);
55  FLOAT_VECT2_ZERO(zero_vector_f);
56  gh_set_ref(zero_vector_i, zero_vector_f, zero_vector_f);
60  gv_set_ref(0, 0, 0);
61  guidance_v_z_ref = 0;
64  }
65  struct NedCoor_i ned_c;
71  gps.hmsl = state.ned_origin_i.hmsl - ned_c.z;
76  gps.fix = GPS_FIX_3D;
79  gps_available = true;
80  }
81  else {
82  struct Int32Vect2 zero_vector_i;
83  struct FloatVect2 zero_vector_f;
84  INT_VECT2_ZERO(zero_vector_i);
85  FLOAT_VECT2_ZERO(zero_vector_f);
86  gh_set_ref(zero_vector_i, zero_vector_f, zero_vector_f);
87  gv_set_ref(0, 0, 0);
88  }
89 
90  // publish gps data
91  uint32_t now_ts = get_sys_time_usec();
94  if (gps.fix == GPS_FIX_3D) {
97  }
98  AbiSendMsgGPS(GPS_SIM_ID, now_ts, &gps);
99  }
100 }
101 
FLOAT_VECT2_ZERO
#define FLOAT_VECT2_ZERO(_v)
Definition: pprz_algebra_float.h:129
GPS_SIM_ID
#define GPS_SIM_ID
Definition: abi_sender_ids.h:268
uint32_t
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
HorizontalGuidance::ref
struct HorizontalGuidanceReference ref
reference calculated from setpoints
Definition: guidance_h.h:104
LlaCoor_i::alt
int32_t alt
in millimeters above WGS84 reference ellipsoid
Definition: pprz_geodetic_int.h:62
gps_sim_hitl_timer
uint32_t gps_sim_hitl_timer
Definition: gps_sim_hitl.c:37
State::ned_origin_i
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
Definition: state.h:166
abi.h
NedCoor_i::y
int32_t y
East.
Definition: pprz_geodetic_int.h:70
guidance_v_zdd_ref
int32_t guidance_v_zdd_ref
vertical acceleration reference in meter/s^2.
Definition: guidance_v.c:134
Int32Vect2::y
int32_t y
Definition: pprz_algebra_int.h:85
HorizontalGuidanceReference::accel
struct Int32Vect2 accel
with INT32_ACCEL_FRAC
Definition: guidance_h.h:84
HorizontalGuidanceReference::speed
struct Int32Vect2 speed
with INT32_SPEED_FRAC
Definition: guidance_h.h:83
SysTimeTimer
#define SysTimeTimer(_t)
Definition: sys_time.h:228
ecef_of_ned_point_i
void ecef_of_ned_point_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, struct NedCoor_i *ned)
Convert a point in local NED to ECEF.
Definition: pprz_geodetic_int.c:250
INT32_POS_OF_CM_NUM
#define INT32_POS_OF_CM_NUM
Definition: pprz_algebra_int.h:70
gh_set_ref
void gh_set_ref(struct Int32Vect2 pos, struct FloatVect2 speed, struct FloatVect2 accel)
Definition: guidance_h_ref.c:95
State::ned_initialized_i
bool ned_initialized_i
true if local int coordinate frame is initialsed
Definition: state.h:171
INT_VECT2_ZERO
#define INT_VECT2_ZERO(_v)
Definition: pprz_algebra_int.h:238
INT32_POS_OF_CM_DEN
#define INT32_POS_OF_CM_DEN
Definition: pprz_algebra_int.h:71
NedCoor_i::z
int32_t z
Down.
Definition: pprz_geodetic_int.h:71
guidance_v_zd_ref
int32_t guidance_v_zd_ref
vertical speed reference in meter/s.
Definition: guidance_v.c:133
autopilot_in_flight
bool autopilot_in_flight(void)
get in_flight flag
Definition: autopilot.c:289
get_sys_time_usec
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:75
FloatVect2
Definition: pprz_algebra_float.h:49
GpsState::last_msg_ticks
uint32_t last_msg_ticks
cpu time ticks at last received GPS message
Definition: gps.h:116
Int32Vect2::x
int32_t x
Definition: pprz_algebra_int.h:84
gps.h
Device independent GPS code (interface)
GpsState::fix
uint8_t fix
status of fix
Definition: gps.h:107
NedCoor_i
vector in North East Down coordinates
Definition: pprz_geodetic_int.h:68
INT32_SPEED_OF_CM_S_DEN
#define INT32_SPEED_OF_CM_S_DEN
Definition: pprz_algebra_int.h:76
Int32Vect2
Definition: pprz_algebra_int.h:83
GpsState::last_3dfix_time
uint32_t last_3dfix_time
cpu time in sec at last valid 3D fix
Definition: gps.h:115
gps_sim_hitl_init
void gps_sim_hitl_init(void)
Definition: gps_sim_hitl.c:39
GpsState::last_3dfix_ticks
uint32_t last_3dfix_ticks
cpu time ticks at last valid 3D fix
Definition: gps.h:114
autopilot.h
gps_sim_hitl_event
void gps_sim_hitl_event(void)
Definition: gps_sim_hitl.c:44
gps_sim_hitl.h
GpsState::last_msg_time
uint32_t last_msg_time
cpu time in sec at last received GPS message
Definition: gps.h:117
GpsState::ecef_pos
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition: gps.h:91
ecef_of_ned_vect_i
void ecef_of_ned_vect_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, struct NedCoor_i *ned)
Rotate a vector from NED to ECEF.
Definition: pprz_geodetic_int.c:225
GpsState::lla_pos
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:92
LtpDef_i::hmsl
int32_t hmsl
Height above mean sea level in mm.
Definition: pprz_geodetic_int.h:102
sys_time
Definition: sys_time.h:71
sys_time::nb_sec_rem
volatile uint32_t nb_sec_rem
remainder of seconds since startup in CPU_TICKS
Definition: sys_time.h:73
GpsState::hmsl
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:94
HorizontalGuidanceReference::pos
struct Int32Vect2 pos
with INT32_POS_FRAC
Definition: guidance_h.h:82
NedCoor_i::x
int32_t x
North.
Definition: pprz_geodetic_int.h:69
gv_set_ref
void gv_set_ref(int32_t alt, int32_t speed, int32_t accel)
Definition: guidance_v_ref.c:87
guidance_h
struct HorizontalGuidance guidance_h
Definition: guidance_h.c:90
state.h
sys_time::nb_sec
volatile uint32_t nb_sec
full seconds since startup
Definition: sys_time.h:72
GpsState::ecef_vel
struct EcefCoor_i ecef_vel
speed ECEF in cm/s
Definition: gps.h:95
state
struct State state
Definition: state.c:36
SysTimeTimerStart
#define SysTimeTimerStart(_t)
Definition: sys_time.h:227
gps
struct GpsState gps
global GPS state
Definition: gps.c:69
GPS_FIX_NONE
#define GPS_FIX_NONE
No GPS fix.
Definition: gps.h:37
GPS_FIX_3D
#define GPS_FIX_3D
3D GPS fix
Definition: gps.h:39
LtpDef_i::lla
struct LlaCoor_i lla
Reference point in lla.
Definition: pprz_geodetic_int.h:100
INT32_SPEED_OF_CM_S_NUM
#define INT32_SPEED_OF_CM_S_NUM
Definition: pprz_algebra_int.h:75
gps_available
bool gps_available
Definition: gps_sim_hitl.c:36
guidance_v_z_ref
int32_t guidance_v_z_ref
altitude reference in meters.
Definition: guidance_v.c:132