fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Butchered MAVLink C++ app to compile and link - there is no hope it will work out of the box 8)

Browse Source
This commit is contained in:
Lorenz Meier 2014-01-28 21:05:00 +01:00
parent 1e3d2acbf6
commit 63b18399c2
8 changed files with 106 additions and 101 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/modules/mavlink/mavlink_bridge_header.h
View File

@ -42,6 +42,8 @@
#ifndef MAVLINK_BRIDGE_HEADER_H
#define MAVLINK_BRIDGE_HEADER_H
__BEGIN_DECLS
#define MAVLINK_USE_CONVENIENCE_FUNCTIONS
/* use efficient approach, see mavlink_helpers.h */
@ -72,11 +74,13 @@ extern mavlink_system_t mavlink_system;
* @param chan MAVLink channel to use, usually MAVLINK_COMM_0 = UART0
* @param ch Character to send
*/
extern void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int length);
void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int length);
extern mavlink_status_t *mavlink_get_channel_status(uint8_t chan);
extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t chan);
#include <v1.0/common/mavlink.h>
__END_DECLS
#endif /* MAVLINK_BRIDGE_HEADER_H */

124
src/modules/mavlink/mavlink_main.cpp
View File

@ -97,6 +97,8 @@
#endif
static const int ERROR = -1;
static Mavlink* _head = nullptr;
/**
* mavlink app start / stop handling function
*
@ -203,10 +205,10 @@ void Mavlink::set_mode(enum MAVLINK_MODE mode)
int Mavlink::instance_count()
{
/* note: a local buffer count will help if this ever is called often */
Mavlink* inst = _head;
Mavlink* inst = ::_head;
unsigned inst_index = 0;
while (inst->_head != nullptr) {
inst = inst->_head;
while (inst->_next != nullptr) {
inst = inst->_next;
inst_index++;
}
@ -216,22 +218,22 @@ int Mavlink::instance_count()
Mavlink* Mavlink::new_instance(const char *port, unsigned baud_rate)
{
Mavlink* inst = new Mavlink(port, baud_rate);
Mavlink* parent = _head;
while (parent->_head != nullptr)
parent = parent->_head;
Mavlink* parent = ::_head;
while (parent->_next != nullptr)
parent = parent->_next;
/* now parent points to a null pointer, fill it */
parent->_head = inst;
parent->_next = inst;
return inst;
}
Mavlink* Mavlink::get_instance(unsigned instance)
{
Mavlink* inst = _head;
Mavlink* inst = ::_head;
unsigned inst_index = 0;
while (inst->_head != nullptr && inst_index < instance) {
inst = inst->_head;
while (inst->_next != nullptr && inst_index < instance) {
inst = inst->_next;
inst_index++;
}
@ -428,9 +430,9 @@ Mavlink::set_hil_on_off(bool hil_enabled)
int ret = OK;
/* Enable HIL */
if (hil_enabled && !mavlink_hil_enabled) {
if (hil_enabled && !_mavlink_hil_enabled) {
mavlink_hil_enabled = true;
_mavlink_hil_enabled = true;
/* ramp up some HIL-related subscriptions */
unsigned hil_rate_interval;
@ -456,8 +458,8 @@ Mavlink::set_hil_on_off(bool hil_enabled)
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, hil_rate_interval);
}
if (!hil_enabled && mavlink_hil_enabled) {
mavlink_hil_enabled = false;
if (!hil_enabled && _mavlink_hil_enabled) {
_mavlink_hil_enabled = false;
orb_set_interval(subs.spa_sub, 200);
} else {
@ -1426,12 +1428,6 @@ Mavlink::mavlink_missionlib_send_gcs_string(const char *string)
}
}
void
Mavlink::task_main_trampoline(int argc, char *argv[])
{
mavlink::g_mavlink->task_main(argc, argv);
}
int
Mavlink::task_main(int argc, char *argv[])
{
@ -1481,43 +1477,43 @@ Mavlink::task_main(int argc, char *argv[])
/* initialize mavlink text message buffering */
mavlink_logbuffer_init(&lb, 10);
// mavlink_logbuffer_init(&lb, 10);
int ch;
char *device_name = "/dev/ttyS1";
baudrate = 57600;
/* work around some stupidity in task_create's argv handling */
argc -= 2;
argv += 2;
// /* work around some stupidity in task_create's argv handling */
// argc -= 2;
// argv += 2;
while ((ch = getopt(argc, argv, "b:d:eo")) != EOF) {
switch (ch) {
case 'b':
baudrate = strtoul(optarg, NULL, 10);
// while ((ch = getopt(argc, argv, "b:d:eo")) != EOF) {
// switch (ch) {
// case 'b':
// baudrate = strtoul(optarg, NULL, 10);
if (baudrate < 9600 || baudrate > 921600)
errx(1, "invalid baud rate '%s'", optarg);
// if (baudrate < 9600 || baudrate > 921600)
// errx(1, "invalid baud rate '%s'", optarg);
break;
// break;
case 'd':
device_name = optarg;
break;
// case 'd':
// device_name = optarg;
// break;
case 'e':
mavlink_link_termination_allowed = true;
break;
// case 'e':
// mavlink_link_termination_allowed = true;
// break;
case 'o':
_mode = MODE_ONBOARD;
break;
// case 'o':
// _mode = MODE_ONBOARD;
// break;
default:
usage();
break;
}
}
// default:
// usage();
// break;
// }
// }
struct termios uart_config_original;
@ -1699,15 +1695,15 @@ Mavlink::task_main(int argc, char *argv[])
/* sleep 10 ms */
usleep(10000);
/* send one string at 10 Hz */
if (!mavlink_logbuffer_is_empty(&lb)) {
struct mavlink_logmessage msg;
int lb_ret = mavlink_logbuffer_read(&lb, &msg);
// /* send one string at 10 Hz */
// if (!mavlink_logbuffer_is_empty(&lb)) {
// struct mavlink_logmessage msg;
// int lb_ret = mavlink_logbuffer_read(&lb, &msg);
if (lb_ret == OK) {
mavlink_missionlib_send_gcs_string(msg.text);
}
}
// if (lb_ret == OK) {
// mavlink_missionlib_send_gcs_string(msg.text);
// }
// }
/* sleep 15 ms */
usleep(15000);
@ -1742,27 +1738,32 @@ Mavlink::task_main(int argc, char *argv[])
_exit(0);
}
int
Mavlink::start()
int Mavlink::start_helper(int argc, char *argv[])
{
// This is beyond evil.. and needs a lock to be safe
return Mavlink::get_instance(Mavlink::instance_count() - 1)->task_main(argc, argv);
}
int
Mavlink::start(Mavlink* mavlink)
{
ASSERT(_mavlink_task == -1);
/* start the task */
char buf[32];
sprintf(buf, "mavlink if%d", Mavlink::instance_count());
_mavlink_task = task_spawn_cmd(buf,
mavlink->_mavlink_task = task_spawn_cmd(buf,
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
2048,
(main_t)&Mavlink::task_main_trampoline,
nullptr);
(main_t)&Mavlink::start_helper,
NULL);
// while (!this->is_running()) {
// usleep(200);
// }
if (_mavlink_task < 0) {
if (mavlink->_mavlink_task < 0) {
warn("task start failed");
return -errno;
}
@ -1794,6 +1795,9 @@ int mavlink_main(int argc, char *argv[])
if (mavlink::g_mavlink == nullptr)
mavlink::g_mavlink = instance;
// Instantiate thread
Mavlink::start(instance);
// if (mavlink::g_mavlink != nullptr) {
// errx(1, "already running");
// }

42
src/modules/mavlink/mavlink_main.h
View File

@ -155,7 +155,7 @@ public:
*
* @return OK on success.
*/
int start();
static int start(Mavlink* mavlink);
/**
* Display the mavlink status.
@ -170,7 +170,7 @@ public:
static int get_uart_fd(unsigned index);
int get_uart_fd() { return _mavlink_fd; }
int get_uart_fd() { return uart; }
enum MAVLINK_MODE {
MODE_TX_HEARTBEAT_ONLY=0,
@ -180,7 +180,7 @@ public:
};
void set_mode(enum MAVLINK_MODE);
enum MAVLINK_MODE get_mode();
enum MAVLINK_MODE get_mode() { return _mode; }
bool hil_enabled() { return _mavlink_hil_enabled; };
@ -189,11 +189,24 @@ public:
*/
void mavlink_wpm_message_handler(const mavlink_message_t *msg);
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(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_on_off(bool hil_enabled);
struct mavlink_subscriptions {
int sensor_sub;
@ -217,12 +230,13 @@ public:
int input_rc_sub;
int optical_flow;
int rates_setpoint_sub;
int home_sub;
int get_sub;
int airspeed_sub;
int navigation_capabilities_sub;
int control_mode_sub;
int rc_sub;
int status_sub;
int home_sub;
};
struct mavlink_subscriptions subs;
@ -252,6 +266,7 @@ protected:
*/
struct file_operations fops;
int _mavlink_fd;
Mavlink* _next;
private:
@ -264,7 +279,6 @@ private:
/* states */
bool _mavlink_hil_enabled; /**< Hardware in the loop mode */
static Mavlink* _head;
int _params_sub;
@ -289,7 +303,6 @@ private:
mavlink_wpm_storage *wpm;
bool verbose;
bool mavlink_hil_enabled;
int uart;
int baudrate;
bool gcs_link;
@ -308,16 +321,6 @@ private:
*/
void parameters_update();
/**
* 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_on_off(bool hil_enabled);
/**
* Send all parameters at once.
*
@ -390,8 +393,6 @@ private:
int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval);
void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);
/**
* Callback for param interface.
*/
@ -399,11 +400,6 @@ private:
static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg);
/**
* Shim for calling task_main from task_create.
*/
void task_main_trampoline(int argc, char *argv[]);
/**
* Main mavlink task.
*/

24
src/modules/mavlink/mavlink_orb_listener.cpp
View File

@ -296,11 +296,11 @@ MavlinkOrbListener::l_vehicle_status(const struct listener *l)
}
void
MavlinkOrbListener::l_rc__mavlink->get_chan()nels(const struct listener *l)
MavlinkOrbListener::l_rc_channels(const struct listener *l)
{
/* copy rc _mavlink->get_chan()nels into local buffer */
orb_copy(ORB_ID(rc__mavlink->get_chan()nels), rc_sub, &rc);
// XXX Add RC _mavlink->get_chan()nels scaled message here
/* copy rc channels into local buffer */
orb_copy(ORB_ID(rc_channels), l->mavlink->get_subs()->rc_sub, &l->listener->rc);
// XXX Add RC channels scaled message here
}
void
@ -315,7 +315,7 @@ MavlinkOrbListener::l_input_rc(const struct listener *l)
for (unsigned i = 0; (i * port_width) < (l->listener->rc_raw.channel_count + port_width); i++) {
/* Channels are sent in MAVLink main loop at a fixed interval */
mavlink_msg_rc_channels_raw_send(_mavlink->get_chan(),
mavlink_msg_rc_channels_raw_send(l->mavlink->get_chan(),
l->listener->rc_raw.timestamp / 1000,
i,
(l->listener->rc_raw.channel_count > (i * port_width) + 0) ? l->listener->rc_raw.values[(i * port_width) + 0] : UINT16_MAX,
@ -335,7 +335,7 @@ void
MavlinkOrbListener::l_global_position(const struct listener *l)
{
/* copy global position data into local buffer */
orb_copy(ORB_ID(vehicle_global_position), _mavlink->get_subs()->global_pos_sub, l->listener->global_pos);
orb_copy(ORB_ID(vehicle_global_position), l->mavlink->get_subs()->global_pos_sub, &l->listener->global_pos);
mavlink_msg_global_position_int_send(l->mavlink->get_chan(),
l->listener->global_pos.timestamp / 1000,
@ -353,7 +353,7 @@ void
MavlinkOrbListener::l_local_position(const struct listener *l)
{
/* copy local position data into local buffer */
orb_copy(ORB_ID(vehicle_local_position), _mavlink->get_subs()->local_pos_sub, l->listener->local_pos);
orb_copy(ORB_ID(vehicle_local_position), l->mavlink->get_subs()->local_pos_sub, &l->listener->local_pos);
if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD)
mavlink_msg_local_position_ned_send(l->mavlink->get_chan(),
@ -407,7 +407,7 @@ MavlinkOrbListener::l_attitude_setpoint(const struct listener *l)
struct vehicle_attitude_setpoint_s att_sp;
/* copy local position data into local buffer */
orb_copy(ORB_ID(vehicle_attitude_setpoint), _mavlink->get_subs()->spa_sub, &att_sp);
orb_copy(ORB_ID(vehicle_attitude_setpoint), l->mavlink->get_subs()->spa_sub, &att_sp);
if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD)
mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(l->mavlink->get_chan(),
@ -464,13 +464,13 @@ MavlinkOrbListener::l_actuator_outputs(const struct listener *l)
act_outputs.output[7]);
/* only send in HIL mode and only send first group for HIL */
if (l->listener->mavlink_hil_enabled && l->listener->armed.armed && ids[l->arg] == ORB_ID(actuator_outputs_0)) {
if (l->mavlink->hil_enabled() && l->listener->armed.armed && ids[l->arg] == ORB_ID(actuator_outputs_0)) {
/* translate the current syste state to mavlink state and mode */
uint8_t mavlink_state = 0;
uint8_t mavlink_base_mode = 0;
uint32_t mavlink_custom_mode = 0;
get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
l->mavlink->get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
/* HIL message as per MAVLink spec */
@ -616,9 +616,9 @@ MavlinkOrbListener::l_optical_flow(const struct listener *l)
void
MavlinkOrbListener::l_home(const struct listener *l)
{
orb_copy(ORB_ID(home_position), l->mavlink->get_subs()->home_sub, &home);
orb_copy(ORB_ID(home_position), l->mavlink->get_subs()->home_sub, &l->listener->home);
mavlink_msg_gps_global_origin_send(l->mavlink->get_chan(), (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.alt)*1e3f);
mavlink_msg_gps_global_origin_send(l->mavlink->get_chan(), (int32_t)(l->listener->home.lat*1e7d), (int32_t)(l->listener->home.lon*1e7d), (int32_t)(l->listener->home.alt)*1e3f);
}
void

1
src/modules/mavlink/mavlink_orb_listener.h
View File

@ -170,6 +170,7 @@ private:
struct actuator_controls_s actuators_0;
struct vehicle_attitude_s att;
struct airspeed_s airspeed;
struct home_position_s home;
int status_sub;
int rc_sub;

5
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -106,7 +106,8 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
telemetry_status_pub(-1),
lat0(0),
lon0(0),
alt0(0)
alt0(0),
thread_should_exit(false)
{
}
@ -818,7 +819,7 @@ MavlinkReceiver::receive_thread(void *arg)
/* if read failed, this loop won't execute */
for (ssize_t i = 0; i < nread; i++) {
if (mavlink_parse_char(chan, buf[i], &msg, &status)) {
if (mavlink_parse_char(_mavlink->get_chan(), buf[i], &msg, &status)) {
/* handle generic messages and commands */
handle_message(&msg);

2
src/modules/mavlink/mavlink_receiver.h
View File

@ -102,7 +102,7 @@ public:
private:
bool _task_should_exit; /**< if true, sensor task should exit */
bool thread_should_exit; /**< if true, sensor task should exit */
perf_counter_t _loop_perf; /**< loop performance counter */

3
src/modules/mavlink/module.mk
View File

@ -39,7 +39,6 @@ MODULE_COMMAND = mavlink
SRCS += mavlink_main.cpp \
mavlink.c \
mavlink_receiver.cpp \
mavlink_orb_listener.cpp \
waypoints.cpp
mavlink_orb_listener.cpp
INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink