Paparazzi UAS  v5.8.2_stable-0-g6260b7c
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2014 Sergey Krukowski <>
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
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  */
27 #include "subsystems/gps.h"
28 #include "subsystems/abi.h"
30 #include "state.h"
31 #include "guidance/guidance_h.h"
32 #include "guidance/guidance_v.h"
37 void gps_impl_init(void)
38 {
40 }
43 {
44  if (SysTimeTimer(gps_sim_hitl_timer) > 100000) {
49  if (!autopilot_in_flight) {
50  struct Int32Vect2 zero_vector;
51  INT_VECT2_ZERO(zero_vector);
52  gh_set_ref(zero_vector, zero_vector, zero_vector);
53  INT_VECT2_ZERO(guidance_h_pos_ref);
54  INT_VECT2_ZERO(guidance_h_speed_ref);
55  INT_VECT2_ZERO(guidance_h_accel_ref);
56  gv_set_ref(0, 0, 0);
57  guidance_v_z_ref = 0;
60  }
61  struct NedCoor_i ned_c;
62  ned_c.x = guidance_h_pos_ref.x * INT32_POS_OF_CM_DEN / INT32_POS_OF_CM_NUM;
63  ned_c.y = guidance_h_pos_ref.y * INT32_POS_OF_CM_DEN / INT32_POS_OF_CM_NUM;
67  gps.hmsl = state.ned_origin_i.hmsl - ned_c.z;
68  ned_c.x = guidance_h_speed_ref.x * INT32_SPEED_OF_CM_S_DEN / INT32_SPEED_OF_CM_S_NUM;
69  ned_c.y = guidance_h_speed_ref.y * INT32_SPEED_OF_CM_S_DEN / INT32_SPEED_OF_CM_S_NUM;
72  gps.fix = GPS_FIX_3D;
76  }
77  else {
78  struct Int32Vect2 zero_vector;
79  INT_VECT2_ZERO(zero_vector);
80  gh_set_ref(zero_vector, zero_vector, zero_vector);
81  gv_set_ref(0, 0, 0);
82  }
84  // publish gps data
85  uint32_t now_ts = get_sys_time_usec();
88  if (gps.fix == GPS_FIX_3D) {
91  }
92  AbiSendMsgGPS(GPS_SIM_ID, now_ts, &gps);
93  }
94 }
void gh_set_ref(struct Int32Vect2 pos, struct Int32Vect2 speed, struct Int32Vect2 accel)
void gps_sim_hitl_event(void)
Definition: gps_sim_hitl.c:42
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).
#define GPS_FIX_3D
3D GPS fix
Definition: gps.h:43
bool_t gps_available
Is set to TRUE when a new REMOTE_GPS packet is received and parsed.
Definition: gps_sim_hitl.c:34
int32_t z
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:89
int32_t alt
in millimeters above WGS84 reference ellipsoid
static uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.h:39
int32_t y
uint32_t last_msg_time
cpu time in sec at last received GPS message
Definition: gps.h:92
#define TRUE
Definition: std.h:4
int32_t hmsl
height above mean sea level in mm
Definition: gps.h:71
#define INT32_POS_OF_CM_NUM
#define GPS_FIX_NONE
No GPS fix.
Definition: gps.h:41
Device independent GPS code (interface)
bool_t autopilot_in_flight
Definition: autopilot.c:68
int32_t x
unsigned long uint32_t
Definition: types.h:18
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition: gps.h:68
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:122
int32_t guidance_v_z_ref
altitude reference in meters.
Definition: guidance_v.c:120
#define GPS_SIM_ID
uint32_t gps_sim_hitl_timer
Definition: gps_sim_hitl.c:35
int32_t guidance_v_zd_ref
vertical speed reference in meter/s.
Definition: guidance_v.c:121
volatile uint32_t nb_sec_rem
remainder of seconds since startup in CPU_TICKS
Definition: sys_time.h:70
#define SysTimeTimer(_t)
Definition: sys_time.h:214
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:162
uint32_t last_3dfix_time
cpu time in sec at last valid 3D fix
Definition: gps.h:90
#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:69
Horizontal guidance for rotorcrafts.
#define INT_VECT2_ZERO(_v)
uint32_t last_msg_ticks
cpu time ticks at last received GPS message
Definition: gps.h:91
struct EcefCoor_i ecef_vel
speed ECEF in cm/s
Definition: gps.h:72
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:69
#define SysTimeTimerStart(_t)
Definition: sys_time.h:213
bool_t ned_initialized_i
true if local int coordinate frame is initialsed
Definition: state.h:167
uint8_t fix
status of fix
Definition: gps.h:82
void gps_impl_init(void)
GPS initialization.
Definition: gps_sim_hitl.c:37
struct GpsState gps
global GPS state
Definition: gps.c:41
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.