diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2013-09-21 15:36:16 +0200 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2013-09-21 15:36:16 +0200 |
commit | 96d8ee3483012e2c48a3ca3d4bd9872b866e3bad (patch) | |
tree | 35fa82a5b59b2fe25eb3d12b9beb69e2d25ee255 /src/modules | |
parent | ab26ecf188198adc328a9e2f8b48dbd179ac45af (diff) | |
parent | 7033aa173bf85b01f6fcfacf6e643095c3f16848 (diff) | |
download | px4-firmware-96d8ee3483012e2c48a3ca3d4bd9872b866e3bad.tar.gz px4-firmware-96d8ee3483012e2c48a3ca3d4bd9872b866e3bad.tar.bz2 px4-firmware-96d8ee3483012e2c48a3ca3d4bd9872b866e3bad.zip |
Merge branch 'master' into mpc_yaw_fix
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/mavlink/orb_listener.c | 23 | ||||
-rw-r--r-- | src/modules/systemlib/param/param.c | 38 |
2 files changed, 52 insertions, 9 deletions
diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index d11a67fc6..cec2fdc43 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -99,6 +99,8 @@ struct listener { uintptr_t arg; }; +uint16_t cm_uint16_from_m_float(float m); + static void l_sensor_combined(const struct listener *l); static void l_vehicle_attitude(const struct listener *l); static void l_vehicle_gps_position(const struct listener *l); @@ -150,6 +152,19 @@ static const struct listener listeners[] = { static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]); +uint16_t +cm_uint16_from_m_float(float m) +{ + if (m < 0.0f) { + return 0; + + } else if (m > 655.35f) { + return 65535; + } + + return (uint16_t)(m * 100.0f); +} + void l_sensor_combined(const struct listener *l) { @@ -235,8 +250,10 @@ l_vehicle_gps_position(const struct listener *l) /* GPS COG is 0..2PI in degrees * 1e2 */ float cog_deg = gps.cog_rad; + if (cog_deg > M_PI_F) cog_deg -= 2.0f * M_PI_F; + cog_deg *= M_RAD_TO_DEG_F; @@ -247,10 +264,10 @@ l_vehicle_gps_position(const struct listener *l) gps.lat, gps.lon, gps.alt, - gps.eph_m * 1e2f, // from m to cm - gps.epv_m * 1e2f, // from m to cm + cm_uint16_from_m_float(gps.eph_m), + cm_uint16_from_m_float(gps.epv_m), gps.vel_m_s * 1e2f, // from m/s to cm/s - cog_deg * 1e2f, // from rad to deg * 100 + cog_deg * 1e2f, // from deg to deg * 100 gps.satellites_visible); /* update SAT info every 10 seconds */ diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index 59cbcf5fb..ccdb2ea38 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -509,31 +509,57 @@ int param_save_default(void) { int result; + unsigned retries = 0; /* delete the file in case it exists */ struct stat buffer; if (stat(param_get_default_file(), &buffer) == 0) { - result = unlink(param_get_default_file()); + + do { + result = unlink(param_get_default_file()); + if (result != 0) { + retries++; + usleep(1000 * retries); + } + } while (result != OK && retries < 10); + if (result != OK) warnx("unlinking file %s failed.", param_get_default_file()); } /* create the file */ - int fd = open(param_get_default_file(), O_WRONLY | O_CREAT | O_EXCL); + int fd; - if (fd < 0) { + do { /* do another attempt in case the unlink call is not synced yet */ - usleep(5000); fd = open(param_get_default_file(), O_WRONLY | O_CREAT | O_EXCL); + if (fd < 0) { + retries++; + usleep(1000 * retries); + } + } while (fd < 0 && retries < 10); + + if (fd < 0) { + warn("opening '%s' for writing failed", param_get_default_file()); return fd; } - result = param_export(fd, false); + do { + result = param_export(fd, false); + + if (result != OK) { + retries++; + usleep(1000 * retries); + } + + } while (result != 0 && retries < 10); + + close(fd); - if (result != 0) { + if (result != OK) { warn("error exporting parameters to '%s'", param_get_default_file()); (void)unlink(param_get_default_file()); return result; |