aboutsummaryrefslogtreecommitdiff
path: root/src/drivers
diff options
context:
space:
mode:
Diffstat (limited to 'src/drivers')
-rw-r--r--src/drivers/airspeed/airspeed.cpp4
-rw-r--r--src/drivers/airspeed/airspeed.h2
-rw-r--r--src/drivers/ets_airspeed/ets_airspeed.cpp7
-rw-r--r--src/drivers/frsky_telemetry/frsky_data.c8
-rw-r--r--src/drivers/hott/messages.cpp4
-rw-r--r--src/drivers/meas_airspeed/meas_airspeed.cpp17
6 files changed, 23 insertions, 19 deletions
diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp
index 771f2128f..524151c90 100644
--- a/src/drivers/airspeed/airspeed.cpp
+++ b/src/drivers/airspeed/airspeed.cpp
@@ -76,8 +76,8 @@
#include <drivers/airspeed/airspeed.h>
-Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) :
- I2C("Airspeed", AIRSPEED_DEVICE_PATH, bus, address, 100000),
+Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const char* path) :
+ I2C("Airspeed", path, bus, address, 100000),
_reports(nullptr),
_buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")),
_max_differential_pressure_pa(0),
diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h
index c27b1bcd8..186602eda 100644
--- a/src/drivers/airspeed/airspeed.h
+++ b/src/drivers/airspeed/airspeed.h
@@ -90,7 +90,7 @@ static const int ERROR = -1;
class __EXPORT Airspeed : public device::I2C
{
public:
- Airspeed(int bus, int address, unsigned conversion_interval);
+ Airspeed(int bus, int address, unsigned conversion_interval, const char* path);
virtual ~Airspeed();
virtual int init();
diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp
index 8bbef5cfa..b6e80ce1d 100644
--- a/src/drivers/ets_airspeed/ets_airspeed.cpp
+++ b/src/drivers/ets_airspeed/ets_airspeed.cpp
@@ -77,6 +77,7 @@
/* I2C bus address */
#define I2C_ADDRESS 0x75 /* 7-bit address. 8-bit address is 0xEA */
+#define ETS_PATH "/dev/ets_airspeed"
/* Register address */
#define READ_CMD 0x07 /* Read the data */
@@ -93,7 +94,7 @@
class ETSAirspeed : public Airspeed
{
public:
- ETSAirspeed(int bus, int address = I2C_ADDRESS);
+ ETSAirspeed(int bus, int address = I2C_ADDRESS, const char* path = ETS_PATH);
protected:
@@ -112,8 +113,8 @@ protected:
*/
extern "C" __EXPORT int ets_airspeed_main(int argc, char *argv[]);
-ETSAirspeed::ETSAirspeed(int bus, int address) : Airspeed(bus, address,
- CONVERSION_INTERVAL)
+ETSAirspeed::ETSAirspeed(int bus, int address, const char* path) : Airspeed(bus, address,
+ CONVERSION_INTERVAL, path)
{
}
diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c
index 63b2d2d29..cfcf91e3f 100644
--- a/src/drivers/frsky_telemetry/frsky_data.c
+++ b/src/drivers/frsky_telemetry/frsky_data.c
@@ -225,16 +225,16 @@ void frsky_send_frame2(int uart)
float course = 0, lat = 0, lon = 0, speed = 0, alt = 0;
char lat_ns = 0, lon_ew = 0;
int sec = 0;
- if (global_pos.valid) {
+ if (global_pos.global_valid) {
time_t time_gps = global_pos.time_gps_usec / 1000000;
struct tm *tm_gps = gmtime(&time_gps);
course = (global_pos.yaw + M_PI_F) / M_PI_F * 180.0f;
- lat = frsky_format_gps(abs(global_pos.lat) / 10000000.0f);
+ lat = frsky_format_gps(abs(global_pos.lat));
lat_ns = (global_pos.lat < 0) ? 'S' : 'N';
- lon = frsky_format_gps(abs(global_pos.lon) / 10000000.0f);
+ lon = frsky_format_gps(abs(global_pos.lon));
lon_ew = (global_pos.lon < 0) ? 'W' : 'E';
- speed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy)
+ speed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e)
* 25.0f / 46.0f;
alt = global_pos.alt;
sec = tm_gps->tm_sec;
diff --git a/src/drivers/hott/messages.cpp b/src/drivers/hott/messages.cpp
index bb8d45bea..90a744015 100644
--- a/src/drivers/hott/messages.cpp
+++ b/src/drivers/hott/messages.cpp
@@ -295,8 +295,8 @@ build_gps_response(uint8_t *buffer, size_t *size)
memset(&home, 0, sizeof(home));
orb_copy(ORB_ID(home_position), _home_sub, &home);
- _home_lat = ((double)(home.lat))*1e-7d;
- _home_lon = ((double)(home.lon))*1e-7d;
+ _home_lat = home.lat;
+ _home_lon = home.lon;
_home_position_set = true;
}
diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp
index 05ae21c1f..06d89abf7 100644
--- a/src/drivers/meas_airspeed/meas_airspeed.cpp
+++ b/src/drivers/meas_airspeed/meas_airspeed.cpp
@@ -50,6 +50,7 @@
* - Interfacing to MEAS Digital Pressure Modules (http://www.meas-spec.com/downloads/Interfacing_to_MEAS_Digital_Pressure_Modules.pdf)
*/
+
#include <nuttx/config.h>
#include <drivers/device/i2c.h>
@@ -89,8 +90,10 @@
/* I2C bus address is 1010001x */
#define I2C_ADDRESS_MS4525DO 0x28 //0x51 /* 7-bit address. */
+#define PATH_MS4525 "/dev/ms4525"
/* The MS5525DSO address is 111011Cx, where C is the complementary value of the pin CSB */
#define I2C_ADDRESS_MS5525DSO 0x77 //0x77/* 7-bit address, addr. pin pulled low */
+#define PATH_MS5525 "/dev/ms5525"
/* Register address */
#define ADDR_READ_MR 0x00 /* write to this address to start conversion */
@@ -101,7 +104,7 @@
class MEASAirspeed : public Airspeed
{
public:
- MEASAirspeed(int bus, int address = I2C_ADDRESS_MS4525DO);
+ MEASAirspeed(int bus, int address = I2C_ADDRESS_MS4525DO, const char* path = PATH_MS4525);
protected:
@@ -120,8 +123,8 @@ protected:
*/
extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]);
-MEASAirspeed::MEASAirspeed(int bus, int address) : Airspeed(bus, address,
- CONVERSION_INTERVAL)
+MEASAirspeed::MEASAirspeed(int bus, int address, const char* path) : Airspeed(bus, address,
+ CONVERSION_INTERVAL, path)
{
}
@@ -304,7 +307,7 @@ start(int i2c_bus)
errx(1, "already started");
/* create the driver, try the MS4525DO first */
- g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS4525DO);
+ g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS4525DO, PATH_MS4525);
/* check if the MS4525DO was instantiated */
if (g_dev == nullptr)
@@ -313,7 +316,7 @@ start(int i2c_bus)
/* try the MS5525DSO next if init fails */
if (OK != g_dev->Airspeed::init()) {
delete g_dev;
- g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS5525DSO);
+ g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS5525DSO, PATH_MS5525);
/* check if the MS5525DSO was instantiated */
if (g_dev == nullptr)
@@ -386,7 +389,7 @@ test()
err(1, "immediate read failed");
warnx("single read");
- warnx("diff pressure: %d pa", (double)report.differential_pressure_pa);
+ warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa);
/* start the sensor polling at 2Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
@@ -411,7 +414,7 @@ test()
err(1, "periodic read failed");
warnx("periodic read %u", i);
- warnx("diff pressure: %d pa", report.differential_pressure_pa);
+ warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa);
warnx("temperature: %d C (0x%02x)", (int)report.temperature, (unsigned) report.temperature);
}