fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 03:53:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'px4/beta_mavlink2' into beta_mavlink2_camera

Browse Source 
Conflicts:
	src/modules/mavlink/mavlink_messages.cpp
This commit is contained in:
Julian Oes
2014-03-21 11:19:26 +01:00
parent bf69a7b647 ca2514b846
commit 3d47ba14f3
32 changed files with 550 additions and 317 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink.c
-25
View File
@@ -45,31 +45,6 @@
#include <string.h>
#include "mavlink_bridge_header.h"
#include <systemlib/param/param.h>
// #include <drivers/drv_hrt.h>
// #include <time.h>
// #include <float.h>
// #include <unistd.h>
// #include <nuttx/sched.h>
// #include <sys/prctl.h>
// #include <termios.h>
// #include <errno.h>
// #include <stdlib.h>
// #include <poll.h>
//
// #include <systemlib/systemlib.h>
// #include <systemlib/err.h>
// #include <mavlink/mavlink_log.h>
// #include <commander/px4_custom_mode.h>
// #include "waypoints.h"
// #include "orb_topics.h"
// #include "mavlink_hil.h"
// #include "util.h"
// #include "waypoints.h"
// #include "mavlink_parameters.h"
// #include <uORB/topics/mission_result.h>
/* define MAVLink specific parameters */
/**
src/modules/mavlink/mavlink_main.cpp
+37 -4
View File
@@ -254,7 +254,6 @@ Mavlink::Mavlink() :
break;
}
LL_APPEND(_mavlink_instances, this);
}
Mavlink::~Mavlink()
@@ -589,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");
}
}
@@ -1769,6 +1768,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;
case MAVLINK_MODE_CAMERA:
@@ -1791,6 +1791,9 @@ Mavlink::task_main(int argc, char *argv[])
/* set main loop delay depending on data rate to minimize CPU overhead */
_main_loop_delay = MAIN_LOOP_DELAY / rate_mult;
/* now the instance is fully initialized and we can bump the instance count */
LL_APPEND(_mavlink_instances, this);
while (!_task_should_exit) {
/* main loop */
usleep(_main_loop_delay);
@@ -1937,10 +1940,18 @@ int Mavlink::start_helper(int argc, char *argv[])
int
Mavlink::start(int argc, char *argv[])
{
// Wait for the instance count to go up one
// before returning to the shell
int ic = Mavlink::instance_count();
// Instantiate thread
char buf[24];
sprintf(buf, "mavlink_if%d", Mavlink::instance_count());
sprintf(buf, "mavlink_if%d", ic);
// This is where the control flow splits
// between the starting task and the spawned
// task - start_helper() only returns
// when the started task exits.
task_spawn_cmd(buf,
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
@@ -1948,6 +1959,25 @@ Mavlink::start(int argc, char *argv[])
(main_t)&Mavlink::start_helper,
(const char **)argv);
// Ensure that this shell command
// does not return before the instance
// is fully initialized. As this is also
// the only path to create a new instance,
// this is effectively a lock on concurrent
// instance starting. XXX do a real lock.
// 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;
}
@@ -2001,7 +2031,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 {
src/modules/mavlink/mavlink_messages.cpp
+91 -68
View File
@@ -230,6 +230,7 @@ protected:
mavlink_base_mode,
mavlink_custom_mode,
mavlink_state);
}
};
@@ -1126,6 +1127,94 @@ 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);
}
}
};
class MavlinkStreamCameraCapture : public MavlinkStream
{
public:
@@ -1168,7 +1257,6 @@ protected:
}
};
MavlinkStream *streams_list[] = {
new MavlinkStreamHeartbeat(),
new MavlinkStreamSysStatus(),
@@ -1192,73 +1280,8 @@ MavlinkStream *streams_list[] = {
new MavlinkStreamRCChannelsRaw(),
new MavlinkStreamManualControl(),
new MavlinkStreamOpticalFlow(),
new MavlinkStreamAttitudeControls(),
new MavlinkStreamNamedValueFloat(),
new MavlinkStreamCameraCapture(),
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);
//
//}