Skip to content

Commit

Permalink
Merge branch 'dev' of github.com:paparazzi/paparazzi into dev
Browse files Browse the repository at this point in the history
  • Loading branch information
@dewagter
dewagter committed Apr 28, 2011
2 parents f6f68da + 0e15a11 commit f160cf4
Show file tree
Hide file tree
Showing 11 changed files with 50 additions and 41 deletions.
3 changes: 3 additions & 0 deletions conf/autopilot/subsystems/fixedwing/autopilot.makefile
View file
Expand Up @@ -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
Expand Down
2 changes: 1 addition & 1 deletion sw/airborne/firmwares/fixedwing/main_ap.c
View file
Expand Up @@ -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 )
Expand Down
5 changes: 2 additions & 3 deletions sw/airborne/firmwares/rotorcraft/main.c
View file
Expand Up @@ -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();
Expand Down
9 changes: 6 additions & 3 deletions sw/airborne/modules/ins/ins_arduimu.c
View file
Expand Up @@ -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];

Expand Down Expand Up @@ -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;

Expand Down
15 changes: 9 additions & 6 deletions sw/airborne/modules/ins/ins_arduimu_basic.c
View file
Expand Up @@ -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"
Expand Down Expand Up @@ -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);

Expand Down
17 changes: 6 additions & 11 deletions sw/airborne/subsystems/gps.h
View file
Expand Up @@ -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
};

Expand All @@ -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
Expand Down
3 changes: 1 addition & 2 deletions sw/airborne/subsystems/gps/gps_sim.h
View file
Expand Up @@ -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; \
Expand Down
2 changes: 1 addition & 1 deletion sw/airborne/subsystems/gps/gps_sim_nps.h
View file
Expand Up @@ -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; \
} \
Expand Down
24 changes: 12 additions & 12 deletions sw/airborne/subsystems/gps/gps_skytraq.h
View file
Expand Up @@ -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() { \
Expand Down
5 changes: 5 additions & 0 deletions sw/airborne/subsystems/gps/gps_ubx.c
View file
Expand Up @@ -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);
}
}
}

Expand Down
6 changes: 4 additions & 2 deletions sw/airborne/subsystems/gps/gps_ubx.h
View file
Expand Up @@ -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;
Expand Down Expand Up @@ -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(); \
} \
Expand Down

0 comments on commit f160cf4

Please sign in to comment.