diff --git a/conf/airframes/examples/separate_fbw_ap.xml b/conf/airframes/examples/separate_fbw_ap.xml index 8878ed286a..eb851f9f0b 100644 --- a/conf/airframes/examples/separate_fbw_ap.xml +++ b/conf/airframes/examples/separate_fbw_ap.xml @@ -202,7 +202,6 @@ - - - - - diff --git a/conf/firmwares/subsystems/rotorcraft/intermcu_uart.makefile b/conf/firmwares/subsystems/rotorcraft/intermcu_uart.makefile index 91b73b7ba3..dc18155cd9 100644 --- a/conf/firmwares/subsystems/rotorcraft/intermcu_uart.makefile +++ b/conf/firmwares/subsystems/rotorcraft/intermcu_uart.makefile @@ -27,6 +27,12 @@ ifneq ($(RADIO_CONTROL_LED),none) $(TARGET).CFLAGS += -DRADIO_CONTROL_LED=$(RADIO_CONTROL_LED) endif +ifneq (,$(findstring $(SECONDARY_GPS), imcu)) + # this is the secondary GPS + $(TARGET).CFLAGS += -DGPS_SECONDARY_TYPE_H=\"subsystems/intermcu/intermcu_ap.h\" + $(TARGET).CFLAGS += -DSECONDARY_GPS=GPS_IMCU -DIMCU_GPS +endif + ap.srcs += subsystems/intermcu/intermcu_ap.c ap.srcs += pprzlink/src/pprz_transport.c $(TARGET).srcs += subsystems/radio_control.c diff --git a/conf/modules/gps_datalink.xml b/conf/modules/gps_datalink.xml index 5987619dd2..c139793660 100644 --- a/conf/modules/gps_datalink.xml +++ b/conf/modules/gps_datalink.xml @@ -17,23 +17,26 @@ - + ifdef SECONDARY_GPS ifneq (,$(findstring $(SECONDARY_GPS), datalink)) # this is the secondary GPS - ap.CFLAGS += -DGPS_SECONDARY_TYPE_H=\"subsystems/gps/gps_datalink.h\" - ap.CFLAGS += -DSECONDARY_GPS=GPS_DATALINK + $(TARGET).CFLAGS += -DGPS_SECONDARY_TYPE_H=\"subsystems/gps/gps_datalink.h\" + $(TARGET).CFLAGS += -DSECONDARY_GPS=GPS_DATALINK else - ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_datalink.h\" - ap.CFLAGS += -DPRIMARY_GPS=GPS_DATALINK + $(TARGET).CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_datalink.h\" + $(TARGET).CFLAGS += -DPRIMARY_GPS=GPS_DATALINK endif else # plain old single GPS usage - ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_datalink.h\" + $(TARGET).CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_datalink.h\" endif + + + diff --git a/conf/modules/gps_furuno.xml b/conf/modules/gps_furuno.xml index fb155050ec..2dc4396a93 100644 --- a/conf/modules/gps_furuno.xml +++ b/conf/modules/gps_furuno.xml @@ -18,7 +18,7 @@ - + @@ -35,16 +35,19 @@ ifdef SECONDARY_GPS ifneq (,$(findstring $(SECONDARY_GPS), nmea furuno)) # this is the secondary GPS - ap.CFLAGS += -DGPS_SECONDARY_TYPE_H=\"subsystems/gps/gps_nmea.h\" - ap.CFLAGS += -DSECONDARY_GPS=GPS_NMEA + $(TARGET).CFLAGS += -DGPS_SECONDARY_TYPE_H=\"subsystems/gps/gps_nmea.h\" + $(TARGET).CFLAGS += -DSECONDARY_GPS=GPS_NMEA else - ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_nmea.h\" - ap.CFLAGS += -DPRIMARY_GPS=GPS_NMEA + $(TARGET).CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_nmea.h\" + $(TARGET).CFLAGS += -DPRIMARY_GPS=GPS_NMEA endif else # plain old single GPS usage - ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_nmea.h\" + $(TARGET).CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_nmea.h\" endif + + + diff --git a/conf/modules/gps_mediatek_diy.xml b/conf/modules/gps_mediatek_diy.xml index 0083a15c82..5bafea50b0 100644 --- a/conf/modules/gps_mediatek_diy.xml +++ b/conf/modules/gps_mediatek_diy.xml @@ -18,7 +18,7 @@ - + @@ -31,16 +31,19 @@ ifdef SECONDARY_GPS ifneq (,$(findstring $(SECONDARY_GPS), mtk mediatek)) # this is the secondary GPS - ap.CFLAGS += -DGPS_SECONDARY_TYPE_H=\"subsystems/gps/gps_mtk.h\" - ap.CFLAGS += -DSECONDARY_GPS=GPS_MTK + $(TARGET).CFLAGS += -DGPS_SECONDARY_TYPE_H=\"subsystems/gps/gps_mtk.h\" + $(TARGET).CFLAGS += -DSECONDARY_GPS=GPS_MTK else - ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_mtk.h\" - ap.CFLAGS += -DPRIMARY_GPS=GPS_MTK + $(TARGET).CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_mtk.h\" + $(TARGET).CFLAGS += -DPRIMARY_GPS=GPS_MTK endif else # plain old single GPS usage - ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_mtk.h\" + $(TARGET).CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_mtk.h\" endif + + + diff --git a/conf/modules/gps_nmea.xml b/conf/modules/gps_nmea.xml index 610c3ed42e..aeccf017ff 100644 --- a/conf/modules/gps_nmea.xml +++ b/conf/modules/gps_nmea.xml @@ -18,7 +18,7 @@ - + @@ -31,16 +31,19 @@ ifdef SECONDARY_GPS ifneq (,$(findstring $(SECONDARY_GPS), nmea)) # this is the secondary GPS - ap.CFLAGS += -DGPS_SECONDARY_TYPE_H=\"subsystems/gps/gps_nmea.h\" - ap.CFLAGS += -DSECONDARY_GPS=GPS_NMEA + $(TARGET).CFLAGS += -DGPS_SECONDARY_TYPE_H=\"subsystems/gps/gps_nmea.h\" + $(TARGET).CFLAGS += -DSECONDARY_GPS=GPS_NMEA else - ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_nmea.h\" - ap.CFLAGS += -DPRIMARY_GPS=GPS_NMEA + $(TARGET).CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_nmea.h\" + $(TARGET).CFLAGS += -DPRIMARY_GPS=GPS_NMEA endif else # plain old single GPS usage - ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_nmea.h\" + $(TARGET).CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_nmea.h\" endif + + + diff --git a/conf/modules/gps_piksi.xml b/conf/modules/gps_piksi.xml index d1379772db..0e9ee3c6b8 100644 --- a/conf/modules/gps_piksi.xml +++ b/conf/modules/gps_piksi.xml @@ -18,7 +18,7 @@ - + @@ -34,16 +34,19 @@ ifdef SECONDARY_GPS ifneq (,$(findstring $(SECONDARY_GPS), piksi)) # this is the secondary GPS - ap.CFLAGS += -DGPS_SECONDARY_TYPE_H=\"subsystems/gps/gps_piksi.h\" - ap.CFLAGS += -DSECONDARY_GPS=GPS_PIKSI + $(TARGET).CFLAGS += -DGPS_SECONDARY_TYPE_H=\"subsystems/gps/gps_piksi.h\" + $(TARGET).CFLAGS += -DSECONDARY_GPS=GPS_PIKSI else - ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_piksi.h\" - ap.CFLAGS += -DPRIMARY_GPS=GPS_PIKSI + $(TARGET).CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_piksi.h\" + $(TARGET).CFLAGS += -DPRIMARY_GPS=GPS_PIKSI endif else # plain old single GPS usage - ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_piksi.h\" + $(TARGET).CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_piksi.h\" endif + + + diff --git a/conf/modules/gps_sirf.xml b/conf/modules/gps_sirf.xml index 0548539f6f..065c142228 100644 --- a/conf/modules/gps_sirf.xml +++ b/conf/modules/gps_sirf.xml @@ -18,7 +18,7 @@ - + @@ -31,16 +31,19 @@ ifdef SECONDARY_GPS ifneq (,$(findstring $(SECONDARY_GPS), sirf)) # this is the secondary GPS - ap.CFLAGS += -DGPS_SECONDARY_TYPE_H=\"subsystems/gps/gps_sirf.h\" - ap.CFLAGS += -DSECONDARY_GPS=GPS_SIRF + $(TARGET).CFLAGS += -DGPS_SECONDARY_TYPE_H=\"subsystems/gps/gps_sirf.h\" + $(TARGET).CFLAGS += -DSECONDARY_GPS=GPS_SIRF else - ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sirf.h\" - ap.CFLAGS += -DPRIMARY_GPS=GPS_SIRF + $(TARGET).CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sirf.h\" + $(TARGET).CFLAGS += -DPRIMARY_GPS=GPS_SIRF endif else # plain old single GPS usage - ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sirf.h\" + $(TARGET).CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sirf.h\" endif + + + diff --git a/conf/modules/gps_skytraq.xml b/conf/modules/gps_skytraq.xml index ead8a7d8e4..8151387497 100644 --- a/conf/modules/gps_skytraq.xml +++ b/conf/modules/gps_skytraq.xml @@ -18,7 +18,7 @@ - + @@ -31,16 +31,19 @@ ifdef SECONDARY_GPS ifneq (,$(findstring $(SECONDARY_GPS), skytraq)) # this is the secondary GPS - ap.CFLAGS += -DGPS_SECONDARY_TYPE_H=\"subsystems/gps/gps_skytraq.h\" - ap.CFLAGS += -DSECONDARY_GPS=GPS_SKYTRAQ + $(TARGET).CFLAGS += -DGPS_SECONDARY_TYPE_H=\"subsystems/gps/gps_skytraq.h\" + $(TARGET).CFLAGS += -DSECONDARY_GPS=GPS_SKYTRAQ else - ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_skytraq.h\" - ap.CFLAGS += -DPRIMARY_GPS=GPS_SKYTRAQ + $(TARGET).CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_skytraq.h\" + $(TARGET).CFLAGS += -DPRIMARY_GPS=GPS_SKYTRAQ endif else # plain old single GPS usage - ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_skytraq.h\" + $(TARGET).CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_skytraq.h\" endif + + + diff --git a/conf/modules/gps_ublox.xml b/conf/modules/gps_ublox.xml index dba5b8870c..70d9290e3e 100644 --- a/conf/modules/gps_ublox.xml +++ b/conf/modules/gps_ublox.xml @@ -18,7 +18,7 @@ - + @@ -31,17 +31,20 @@ ifdef SECONDARY_GPS ifneq (,$(findstring $(SECONDARY_GPS), ublox)) # this is the secondary GPS - ap.CFLAGS += -DGPS_SECONDARY_TYPE_H=\"subsystems/gps/gps_ubx.h\" - ap.CFLAGS += -DSECONDARY_GPS=GPS_UBX + $(TARGET).CFLAGS += -DGPS_SECONDARY_TYPE_H=\"subsystems/gps/gps_ubx.h\" + $(TARGET).CFLAGS += -DSECONDARY_GPS=GPS_UBX else - ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_ubx.h\" - ap.CFLAGS += -DPRIMARY_GPS=GPS_UBX + $(TARGET).CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_ubx.h\" + $(TARGET).CFLAGS += -DPRIMARY_GPS=GPS_UBX endif else # plain old single GPS usage - ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_ubx.h\" + $(TARGET).CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_ubx.h\" endif + + + diff --git a/conf/modules/gps_ubx_i2c.xml b/conf/modules/gps_ubx_i2c.xml index 3ab8e9f00a..e58a5cb372 100644 --- a/conf/modules/gps_ubx_i2c.xml +++ b/conf/modules/gps_ubx_i2c.xml @@ -18,7 +18,7 @@ - + diff --git a/conf/modules/gps_ubx_ucenter.xml b/conf/modules/gps_ubx_ucenter.xml index 292ed4dec4..ae407bb937 100644 --- a/conf/modules/gps_ubx_ucenter.xml +++ b/conf/modules/gps_ubx_ucenter.xml @@ -40,7 +40,7 @@ Warning: you still need to tell the driver, which paparazzi port you use. - + diff --git a/sw/airborne/subsystems/abi_sender_ids.h b/sw/airborne/subsystems/abi_sender_ids.h index 19b19b133e..6352d5449f 100644 --- a/sw/airborne/subsystems/abi_sender_ids.h +++ b/sw/airborne/subsystems/abi_sender_ids.h @@ -178,6 +178,10 @@ #define GPS_VECTORNAV_ID 13 #endif +#ifndef GPS_IMCU_ID +#define GPS_IMCU_ID 14 +#endif + /* * IDs of IMU sensors (accel, gyro) */ diff --git a/sw/airborne/subsystems/gps.c b/sw/airborne/subsystems/gps.c index facf24e863..e3d5a6a27d 100644 --- a/sw/airborne/subsystems/gps.c +++ b/sw/airborne/subsystems/gps.c @@ -77,7 +77,7 @@ struct GpsState gps; struct GpsTimeSync gps_time_sync; #ifdef SECONDARY_GPS -static uint8_t current_gps_id = 0; +static uint8_t current_gps_id = GpsId(PRIMARY_GPS); #endif uint8_t multi_gps_mode; diff --git a/sw/airborne/subsystems/intermcu/intermcu_ap.c b/sw/airborne/subsystems/intermcu/intermcu_ap.c index 612d3ef538..95cfdd97d6 100644 --- a/sw/airborne/subsystems/intermcu/intermcu_ap.c +++ b/sw/airborne/subsystems/intermcu/intermcu_ap.c @@ -47,6 +47,13 @@ uint8_t imcu_msg_buf[128] __attribute__((aligned)); ///< The InterMCU message b static struct fbw_status_t fbw_status; static inline void intermcu_parse_msg(void (*rc_frame_handler)(void)); +#if IMCU_GPS +#include "std.h" +#include "subsystems/abi.h" +#include "subsystems/gps.h" +static struct GpsState gps_imcu; +#endif + #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" @@ -65,6 +72,15 @@ void intermcu_init(void) { pprz_transport_init(&intermcu.transport); +#if IMCU_GPS + gps_imcu.fix = GPS_FIX_NONE; + gps_imcu.pdop = 0; + gps_imcu.sacc = 0; + gps_imcu.pacc = 0; + gps_imcu.cacc = 0; + gps_imcu.comp_id = GPS_IMCU_ID; +#endif + #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_FBW_STATUS, send_status); #endif @@ -79,6 +95,10 @@ void intermcu_periodic(void) } else { intermcu.time_since_last_frame++; } + +#if IMCU_GPS + RunOnceEvery(PERIODIC_FREQUENCY, gps_periodic_check(&gps_imcu)); +#endif } /* Enable or disable the communication of the InterMCU */ @@ -151,6 +171,44 @@ static inline void intermcu_parse_msg(void (*rc_frame_handler)(void)) telemetry_intermcu_on_msg(0, msg, size); break; } +#if IMCU_GPS + case DL_IMCU_REMOTE_GPS: { + uint32_t now_ts = get_sys_time_usec(); + gps_imcu.ecef_pos.x = DL_IMCU_REMOTE_GPS_ecef_x(imcu_msg_buf); + gps_imcu.ecef_pos.y = DL_IMCU_REMOTE_GPS_ecef_y(imcu_msg_buf); + gps_imcu.ecef_pos.z = DL_IMCU_REMOTE_GPS_ecef_z(imcu_msg_buf); + SetBit(gps_imcu.valid_fields, GPS_VALID_POS_ECEF_BIT); + + gps_imcu.lla_pos.alt = DL_IMCU_REMOTE_GPS_alt(imcu_msg_buf); + gps_imcu.hmsl = DL_IMCU_REMOTE_GPS_hmsl(imcu_msg_buf); + SetBit(gps_imcu.valid_fields, GPS_VALID_HMSL_BIT); + + gps_imcu.ecef_vel.x = DL_IMCU_REMOTE_GPS_ecef_xd(imcu_msg_buf); + gps_imcu.ecef_vel.y = DL_IMCU_REMOTE_GPS_ecef_yd(imcu_msg_buf); + gps_imcu.ecef_vel.z = DL_IMCU_REMOTE_GPS_ecef_zd(imcu_msg_buf); + SetBit(gps_imcu.valid_fields, GPS_VALID_VEL_ECEF_BIT); + + gps_imcu.course = DL_IMCU_REMOTE_GPS_course(imcu_msg_buf); + gps_imcu.gspeed = DL_IMCU_REMOTE_GPS_gspeed(imcu_msg_buf); + SetBit(gps_imcu.valid_fields, GPS_VALID_COURSE_BIT); + + gps_imcu.pacc = DL_IMCU_REMOTE_GPS_pacc(imcu_msg_buf); + gps_imcu.sacc = DL_IMCU_REMOTE_GPS_sacc(imcu_msg_buf); + gps_imcu.num_sv = DL_IMCU_REMOTE_GPS_numsv(imcu_msg_buf); + gps_imcu.fix = DL_IMCU_REMOTE_GPS_fix(imcu_msg_buf); + + // set gps msg time + gps_imcu.last_msg_ticks = sys_time.nb_sec_rem; + gps_imcu.last_msg_time = sys_time.nb_sec; + + if (gps_imcu.fix >= GPS_FIX_3D) { + gps_imcu.last_3dfix_ticks = sys_time.nb_sec_rem; + gps_imcu.last_3dfix_time = sys_time.nb_sec; + } + + AbiSendMsgGPS(GPS_IMCU_ID, now_ts, &gps_imcu); + break; + } #endif diff --git a/sw/airborne/subsystems/intermcu/intermcu_fbw.c b/sw/airborne/subsystems/intermcu/intermcu_fbw.c index 8e10e0b656..7fc336b3a3 100644 --- a/sw/airborne/subsystems/intermcu/intermcu_fbw.c +++ b/sw/airborne/subsystems/intermcu/intermcu_fbw.c @@ -66,10 +66,26 @@ static void intermcu_parse_msg(void (*commands_frame_handler)(void)); static void checkPx4RebootCommand(unsigned char b); #endif +#ifdef USE_GPS + +#ifndef IMCU_GPS_ID +#define IMCU_GPS_ID GPS_MULTI_ID +#endif + +#include "subsystems/abi.h" +#include "subsystems/gps.h" +static abi_event gps_ev; +static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s); +#endif + void intermcu_init(void) { pprz_transport_init(&intermcu.transport); +#if USE_GPS + AbiBindMsgGPS(IMCU_GPS_ID, &gps_ev, gps_cb); +#endif + #ifdef BOARD_PX4IO px4bl_tid = sys_time_register_timer(10.0, NULL); #endif @@ -205,6 +221,33 @@ void InterMcuEvent(void (*frame_handler)(void)) } } +#if USE_GPS +static void gps_cb(uint8_t sender_id __attribute__((unused)), + uint32_t stamp __attribute__((unused)), + struct GpsState *gps_s) { + pprz_msg_send_IMCU_REMOTE_GPS(&(intermcu.transport.trans_tx), intermcu.device, INTERMCU_FBW, + &gps_s->ecef_pos.x, + &gps_s->ecef_pos.y, + &gps_s->ecef_pos.z, + &gps_s->lla_pos.alt, + &gps_s->hmsl, + &gps_s->ecef_vel.x, + &gps_s->ecef_vel.y, + &gps_s->ecef_vel.z, + &gps_s->course, + &gps_s->gspeed, + &gps_s->pacc, + &gps_s->sacc, + &gps_s->num_sv, + &gps_s->fix); +} + +void gps_periodic_check(struct GpsState *gps_s) { + if (sys_time.nb_sec - gps_s->last_msg_time > GPS_TIMEOUT) { + gps_s->fix = GPS_FIX_NONE; + } +} +#endif /* SOME STUFF FOR PX4IO BOOTLOADER (TODO: move this code) */ #ifdef BOARD_PX4IO