diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-03-21 10:44:31 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-03-21 10:44:31 +0100 |
commit | 2988136e7eaa1f55c41376b74b58766af9f8fcb9 (patch) | |
tree | fc70f4da004976adb9323cc0a527a7172f32c32b /src | |
parent | 055e45935534b409cd3ed68c837399461e448b3e (diff) | |
download | px4-firmware-2988136e7eaa1f55c41376b74b58766af9f8fcb9.tar.gz px4-firmware-2988136e7eaa1f55c41376b74b58766af9f8fcb9.tar.bz2 px4-firmware-2988136e7eaa1f55c41376b74b58766af9f8fcb9.zip |
Implemented last missing messages, added stream config for USB, made stream config fails for non-existing mavlink links non-fatal
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/mavlink/mavlink_main.cpp | 21 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_messages.cpp | 156 |
2 files changed, 106 insertions, 71 deletions
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index c9f4835ba..00d906dcd 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -588,7 +588,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * /* setup output flow control */ if (enable_flow_control(true)) { - warnx("ERR FLOW CTRL EN"); + warnx("hardware flow control not supported"); } } @@ -1762,6 +1762,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("GLOBAL_POSITION_INT", 3.0f * rate_mult); configure_stream("LOCAL_POSITION_NED", 3.0f * rate_mult); configure_stream("RC_CHANNELS_RAW", 1.0f * rate_mult); + configure_stream("NAMED_VALUE_FLOAT", 1.0f * rate_mult); break; default: @@ -1951,8 +1952,17 @@ Mavlink::start(int argc, char *argv[]) // the only path to create a new instance, // this is effectively a lock on concurrent // instance starting. XXX do a real lock. - while (ic == Mavlink::instance_count()) { - ::usleep(500); + + // Sleep 500 us between each attempt + const unsigned sleeptime = 500; + + // Wait 100 ms max for the startup. + const unsigned limit = 100 * 1000 / sleeptime; + + unsigned count = 0; + while (ic == Mavlink::instance_count() && count < limit) { + ::usleep(sleeptime); + count++; } return OK; @@ -2008,7 +2018,10 @@ Mavlink::stream(int argc, char *argv[]) inst->configure_stream_threadsafe(stream_name, rate); } else { - errx(1, "mavlink for device %s is not running", device_name); + + // If the link is not running we should complain, but not fall over + // because this is so easy to get wrong and not fatal. Warning is sufficient. + errx(0, "mavlink for device %s is not running", device_name); } } else { diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 7d388f88d..dc01935ca 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1127,6 +1127,93 @@ protected: } }; +class MavlinkStreamAttitudeControls : public MavlinkStream +{ +public: + const char *get_name() + { + return "ATTITUDE_CONTROLS"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamAttitudeControls(); + } + +private: + MavlinkOrbSubscription *att_ctrl_sub; + struct actuator_controls_s *att_ctrl; + +protected: + void subscribe(Mavlink *mavlink) + { + att_ctrl_sub = mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + att_ctrl = (struct actuator_controls_s *)att_ctrl_sub->get_data(); + } + + void send(const hrt_abstime t) + { + if (att_ctrl_sub->update(t)) { + + // send, add spaces so that string buffer is at least 10 chars long + mavlink_msg_named_value_float_send(_channel, + att_ctrl->timestamp / 1000, + "rll ctrl ", + att_ctrl->control[0]); + mavlink_msg_named_value_float_send(_channel, + att_ctrl->timestamp / 1000, + "ptch ctrl ", + att_ctrl->control[1]); + mavlink_msg_named_value_float_send(_channel, + att_ctrl->timestamp / 1000, + "yaw ctrl ", + att_ctrl->control[2]); + mavlink_msg_named_value_float_send(_channel, + att_ctrl->timestamp / 1000, + "thr ctrl ", + att_ctrl->control[3]); + } + } +}; + +class MavlinkStreamNamedValueFloat : public MavlinkStream +{ +public: + const char *get_name() + { + return "NAMED_VALUE_FLOAT"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamNamedValueFloat(); + } + +private: + MavlinkOrbSubscription *debug_sub; + struct debug_key_value_s *debug; + +protected: + void subscribe(Mavlink *mavlink) + { + debug_sub = mavlink->add_orb_subscription(ORB_ID(debug_key_value)); + debug = (struct debug_key_value_s *)debug_sub->get_data(); + } + + void send(const hrt_abstime t) + { + if (debug_sub->update(t)) { + + // Enforce null termination + debug->key[sizeof(debug->key) - 1] = '\0'; + + mavlink_msg_named_value_float_send(_channel, + debug->timestamp_ms, + debug->key, + debug->value); + } + } +}; MavlinkStream *streams_list[] = { new MavlinkStreamHeartbeat(), @@ -1151,72 +1238,7 @@ MavlinkStream *streams_list[] = { new MavlinkStreamRCChannelsRaw(), new MavlinkStreamManualControl(), new MavlinkStreamOpticalFlow(), + new MavlinkStreamAttitudeControls(), + new MavlinkStreamNamedValueFloat(), nullptr }; - - - - - - - -// -// -// -// -// -// -//void -//MavlinkOrbListener::l_vehicle_attitude_controls(const struct listener *l) -//{ -// orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, l->mavlink->get_subs()->actuators_sub, &l->listener->actuators_0); -// -// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) { -// /* send, add spaces so that string buffer is at least 10 chars long */ -// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), -// l->listener->last_sensor_timestamp / 1000, -// "ctrl0 ", -// l->listener->actuators_0.control[0]); -// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), -// l->listener->last_sensor_timestamp / 1000, -// "ctrl1 ", -// l->listener->actuators_0.control[1]); -// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), -// l->listener->last_sensor_timestamp / 1000, -// "ctrl2 ", -// l->listener->actuators_0.control[2]); -// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), -// l->listener->last_sensor_timestamp / 1000, -// "ctrl3 ", -// l->listener->actuators_0.control[3]); -// } -//} -// -//void -//MavlinkOrbListener::l_debug_key_value(const struct listener *l) -//{ -// struct debug_key_value_s debug; -// -// orb_copy(ORB_ID(debug_key_value), l->mavlink->get_subs()->debug_key_value, &debug); -// -// /* Enforce null termination */ -// debug.key[sizeof(debug.key) - 1] = '\0'; -// -// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), -// l->listener->last_sensor_timestamp / 1000, -// debug.key, -// debug.value); -//} -// -//void -//MavlinkOrbListener::l_nav_cap(const struct listener *l) -//{ -// -// orb_copy(ORB_ID(navigation_capabilities), l->mavlink->get_subs()->navigation_capabilities_sub, &l->listener->nav_cap); -// -// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), -// hrt_absolute_time() / 1000, -// "turn dist", -// l->listener->nav_cap.turn_distance); -// -//} |