Merge branch 'master' into mavlink-ftp

This commit is contained in:
px4dev
2014-06-07 10:54:17 -07:00
271 changed files with 9868 additions and 12186 deletions
+64 -72
View File
@@ -105,7 +105,8 @@ static struct file_operations fops;
*/
extern "C" __EXPORT int mavlink_main(int argc, char *argv[]);
static uint64_t last_write_times[6] = {0};
static uint64_t last_write_success_times[6] = {0};
static uint64_t last_write_try_times[6] = {0};
/*
* Internal function to send the bytes through the right serial port
@@ -149,10 +150,7 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
instance = Mavlink::get_instance(6);
break;
#endif
}
/* no valid instance, bail */
if (!instance) {
default:
return;
}
@@ -169,29 +167,40 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
if (instance->get_flow_control_enabled()
&& ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) {
if (buf_free == 0) {
if (last_write_times[(unsigned)channel] != 0 &&
hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500 * 1000UL) {
warnx("DISABLING HARDWARE FLOW CONTROL");
instance->enable_flow_control(false);
}
} else {
/* apparently there is space left, although we might be
* partially overflooding the buffer already */
last_write_times[(unsigned)channel] = hrt_absolute_time();
/* Disable hardware flow control:
* if no successful write since a defined time
* and if the last try was not the last successful write
*/
if (last_write_try_times[(unsigned)channel] != 0 &&
hrt_elapsed_time(&last_write_success_times[(unsigned)channel]) > 500 * 1000UL &&
last_write_success_times[(unsigned)channel] !=
last_write_try_times[(unsigned)channel])
{
warnx("DISABLING HARDWARE FLOW CONTROL");
instance->enable_flow_control(false);
}
}
/* If the wait until transmit flag is on, only transmit after we've received messages.
Otherwise, transmit all the time. */
if (instance->should_transmit()) {
ssize_t ret = write(uart, ch, desired);
last_write_try_times[(unsigned)channel] = hrt_absolute_time();
/* check if there is space in the buffer, let it overflow else */
if (!ioctl(uart, FIONWRITE, (unsigned long)&buf_free)) {
if (buf_free < desired) {
/* we don't want to send anything just in half, so return */
return;
}
}
ssize_t ret = write(uart, ch, desired);
if (ret != desired) {
// XXX do something here, but change to using FIONWRITE and OS buf size for detection
warnx("TX FAIL");
} else {
last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel];
}
}
@@ -202,9 +211,9 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
static void usage(void);
Mavlink::Mavlink() :
next(nullptr),
_device_name(DEFAULT_DEVICE_NAME),
_task_should_exit(false),
next(nullptr),
_mavlink_fd(-1),
_task_running(false),
_hil_enabled(false),
@@ -216,6 +225,8 @@ Mavlink::Mavlink() :
_subscriptions(nullptr),
_streams(nullptr),
_mission_pub(-1),
_mode(MAVLINK_MODE_NORMAL),
_total_counter(0),
_verbose(false),
_forwarding_on(false),
_passing_on(false),
@@ -225,7 +236,6 @@ Mavlink::Mavlink() :
_subscribe_to_stream_rate(0.0f),
_flow_control_enabled(true),
_message_buffer({}),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "mavlink"))
{
@@ -410,7 +420,7 @@ Mavlink::instance_exists(const char *device_name, Mavlink *self)
void
Mavlink::forward_message(mavlink_message_t *msg, Mavlink *self)
{
Mavlink *inst;
LL_FOREACH(_mavlink_instances, inst) {
if (inst != self) {
@@ -727,9 +737,9 @@ int Mavlink::mavlink_pm_send_param(param_t param)
if (param == PARAM_INVALID) { return 1; }
/* buffers for param transmission */
static char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN];
char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN];
float val_buf;
static mavlink_message_t tx_msg;
mavlink_message_t tx_msg;
/* query parameter type */
param_type_t type = param_type(param);
@@ -780,9 +790,14 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav
{
switch (msg->msgid) {
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
/* Start sending parameters */
mavlink_pm_start_queued_send();
mavlink_missionlib_send_gcs_string("[mavlink pm] sending list");
mavlink_param_request_list_t req;
mavlink_msg_param_request_list_decode(msg, &req);
if (req.target_system == mavlink_system.sysid &&
(req.target_component == mavlink_system.compid || req.target_component == MAV_COMP_ID_ALL)) {
/* Start sending parameters */
mavlink_pm_start_queued_send();
mavlink_missionlib_send_gcs_string("[mavlink pm] sending list");
}
} break;
case MAVLINK_MSG_ID_PARAM_SET: {
@@ -881,11 +896,12 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item
switch (mavlink_mission_item->command) {
case MAV_CMD_NAV_TAKEOFF:
mission_item->pitch_min = mavlink_mission_item->param2;
mission_item->pitch_min = mavlink_mission_item->param1;
break;
default:
mission_item->acceptance_radius = mavlink_mission_item->param2;
mission_item->time_inside = mavlink_mission_item->param1;
break;
}
@@ -894,7 +910,6 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item
mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */
mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
mission_item->time_inside = mavlink_mission_item->param1;
mission_item->autocontinue = mavlink_mission_item->autocontinue;
// mission_item->index = mavlink_mission_item->seq;
mission_item->origin = ORIGIN_MAVLINK;
@@ -913,11 +928,12 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_
switch (mission_item->nav_cmd) {
case NAV_CMD_TAKEOFF:
mavlink_mission_item->param2 = mission_item->pitch_min;
mavlink_mission_item->param1 = mission_item->pitch_min;
break;
default:
mavlink_mission_item->param2 = mission_item->acceptance_radius;
mavlink_mission_item->param1 = mission_item->time_inside;
break;
}
@@ -928,7 +944,6 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_
mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F;
mavlink_mission_item->param3 = mission_item->loiter_radius * (float)mission_item->loiter_direction;
mavlink_mission_item->command = mission_item->nav_cmd;
mavlink_mission_item->param1 = mission_item->time_inside;
mavlink_mission_item->autocontinue = mission_item->autocontinue;
// mavlink_mission_item->seq = mission_item->index;
@@ -944,6 +959,7 @@ void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state)
state->current_partner_compid = 0;
state->timestamp_lastaction = 0;
state->timestamp_last_send_setpoint = 0;
state->timestamp_last_send_request = 0;
state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT;
state->current_dataman_id = 0;
}
@@ -1044,6 +1060,7 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t
} else {
mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR);
mavlink_missionlib_send_gcs_string("#audio: Unable to read from micro SD");
if (_verbose) { warnx("ERROR: could not read WP%u", seq); }
}
@@ -1059,6 +1076,7 @@ void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, u
wpr.seq = seq;
mavlink_msg_mission_request_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpr);
mavlink_missionlib_send_message(&msg);
_wpm->timestamp_last_send_request = hrt_absolute_time();
if (_verbose) { warnx("Sent waypoint request %u to ID %u", wpr.seq, wpr.target_system); }
@@ -1101,6 +1119,10 @@ void Mavlink::mavlink_waypoint_eventloop(uint64_t now)
_wpm->current_state = MAVLINK_WPM_STATE_IDLE;
_wpm->current_partner_sysid = 0;
_wpm->current_partner_compid = 0;
} else if (now - _wpm->timestamp_last_send_request > 500000 && _wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) {
/* try to get WP again after short timeout */
mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id);
}
}
@@ -1163,11 +1185,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (_verbose) { warnx("IGN WP CURR CMD: Busy"); }
}
} else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
if (_verbose) { warnx("REJ. WP CMD: target id mismatch"); }
}
break;
@@ -1200,11 +1217,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (_verbose) { warnx("IGN REQUEST LIST: Busy"); }
}
} else {
mavlink_missionlib_send_gcs_string("REJ. REQUEST LIST: target id mismatch");
if (_verbose) { warnx("REJ. REQUEST LIST: target id mismatch"); }
}
break;
@@ -1293,12 +1305,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.", msg->sysid, _wpm->current_partner_sysid); }
} else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
if (_verbose) { warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); }
}
}
@@ -1357,12 +1363,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (_verbose) { warnx("IGN MISSION_COUNT CMD: Busy"); }
}
} else {
mavlink_missionlib_send_gcs_string("REJ. WP COUNT CMD: target id mismatch");
if (_verbose) { warnx("IGNORED WAYPOINT COUNT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); }
}
}
break;
@@ -1430,6 +1430,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (dm_write(dm_next, wp.seq, DM_PERSIST_IN_FLIGHT_RESET, &mission_item, len) != len) {
mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR);
mavlink_missionlib_send_gcs_string("#audio: Unable to write on micro SD");
_wpm->current_state = MAVLINK_WPM_STATE_IDLE;
break;
}
@@ -1461,11 +1462,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
} else {
mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id);
}
} else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
if (_verbose) { warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); }
}
break;
@@ -1501,13 +1497,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (_verbose) { warnx("IGN WP CLEAR CMD: Busy"); }
}
} else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && _wpm->current_state != MAVLINK_WPM_STATE_IDLE) {
mavlink_missionlib_send_gcs_string("REJ. WP CLERR CMD: target id mismatch");
if (_verbose) { warnx("IGNORED WAYPOINT CLEAR COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); }
}
break;
@@ -1523,6 +1512,8 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
void
Mavlink::mavlink_missionlib_send_message(mavlink_message_t *msg)
{
uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN];
uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg);
mavlink_send_uart_bytes(_channel, missionlib_msg_buf, len);
@@ -1535,6 +1526,8 @@ Mavlink::mavlink_missionlib_send_gcs_string(const char *string)
{
const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN;
mavlink_statustext_t statustext;
statustext.severity = MAV_SEVERITY_INFO;
int i = 0;
while (i < len - 1) {
@@ -2017,14 +2010,14 @@ Mavlink::task_main(int argc, char *argv[])
if (_subscribe_to_stream != nullptr) {
if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) {
if (_subscribe_to_stream_rate > 0.0f) {
warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name, _subscribe_to_stream_rate);
warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name, (double)_subscribe_to_stream_rate);
} else {
warnx("stream %s on device %s disabled", _subscribe_to_stream, _device_name);
}
} else {
warnx("stream %s not found", _subscribe_to_stream, _device_name);
warnx("stream %s on device %s not found", _subscribe_to_stream, _device_name);
}
delete _subscribe_to_stream;
@@ -2191,7 +2184,7 @@ Mavlink::start(int argc, char *argv[])
task_spawn_cmd(buf,
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
2048,
1950,
(main_t)&Mavlink::start_helper,
(const char **)argv);
@@ -2230,7 +2223,6 @@ Mavlink::stream(int argc, char *argv[])
const char *device_name = DEFAULT_DEVICE_NAME;
float rate = -1.0f;
const char *stream_name = nullptr;
int ch;
argc -= 2;
argv += 2;
@@ -2267,7 +2259,7 @@ Mavlink::stream(int argc, char *argv[])
i++;
}
if (!err_flag && rate >= 0.0 && stream_name != nullptr) {
if (!err_flag && rate >= 0.0f && stream_name != nullptr) {
Mavlink *inst = get_instance_for_device(device_name);
if (inst != nullptr) {
+2 -4
View File
@@ -93,6 +93,7 @@ struct mavlink_wpm_storage {
uint8_t current_partner_compid;
uint64_t timestamp_lastaction;
uint64_t timestamp_last_send_setpoint;
uint64_t timestamp_last_send_request;
uint32_t timeout;
int current_dataman_id;
};
@@ -221,8 +222,6 @@ private:
int _mavlink_fd;
bool _task_running;
perf_counter_t _loop_perf; /**< loop performance counter */
/* 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) */
@@ -237,7 +236,6 @@ private:
orb_advert_t _mission_pub;
struct mission_s mission;
uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN];
MAVLINK_MODE _mode;
uint8_t _mavlink_wpm_comp_id;
@@ -283,7 +281,7 @@ private:
pthread_mutex_t _message_buffer_mutex;
perf_counter_t _loop_perf; /**< loop performance counter */
/**
* Send one parameter.
+27 -22
View File
@@ -124,18 +124,22 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
} else if (status->main_state == MAIN_STATE_SEATBELT) {
} else if (status->main_state == MAIN_STATE_ALTCTL) {
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL;
} else if (status->main_state == MAIN_STATE_EASY) {
} else if (status->main_state == MAIN_STATE_POSCTL) {
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL;
} else if (status->main_state == MAIN_STATE_AUTO) {
*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
} else if (status->main_state == MAIN_STATE_ACRO) {
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO;
}
} else {
@@ -270,7 +274,7 @@ protected:
status->onboard_control_sensors_health,
status->load * 1000.0f,
status->battery_voltage * 1000.0f,
status->battery_current * 1000.0f,
status->battery_current * 100.0f,
status->battery_remaining * 100.0f,
status->drop_rate_comm,
status->errors_comm,
@@ -1138,10 +1142,10 @@ protected:
if (manual_sub->update(t)) {
mavlink_msg_manual_control_send(_channel,
mavlink_system.sysid,
manual->roll * 1000,
manual->pitch * 1000,
manual->yaw * 1000,
manual->throttle * 1000,
manual->x * 1000,
manual->y * 1000,
manual->z * 1000,
manual->r * 1000,
0);
}
}
@@ -1339,22 +1343,23 @@ protected:
void send(const hrt_abstime t)
{
(void)range_sub->update(t);
if (range_sub->update(t)) {
uint8_t type;
uint8_t type;
switch (range->type) {
case RANGE_FINDER_TYPE_LASER:
type = MAV_DISTANCE_SENSOR_LASER;
break;
switch (range->type) {
case RANGE_FINDER_TYPE_LASER:
type = MAV_DISTANCE_SENSOR_LASER;
break;
}
uint8_t id = 0;
uint8_t orientation = 0;
uint8_t covariance = 20;
mavlink_msg_distance_sensor_send(_channel, range->timestamp / 1000, type, id, orientation,
range->minimum_distance*100, range->maximum_distance*100, range->distance*100, covariance);
}
uint8_t id = 0;
uint8_t orientation = 0;
uint8_t covariance = 20;
mavlink_msg_distance_sensor_send(_channel, range->timestamp / 1000, type, id, orientation,
range->minimum_distance*100, range->maximum_distance*100, range->distance*100, covariance);
}
};
@@ -63,7 +63,7 @@ MavlinkOrbSubscription::~MavlinkOrbSubscription()
free(_data);
}
const orb_id_t
orb_id_t
MavlinkOrbSubscription::get_topic()
{
return _topic;
@@ -63,7 +63,7 @@ public:
*/
bool is_published();
void *get_data();
const orb_id_t get_topic();
orb_id_t get_topic();
private:
const orb_id_t _topic; /*< topic metadata */
+14 -6
View File
@@ -198,7 +198,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
}
}
/* If we've received a valid message, mark the flag indicating so.
This is used in the '-w' command-line flag. */
_mavlink->set_has_received_messages(true);
@@ -224,6 +224,12 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
_mavlink->_task_should_exit = true;
} else {
if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) {
warnx("ignoring CMD spoofed with same SYS/COMP ID");
return;
}
struct vehicle_command_s vcmd;
memset(&vcmd, 0, sizeof(vcmd));
@@ -439,10 +445,10 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
memset(&manual, 0, sizeof(manual));
manual.timestamp = hrt_absolute_time();
manual.pitch = man.x / 1000.0f;
manual.roll = man.y / 1000.0f;
manual.yaw = man.r / 1000.0f;
manual.throttle = man.z / 1000.0f;
manual.x = man.x / 1000.0f;
manual.y = man.y / 1000.0f;
manual.r = man.r / 1000.0f;
manual.z = man.z / 1000.0f;
if (_manual_pub < 0) {
_manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual);
@@ -933,6 +939,8 @@ void *MavlinkReceiver::start_helper(void *context)
rcv->receive_thread(NULL);
delete rcv;
return nullptr;
}
pthread_t
@@ -950,7 +958,7 @@ MavlinkReceiver::receive_start(Mavlink *parent)
param.sched_priority = SCHED_PRIORITY_MAX - 40;
(void)pthread_attr_setschedparam(&receiveloop_attr, &param);
pthread_attr_setstacksize(&receiveloop_attr, 3000);
pthread_attr_setstacksize(&receiveloop_attr, 2900);
pthread_t thread;
pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void *)parent);
+4
View File
@@ -81,5 +81,9 @@ MavlinkStream::update(const hrt_abstime t)
/* interval expired, send message */
send(t);
_last_sent = (t / _interval) * _interval;
return 0;
}
return -1;
}
+5 -1
View File
@@ -63,9 +63,13 @@ public:
MavlinkStream *next;
MavlinkStream();
~MavlinkStream();
virtual ~MavlinkStream();
void set_interval(const unsigned int interval);
void set_channel(mavlink_channel_t channel);
/**
* @return 0 if updated / sent, -1 if unchanged
*/
int update(const hrt_abstime t);
virtual MavlinkStream *new_instance() = 0;
virtual void subscribe(Mavlink *mavlink) = 0;
+2
View File
@@ -49,3 +49,5 @@ SRCS += mavlink_main.cpp \
INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
MAXOPTIMIZATION = -Os
MODULE_STACKSIZE = 1024