Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-06 07:53:43 +08:00
Code Issues Actions 24 Packages Projects Releases Wiki Activity

Merge branch 'master' of github.com:paparazzi/paparazzi

Browse Source 
Conflicts:
	sw/airborne/math/pprz_geodetic_double.c
This commit is contained in:
Antoine Drouin
2011-02-24 14:52:25 +01:00
parent bf0a6b2cd5 62943d6115
commit 29e4f169ee
10 changed files with 227 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitignore
+6
View File
@@ -122,3 +122,9 @@
/sw/airborne/arch/lpc21/test/bootloader/bl_ram.map
/sw/airborne/arch/lpc21/test/bootloader/bl_ram.elf
/sw/airborne/arch/lpc21/test/bootloader/crt.lst
# /sw/airborne/test
/sw/airborne/test/test_geodetic
/sw/airborne/test/test_algebra
/sw/airborne/test/test_matrix
/sw/airborne/test/test_att
TODO
+6
View File
@@ -1,3 +1,9 @@
This is so totally out of date....
.... it seems.
add date/time of generation in generated code
conf/airframes/jsbsim.xml
+31 -13
View File
@@ -191,25 +191,43 @@
<section name="SIMU">
<define name="JSBSIM_MODEL" value="&quot;Malolo1&quot;"/>
<define name="JSBSIM_INIT" value="&quot;Malolo1-IC&quot;"/>
<define name="JSBSIM_LAUNCHSPEED" value="15.0"/>
<define name="JSBSIM_IR_ROLL_NEUTRAL" value="RadOfDeg(0.)"/>
<define name="JSBSIM_IR_PITCH_NEUTRAL" value="RadOfDeg(0.)"/>
</section>
<makefile>
#### Config for SITL simulation
include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
<firmware name="fixedwing">
<target name="sim" board="pc" />
<target name="jsbsim" board="pc"/>
<target name="ap" board="tiny_2.1"/>
sim.CFLAGS += -DBOARD_CONFIG=\"boards/tiny_sim.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
sim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
<define name="AGR_CLIMB" />
<define name="LOITER_TRIM" />
<define name="ALT_KALMAN" />
<subsystem name="radio_control" type="ppm"/>
<!-- Communication -->
<subsystem name="telemetry" type="transparent">
<configure name="MODEM_BAUD" value="B9600"/>
</subsystem>
<subsystem name="control"/>
<!-- Sensors -->
<subsystem name="gyro" type="roll"/>
<subsystem name="attitude" type="infrared"/>
<subsystem name="gps" type="ublox_lea4p"/>
<subsystem name="navigation"/>
</firmware>
#### Config for SITL simulation with JSBSim
SRC_FIRMWARE = firmwares/fixedwing
include $(PAPARAZZI_SRC)/conf/autopilot/sitl_jsbsim.makefile
<firmware name="setup">
<target name="tunnel" board="tiny_2.1" />
<target name="usb_tunnel_0" board="tiny_2.1" />
<target name="usb_tunnel_1" board="tiny_2.1" />
<target name="setup_actuators" board="tiny_2.1" />
</firmware>
jsbsim.CFLAGS += -DBOARD_CONFIG=\"boards/tiny_sim.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
jsbsim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
jsbsim.srcs += $(SIMDIR)/sim_ac_fw.c
</makefile>
</airframe>
conf/simulator/Malolo1/Malolo1-IC.xml
+49
View File
@@ -0,0 +1,49 @@
<?xml version="1.0"?>
<!--
This file sets up the aircraft initial conditions.
Allowable units are:
Length: M IN FT
Angles: DEG RAD
Velocity: M/S FT/SEC KTS
Area: M2 FT2
Volume: CC LTR M3 IN3 FT3
Mass/Weight: KG LBS
Moments of Inertia: KG*M2 SLUG*FT2
Torque: N*M FT*LBS
Force: N LBS
Spring Force: N/M LBS/FT
Damping Force: N/M/SEC LBS/FT/SEC
Power: WATTS HP
Pressure: PA ATM PSI PSF INHG
Some example initial conditions are given below. Not all are needed.
Note that the launch condition body aligned forward velocity is given in sim_ac_fw.c
and can be defined in the SIMU section of the airframe file. See jsbsim.xml for example.
Other ICs are allowable, see JSBSim source for more info:
FGInitialCondition::Load ()
FGInitialCondition::Load_v1()
FGInitialCondition::Load_v2()
-->
<initialize name="beforelaunch">
<!-- Location -->
<latitude unit="DEG"> 43.46223 </latitude> <!-- Muret, FR -->
<longitude unit="DEG"> 1.27289 </longitude> <!-- Muret, FR -->
<elevation unit="M" > 186.0 </elevation> <!-- terrain elevation -->
<altitudeAGL unit="M"> 2.0 </altitudeAGL> <!-- altitude above ground level -->
<!-- Orientation -->
<psi unit="DEG"> 0.0 </psi> <!-- heading (yaw) -->
<theta unit="DEG"> 0.0 </theta> <!-- pitch -->
<phi unit="DEG"> 0.0 </phi> <!-- roll -->
<!-- Wind -->
<winddir unit="DEG"> 0.0 </winddir> <!-- positive velocity direction (0.0 is wind from south) -->
<vwind unit="KTS"> 20.0 </vwind>
<!--Do not modify-->
<vground unit="KTS"> 0.0 </vground> <!-- ground speed -->
<running> -1 </running> <!-- set all engines running -->
</initialize>
sw/airborne/gps_ubx.c
+8 -8
View File
@@ -257,7 +257,7 @@ void parse_gps_msg( void ) {
gps_status_flags = UBX_NAV_STATUS_Flags(ubx_msg_buf);
gps_sol_flags = UBX_NAV_SOL_Flags(ubx_msg_buf);
#ifdef GPS_USE_LATLONG
/* Computes from (lat, long) in the referenced UTM zone */
/* Computes from (lat, long) in the referenced UTM zone */
} else if (ubx_id == UBX_NAV_POSLLH_ID) {
gps_lat = UBX_NAV_POSLLH_LAT(ubx_msg_buf);
gps_lon = UBX_NAV_POSLLH_LON(ubx_msg_buf);
@@ -275,7 +275,7 @@ void parse_gps_msg( void ) {
gps_utm_north = UBX_NAV_POSUTM_NORTH(ubx_msg_buf);
uint8_t hem = UBX_NAV_POSUTM_HEM(ubx_msg_buf);
if (hem == UTM_HEM_SOUTH)
gps_utm_north -= 1000000000; /* Subtract false northing: -10000km */
gps_utm_north -= 1000000000; /* Subtract false northing: -10000km */
gps_alt = UBX_NAV_POSUTM_ALT(ubx_msg_buf);
gps_utm_zone = UBX_NAV_POSUTM_ZONE(ubx_msg_buf);
#endif
@@ -306,12 +306,12 @@ void parse_gps_msg( void ) {
gps_nb_channels = Min(UBX_NAV_SVINFO_NCH(ubx_msg_buf), GPS_NB_CHANNELS);
uint8_t i;
for(i = 0; i < gps_nb_channels; i++) {
gps_svinfos[i].svid = UBX_NAV_SVINFO_SVID(ubx_msg_buf, i);
gps_svinfos[i].flags = UBX_NAV_SVINFO_Flags(ubx_msg_buf, i);
gps_svinfos[i].qi = UBX_NAV_SVINFO_QI(ubx_msg_buf, i);
gps_svinfos[i].cno = UBX_NAV_SVINFO_CNO(ubx_msg_buf, i);
gps_svinfos[i].elev = UBX_NAV_SVINFO_Elev(ubx_msg_buf, i);
gps_svinfos[i].azim = UBX_NAV_SVINFO_Azim(ubx_msg_buf, i);
gps_svinfos[i].svid = UBX_NAV_SVINFO_SVID(ubx_msg_buf, i);
gps_svinfos[i].flags = UBX_NAV_SVINFO_Flags(ubx_msg_buf, i);
gps_svinfos[i].qi = UBX_NAV_SVINFO_QI(ubx_msg_buf, i);
gps_svinfos[i].cno = UBX_NAV_SVINFO_CNO(ubx_msg_buf, i);
gps_svinfos[i].elev = UBX_NAV_SVINFO_Elev(ubx_msg_buf, i);
gps_svinfos[i].azim = UBX_NAV_SVINFO_Azim(ubx_msg_buf, i);
}
}
}
sw/airborne/math/pprz_geodetic_double.c
+93
View File
@@ -71,6 +71,96 @@ static inline double inverse_isometric_latitude(double lat, double e, double eps
void lla_of_utm(struct LlaCoor_d* out, struct UTMCoor_d* in) {
// struct DoubleVect2 v = {in->east - YS, in->north - XS};
struct DoubleVect2 v = {in->north - DELTA_NORTH, in->east - DELTA_EAST};
double scale = 1 / N / serie_coeff_proj_mercator[0];
VECT2_SMUL(v, v, scale);
// first order taylor serie of something ?
struct DoubleVect2 v1;
VECT2_SMUL(v1, v, 2.);
CSin(v1);
VECT2_SMUL(v1, v1, serie_coeff_proj_mercator[1]);
VECT2_SUB(v, v1);
double lambda_c = LambdaOfUtmZone(in->zone);
out->lon = lambda_c + atan(sinh(v.y) / cos(v.x));
double phi = asin (sin(v.x) / cosh(v.y));
double il = isometric_latitude_fast(phi);
out->lat = inverse_isometric_latitude(il, E, 1e-8);
}
/* Computation for the WGS84 geoid only */
#define E 0.08181919106
#define K0 0.9996
#define DELTA_EAST 500000.
#define DELTA_NORTH 0.
#define A 6378137.0
#define N (K0*A)
#define LambdaOfUtmZone(utm_zone) RadOfDeg((utm_zone-1)*6-180+3)
static const float serie_coeff_proj_mercator[5] = {
0.99832429842242842444,
0.00083632803657738403,
0.00000075957783563707,
0.00000000119563131778,
0.00000000000241079916
};
static inline double isometric_latitude(double phi, double e) {
return log (tan (M_PI_4 + phi / 2.0)) - e / 2.0 * log((1.0 + e * sin(phi)) / (1.0 - e * sin(phi)));
}
static inline double isometric_latitude_fast(double phi) {
return log (tan (M_PI_4 + phi / 2.0));
}
static inline double inverse_isometric_latitude(double lat, double e, double epsilon) {
double exp_l = exp(lat);
double phi0 = 2 * atan(exp_l) - M_PI_2;
double phi_;
uint8_t max_iter = 3; /* To be sure to return */
do {
phi_ = phi0;
double sin_phi = e * sin(phi_);
phi0 = 2 * atan (pow((1 + sin_phi) / (1. - sin_phi), e/2.) * exp_l) - M_PI_2;
max_iter--;
}
while (max_iter && fabs(phi_ - phi0) > epsilon);
return phi0;
}
#define CI(v) { \
double tmp = v.x; \
v.x = -v.y; \
v.y = tmp; \
}
#define CExp(v) { \
double e = exp(v.x); \
v.x = e*cosf(v.y); \
v.y = e*sinf(v.y); \
}
#define CSin(v) { \
CI(v); \
struct DoubleVect2 vstar = {-v.x, -v.y}; \
CExp(v); \
CExp(vstar); \
VECT2_SUB(v, vstar); \
VECT2_SMUL(v, v, -0.5); \
CI(v); \
}
void lla_of_utm(struct LlaCoor_d* out, struct UTMCoor_d* in) {
// struct DoubleVect2 v = {in->east - YS, in->north - XS};
@@ -226,6 +316,7 @@ double gc_of_gd_lat_d(double gd_lat, double hmsl) {
return atan2(hmsl*sin(gd_lat) + a*sin(ls), hmsl*cos(gd_lat) + a*cos(ls));
}
<<<<<<< HEAD
<<<<<<< HEAD
@@ -317,3 +408,5 @@ void lla_of_utm(struct LlaCoor_d* out, struct UTMCoor_d* in) {
}
=======
>>>>>>> 62943d6115cdac629878cb488a8592aab4a7ded4
=======
>>>>>>> 62943d6115cdac629878cb488a8592aab4a7ded4
sw/airborne/math/pprz_geodetic_double.h
+11
View File
@@ -32,6 +32,7 @@
#include "pprz_geodetic.h"
#include "pprz_algebra_double.h"
#include "std.h"
/**
* @brief vector in EarthCenteredEarthFixed coordinates
@@ -74,6 +75,15 @@ struct EnuCoor_d {
double z; ///< in meters
};
/**
* @brief position in UTM coordinates
* Units: meters */
struct UTMCoor_d {
double north; ///< in meters
double east; ///< in meters
uint8_t zone; ///< UTM zone number
};
/**
* @brief definition of the local (flat earth) coordinate system
* @details Defines the origin of the local coordinate system
@@ -85,6 +95,7 @@ struct LtpDef_d {
struct DoubleMat33 ltp_of_ecef; ///< rotation from ECEF to local frame
};
extern void lla_of_utm(struct LlaCoor_d* out, struct UTMCoor_d* in);
extern void ltp_def_from_ecef_d(struct LtpDef_d* def, struct EcefCoor_d* ecef);
extern void lla_of_ecef_d(struct LlaCoor_d* out, struct EcefCoor_d* in);
extern void ecef_of_lla_d(struct EcefCoor_d* out, struct LlaCoor_d* in);
sw/airborne/test/test_geodetic.c
+16 -1
View File
@@ -21,9 +21,10 @@
#define EM7RAD_OF_RAD(_r) (_r*1e7)
#define RAD_OF_EM7RAD(_r) (_r/1e7)
static void test_lla_of_utm(void);
static void test_floats(void);
static void test_doubles(void);
static void test_enu_of_ecef_int(void);
static void test_enu_of_ecef_int(void);
static void test_ned_to_ecef_to_ned(void);
static void test_enu_to_ecef_to_enu( void );
@@ -35,6 +36,9 @@ int main(int argc, char** argv) {
test_floats();
test_doubles();
test_lla_of_utm();
// test_enu_of_ecef_int();
// test_ned_to_ecef_to_ned();
@@ -43,6 +47,17 @@ int main(int argc, char** argv) {
}
static void test_lla_of_utm(void) {
printf("\n--- lla of UTM double ---\n");
struct UTMCoor_d u = { .east=348805.71, .north=4759354.89, .zone=31 };
struct LlaCoor_d l;
struct LlaCoor_d l_ref = {.lat=0.749999999392454875,
.lon=0.019999999054505127};
lla_of_utm(&l, &u);
printf(" lat=%.16f lon=%.16f\nref_lat=%.16f ref_lon=%.16f\n",
l.lat, l.lon, l_ref.lat, l_ref.lon);
}
static void test_floats(void) {
sw/simulator/sim_ac_fw.c
+6 -2
View File
@@ -127,6 +127,9 @@ static inline double normalize_from_pprz(int command) {
void copy_inputs_to_jsbsim(FGFDMExec* FDMExec) {
static double throttle_slewed = 0.;
static double th = 0.;
#ifndef JSBSIM_LAUNCHSPEED
#define JSBSIM_LAUNCHSPEED 20.0 //launch speed in m/s aligned with airframe body forward
#endif
if (run_model) th += 0.01;
if (th >= 1) th = 1;
// detect launch
@@ -134,8 +137,9 @@ void copy_inputs_to_jsbsim(FGFDMExec* FDMExec) {
run_model = true;
//set_value(FDMExec, "propulsion/set-running", 1);
// set initial speed
FDMExec->GetIC()->SetAltitudeAGLFtIC(5.0 / FT2M);
FDMExec->GetIC()->SetVgroundFpsIC(20./FT2M);
//FDMExec->GetIC()->SetAltitudeAGLFtIC(5.0 / FT2M);
//FDMExec->GetIC()->SetVgroundFpsIC(20./FT2M);
FDMExec->GetIC()->SetUBodyFpsIC( JSBSIM_LAUNCHSPEED / FT2M);
FDMExec->RunIC();
th = 0.;
}
sw/simulator/sim_ac_jsbsim.c
+1 -1
View File
@@ -253,7 +253,7 @@ void jsbsim_init(void) {
IC->SetLatitudeDegIC(NAV_LAT0 / 1e7);
IC->SetLongitudeDegIC(NAV_LON0 / 1e7);
IC->SetAltitudeASLFtIC(GROUND_ALT / FT2M);
IC->SetAltitudeASLFtIC((GROUND_ALT + 2.0) / FT2M);
IC->SetTerrainElevationFtIC(GROUND_ALT / FT2M);
IC->SetPsiDegIC(QFU);
IC->SetVgroundFpsIC(0.);