diff options
author | Julian Oes <julian@oes.ch> | 2014-07-02 15:15:26 +0200 |
---|---|---|
committer | Julian Oes <julian@oes.ch> | 2014-07-02 15:15:26 +0200 |
commit | 73d6121a9dbcd438f407edcd9b58bbb31bd94a42 (patch) | |
tree | cd82ecf355e68b80db6403716b394737f45f5231 /src/modules/mavlink | |
parent | fdceb8b0620c347c9f7f477dbf295dcfff12012c (diff) | |
parent | f428ebb04f0610c31639d8fe6d121f632c1cad1b (diff) | |
download | px4-firmware-73d6121a9dbcd438f407edcd9b58bbb31bd94a42.tar.gz px4-firmware-73d6121a9dbcd438f407edcd9b58bbb31bd94a42.tar.bz2 px4-firmware-73d6121a9dbcd438f407edcd9b58bbb31bd94a42.zip |
Merge branch 'master' into offboard2_merge
Conflicts:
src/modules/uORB/topics/rc_channels.h
Diffstat (limited to 'src/modules/mavlink')
-rw-r--r-- | src/modules/mavlink/mavlink_main.cpp | 10 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_main.h | 16 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_messages.cpp | 3 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_orb_subscription.cpp | 6 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 3 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_stream.cpp | 6 |
6 files changed, 23 insertions, 21 deletions
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 9a5e31ef4..026a4d6c9 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -474,7 +474,7 @@ Mavlink::get_instance_id() return _instance_id; } -const mavlink_channel_t +mavlink_channel_t Mavlink::get_channel() { return _channel; @@ -2109,7 +2109,7 @@ Mavlink::task_main(int argc, char *argv[]) write_ptr = (uint8_t*)&msg; // Pull a single message from the buffer - int read_count = available; + size_t read_count = available; if (read_count > sizeof(mavlink_message_t)) { read_count = sizeof(mavlink_message_t); } @@ -2266,13 +2266,13 @@ Mavlink::start(int argc, char *argv[]) } void -Mavlink::status() +Mavlink::display_status() { warnx("running"); } int -Mavlink::stream(int argc, char *argv[]) +Mavlink::stream_command(int argc, char *argv[]) { const char *device_name = DEFAULT_DEVICE_NAME; float rate = -1.0f; @@ -2360,7 +2360,7 @@ int mavlink_main(int argc, char *argv[]) // mavlink::g_mavlink->status(); } else if (!strcmp(argv[1], "stream")) { - return Mavlink::stream(argc, argv); + return Mavlink::stream_command(argc, argv); } else { usage(); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 6777d56c3..f94036a17 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -123,9 +123,9 @@ public: /** * Display the mavlink status. */ - void status(); + void display_status(); - static int stream(int argc, char *argv[]); + static int stream_command(int argc, char *argv[]); static int instance_count(); @@ -213,15 +213,15 @@ public: */ int enable_flow_control(bool enabled); - const mavlink_channel_t get_channel(); + mavlink_channel_t get_channel(); - void configure_stream_threadsafe(const char *stream_name, const float rate); + 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; } - MavlinkStream * get_streams() { return _streams; } const + MavlinkStream * get_streams() const { return _streams; } /* Functions for waiting to start transmission until message received. */ @@ -311,15 +311,15 @@ private: pthread_mutex_t _message_buffer_mutex; - perf_counter_t _loop_perf; /**< loop performance counter */ - perf_counter_t _txerr_perf; /**< TX error counter */ - bool _param_initialized; param_t _param_system_id; param_t _param_component_id; param_t _param_system_type; param_t _param_use_hil_gps; + perf_counter_t _loop_perf; /**< loop performance counter */ + perf_counter_t _txerr_perf; /**< TX error counter */ + /** * Send one parameter. * diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index e1ebc16cc..667be87b7 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -45,7 +45,6 @@ #include <uORB/uORB.h> #include <uORB/topics/sensor_combined.h> -#include <uORB/topics/rc_channels.h> #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_gps_position.h> #include <uORB/topics/vehicle_global_position.h> @@ -678,7 +677,7 @@ protected: cm_uint16_from_m_float(gps.epv), gps.vel_m_s * 100.0f, _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f, - gps.satellites_visible); + gps.satellites_used); } } }; diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index 901fa8f05..734f0903a 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -47,10 +47,10 @@ #include "mavlink_orb_subscription.h" MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) : - _fd(orb_subscribe(_topic)), - _published(false), + next(nullptr), _topic(topic), - next(nullptr) + _fd(orb_subscribe(_topic)), + _published(false) { } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 6aba72161..3747ad3ba 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -728,9 +728,8 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg) hil_gps.vel_ned_valid = true; hil_gps.cog_rad = _wrap_pi(gps.cog * M_DEG_TO_RAD_F * 1e-2f); - hil_gps.timestamp_satellites = timestamp; hil_gps.fix_type = gps.fix_type; - hil_gps.satellites_visible = gps.satellites_visible; + hil_gps.satellites_used = gps.satellites_visible; //TODO: rename mavlink_hil_gps_t sats visible to used? if (_gps_pub < 0) { _gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps); diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index 5ec30bd33..7279206db 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -43,7 +43,11 @@ #include "mavlink_stream.h" #include "mavlink_main.h" -MavlinkStream::MavlinkStream() : _interval(1000000), _last_sent(0), _channel(MAVLINK_COMM_0), next(nullptr) +MavlinkStream::MavlinkStream() : + next(nullptr), + _channel(MAVLINK_COMM_0), + _interval(1000000), + _last_sent(0) { } |