diff --git a/conf/autopilot/subsystems/fixedwing/autopilot.makefile b/conf/autopilot/subsystems/fixedwing/autopilot.makefile index 2faf78d71e0..a1994bcd334 100644 --- a/conf/autopilot/subsystems/fixedwing/autopilot.makefile +++ b/conf/autopilot/subsystems/fixedwing/autopilot.makefile @@ -218,6 +218,9 @@ jsbsim.LDFLAGS += `pkg-config glib-2.0 --libs` -lm -lpcre -lglibivy -L/usr/lib jsbsim.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport jsbsim.srcs += downlink.c $(SRC_FIRMWARE)/datalink.c $(SRC_ARCH)/jsbsim_hw.c $(SRC_ARCH)/jsbsim_gps.c $(SRC_ARCH)/ivy_transport.c $(SRC_ARCH)/jsbsim_transport.c +jsbsim.srcs += subsystems/settings.c +jsbsim.srcs += $(SRC_ARCH)/subsystems/settings_arch.c + ###################################################################### ## ## Final Target Allocations diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c index 653304e0007..0cd47e82c32 100644 --- a/sw/airborne/firmwares/fixedwing/main_ap.c +++ b/sw/airborne/firmwares/fixedwing/main_ap.c @@ -269,7 +269,7 @@ static inline void telecommand_task( void ) { #ifdef FAILSAFE_DELAY_WITHOUT_GPS -#define GpsTimeoutError (cpu_time_sec - gps.last_msg_time > FAILSAFE_DELAY_WITHOUT_GPS) +#define GpsTimeoutError (cpu_time_sec - gps.last_fix_time > FAILSAFE_DELAY_WITHOUT_GPS) #endif /** \fn void navigation_task( void ) diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c index 12da21923b4..b0808d6202f 100644 --- a/sw/airborne/firmwares/rotorcraft/main.c +++ b/sw/airborne/firmwares/rotorcraft/main.c @@ -169,10 +169,9 @@ STATIC_INLINE void main_periodic( void ) { } ); #ifdef USE_GPS - if (radio_control.status != RC_OK && \ + if (radio_control.status != RC_OK && \ autopilot_mode == AP_MODE_NAV && GpsIsLost()) \ - autopilot_set_mode(AP_MODE_FAILSAFE); \ - gps_periodic(); + autopilot_set_mode(AP_MODE_FAILSAFE); #endif modules_periodic_task(); diff --git a/sw/airborne/modules/ins/ins_arduimu.c b/sw/airborne/modules/ins/ins_arduimu.c index cc10e38d0c8..dbea8c92f2d 100644 --- a/sw/airborne/modules/ins/ins_arduimu.c +++ b/sw/airborne/modules/ins/ins_arduimu.c @@ -14,6 +14,9 @@ Autoren@ZHAW: schmiemi #include "estimator.h" // für das Senden von GPS-Daten an den ArduIMU +#ifndef UBX +#error "currently only compatible with uBlox GPS modules" +#endif #include "subsystems/gps.h" int32_t GPS_Data[14]; @@ -87,11 +90,11 @@ void ArduIMU_periodicGPS( void ) { GPS_Data [6] = gps.gspeed; //ground speed GPS_Data [7] = DegOfRad(gps.course / 1e6); //Kurs //status - GPS_Data [8] = gps_mode; //fix - GPS_Data [9] = gps_status_flags; //flags + GPS_Data [8] = gps.fix; //fix + GPS_Data [9] = gps_ubx.status_flags; //flags //sol GPS_Data [10] = gps.fix; //fix - //GPS_Data [11] = gps_sol_flags; //flags + //GPS_Data [11] = gps_ubx.sol_flags; //flags GPS_Data [12] = -gps.ned_vel.z; GPS_Data [13] = gps.num_sv; diff --git a/sw/airborne/modules/ins/ins_arduimu_basic.c b/sw/airborne/modules/ins/ins_arduimu_basic.c index 8d1f3c59533..0352c58b2d0 100644 --- a/sw/airborne/modules/ins/ins_arduimu_basic.c +++ b/sw/airborne/modules/ins/ins_arduimu_basic.c @@ -14,7 +14,10 @@ Autoren@ZHAW: schmiemi #include "estimator.h" // GPS data for ArduIMU -#include "gps.h" +#ifndef UBX +#error "currently only compatible with uBlox GPS modules" +#endif +#include "subsystems/gps.h" // Command vector for thrust #include "generated/airframe.h" @@ -90,11 +93,11 @@ void ArduIMU_periodicGPS( void ) { high_accel_flag = FALSE; } - FillBufWith32bit(ardu_gps_trans.buf, 0, (int32_t)gps_speed_3d); // speed 3D - FillBufWith32bit(ardu_gps_trans.buf, 4, (int32_t)gps_gspeed); // ground speed - FillBufWith32bit(ardu_gps_trans.buf, 8, (int32_t)gps_course); // course - ardu_gps_trans.buf[12] = gps_mode; // status gps fix - ardu_gps_trans.buf[13] = gps_status_flags; // status flags + FillBufWith32bit(ardu_gps_trans.buf, 0, (int32_t)gps.speed_3d); // speed 3D + FillBufWith32bit(ardu_gps_trans.buf, 4, (int32_t)gps.gspeed); // ground speed + FillBufWith32bit(ardu_gps_trans.buf, 8, (int32_t)DegOfRad(gps.course / 1e6)); // course + ardu_gps_trans.buf[12] = gps.fix; // status gps fix + ardu_gps_trans.buf[13] = gps_ubx.status_flags; // status flags ardu_gps_trans.buf[14] = (uint8_t)high_accel_flag; // high acceleration flag (disable accelerometers in the arduimu filter) I2CTransmit(ARDUIMU_I2C_DEV, ardu_gps_trans, ArduIMU_SLAVE_ADDR, 15); diff --git a/sw/airborne/subsystems/gps.h b/sw/airborne/subsystems/gps.h index ace5586b3db..21a30c6bb1f 100644 --- a/sw/airborne/subsystems/gps.h +++ b/sw/airborne/subsystems/gps.h @@ -78,8 +78,7 @@ struct GpsState { uint8_t nb_channels; ///< Number of scanned satellites struct SVinfo svinfos[GPS_NB_CHANNELS]; - uint8_t lost_counter; /* updated at 4Hz */ - uint16_t last_msg_time; + uint16_t last_fix_time; ///< cpu time in sec at last valid fix uint16_t reset; ///< hotstart, warmstart, coldstart }; @@ -99,15 +98,11 @@ extern void gps_init(void); extern void gps_impl_init(void); - -//TODO -// this is only true for a 512Hz main loop -// needs to work with different main loop frequencies -static inline void gps_periodic( void ) { - RunOnceEvery(128, gps.lost_counter++; ); -} - -#define GpsIsLost() (gps.lost_counter > 20) /* 4Hz -> 5s */ +/* mark GPS as lost when no valid 3D fix was received for GPS_TIMEOUT secs */ +#ifndef GPS_TIMEOUT +#define GPS_TIMEOUT 5 +#endif +#define GpsIsLost() (cpu_time_sec - gps.last_fix_time > GPS_TIMEOUT) //TODO diff --git a/sw/airborne/subsystems/gps/gps_sim.h b/sw/airborne/subsystems/gps/gps_sim.h index 7e20ed92351..7794ea9c248 100644 --- a/sw/airborne/subsystems/gps/gps_sim.h +++ b/sw/airborne/subsystems/gps/gps_sim.h @@ -14,8 +14,7 @@ extern void gps_impl_init(void); #define GpsEvent(_sol_available_callback) { \ if (gps_available) { \ if (gps.fix == GPS_FIX_3D) { \ - gps.lost_counter = 0; \ - gps.last_msg_time = cpu_time_sec; \ + gps.last_fix_time = cpu_time_sec; \ } \ _sol_available_callback(); \ gps_available = FALSE; \ diff --git a/sw/airborne/subsystems/gps/gps_sim_nps.h b/sw/airborne/subsystems/gps/gps_sim_nps.h index c5284b2e64a..c14e3f8f946 100644 --- a/sw/airborne/subsystems/gps/gps_sim_nps.h +++ b/sw/airborne/subsystems/gps/gps_sim_nps.h @@ -14,7 +14,7 @@ extern void gps_impl_init(); #define GpsEvent(_sol_available_callback) { \ if (gps_available) { \ if (gps.fix == GPS_FIX_3D) \ - gps.lost_counter = 0; \ + gps.last_fix_time = cpu_time_sec; \ _sol_available_callback(); \ gps_available = FALSE; \ } \ diff --git a/sw/airborne/subsystems/gps/gps_skytraq.h b/sw/airborne/subsystems/gps/gps_skytraq.h index c5d6964c92b..fea8398af0c 100644 --- a/sw/airborne/subsystems/gps/gps_skytraq.h +++ b/sw/airborne/subsystems/gps/gps_skytraq.h @@ -88,19 +88,19 @@ extern struct GpsSkytraq gps_skytraq; #define GpsBuffer() GpsLink(ChAvailable()) -#define GpsEvent(_sol_available_callback) { \ - if (GpsBuffer()) { \ - ReadGpsBuffer(); \ - } \ - if (gps_skytraq.msg_available) { \ - gps_skytraq_read_message(); \ +#define GpsEvent(_sol_available_callback) { \ + if (GpsBuffer()) { \ + ReadGpsBuffer(); \ + } \ + if (gps_skytraq.msg_available) { \ + gps_skytraq_read_message(); \ if (gps_skytraq.msg_id == SKYTRAQ_ID_NAVIGATION_DATA) { \ - if (gps.fix == GPS_FIX_3D) \ - gps.lost_counter = 0; \ - _sol_available_callback(); \ - } \ - gps_skytraq.msg_available = FALSE; \ - } \ + if (gps.fix == GPS_FIX_3D) \ + gps.last_fix_time = cpu_time_sec; \ + _sol_available_callback(); \ + } \ + gps_skytraq.msg_available = FALSE; \ + } \ } #define ReadGpsBuffer() { \ diff --git a/sw/airborne/subsystems/gps/gps_ubx.c b/sw/airborne/subsystems/gps/gps_ubx.c index 911ead5a351..cdb834db13c 100644 --- a/sw/airborne/subsystems/gps/gps_ubx.c +++ b/sw/airborne/subsystems/gps/gps_ubx.c @@ -179,6 +179,11 @@ void gps_ubx_read_message(void) { gps.svinfos[i].azim = UBX_NAV_SVINFO_Azim(gps_ubx.msg_buf, i); } } + else if (gps_ubx.msg_id == UBX_NAV_STATUS_ID) { + gps.fix = UBX_NAV_STATUS_GPSfix(gps_ubx.msg_buf); + gps_ubx.status_flags = UBX_NAV_STATUS_Flags(gps_ubx.msg_buf); + gps_ubx.sol_flags = UBX_NAV_SOL_Flags(gps_ubx.msg_buf); + } } } diff --git a/sw/airborne/subsystems/gps/gps_ubx.h b/sw/airborne/subsystems/gps/gps_ubx.h index 321bd24810e..e1f85d4d410 100644 --- a/sw/airborne/subsystems/gps/gps_ubx.h +++ b/sw/airborne/subsystems/gps/gps_ubx.h @@ -48,6 +48,9 @@ struct GpsUbx { uint8_t send_ck_a, send_ck_b; uint8_t error_cnt; uint8_t error_last; + + uint8_t status_flags; + uint8_t sol_flags; }; extern struct GpsUbx gps_ubx; @@ -83,8 +86,7 @@ extern bool_t gps_configuring; if (gps_ubx.msg_class == UBX_NAV_ID && \ gps_ubx.msg_id == UBX_NAV_VELNED_ID) { \ if (gps.fix == GPS_FIX_3D) { \ - gps.lost_counter = 0; \ - gps.last_msg_time = cpu_time_sec; \ + gps.last_fix_time = cpu_time_sec; \ } \ _sol_available_callback(); \ } \