diff --git a/.gitignore b/.gitignore
index 91fcea346b..0d10b010dd 100644
--- a/.gitignore
+++ b/.gitignore
@@ -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
diff --git a/TODO b/TODO
index 5911b6afbd..0012cf0db3 100644
--- a/TODO
+++ b/TODO
@@ -1,3 +1,9 @@
+This is so totally out of date....
+
+
+.... it seems.
+
+
add date/time of generation in generated code
diff --git a/conf/airframes/jsbsim.xml b/conf/airframes/jsbsim.xml
index 67fe2f0a1a..18dc8cc3ca 100644
--- a/conf/airframes/jsbsim.xml
+++ b/conf/airframes/jsbsim.xml
@@ -191,25 +191,43 @@
-
-#### Config for SITL simulation
-include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
+
+
+
+
-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
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-#### Config for SITL simulation with JSBSim
-SRC_FIRMWARE = firmwares/fixedwing
-include $(PAPARAZZI_SRC)/conf/autopilot/sitl_jsbsim.makefile
+
+
+
+
+
+
-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
-
-
diff --git a/conf/simulator/Malolo1/Malolo1-IC.xml b/conf/simulator/Malolo1/Malolo1-IC.xml
new file mode 100644
index 0000000000..89c46bef55
--- /dev/null
+++ b/conf/simulator/Malolo1/Malolo1-IC.xml
@@ -0,0 +1,49 @@
+
+
+
+
+
+
+ 43.46223
+ 1.27289
+ 186.0
+ 2.0
+
+ 0.0
+ 0.0
+ 0.0
+
+ 0.0
+ 20.0
+
+ 0.0
+ -1
+
diff --git a/sw/airborne/gps_ubx.c b/sw/airborne/gps_ubx.c
index 581a3cb4ce..d2254f69a8 100644
--- a/sw/airborne/gps_ubx.c
+++ b/sw/airborne/gps_ubx.c
@@ -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);
}
}
}
diff --git a/sw/airborne/math/pprz_geodetic_double.c b/sw/airborne/math/pprz_geodetic_double.c
index 6d6576608e..0d1c38d260 100644
--- a/sw/airborne/math/pprz_geodetic_double.c
+++ b/sw/airborne/math/pprz_geodetic_double.c
@@ -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
diff --git a/sw/airborne/math/pprz_geodetic_double.h b/sw/airborne/math/pprz_geodetic_double.h
index 2bed3451c2..4f2b8eba17 100644
--- a/sw/airborne/math/pprz_geodetic_double.h
+++ b/sw/airborne/math/pprz_geodetic_double.h
@@ -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);
diff --git a/sw/airborne/test/test_geodetic.c b/sw/airborne/test/test_geodetic.c
index 9d04e24b60..6f6b138eb2 100644
--- a/sw/airborne/test/test_geodetic.c
+++ b/sw/airborne/test/test_geodetic.c
@@ -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) {
diff --git a/sw/simulator/sim_ac_fw.c b/sw/simulator/sim_ac_fw.c
index fac1a433b5..e9dde43ab0 100644
--- a/sw/simulator/sim_ac_fw.c
+++ b/sw/simulator/sim_ac_fw.c
@@ -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.;
}
diff --git a/sw/simulator/sim_ac_jsbsim.c b/sw/simulator/sim_ac_jsbsim.c
index 3806af2909..55f66a17a8 100644
--- a/sw/simulator/sim_ac_jsbsim.c
+++ b/sw/simulator/sim_ac_jsbsim.c
@@ -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.);