Paparazzi UAS  v5.12_stable-4-g9b43e9b
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 "subsystems/gps.h"
29 #include "subsystems/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;
53  INT_VECT2_ZERO(zero_vector);
54  gh_set_ref(zero_vector, zero_vector, zero_vector);
58  gv_set_ref(0, 0, 0);
59  guidance_v_z_ref = 0;
62  }
63  struct NedCoor_i ned_c;
69  gps.hmsl = state.ned_origin_i.hmsl - ned_c.z;
74  gps.fix = GPS_FIX_3D;
77  gps_available = true;
78  }
79  else {
80  struct Int32Vect2 zero_vector;
81  INT_VECT2_ZERO(zero_vector);
82  gh_set_ref(zero_vector, zero_vector, zero_vector);
83  gv_set_ref(0, 0, 0);
84  }
85 
86  // publish gps data
87  uint32_t now_ts = get_sys_time_usec();
90  if (gps.fix == GPS_FIX_3D) {
93  }
94  AbiSendMsgGPS(GPS_SIM_ID, now_ts, &gps);
95  }
96 }
97 
void gps_sim_hitl_init(void)
Definition: gps_sim_hitl.c:39
void gh_set_ref(struct Int32Vect2 pos, struct Int32Vect2 speed, struct Int32Vect2 accel)
void gps_sim_hitl_event(void)
Definition: gps_sim_hitl.c:44
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.
Main include for ABI (AirBorneInterface).
struct HorizontalGuidanceReference ref
reference calculated from setpoints
Definition: guidance_h.h:101
#define INT32_SPEED_OF_CM_S_DEN
#define GPS_FIX_3D
3D GPS fix
Definition: gps.h:39
int32_t z
Down.
int32_t hmsl
Height above mean sea level in mm.
uint32_t last_3dfix_ticks
cpu time ticks at last valid 3D fix
Definition: gps.h:106
int32_t alt
in millimeters above WGS84 reference ellipsoid
int32_t y
East.
uint32_t last_msg_time
cpu time in sec at last received GPS message
Definition: gps.h:109
struct Int32Vect2 speed
with INT32_SPEED_FRAC
Definition: guidance_h.h:80
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:88
bool autopilot_in_flight(void)
get in_flight flag
Definition: autopilot.c:252
#define INT32_POS_OF_CM_NUM
#define GPS_FIX_NONE
No GPS fix.
Definition: gps.h:37
Device independent GPS code (interface)
int32_t x
North.
unsigned long uint32_t
Definition: types.h:18
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition: gps.h:85
bool gps_available
Definition: gps_sim_hitl.c:36
struct LlaCoor_i lla
Reference point in lla.
int32_t guidance_v_zdd_ref
vertical acceleration reference in meter/s^2.
Definition: guidance_v.c:130
struct HorizontalGuidance guidance_h
Definition: guidance_h.c:80
int32_t guidance_v_z_ref
altitude reference in meters.
Definition: guidance_v.c:128
#define GPS_SIM_ID
uint32_t gps_sim_hitl_timer
Definition: gps_sim_hitl.c:37
int32_t guidance_v_zd_ref
vertical speed reference in meter/s.
Definition: guidance_v.c:129
volatile uint32_t nb_sec_rem
remainder of seconds since startup in CPU_TICKS
Definition: sys_time.h:73
bool ned_initialized_i
true if local int coordinate frame is initialsed
Definition: state.h:171
#define SysTimeTimer(_t)
Definition: sys_time.h:219
GPS subsystem simulation from rotorcrafts horizontal/vertical reference system.
void gv_set_ref(int32_t alt, int32_t speed, int32_t accel)
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
Definition: state.h:166
Core autopilot interface common to all firmwares.
struct Int32Vect2 pos
with INT32_POS_FRAC
Definition: guidance_h.h:79
uint32_t last_3dfix_time
cpu time in sec at last valid 3D fix
Definition: gps.h:107
#define INT32_POS_OF_CM_DEN
API to get/set the generic vehicle states.
vector in North East Down coordinates
volatile uint32_t nb_sec
full seconds since startup
Definition: sys_time.h:72
#define INT32_SPEED_OF_CM_S_NUM
#define INT_VECT2_ZERO(_v)
uint32_t last_msg_ticks
cpu time ticks at last received GPS message
Definition: gps.h:108
struct Int32Vect2 accel
with INT32_ACCEL_FRAC
Definition: guidance_h.h:81
struct EcefCoor_i ecef_vel
speed ECEF in cm/s
Definition: gps.h:89
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:86
#define SysTimeTimerStart(_t)
Definition: sys_time.h:218
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:68
uint8_t fix
status of fix
Definition: gps.h:99
struct GpsState gps
global GPS state
Definition: gps.c:75
struct State state
Definition: state.c:36
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.