Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-27 00:37:37 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

[gps] directly send ABI messages from implementation

Browse Source 
and get rid of the on_gps_solution callback in main
This commit is contained in:
Felix Ruess
2015-03-30 18:48:23 +02:00
parent 38e0c497ad
commit 6759ea17b2
33 changed files with 292 additions and 277 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/arch/sim/sim_gps.c
+1 -1
View File
@@ -74,7 +74,7 @@ value sim_use_gps_pos(value x, value y, value z, value c, value a, value s, valu
//gps_verbose_downlink = !launch;
//gps_downlink();
gps_available = TRUE;
gps_sim_publish();
return Val_unit;
}
sw/airborne/firmwares/fixedwing/main_ap.c
+1 -20
View File
@@ -148,10 +148,6 @@ static inline void on_accel_event(void);
static inline void on_mag_event(void);
#endif // USE_IMU
#if USE_GPS
static inline void on_gps_solution(void);
#endif
#if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1
static uint8_t mcu1_ppm_cpt;
#endif
@@ -688,7 +684,7 @@ void event_task_ap(void)
#endif
#if USE_GPS
GpsEvent(on_gps_solution);
GpsEvent();
#endif /* USE_GPS */
#if USE_BARO_BOARD
@@ -720,21 +716,6 @@ void event_task_ap(void)
} /* event_task_ap() */
#if USE_GPS
static inline void on_gps_solution(void)
{
// current timestamp
uint32_t now_ts = get_sys_time_usec();
AbiSendMsgGPS(1, now_ts, &gps);
#ifdef GPS_TRIGGERED_FUNCTION
GPS_TRIGGERED_FUNCTION();
#endif
}
#endif
#if USE_IMU
static inline void on_accel_event(void)
{
sw/airborne/firmwares/rotorcraft/main.c
+1 -16
View File
@@ -107,7 +107,6 @@ INFO_VALUE("it is recommended to configure in your airframe PERIODIC_FREQUENCY t
static inline void on_gyro_event(void);
static inline void on_accel_event(void);
static inline void on_gps_event(void);
static inline void on_mag_event(void);
@@ -327,7 +326,7 @@ STATIC_INLINE void main_event(void)
#endif
#if USE_GPS
GpsEvent(on_gps_event);
GpsEvent();
#endif
#if FAILSAFE_GROUND_DETECT || KILL_ON_GROUND_DETECT
@@ -371,20 +370,6 @@ static inline void on_gyro_event( void ) {
#endif
}
static inline void on_gps_event(void)
{
// current timestamp
uint32_t now_ts = get_sys_time_usec();
AbiSendMsgGPS(1, now_ts, &gps);
#ifdef USE_VEHICLE_INTERFACE
if (gps.fix == GPS_FIX_3D) {
vi_notify_gps_available();
}
#endif
}
static inline void on_mag_event(void)
{
imu_scale_mag(&imu);
sw/airborne/modules/ahrs/ahrs_infrared.c
+17 -3
View File
@@ -52,6 +52,12 @@ static float heading;
#endif
static abi_event gyro_ev;
#ifndef AHRS_INFRARED_GPS_ID
#define AHRS_INFRARED_GPS_ID ABI_BROADCAST
#endif
static abi_event gps_ev;
void ahrs_infrared_update_gps(struct GpsState *gps_s);
static void gyro_cb(uint8_t sender_id __attribute__((unused)),
uint32_t stamp __attribute__((unused)),
struct Int32Rates *gyro)
@@ -59,6 +65,13 @@ static void gyro_cb(uint8_t sender_id __attribute__((unused)),
stateSetBodyRates_i(gyro);
}
static void gps_cb(uint8_t sender_id __attribute__((unused)),
uint32_t stamp __attribute__((unused)),
struct GpsState *gps_s)
{
ahrs_infrared_update_gps(gps_s);
}
#if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h"
@@ -85,6 +98,7 @@ void ahrs_infrared_init(void)
heading = 0.;
AbiBindMsgIMU_GYRO_INT32(AHRS_INFRARED_GYRO_ID, &gyro_ev, gyro_cb);
AbiBindMsgGPS(AHRS_INFRARED_GPS_ID, &gps_ev, &gps_cb);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "IR_SENSORS", send_infrared);
@@ -93,10 +107,10 @@ void ahrs_infrared_init(void)
}
void ahrs_infrared_update_gps(void)
void ahrs_infrared_update_gps(struct GpsState *gps_s)
{
float hspeed_mod_f = gps.gspeed / 100.;
float course_f = gps.course / 1e7;
float hspeed_mod_f = gps_s->gspeed / 100.;
float course_f = gps_s->course / 1e7;
// Heading estimator from wind-information, usually computed with -DWIND_INFO
// wind_north and wind_east initialized to 0, so still correct if not updated
sw/airborne/modules/ahrs/ahrs_infrared.h
-3
View File
@@ -33,8 +33,5 @@
extern void ahrs_infrared_init(void);
extern void ahrs_infrared_periodic(void);
extern void ahrs_infrared_update_gps(void);
#define GPS_TRIGGERED_FUNCTION ahrs_infrared_update_gps
#endif /* AHRS_INFRARED_H */
sw/airborne/modules/ins/ins_xsens.c
+16 -3
View File
@@ -40,6 +40,7 @@
#error "USE_GPS needs to be 1 to use the Xsens GPS!"
#endif
#include "subsystems/gps.h"
#include "subsystems/abi.h"
#include "math/pprz_geodetic_wgs84.h"
#include "math/pprz_geodetic_float.h"
#include "subsystems/navigation/common_nav.h" /* needed for nav_utm_zone0 */
@@ -297,7 +298,19 @@ void ins_xsens_update_gps(struct GpsState *gps_s)
void gps_impl_init(void)
{
gps.nb_channels = 0;
gps_xsens_msg_available = FALSE;
}
static void gps_xsens_publish(void)
{
// publish gps data
uint32_t now_ts = get_sys_time_usec();
gps.last_msg_ticks = sys_time.nb_sec_rem;
gps.last_msg_time = sys_time.nb_sec;
if (gps.fix == GPS_FIX_3D) {
gps.last_3dfix_ticks = sys_time.nb_sec_rem;
gps.last_3dfix_time = sys_time.nb_sec;
}
AbiSendMsgGPS(GPS_XSENS_ID, now_ts, &gps);
}
#endif
@@ -533,7 +546,7 @@ void parse_ins_msg(void)
gps.sacc = XSENS_DATA_RAWGPS_sacc(xsens_msg_buf, offset) / 100;
gps.pdop = 5; // FIXME Not output by XSens
gps_xsens_msg_available = TRUE;
gps_xsens_publish();
#endif
offset += XSENS_DATA_RAWGPS_LENGTH;
}
@@ -627,7 +640,7 @@ void parse_ins_msg(void)
gps.hmsl = ins_z * 1000;
// what about gps.lla_pos.alt and gps.utm_pos.alt ?
gps_xsens_msg_available = TRUE;
gps_xsens_publish();
#endif
offset += XSENS_DATA_Position_LENGTH;
}
sw/airborne/modules/ins/ins_xsens.h
+1 -13
View File
@@ -91,19 +91,7 @@ extern void ins_xsens_register(void);
#if USE_GPS_XSENS
extern bool_t gps_xsens_msg_available;
#define GpsEvent(_sol_available_callback) { \
if (gps_xsens_msg_available) { \
gps.last_msg_ticks = sys_time.nb_sec_rem; \
gps.last_msg_time = sys_time.nb_sec; \
if (gps.fix == GPS_FIX_3D) { \
gps.last_3dfix_ticks = sys_time.nb_sec_rem; \
gps.last_3dfix_time = sys_time.nb_sec; \
} \
_sol_available_callback(); \
gps_xsens_msg_available = FALSE; \
} \
}
#define GpsEvent() {}
#endif
#endif
sw/airborne/modules/ins/ins_xsens700.c
+15 -3
View File
@@ -42,10 +42,10 @@
#error "USE_GPS needs to be 1 to use the Xsens GPS!"
#endif
#include "subsystems/gps.h"
#include "subsystems/abi.h"
#include "math/pprz_geodetic_wgs84.h"
#include "math/pprz_geodetic_float.h"
#include "subsystems/navigation/common_nav.h" /* needed for nav_utm_zone0 */
bool_t gps_xsens_msg_available;
#endif
// positions
@@ -213,7 +213,19 @@ void ins_update_gps(void)
void gps_impl_init(void)
{
gps.nb_channels = 0;
gps_xsens_msg_available = FALSE;
}
static void gps_xsens_publish(void)
{
// publish gps data
uint32_t now_ts = get_sys_time_usec();
gps.last_msg_ticks = sys_time.nb_sec_rem;
gps.last_msg_time = sys_time.nb_sec;
if (gps.fix == GPS_FIX_3D) {
gps.last_3dfix_ticks = sys_time.nb_sec_rem;
gps.last_3dfix_time = sys_time.nb_sec;
}
AbiSendMsgGPS(GPS_XSENS_ID, now_ts, &gps);
}
#endif
@@ -485,7 +497,7 @@ void parse_ins_msg(void)
gps.utm_pos.east = utm_f.east * 100;
gps.utm_pos.north = utm_f.north * 100;
gps_xsens_msg_available = TRUE;
gps_xsens_publish();
}
} else if (code1 == 0xD0) {
if (code2 == 0x10) {
sw/airborne/subsystems/abi_sender_ids.h
+47
View File
@@ -111,4 +111,51 @@
#define MAG_HMC58XX_SENDER_ID 2
#endif
/*
* IDs of GPS sensors
*/
#ifndef GPS_UBX_ID
#define GPS_UBX_ID 1
#endif
#ifndef GPS_NMEA_ID
#define GPS_NMEA_ID 2
#endif
#ifndef GPS_SIRF_ID
#define GPS_SIRF_ID 3
#endif
#ifndef GPS_SKYTRAQ_ID
#define GPS_SKYTRAQ_ID 4
#endif
#ifndef GPS_MTK_ID
#define GPS_MTK_ID 5
#endif
#ifndef GPS_PIKSI_ID
#define GPS_PIKSI_ID 6
#endif
#ifndef GPS_XSENS_ID
#define GPS_XSENS_ID 7
#endif
#ifndef GPS_DATALINK_ID
#define GPS_DATALINK_ID 8
#endif
#ifndef GPS_UDP_ID
#define GPS_UDP_ID 9
#endif
#ifndef GPS_ARDRONE2_ID
#define GPS_ARDRONE2_ID 10
#endif
#ifndef GPS_SIM_ID
#define GPS_SIM_ID 11
#endif
#endif /* ABI_SENDER_IDS_H */
sw/airborne/subsystems/gps/gps_ardrone2.c
+10 -5
View File
@@ -31,13 +31,11 @@
#endif
#include "subsystems/gps.h"
#include "subsystems/abi.h"
#include "math/pprz_geodetic_double.h"
bool_t gps_ardrone2_available;
void gps_impl_init(void)
{
gps_ardrone2_available = FALSE;
}
void gps_ardrone2_parse(navdata_gps_t *navdata_gps)
@@ -76,6 +74,13 @@ void gps_ardrone2_parse(navdata_gps_t *navdata_gps)
gps.fix = GPS_FIX_NONE;
}
// Set that there is a packet
gps_ardrone2_available = TRUE;
gps.last_msg_ticks = sys_time.nb_sec_rem;
gps.last_msg_time = sys_time.nb_sec;
if (gps.fix == GPS_FIX_3D) {
gps.last_3dfix_ticks = sys_time.nb_sec_rem;
gps.last_3dfix_time = sys_time.nb_sec;
}
uint32_t now_ts = get_sys_time_usec();
AbiSendMsgGPS(GPS_ARDRONE2_ID, now_ts, &gps);
}
sw/airborne/subsystems/gps/gps_ardrone2.h
+2 -14
View File
@@ -31,23 +31,11 @@
#include "boards/ardrone/at_com.h"
#define GPS_NB_CHANNELS 12
extern bool_t gps_ardrone2_available;
/*
* The GPS event
* The GPS event (dummy)
*/
#define GpsEvent(_sol_available_callback) { \
if (gps_ardrone2_available) { \
gps.last_msg_ticks = sys_time.nb_sec_rem; \
gps.last_msg_time = sys_time.nb_sec; \
if (gps.fix == GPS_FIX_3D) { \
gps.last_3dfix_ticks = sys_time.nb_sec_rem; \
gps.last_3dfix_time = sys_time.nb_sec; \
} \
_sol_available_callback(); \
gps_ardrone2_available = FALSE; \
} \
}
#define GpsEvent() {}
void gps_ardrone2_parse(navdata_gps_t *navdata_gps);
sw/airborne/subsystems/gps/gps_datalink.c
+11 -4
View File
@@ -28,14 +28,12 @@
*/
#include "subsystems/gps.h"
bool_t gps_available; ///< Is set to TRUE when a new REMOTE_GPS packet is received and parsed
#include "subsystems/abi.h"
/** GPS initialization */
void gps_impl_init(void)
{
gps.fix = GPS_FIX_NONE;
gps_available = FALSE;
gps.gspeed = 700; // To enable course setting
gps.cacc = 0; // To enable course setting
}
@@ -63,7 +61,6 @@ void parse_gps_datalink(uint8_t numsv, int32_t ecef_x, int32_t ecef_y, int32_t e
gps.num_sv = numsv;
gps.tow = tow;
gps.fix = GPS_FIX_3D;
gps_available = TRUE;
#if GPS_USE_LATLONG
// Computes from (lat, long) in the referenced UTM zone
@@ -79,5 +76,15 @@ void parse_gps_datalink(uint8_t numsv, int32_t ecef_x, int32_t ecef_y, int32_t e
gps.utm_pos.alt = gps.lla_pos.alt;
gps.utm_pos.zone = nav_utm_zone0;
#endif
// publish new GPS data
uint32_t now_ts = get_sys_time_usec();
gps.last_msg_ticks = sys_time.nb_sec_rem;
gps.last_msg_time = sys_time.nb_sec;
if (gps.fix == GPS_FIX_3D) {
gps.last_3dfix_ticks = sys_time.nb_sec_rem;
gps.last_3dfix_time = sys_time.nb_sec;
}
AbiSendMsgGPS(GPS_DATALINK_ID, now_ts, &gps);
}
sw/airborne/subsystems/gps/gps_datalink.h
+6 -18
View File
@@ -33,24 +33,12 @@
#include "std.h"
#define GPS_NB_CHANNELS 0
extern bool_t gps_available;
extern void parse_gps_datalink(uint8_t numsv, int32_t ecef_x, int32_t ecef_y, int32_t ecef_z, int32_t lat, int32_t lon,
int32_t alt,
int32_t hmsl, int32_t ecef_xd, int32_t ecef_yd, int32_t ecef_zd, uint32_t tow, int32_t course);
#define GpsEvent(_sol_available_callback) { \
if (gps_available) { \
gps.last_msg_ticks = sys_time.nb_sec_rem; \
gps.last_msg_time = sys_time.nb_sec; \
if (gps.fix == GPS_FIX_3D) { \
gps.last_3dfix_ticks = sys_time.nb_sec_rem; \
gps.last_3dfix_time = sys_time.nb_sec; \
} \
_sol_available_callback(); \
gps_available = FALSE; \
} \
}
extern void parse_gps_datalink(uint8_t numsv, int32_t ecef_x, int32_t ecef_y, int32_t ecef_z,
int32_t lat, int32_t lon, int32_t alt, int32_t hmsl,
int32_t ecef_xd, int32_t ecef_yd, int32_t ecef_zd,
uint32_t tow, int32_t course);
// dummy
#define GpsEvent() {}
#endif /* GPS_DATALINK_H */
sw/airborne/subsystems/gps/gps_mtk.c
+7 -4
View File
@@ -32,7 +32,7 @@
*/
#include "subsystems/gps.h"
#include "subsystems/abi.h"
#include "led.h"
/* currently needed to get nav_utm_zone0 */
@@ -115,8 +115,11 @@ void gps_impl_init(void)
#endif
}
void gps_mtk_msg(void (* _cb)(void))
void gps_mtk_msg(void)
{
// current timestamp
uint32_t now_ts = get_sys_time_usec();
gps.last_msg_ticks = sys_time.nb_sec_rem;
gps.last_msg_time = sys_time.nb_sec;
gps_mtk_read_message();
@@ -126,7 +129,7 @@ void gps_mtk_msg(void (* _cb)(void))
gps.last_3dfix_ticks = sys_time.nb_sec_rem;
gps.last_3dfix_time = sys_time.nb_sec;
}
_sol_available_callback();
AbiSendMsgGPS(GPS_MTK_ID, now_ts, &gps);
}
if (gps_mtk.msg_class == MTK_DIY16_ID &&
gps_mtk.msg_id == MTK_DIY16_NAV_ID) {
@@ -134,7 +137,7 @@ void gps_mtk_msg(void (* _cb)(void))
gps.last_3dfix_ticks = sys_time.nb_sec_rem;
gps.last_3dfix_time = sys_time.nb_sec;
}
_cb();
AbiSendMsgGPS(GPS_MTK_ID, now_ts, &gps);
}
gps_mtk.msg_available = FALSE;
}
sw/airborne/subsystems/gps/gps_mtk.h
+2 -2
View File
@@ -69,7 +69,7 @@ extern bool_t gps_configuring;
extern void gps_mtk_read_message(void);
extern void gps_mtk_parse(uint8_t c);
static inline void GpsEvent(void (* _sol_available_callback)(void))
static inline void GpsEvent(void)
{
struct link_device *dev = &((GPS_LINK).device);
@@ -80,7 +80,7 @@ static inline void GpsEvent(void (* _sol_available_callback)(void))
GpsConfigure();
}
if (gps_mtk.msg_available) {
gps_mtk_msg(_sol_available_callback);
gps_mtk_msg();
}
}
sw/airborne/subsystems/gps/gps_nmea.c
+6 -3
View File
@@ -31,7 +31,7 @@
*/
#include "subsystems/gps.h"
#include "subsystems/abi.h"
#include "led.h"
#if GPS_USE_LATLONG
@@ -74,8 +74,11 @@ void gps_impl_init(void)
nmea_configure();
}
void gps_nmea_msg(void (* _cb)(void))
void gps_nmea_msg(void)
{
// current timestamp
uint32_t now_ts = get_sys_time_usec();
gps.last_msg_ticks = sys_time.nb_sec_rem;
gps.last_msg_time = sys_time.nb_sec;
nmea_parse_msg();
@@ -84,7 +87,7 @@ void gps_nmea_msg(void (* _cb)(void))
gps.last_3dfix_ticks = sys_time.nb_sec_rem;
gps.last_3dfix_time = sys_time.nb_sec;
}
_cb();
AbiSendMsgGPS(GPS_NMEA_ID, now_ts, &gps);
}
gps_nmea.msg_available = FALSE;
}
sw/airborne/subsystems/gps/gps_nmea.h
+3 -3
View File
@@ -62,9 +62,9 @@ extern void nmea_parse_msg(void);
extern uint8_t nmea_calc_crc(const char *buff, int buff_sz);
extern void nmea_parse_prop_init(void);
extern void nmea_parse_prop_msg(void);
extern void gps_nmea_msg(void (* _cb)(void));
extern void gps_nmea_msg(void);
static inline void GpsEvent(void (* _sol_available_callback)(void))
static inline void GpsEvent(void)
{
struct link_device *dev = &((GPS_LINK).device);
@@ -78,7 +78,7 @@ static inline void GpsEvent(void (* _sol_available_callback)(void))
}
}
if (gps_nmea.msg_available) {
gps_nmea_msg(_sol_available_callback);
gps_nmea_msg();
}
}
sw/airborne/subsystems/gps/gps_piksi.c
+19 -6
View File
@@ -32,6 +32,7 @@
#include <sbp.h>
#include <sbp_messages.h>
#include "subsystems/gps.h"
#include "subsystems/abi.h"
#include "mcu_periph/uart.h"
#include "math/pprz_geodetic_double.h"
#if GPS_USE_LATLONG
@@ -67,6 +68,8 @@ sbp_msg_callbacks_node_t baseline_ecef_node;
//sbp_msg_callbacks_node_t baseline_ned_node;
//#endif
static void gps_piksi_publish(void);
/*
* Callback functions to interpret SBP messages.
* Every message ID has a callback associated with it to
@@ -102,7 +105,7 @@ static void sbp_baseline_ecef_callback(uint16_t sender_id __attribute__((unused)
gps.tow = baseline_ecef.tow;
// High precision solution available
gps_piksi_available = TRUE;
gps_piksi_publish();
}
#endif
@@ -118,7 +121,7 @@ static void sbp_vel_ecef_callback(uint16_t sender_id __attribute__((unused)),
gps.sacc = (uint32_t)(vel_ecef.accuracy);
// Solution available (VEL_ECEF is the last message to be send)
gps_piksi_available = TRUE;
gps_piksi_publish();
}
static void sbp_pos_llh_callback(uint16_t sender_id __attribute__((unused)),
@@ -205,12 +208,8 @@ static void sbp_gps_time_callback(uint16_t sender_id __attribute__((unused)),
gps.tow = gps_time.tow;
}
bool_t gps_piksi_available;
void gps_impl_init(void)
{
gps_piksi_available = FALSE;
/* Setup SBP nodes */
sbp_state_init(&sbp_state);
/* Register a node and callback, and associate them with a specific message ID. */
@@ -258,6 +257,20 @@ void gps_piksi_event(void)
sbp_process(&sbp_state, &fifo_read);
}
static void gps_piksi_publish(void)
{
// current timestamp
uint32_t now_ts = get_sys_time_usec();
gps.last_msg_ticks = sys_time.nb_sec_rem;
gps.last_msg_time = sys_time.nb_sec;
if (gps.fix == GPS_FIX_3D) {
gps.last_3dfix_ticks = sys_time.nb_sec_rem;
gps.last_3dfix_time = sys_time.nb_sec;
}
AbiSendMsgGPS(GPS_PIKSI_ID, now_ts, &gps);
}
/*
* FIFO implementation
*/
sw/airborne/subsystems/gps/gps_piksi.h
+1 -16
View File
@@ -32,26 +32,11 @@
#ifndef GPS_PIKSI_H
#define GPS_PIKSI_H
extern bool_t gps_piksi_available;
void gps_piksi_event(void);
/*
* The GPS event
*/
#define GpsEvent(_sol_available_callback) { \
gps_piksi_event(); \
if (gps_piksi_available) { \
gps.last_msg_ticks = sys_time.nb_sec_rem; \
gps.last_msg_time = sys_time.nb_sec; \
if (gps.fix == GPS_FIX_3D) { \
gps.last_3dfix_ticks = sys_time.nb_sec_rem; \
gps.last_3dfix_time = sys_time.nb_sec; \
} \
_sol_available_callback(); \
gps_piksi_available = FALSE; \
} \
}
#define GpsEvent gps_piksi_event
#endif /* GPS_PIKSI_H */
sw/airborne/subsystems/gps/gps_sim.c
+13 -19
View File
@@ -20,27 +20,21 @@
*/
#include "subsystems/gps.h"
bool_t gps_available;
#if 0
void gps_feed_values(double utm_north, double utm_east, double utm_alt, double gspeed, double course, double climb)
{
gps.utm_pos.north = CM_OF_M(utm_north);
gps.utm_pos.east = CM_OF_M(utm_east);
//TODO set height above ellipsoid properly
gps.hmsl = utm_alt * 1000.;
gps.gspeed = CM_OF_M(gspeed);
gps.course = EM7RAD_OF_RAD(RadOfDeg(course / 10.));
gps.ned_vel.z = -climb * 100.;
gps.fix = GPS_FIX_3D;
gps_available = TRUE;
}
#endif
#include "subsystems/abi.h"
void gps_impl_init(void)
{
gps.fix = GPS_FIX_NONE;
gps_available = FALSE;
}
void gps_sim_publish(void)
{
uint32_t now_ts = get_sys_time_usec();
gps.last_msg_ticks = sys_time.nb_sec_rem;
gps.last_msg_time = sys_time.nb_sec;
if (gps.fix == GPS_FIX_3D) {
gps.last_3dfix_ticks = sys_time.nb_sec_rem;
gps.last_3dfix_time = sys_time.nb_sec;
}
AbiSendMsgGPS(GPS_SIM_ID, now_ts, &gps);
}
sw/airborne/subsystems/gps/gps_sim.h
+3 -16
View File
@@ -5,23 +5,10 @@
#define GPS_NB_CHANNELS 16
extern bool_t gps_available;
//extern void gps_feed_values(double utm_north, double utm_east, double utm_alt, double gspeed, double course, double climb);
extern void gps_impl_init(void);
#define GpsEvent(_sol_available_callback) { \
if (gps_available) { \
gps.last_msg_ticks = sys_time.nb_sec_rem; \
gps.last_msg_time = sys_time.nb_sec; \
if (gps.fix == GPS_FIX_3D) { \
gps.last_3dfix_ticks = sys_time.nb_sec_rem; \
gps.last_3dfix_time = sys_time.nb_sec; \
} \
_sol_available_callback(); \
gps_available = FALSE; \
} \
}
extern void gps_sim_publish(void);
#define GpsEvent() {}
#endif /* GPS_SIM_H */
sw/airborne/subsystems/gps/gps_sim_hitl.c
+59 -1
View File
@@ -25,6 +25,11 @@
*/
#include "subsystems/gps.h"
#include "subsystems/abi.h"
#include "state.h"
#include "guidance/guidance_h.h"
#include "guidance/guidance_v.h"
bool_t gps_available;
uint32_t gps_sim_hitl_timer;
@@ -32,5 +37,58 @@ uint32_t gps_sim_hitl_timer;
void gps_impl_init(void)
{
gps.fix = GPS_FIX_NONE;
gps_available = FALSE;
}
void gps_sim_hitl_event(void)
{
if (SysTimeTimer(gps_sim_hitl_timer) > 100000) {
SysTimeTimerStart(gps_sim_hitl_timer);
gps.last_msg_ticks = sys_time.nb_sec_rem;
gps.last_msg_time = sys_time.nb_sec;
if (state.ned_initialized_i) {
if (!autopilot_in_flight) {
struct Int32Vect2 zero_vector;
INT_VECT2_ZERO(zero_vector);
gh_set_ref(zero_vector, zero_vector, zero_vector);
INT_VECT2_ZERO(guidance_h_pos_ref);
INT_VECT2_ZERO(guidance_h_speed_ref);
INT_VECT2_ZERO(guidance_h_accel_ref);
gv_set_ref(0, 0, 0);
guidance_v_z_ref = 0;
guidance_v_zd_ref = 0;
guidance_v_zdd_ref = 0;
}
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;
gps_available = TRUE;
}
else {
struct Int32Vect2 zero_vector;
INT_VECT2_ZERO(zero_vector);
gh_set_ref(zero_vector, zero_vector, zero_vector);
gv_set_ref(0, 0, 0);
}
// publish gps data
uint32_t now_ts = get_sys_time_usec();
gps.last_msg_ticks = sys_time.nb_sec_rem;
gps.last_msg_time = sys_time.nb_sec;
if (gps.fix == GPS_FIX_3D) {
gps.last_3dfix_ticks = sys_time.nb_sec_rem;
gps.last_3dfix_time = sys_time.nb_sec;
}
AbiSendMsgGPS(GPS_SIM_ID, now_ts, &gps);
}
}
sw/airborne/subsystems/gps/gps_sim_hitl.h
+2 -49
View File
@@ -28,56 +28,9 @@
#define GPS_SIM_HITL_H
#include "std.h"
#include "state.h"
#include "guidance/guidance_h.h"
#include "guidance/guidance_v.h"
extern bool_t gps_available;
extern uint32_t gps_sim_hitl_timer;
extern void gps_sim_hitl_event(void);
#define GpsEvent(_sol_available_callback) { \
if (SysTimeTimer(gps_sim_hitl_timer) > 100000) { \
SysTimeTimerStart(gps_sim_hitl_timer); \
gps.last_msg_ticks = sys_time.nb_sec_rem; \
gps.last_msg_time = sys_time.nb_sec; \
if (state.ned_initialized_i) { \
if (!autopilot_in_flight) { \
struct Int32Vect2 zero_vector; \
INT_VECT2_ZERO(zero_vector); \
gh_set_ref(zero_vector, zero_vector, zero_vector); \
INT_VECT2_ZERO(guidance_h_pos_ref); \
INT_VECT2_ZERO(guidance_h_speed_ref); \
INT_VECT2_ZERO(guidance_h_accel_ref); \
gv_set_ref(0, 0, 0); \
guidance_v_z_ref = 0; \
guidance_v_zd_ref = 0; \
guidance_v_zdd_ref = 0; \
} \
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; \
gps_available = TRUE; \
} \
else { \
struct Int32Vect2 zero_vector; \
INT_VECT2_ZERO(zero_vector); \
gh_set_ref(zero_vector, zero_vector, zero_vector); \
gv_set_ref(0, 0, 0); \
} \
_sol_available_callback(); \
gps_available = FALSE; \
} \
}
#define GpsEvent gps_sim_hitl_event
#endif /* GPS_SIM_HITL_H */
sw/airborne/subsystems/gps/gps_sim_nps.c
+11 -4
View File
@@ -21,7 +21,7 @@
#include "subsystems/gps/gps_sim_nps.h"
#include "subsystems/gps.h"
#include "subsystems/abi.h"
#include "nps_sensors.h"
#include "nps_fdm.h"
@@ -31,7 +31,6 @@
#include "math/pprz_geodetic_float.h"
#endif
bool_t gps_available;
bool_t gps_has_fix;
void gps_feed_value()
@@ -88,11 +87,19 @@ void gps_feed_value()
} else {
gps.fix = GPS_FIX_NONE;
}
gps_available = TRUE;
// publish gps data
uint32_t now_ts = get_sys_time_usec();
gps.last_msg_ticks = sys_time.nb_sec_rem;
gps.last_msg_time = sys_time.nb_sec;
if (gps.fix == GPS_FIX_3D) {
gps.last_3dfix_ticks = sys_time.nb_sec_rem;
gps.last_3dfix_time = sys_time.nb_sec;
}
AbiSendMsgGPS(GPS_SIM_ID, now_ts, &gps);
}
void gps_impl_init()
{
gps_available = FALSE;
gps_has_fix = TRUE;
}
sw/airborne/subsystems/gps/gps_sim_nps.h
+1 -13
View File
@@ -5,24 +5,12 @@
#define GPS_NB_CHANNELS 16
extern bool_t gps_available;
extern bool_t gps_has_fix;
extern void gps_feed_value();
extern void gps_impl_init();
#define GpsEvent(_sol_available_callback) { \
if (gps_available) { \
gps.last_msg_ticks = sys_time.nb_sec_rem; \
gps.last_msg_time = sys_time.nb_sec; \
if (gps.fix == GPS_FIX_3D) { \
gps.last_3dfix_ticks = sys_time.nb_sec_rem; \
gps.last_3dfix_time = sys_time.nb_sec; \
} \
_sol_available_callback(); \
gps_available = FALSE; \
} \
}
#define GpsEvent() {}
#endif /* GPS_SIM_NPS_H */
sw/airborne/subsystems/gps/gps_sirf.c
+5 -3
View File
@@ -22,7 +22,7 @@
#include "subsystems/gps.h"
#include "subsystems/abi.h"
#include "led.h"
#if GPS_USE_LATLONG
@@ -48,8 +48,10 @@ void gps_impl_init(void)
gps_sirf.read_state = 0;
}
void gps_sirf_msg(void (* _cb)(void))
void gps_sirf_msg(void)
{
// current timestamp
uint32_t now_ts = get_sys_time_usec();
gps.last_msg_ticks = sys_time.nb_sec_rem;
gps.last_msg_time = sys_time.nb_sec;
sirf_parse_msg();
@@ -58,7 +60,7 @@ void gps_sirf_msg(void (* _cb)(void))
gps.last_3dfix_ticks = sys_time.nb_sec_rem;
gps.last_3dfix_time = sys_time.nb_sec;
}
_cb();
AbiSendMsgGPS(GPS_SIRF_ID, now_ts, &gps);
}
gps_sirf.msg_available = FALSE;
}
sw/airborne/subsystems/gps/gps_sirf.h
+3 -3
View File
@@ -131,9 +131,9 @@ struct sirf_msg_41 {
extern void sirf_parse_char(uint8_t c);
extern void sirf_parse_msg(void);
extern void gps_sirf_msg(void (* _cb)(void));
extern void gps_sirf_msg(void);
static inline void GpsEvent(void (* _sol_available_callback)(void))
static inline void GpsEvent(void)
{
struct link_device *dev = &((GPS_LINK).device);
@@ -143,7 +143,7 @@ static inline void GpsEvent(void (* _sol_available_callback)(void))
}
}
if (gps_sirf.msg_available) {
gps_sirf_msg(_sol_available_callback);
gps_sirf_msg();
}
}
sw/airborne/subsystems/gps/gps_skytraq.c
+5 -2
View File
@@ -20,6 +20,7 @@
*/
#include "subsystems/gps.h"
#include "subsystems/abi.h"
#include "led.h"
#if GPS_USE_LATLONG
@@ -97,8 +98,10 @@ void gps_impl_init(void)
}
void gps_skytraq_msg(void (* _cb)(void))
void gps_skytraq_msg(void)
{
// current timestamp
uint32_t now_ts = get_sys_time_usec();
gps.last_msg_ticks = sys_time.nb_sec_rem;
gps.last_msg_time = sys_time.nb_sec;
gps_skytraq_read_message();
@@ -107,7 +110,7 @@ void gps_skytraq_msg(void (* _cb)(void))
gps.last_3dfix_ticks = sys_time.nb_sec_rem;
gps.last_3dfix_time = sys_time.nb_sec;
}
_cb();
AbiSendMsgGPS(GPS_SKYTRAQ_ID, now_ts, &gps);
}
gps_skytraq.msg_available = FALSE;
}
sw/airborne/subsystems/gps/gps_skytraq.h
+3 -3
View File
@@ -62,9 +62,9 @@ extern struct GpsSkytraq gps_skytraq;
extern void gps_skytraq_read_message(void);
extern void gps_skytraq_parse(uint8_t c);
extern void gps_skytraq_msg(void (* _cb)(void));
extern void gps_skytraq_msg(void);
static inline void GpsEvent(void (* _sol_available_callback)(void))
static inline void GpsEvent(void)
{
struct link_device *dev = &((GPS_LINK).device);
@@ -74,7 +74,7 @@ static inline void GpsEvent(void (* _sol_available_callback)(void))
}
}
if (gps_skytraq.msg_available) {
gps_skytraq_msg(_sol_available_callback);
gps_skytraq_msg();
}
}
sw/airborne/subsystems/gps/gps_ubx.c
+6 -3
View File
@@ -21,7 +21,7 @@
#include "subsystems/gps.h"
#include "subsystems/abi.h"
#include "led.h"
#if GPS_USE_LATLONG
@@ -320,8 +320,11 @@ void ubx_send_cfg_rst(struct link_device *dev, uint16_t bbr , uint8_t reset_mode
#include "modules/gps/gps_ubx_ucenter.h"
#endif
void gps_ubx_msg(void (* _cb)(void))
void gps_ubx_msg(void)
{
// current timestamp
uint32_t now_ts = get_sys_time_usec();
gps.last_msg_ticks = sys_time.nb_sec_rem;
gps.last_msg_time = sys_time.nb_sec;
gps_ubx_read_message();
@@ -334,7 +337,7 @@ void gps_ubx_msg(void (* _cb)(void))
gps.last_3dfix_ticks = sys_time.nb_sec_rem;
gps.last_3dfix_time = sys_time.nb_sec;
}
_cb();
AbiSendMsgGPS(GPS_UBX_ID, now_ts, &gps);
}
gps_ubx.msg_available = FALSE;
}
sw/airborne/subsystems/gps/gps_ubx.h
+3 -3
View File
@@ -91,14 +91,14 @@ extern void ubx_send_cfg_rst(struct link_device *dev, uint16_t bbr, uint8_t rese
extern void gps_ubx_read_message(void);
extern void gps_ubx_parse(uint8_t c);
extern void gps_ubx_msg(void (* _cb)(void));
extern void gps_ubx_msg(void);
/* Gps callback is called when receiving a VELNED or a SOL message
* All position/speed messages are sent in one shot and VELNED is the last one on fixedwing
* For rotorcraft, only SOL message is needed for pos/speed data
*/
static inline void GpsEvent(void (* _sol_available_callback)(void))
static inline void GpsEvent(void)
{
struct link_device *dev = &((GPS_LINK).device);
@@ -108,7 +108,7 @@ static inline void GpsEvent(void (* _sol_available_callback)(void))
}
}
if (gps_ubx.msg_available) {
gps_ubx_msg(_sol_available_callback);
gps_ubx_msg();
}
}
sw/airborne/subsystems/gps/gps_udp.c
+11 -5
View File
@@ -20,9 +20,7 @@
*/
#include "subsystems/gps.h"
bool_t gps_available;
#include "subsystems/abi.h"
#include "fms/fms_network.h"
#include <string.h>
@@ -47,7 +45,6 @@ struct FmsNetwork *gps_network = NULL;
void gps_impl_init(void)
{
gps.fix = GPS_FIX_NONE;
gps_available = FALSE;
gps_network = network_new(GPS_UDP_HOST, 6000 /*out*/, 7000 /*in*/, TRUE);
}
@@ -79,7 +76,6 @@ void gps_parse(void)
gps.ecef_vel.z = UDP_GPS_INT(gps_udp_read_buffer + 40);
gps.fix = GPS_FIX_3D;
gps_available = TRUE;
#if GPS_USE_LATLONG
// Computes from (lat, long) in the referenced UTM zone
@@ -96,6 +92,16 @@ void gps_parse(void)
gps.utm_pos.zone = nav_utm_zone0;
#endif
// publish new GPS data
uint32_t now_ts = get_sys_time_usec();
gps.last_msg_ticks = sys_time.nb_sec_rem;
gps.last_msg_time = sys_time.nb_sec;
if (gps.fix == GPS_FIX_3D) {
gps.last_3dfix_ticks = sys_time.nb_sec_rem;
gps.last_3dfix_time = sys_time.nb_sec;
}
AbiSendMsgGPS(GPS_UDP_ID, now_ts, &gps);
} else {
printf("gps_udp error: msg len invalid %d bytes\n", size);
}
sw/airborne/subsystems/gps/gps_udp.h
+1 -16
View File
@@ -5,23 +5,8 @@
#define GPS_NB_CHANNELS 16
extern bool_t gps_available;
extern void gps_parse(void);
#define GpsEvent(_sol_available_callback) { \
gps_parse(); \
if (gps_available) { \
gps.last_msg_ticks = sys_time.nb_sec_rem; \
gps.last_msg_time = sys_time.nb_sec; \
if (gps.fix == GPS_FIX_3D) { \
gps.last_3dfix_ticks = sys_time.nb_sec_rem; \
gps.last_3dfix_time = sys_time.nb_sec; \
} \
_sol_available_callback(); \
gps_available = FALSE; \
} \
}
#define GpsEvent gps_parse
#endif /* GPS_UDP_H */