Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-01 12:57:27 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

updated gps for jsbsim, fixed sim if GPS_USE_LATLONG, removed superfluous jsbsim file

Browse Source
This commit is contained in:
Felix Ruess
2011-02-27 13:03:21 +01:00
parent d29aa6c30e
commit 5cf52e44e6
6 changed files with 55 additions and 120 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/autopilot/subsystems/fixedwing/gps_ublox_lea4p.makefile
+3
View File
@@ -17,3 +17,6 @@ $(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c
sim.CFLAGS += -DUSE_GPS -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c
jsbsim.CFLAGS += -DUSE_GPS -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
jsbsim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c
conf/autopilot/subsystems/fixedwing/gps_ublox_lea5h.makefile
+5 -1
View File
@@ -15,5 +15,9 @@ ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_ubx.c
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c
sim.CFLAGS += -DUSE_GPS -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
sim.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG
sim.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c
jsbsim.CFLAGS += -DUSE_GPS -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
jsbsim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c
sw/airborne/arch/sim/jsbsim_gps.c
+34 -46
View File
@@ -8,69 +8,57 @@
#include "generated/airframe.h"
#include "generated/flight_plan.h"
#include "autopilot.h"
#include "gps.h"
#include "subsystems/gps.h"
#include "estimator.h"
#include "latlong.h"
#include "math/pprz_geodetic_float.h"
#include "math/pprz_geodetic_int.h"
#include "subsystems/navigation/common_nav.h"
uint8_t gps_mode;
uint16_t gps_week; /* weeks */
uint32_t gps_itow; /* ms */
int32_t gps_alt; /* cm */
uint16_t gps_gspeed; /* cm/s */
int16_t gps_climb; /* cm/s */
int16_t gps_course; /* decideg */
int32_t gps_utm_east, gps_utm_north;
uint8_t gps_utm_zone;
int32_t gps_lat, gps_lon;
struct svinfo gps_svinfos[GPS_NB_CHANNELS];
uint8_t gps_nb_channels = 0;
uint16_t gps_PDOP;
uint32_t gps_Pacc, gps_Sacc;
uint8_t gps_numSV;
uint16_t gps_reset;
void sim_use_gps_pos(double lat, double lon, double alt, double course, double gspeed, double climb, double time) {
gps_mode = 3; // Mode 3D
gps_course = DegOfRad(course) * 10.;
gps_alt = alt * 100.;
gps_gspeed = gspeed * 100.;
gps_climb = climb * 100.;
gps_week = 0; // FIXME
gps_itow = time * 1000.;
gps.fix = 3; // Mode 3D
gps.course = course * 1e7;
gps.hmsl = alt * 1000.;
gps.gspeed = gspeed * 100.;
gps.ned_vel.z = -climb * 100.;
gps.week = 0; // FIXME
gps.tow = time * 1000.;
gps_lat = DegOfRad(lat)*1e7;
gps_lon = DegOfRad(lon)*1e7;
latlong_utm_of(lat, lon, nav_utm_zone0);
gps_utm_east = latlong_utm_x * 100;
gps_utm_north = latlong_utm_y * 100;
gps_utm_zone = nav_utm_zone0;
//TODO set alt above ellipsoid and hmsl
struct LlaCoor_f lla_f;
struct UtmCoor_f utm_f;
lla_f.lat = lat;
lla_f.lon = lon;
utm_f.zone = nav_utm_zone0;
utm_of_lla_f(&utm_f, &lla_f);
LLA_BFP_OF_REAL(gps.lla_pos, lla_f);
gps.utm_pos.east = utm_f.east*100;
gps.utm_pos.north = utm_f.north*100;
gps.utm_pos.zone = nav_utm_zone0;
gps_available = TRUE;
}
/** Space vehicle info simulation */
void sim_update_sv(void) {
gps_nb_channels=7;
gps.nb_channels=7;
int i;
static int time;
time++;
for(i = 0; i < gps_nb_channels; i++) {
gps_svinfos[i].svid = 7 + i;
gps_svinfos[i].elev = (cos(((100*i)+time)/100.) + 1) * 45;
gps_svinfos[i].azim = (time/gps_nb_channels + 50 * i) % 360;
gps_svinfos[i].cno = 40 + sin((time+i*10)/100.) * 10.;
gps_svinfos[i].flags = ((time/10) % (i+1) == 0 ? 0x00 : 0x01);
gps_svinfos[i].qi = (int)((time / 1000.) + i) % 8;
for(i = 0; i < gps.nb_channels; i++) {
gps.svinfos[i].svid = 7 + i;
gps.svinfos[i].elev = (cos(((100*i)+time)/100.) + 1) * 45;
gps.svinfos[i].azim = (time/gps.nb_channels + 50 * i) % 360;
gps.svinfos[i].cno = 40 + sin((time+i*10)/100.) * 10.;
gps.svinfos[i].flags = ((time/10) % (i+1) == 0 ? 0x00 : 0x01);
gps.svinfos[i].qi = (int)((time / 1000.) + i) % 8;
}
gps_PDOP = gps_Sacc = gps_Pacc = 500+200*sin(time/100.);
gps_numSV = 7;
gps_verbose_downlink = !launch;
UseGpsPosNoSend(estimator_update_state_gps);
gps_downlink();
gps.pdop = gps.sacc = gps.pacc = 500+200*sin(time/100.);
gps.num_sv = 7;
}
sw/airborne/arch/sim/jsbsim_hw.h
+1 -1
View File
@@ -32,7 +32,7 @@
#include "inter_mcu.h"
#include "firmwares/fixedwing/autopilot.h"
#include "estimator.h"
#include "gps.h"
#include "subsystems/gps.h"
#include "subsystems/navigation/traffic_info.h"
#include "generated/flight_plan.h"
#include "generated/airframe.h"
sw/airborne/arch/sim/sim_gps.c
+12 -3
View File
@@ -11,6 +11,7 @@
#include "subsystems/gps.h"
#include "estimator.h"
#include "math/pprz_geodetic_float.h"
#include "math/pprz_geodetic_int.h"
#include "subsystems/navigation/common_nav.h"
#include <caml/mlvalues.h>
@@ -25,11 +26,19 @@ value sim_use_gps_pos(value x, value y, value z, value c, value a, value s, valu
gps.week = 0; // FIXME
gps.tow = Double_val(t) * 1000.;
//TODO set alt above ellipsoid and hmsl
#ifdef GPS_USE_LATLONG
gps.lla_pos.lat = Double_val(lat)*1e7;
gps.lla_pos.lon = Double_val(lon)*1e7;
struct LlaCoor_f lla_f;
struct UtmCoor_f utm_f;
lla_f.lat = Double_val(lat);
lla_f.lon = Double_val(lon);
utm_f.zone = nav_utm_zone0;
utm_of_lla_f(&utm_f, &lla_f);
LLA_BFP_OF_REAL(gps.lla_pos, lla_f);
gps.utm_pos.east = utm_f.east*100;
gps.utm_pos.north = utm_f.north*100;
gps.utm_pos.zone = nav_utm_zone0;
utm_of_lla_f(&gps.utm_pos, &gps.lla_pos);
x = y = z; /* Just to get rid of the "unused arg" warning */
#else // GPS_USE_LATLONG
gps.utm_pos.east = Int_val(x);
sw/airborne/arch/sim/sim_jsbsim.c
-69
View File
@@ -1,69 +0,0 @@
/* Definitions and declarations required to compile autopilot code on a
i386 architecture */
#include <stdio.h>
#include <assert.h>
#include <sys/time.h>
#include <sys/stat.h>
#include <time.h>
#include <string.h>
#include "std.h"
#include "inter_mcu.h"
#include "autopilot.h"
#include "estimator.h"
#include "gps.h"
#include "subsystems/navigation/traffic_info.h"
#include "generated/flight_plan.h"
#include "generated/settings.h"
#include "subsystems/nav.h"
#include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
#include "ffirmwares/fixedwing/guidance/guidance_v.h"
#include "subsystems/sensors/infrared.h"
#include "commands.h"
#include "firmwares/fixedwing/main_ap.h"
#include "ap_downlink.h"
#include "sim_uart.h"
#include "latlong.h"
#include "datalink.h"
#include "adc_generic.h"
#include "ppm.h"
/* Dummy definitions to replace the ones from the files not compiled in the
simulator */
uint8_t ir_estim_mode;
uint8_t vertical_mode;
uint8_t inflight_calib_mode;
bool_t rc_event_1, rc_event_2;
uint8_t gps_mode;
uint16_t gps_week; /* weeks */
uint32_t gps_itow; /* ms */
int32_t gps_alt; /* cm */
uint16_t gps_gspeed; /* cm/s */
int16_t gps_climb; /* cm/s */
int16_t gps_course; /* decideg */
int32_t gps_utm_east, gps_utm_north;
uint8_t gps_utm_zone;
int32_t gps_lat, gps_lon;
struct svinfo gps_svinfos[GPS_NB_CHANNELS];
uint8_t gps_nb_channels = 0;
uint16_t gps_PDOP;
uint32_t gps_Pacc, gps_Sacc;
uint8_t gps_numSV;
uint16_t gps_reset;
uint8_t gps_nb_ovrn, modem_nb_ovrn, link_fbw_fbw_nb_err, link_fbw_nb_err;
float alt_roll_pgain;
float roll_rate_pgain;
bool_t gpio1_status;
uint16_t adc_generic_val1;
uint16_t adc_generic_val2;
uint16_t ppm_pulses[ PPM_NB_PULSES ];
volatile bool_t ppm_valid;
uint8_t ac_id;
void ir_gain_calib(void) {}
void adc_buf_channel(uint8_t adc_channel __attribute__ ((unused)), struct adc_buf* s __attribute__ ((unused)), uint8_t av_nb_sample __attribute__ ((unused))) {}
void ubxsend_cfg_rst(uint16_t bbr __attribute__ ((unused)), uint8_t reset_mode __attribute__ ((unused))) {}
void adc_generic_init( void ) {}
void adc_generic_periodic( void ) {}