aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
authorMark Charlebois <charlebm@gmail.com>2015-03-16 19:35:21 -0700
committerMark Charlebois <charlebm@gmail.com>2015-04-20 11:13:24 -0700
commit2feeecdab1116b3ef09a7a903bfefb8c5d604d81 (patch)
treecc898e44a49f76444fc1f55872a0abd98a93d58a /src
parent598b05fcbace33ec8e4b9d5b46f0136bf693339d (diff)
downloadpx4-firmware-2feeecdab1116b3ef09a7a903bfefb8c5d604d81.tar.gz
px4-firmware-2feeecdab1116b3ef09a7a903bfefb8c5d604d81.tar.bz2
px4-firmware-2feeecdab1116b3ef09a7a903bfefb8c5d604d81.zip
Linux: Added config and stubs to compile I2C device for Linux
Not yet functional. Full implementation will provide an IOCTL interface to do bi-directional transfer. will model the interface after Linux. Signed-off-by: Mark Charlebois <charlebm@gmail.com>
Diffstat (limited to 'src')
-rw-r--r--src/drivers/device/i2c.h128
-rw-r--r--src/drivers/device/i2c_linux.cpp3
-rw-r--r--src/drivers/device/i2c_linux.h152
-rw-r--r--src/drivers/device/i2c_nuttx.h153
-rw-r--r--src/drivers/ms5611/ms5611_linux.cpp1
-rw-r--r--src/modules/mavlink/mavlink_main.h375
-rw-r--r--src/modules/mavlink/mavlink_main_linux.cpp3
-rw-r--r--src/platforms/linux/tests/hrt_test/hrt_test.cpp8
-rw-r--r--src/platforms/px4_config.h1
9 files changed, 321 insertions, 503 deletions
diff --git a/src/drivers/device/i2c.h b/src/drivers/device/i2c.h
index 97ab25672..fb0be9bd5 100644
--- a/src/drivers/device/i2c.h
+++ b/src/drivers/device/i2c.h
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * Copyright (C) 2015 Mark Charlebois. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -30,124 +30,10 @@
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
+#pragma once
-/**
- * @file i2c.h
- *
- * Base class for devices connected via I2C.
- */
-
-#ifndef _DEVICE_I2C_H
-#define _DEVICE_I2C_H
-
-#include "device.h"
-
-#include <px4_i2c.h>
-
-namespace device __EXPORT
-{
-
-/**
- * Abstract class for character device on I2C
- */
-class __EXPORT I2C : public CDev
-{
-
-public:
-
- /**
- * Get the address
- */
- int16_t get_address() const { return _address; }
-
- static int set_bus_clock(unsigned bus, unsigned clock_hz);
-
- static unsigned int _bus_clocks[3];
-
-protected:
- /**
- * The number of times a read or write operation will be retried on
- * error.
- */
- unsigned _retries;
-
- /**
- * The I2C bus number the device is attached to.
- */
- int _bus;
-
- /**
- * @ Constructor
- *
- * @param name Driver name
- * @param devname Device node name
- * @param bus I2C bus on which the device lives
- * @param address I2C bus address, or zero if set_address will be used
- * @param frequency I2C bus frequency for the device (currently not used)
- * @param irq Interrupt assigned to the device (or zero if none)
- */
- I2C(const char *name,
- const char *devname,
- int bus,
- uint16_t address,
- uint32_t frequency,
- int irq = 0);
- virtual ~I2C();
-
- virtual int init();
-
- /**
- * Check for the presence of the device on the bus.
- */
- virtual int probe();
-
- /**
- * Perform an I2C transaction to the device.
- *
- * At least one of send_len and recv_len must be non-zero.
- *
- * @param send Pointer to bytes to send.
- * @param send_len Number of bytes to send.
- * @param recv Pointer to buffer for bytes received.
- * @param recv_len Number of bytes to receive.
- * @return OK if the transfer was successful, -errno
- * otherwise.
- */
- int transfer(const uint8_t *send, unsigned send_len,
- uint8_t *recv, unsigned recv_len);
-
- /**
- * Perform a multi-part I2C transaction to the device.
- *
- * @param msgv An I2C message vector.
- * @param msgs The number of entries in the message vector.
- * @return OK if the transfer was successful, -errno
- * otherwise.
- */
- int transfer(px4_i2c_msg_t *msgv, unsigned msgs);
-
- /**
- * Change the bus address.
- *
- * Most often useful during probe() when the driver is testing
- * several possible bus addresses.
- *
- * @param address The new bus address to set.
- */
- void set_address(uint16_t address) {
- _address = address;
- _device_id.devid_s.address = _address;
- }
-
-private:
- uint16_t _address;
- uint32_t _frequency;
- px4_i2c_dev_t *_dev;
-
- I2C(const device::I2C &);
- I2C operator=(const device::I2C &);
-};
-
-} // namespace device
-
-#endif /* _DEVICE_I2C_H */
+#ifdef __PX4_NUTTX
+#include "i2c_nuttx.h"
+#else
+#include "i2c_linux.h"
+#endif
diff --git a/src/drivers/device/i2c_linux.cpp b/src/drivers/device/i2c_linux.cpp
index 4ab3ff172..52f336b3a 100644
--- a/src/drivers/device/i2c_linux.cpp
+++ b/src/drivers/device/i2c_linux.cpp
@@ -51,8 +51,7 @@ I2C::I2C(const char *name,
const char *devname,
int bus,
uint16_t address,
- uint32_t frequency,
- int irq) :
+ uint32_t frequency) :
// base class
CDev(name, devname),
// public
diff --git a/src/drivers/device/i2c_linux.h b/src/drivers/device/i2c_linux.h
new file mode 100644
index 000000000..3de6c9658
--- /dev/null
+++ b/src/drivers/device/i2c_linux.h
@@ -0,0 +1,152 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file i2c.h
+ *
+ * Base class for devices connected via I2C.
+ */
+
+#ifndef _DEVICE_I2C_H
+#define _DEVICE_I2C_H
+
+#include "device.h"
+
+#include <px4_i2c.h>
+
+namespace device __EXPORT
+{
+
+/**
+ * Abstract class for character device on I2C
+ */
+class __EXPORT I2C : public CDev
+{
+
+public:
+
+ /**
+ * Get the address
+ */
+ int16_t get_address() const { return _address; }
+
+ static int set_bus_clock(unsigned bus, unsigned clock_hz);
+
+ static unsigned int _bus_clocks[3];
+
+protected:
+ /**
+ * The number of times a read or write operation will be retried on
+ * error.
+ */
+ unsigned _retries;
+
+ /**
+ * The I2C bus number the device is attached to.
+ */
+ int _bus;
+
+ /**
+ * @ Constructor
+ *
+ * @param name Driver name
+ * @param devname Device node name
+ * @param bus I2C bus on which the device lives
+ * @param address I2C bus address, or zero if set_address will be used
+ * @param frequency I2C bus frequency for the device (currently not used)
+ * @param irq Interrupt assigned to the device (or zero if none)
+ */
+ I2C(const char *name,
+ const char *devname,
+ int bus,
+ uint16_t address,
+ uint32_t frequency);
+ virtual ~I2C();
+
+ virtual int init();
+
+ /**
+ * Check for the presence of the device on the bus.
+ */
+ virtual int probe();
+
+ /**
+ * Perform an I2C transaction to the device.
+ *
+ * At least one of send_len and recv_len must be non-zero.
+ *
+ * @param send Pointer to bytes to send.
+ * @param send_len Number of bytes to send.
+ * @param recv Pointer to buffer for bytes received.
+ * @param recv_len Number of bytes to receive.
+ * @return OK if the transfer was successful, -errno
+ * otherwise.
+ */
+ int transfer(const uint8_t *send, unsigned send_len,
+ uint8_t *recv, unsigned recv_len);
+
+ /**
+ * Perform a multi-part I2C transaction to the device.
+ *
+ * @param msgv An I2C message vector.
+ * @param msgs The number of entries in the message vector.
+ * @return OK if the transfer was successful, -errno
+ * otherwise.
+ */
+ int transfer(px4_i2c_msg_t *msgv, unsigned msgs);
+
+ /**
+ * Change the bus address.
+ *
+ * Most often useful during probe() when the driver is testing
+ * several possible bus addresses.
+ *
+ * @param address The new bus address to set.
+ */
+ void set_address(uint16_t address) {
+ _address = address;
+ _device_id.devid_s.address = _address;
+ }
+
+private:
+ uint16_t _address;
+ uint32_t _frequency;
+ px4_i2c_dev_t *_dev;
+
+ I2C(const device::I2C&);
+ I2C operator=(const device::I2C&);
+};
+
+} // namespace device
+
+#endif /* _DEVICE_I2C_H */
diff --git a/src/drivers/device/i2c_nuttx.h b/src/drivers/device/i2c_nuttx.h
new file mode 100644
index 000000000..97ab25672
--- /dev/null
+++ b/src/drivers/device/i2c_nuttx.h
@@ -0,0 +1,153 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file i2c.h
+ *
+ * Base class for devices connected via I2C.
+ */
+
+#ifndef _DEVICE_I2C_H
+#define _DEVICE_I2C_H
+
+#include "device.h"
+
+#include <px4_i2c.h>
+
+namespace device __EXPORT
+{
+
+/**
+ * Abstract class for character device on I2C
+ */
+class __EXPORT I2C : public CDev
+{
+
+public:
+
+ /**
+ * Get the address
+ */
+ int16_t get_address() const { return _address; }
+
+ static int set_bus_clock(unsigned bus, unsigned clock_hz);
+
+ static unsigned int _bus_clocks[3];
+
+protected:
+ /**
+ * The number of times a read or write operation will be retried on
+ * error.
+ */
+ unsigned _retries;
+
+ /**
+ * The I2C bus number the device is attached to.
+ */
+ int _bus;
+
+ /**
+ * @ Constructor
+ *
+ * @param name Driver name
+ * @param devname Device node name
+ * @param bus I2C bus on which the device lives
+ * @param address I2C bus address, or zero if set_address will be used
+ * @param frequency I2C bus frequency for the device (currently not used)
+ * @param irq Interrupt assigned to the device (or zero if none)
+ */
+ I2C(const char *name,
+ const char *devname,
+ int bus,
+ uint16_t address,
+ uint32_t frequency,
+ int irq = 0);
+ virtual ~I2C();
+
+ virtual int init();
+
+ /**
+ * Check for the presence of the device on the bus.
+ */
+ virtual int probe();
+
+ /**
+ * Perform an I2C transaction to the device.
+ *
+ * At least one of send_len and recv_len must be non-zero.
+ *
+ * @param send Pointer to bytes to send.
+ * @param send_len Number of bytes to send.
+ * @param recv Pointer to buffer for bytes received.
+ * @param recv_len Number of bytes to receive.
+ * @return OK if the transfer was successful, -errno
+ * otherwise.
+ */
+ int transfer(const uint8_t *send, unsigned send_len,
+ uint8_t *recv, unsigned recv_len);
+
+ /**
+ * Perform a multi-part I2C transaction to the device.
+ *
+ * @param msgv An I2C message vector.
+ * @param msgs The number of entries in the message vector.
+ * @return OK if the transfer was successful, -errno
+ * otherwise.
+ */
+ int transfer(px4_i2c_msg_t *msgv, unsigned msgs);
+
+ /**
+ * Change the bus address.
+ *
+ * Most often useful during probe() when the driver is testing
+ * several possible bus addresses.
+ *
+ * @param address The new bus address to set.
+ */
+ void set_address(uint16_t address) {
+ _address = address;
+ _device_id.devid_s.address = _address;
+ }
+
+private:
+ uint16_t _address;
+ uint32_t _frequency;
+ px4_i2c_dev_t *_dev;
+
+ I2C(const device::I2C &);
+ I2C operator=(const device::I2C &);
+};
+
+} // namespace device
+
+#endif /* _DEVICE_I2C_H */
diff --git a/src/drivers/ms5611/ms5611_linux.cpp b/src/drivers/ms5611/ms5611_linux.cpp
index 7f020c6c3..e914dfaec 100644
--- a/src/drivers/ms5611/ms5611_linux.cpp
+++ b/src/drivers/ms5611/ms5611_linux.cpp
@@ -890,6 +890,7 @@ start_bus(struct ms5611_bus_option &bus)
if (bus.dev != nullptr && OK != bus.dev->init()) {
delete bus.dev;
bus.dev = NULL;
+ warnx("bus init failed %p", bus.dev);
return false;
}
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 216abe954..3153255b3 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -32,383 +32,8 @@
****************************************************************************/
#pragma once
-<<<<<<< HEAD
-#include <stdbool.h>
-#include <nuttx/fs/fs.h>
-#include <systemlib/param/param.h>
-#include <systemlib/perf_counter.h>
-#include <pthread.h>
-#include <mavlink/mavlink_log.h>
-
-#include <uORB/uORB.h>
-#include <uORB/topics/mission.h>
-#include <uORB/topics/mission_result.h>
-#include <uORB/topics/telemetry_status.h>
-
-#include "mavlink_bridge_header.h"
-#include "mavlink_orb_subscription.h"
-#include "mavlink_stream.h"
-#include "mavlink_messages.h"
-#include "mavlink_mission.h"
-#include "mavlink_parameters.h"
-
-class Mavlink
-{
-
-public:
- /**
- * Constructor
- */
- Mavlink();
-
- /**
- * Destructor, also kills the mavlinks task.
- */
- ~Mavlink();
-
- /**
- * Start the mavlink task.
- *
- * @return OK on success.
- */
- static int start(int argc, char *argv[]);
-
- /**
- * Display the mavlink status.
- */
- void display_status();
-
- static int stream_command(int argc, char *argv[]);
-
- static int instance_count();
-
- static Mavlink *new_instance();
-
- static Mavlink *get_instance(unsigned instance);
-
- static Mavlink *get_instance_for_device(const char *device_name);
-
- static int destroy_all_instances();
-
- static int get_status_all_instances();
-
- static bool instance_exists(const char *device_name, Mavlink *self);
-
- static void forward_message(const mavlink_message_t *msg, Mavlink *self);
-
- static int get_uart_fd(unsigned index);
-
- int get_uart_fd();
-
- /**
- * Get the MAVLink system id.
- *
- * @return The system ID of this vehicle
- */
- int get_system_id();
-
- /**
- * Get the MAVLink component id.
- *
- * @return The component ID of this vehicle
- */
- int get_component_id();
-
- const char *_device_name;
-
- enum MAVLINK_MODE {
- MAVLINK_MODE_NORMAL = 0,
- MAVLINK_MODE_CUSTOM,
- MAVLINK_MODE_ONBOARD
- };
-
- void set_mode(enum MAVLINK_MODE);
- enum MAVLINK_MODE get_mode() { return _mode; }
-
- bool get_hil_enabled() { return _hil_enabled; }
-
- bool get_use_hil_gps() { return _use_hil_gps; }
-
- bool get_forward_externalsp() { return _forward_externalsp; }
-
- bool get_flow_control_enabled() { return _flow_control_enabled; }
-
- bool get_forwarding_on() { return _forwarding_on; }
-
- /**
- * Get the free space in the transmit buffer
- *
- * @return free space in the UART TX buffer
- */
- unsigned get_free_tx_buf();
-
- static int start_helper(int argc, char *argv[]);
-
- /**
- * Handle parameter related messages.
- */
- void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg);
-
- void get_mavlink_mode_and_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);
-
- /**
- * Enable / disable Hardware in the Loop simulation mode.
- *
- * @param hil_enabled The new HIL enable/disable state.
- * @return OK if the HIL state changed, ERROR if the
- * requested change could not be made or was
- * redundant.
- */
- int set_hil_enabled(bool hil_enabled);
-
- void send_message(const uint8_t msgid, const void *msg, uint8_t component_ID = 0);
-
- /**
- * Resend message as is, don't change sequence number and CRC.
- */
- void resend_message(mavlink_message_t *msg);
-
- void handle_message(const mavlink_message_t *msg);
-
- MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic, int instance=0);
-
- int get_instance_id();
-
- /**
- * Enable / disable hardware flow control.
- *
- * @param enabled True if hardware flow control should be enabled
- */
- int enable_flow_control(bool enabled);
-
- mavlink_channel_t get_channel();
-
- void configure_stream_threadsafe(const char *stream_name, float rate);
-
- bool _task_should_exit; /**< if true, mavlink task should exit */
-
- int get_mavlink_fd() { return _mavlink_fd; }
-
- /**
- * Send a status text with loglevel INFO
- *
- * @param string the message to send (will be capped by mavlink max string length)
- */
- void send_statustext_info(const char *string);
-
- /**
- * Send a status text with loglevel CRITICAL
- *
- * @param string the message to send (will be capped by mavlink max string length)
- */
- void send_statustext_critical(const char *string);
-
- /**
- * Send a status text with loglevel EMERGENCY
- *
- * @param string the message to send (will be capped by mavlink max string length)
- */
- void send_statustext_emergency(const char *string);
-
- /**
- * Send a status text with loglevel, the difference from mavlink_log_xxx() is that message sent
- * only on this mavlink connection. Useful for reporting communication specific, not system-wide info
- * only to client interested in it. Message will be not sent immediately but queued in buffer as
- * for mavlink_log_xxx().
- *
- * @param string the message to send (will be capped by mavlink max string length)
- * @param severity the log level
- */
- void send_statustext(unsigned char severity, const char *string);
-
- MavlinkStream * get_streams() const { return _streams; }
-
- float get_rate_mult();
-
- /* Functions for waiting to start transmission until message received. */
- void set_has_received_messages(bool received_messages) { _received_messages = received_messages; }
- bool get_has_received_messages() { return _received_messages; }
- void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; }
- bool get_wait_to_transmit() { return _wait_to_transmit; }
- bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); }
-
- bool message_buffer_write(const void *ptr, int size);
-
- void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); }
- void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); }
-
- /**
- * Count a transmision error
- */
- void count_txerr();
-
- /**
- * Count transmitted bytes
- */
- void count_txbytes(unsigned n) { _bytes_tx += n; };
-
- /**
- * Count bytes not transmitted because of errors
- */
- void count_txerrbytes(unsigned n) { _bytes_txerr += n; };
-
- /**
- * Count received bytes
- */
- void count_rxbytes(unsigned n) { _bytes_rx += n; };
-
- /**
- * Get the receive status of this MAVLink link
- */
- struct telemetry_status_s& get_rx_status() { return _rstatus; }
-
- struct mavlink_logbuffer *get_logbuffer() { return &_logbuffer; }
-
- unsigned get_system_type() { return _system_type; }
-
-protected:
- Mavlink *next;
-
-private:
- int _instance_id;
-
- int _mavlink_fd;
- bool _task_running;
-
- /* states */
- bool _hil_enabled; /**< Hardware In the Loop mode */
- bool _use_hil_gps; /**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */
- bool _forward_externalsp; /**< Forward external setpoint messages to controllers directly if in offboard mode */
- bool _is_usb_uart; /**< Port is USB */
- bool _wait_to_transmit; /**< Wait to transmit until received messages. */
- bool _received_messages; /**< Whether we've received valid mavlink messages. */
-
- unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */
-
- MavlinkOrbSubscription *_subscriptions;
- MavlinkStream *_streams;
-
- MavlinkMissionManager *_mission_manager;
- MavlinkParametersManager *_parameters_manager;
-
- MAVLINK_MODE _mode;
-
- mavlink_channel_t _channel;
-
- struct mavlink_logbuffer _logbuffer;
- unsigned int _total_counter;
-
- pthread_t _receive_thread;
-
- bool _verbose;
- bool _forwarding_on;
- bool _passing_on;
- bool _ftp_on;
- int _uart_fd;
- int _baudrate;
- int _datarate; ///< data rate for normal streams (attitude, position, etc.)
- int _datarate_events; ///< data rate for params, waypoints, text messages
- float _rate_mult;
-
- /**
- * If the queue index is not at 0, the queue sending
- * logic will send parameters from the current index
- * to len - 1, the end of the param list.
- */
- unsigned int _mavlink_param_queue_index;
-
- bool mavlink_link_termination_allowed;
-
- char *_subscribe_to_stream;
- float _subscribe_to_stream_rate;
-
- bool _flow_control_enabled;
- uint64_t _last_write_success_time;
- uint64_t _last_write_try_time;
-
- unsigned _bytes_tx;
- unsigned _bytes_txerr;
- unsigned _bytes_rx;
- uint64_t _bytes_timestamp;
- float _rate_tx;
- float _rate_txerr;
- float _rate_rx;
-
- struct telemetry_status_s _rstatus; ///< receive status
-
- struct mavlink_message_buffer {
- int write_ptr;
- int read_ptr;
- int size;
- char *data;
- };
-
- mavlink_message_buffer _message_buffer;
-
- pthread_mutex_t _message_buffer_mutex;
- pthread_mutex_t _send_mutex;
-
- bool _param_initialized;
- param_t _param_system_id;
- param_t _param_component_id;
- param_t _param_system_type;
- param_t _param_use_hil_gps;
- param_t _param_forward_externalsp;
-
- unsigned _system_type;
-
- perf_counter_t _loop_perf; /**< loop performance counter */
- perf_counter_t _txerr_perf; /**< TX error counter */
-
- void mavlink_update_system();
-
- int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb);
-
- static unsigned int interval_from_rate(float rate);
-
- int configure_stream(const char *stream_name, const float rate);
-
- /**
- * Adjust the stream rates based on the current rate
- *
- * @param multiplier if greater than 1, the transmission rate will increase, if smaller than one decrease
- */
- void adjust_stream_rates(const float multiplier);
-
- int message_buffer_init(int size);
-
- void message_buffer_destroy();
-
- int message_buffer_count();
-
- int message_buffer_is_empty();
-
- int message_buffer_get_ptr(void **ptr, bool *is_part);
-
- void message_buffer_mark_read(int n);
-
- void pass_message(const mavlink_message_t *msg);
-
- /**
- * Update rate mult so total bitrate will be equal to _datarate.
- */
- void update_rate_mult();
-
- static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg);
-
- /**
- * Main mavlink task.
- */
- int task_main(int argc, char *argv[]);
-
- /* do not allow copying this class */
- Mavlink(const Mavlink&);
- Mavlink operator=(const Mavlink&);
-};
-=======
#ifdef __PX4_NUTTX
#include "mavlink_main_nuttx.h"
#else
#include "mavlink_main_linux.h"
#endif
->>>>>>> Support for building more modules with Linux
diff --git a/src/modules/mavlink/mavlink_main_linux.cpp b/src/modules/mavlink/mavlink_main_linux.cpp
index f2d8ea90f..694a9044d 100644
--- a/src/modules/mavlink/mavlink_main_linux.cpp
+++ b/src/modules/mavlink/mavlink_main_linux.cpp
@@ -244,7 +244,8 @@ Mavlink::~Mavlink()
} while (_task_running);
}
- LL_DELETE(_mavlink_instances, this);
+ if (_mavlink_instances)
+ LL_DELETE(_mavlink_instances, this);
}
void
diff --git a/src/platforms/linux/tests/hrt_test/hrt_test.cpp b/src/platforms/linux/tests/hrt_test/hrt_test.cpp
index d5e446bcc..5677f4e1b 100644
--- a/src/platforms/linux/tests/hrt_test/hrt_test.cpp
+++ b/src/platforms/linux/tests/hrt_test/hrt_test.cpp
@@ -53,14 +53,14 @@ int HRTTest::main()
hrt_abstime t = hrt_absolute_time();
usleep(1000000);
hrt_abstime elt = hrt_elapsed_time(&t);
- printf("Elapsed time %llu in 1 sec (usleep)\n", elt);
- printf("Start time %llu\n", t);
+ printf("Elapsed time %lu in 1 sec (usleep)\n", elt);
+ printf("Start time %lu\n", t);
t = hrt_absolute_time();
sleep(1);
elt = hrt_elapsed_time(&t);
- printf("Elapsed time %llu in 1 sec (sleep)\n", elt);
- printf("Start time %llu\n", t);
+ printf("Elapsed time %lu in 1 sec (sleep)\n", elt);
+ printf("Start time %lu\n", t);
return 0;
}
diff --git a/src/platforms/px4_config.h b/src/platforms/px4_config.h
index c53fd63d4..a7b9e5500 100644
--- a/src/platforms/px4_config.h
+++ b/src/platforms/px4_config.h
@@ -44,6 +44,7 @@
#include <px4_config.h>
#elif defined (__PX4_LINUX)
#define CONFIG_NFILE_STREAMS 1
+#define PX4_I2C_BUS_ONBOARD 1
#define px4_errx(x, ...) errx(x, __VA_ARGS__)