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

Merge master with the exception of ubx.cpp and ubx.h

Browse Source
This commit is contained in:
Kynos
2014-06-15 12:30:06 +02:00
parent 7724e3d889 251760679a
commit 137564b4a6
19 changed files with 520 additions and 279 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_main.cpp
+41 -57
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
@@ -166,38 +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()) {
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 (desired > buf_free) {
desired = 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) {
warnx("TX FAIL");
} else {
last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel];
}
}
@@ -222,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),
@@ -785,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: {
@@ -891,6 +901,7 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item
default:
mission_item->acceptance_radius = mavlink_mission_item->param2;
mission_item->time_inside = mavlink_mission_item->param1;
break;
}
@@ -899,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;
@@ -918,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;
}
@@ -933,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;
@@ -949,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;
}
@@ -1049,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); }
}
@@ -1064,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); }
@@ -1106,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);
}
}
@@ -1168,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;
@@ -1205,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;
@@ -1298,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"); }
}
}
@@ -1362,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;
@@ -1435,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;
}
@@ -1466,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;
@@ -1506,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;
src/modules/mavlink/mavlink_main.h
+1
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;
};
src/modules/mavlink/mavlink_messages.cpp
+9 -9
View File
@@ -274,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,
@@ -938,14 +938,14 @@ protected:
void send(const hrt_abstime t)
{
if (pos_sp_triplet_sub->update(t)) {
mavlink_msg_global_position_setpoint_int_send(_channel,
MAV_FRAME_GLOBAL,
(int32_t)(pos_sp_triplet->current.lat * 1e7),
(int32_t)(pos_sp_triplet->current.lon * 1e7),
(int32_t)(pos_sp_triplet->current.alt * 1000),
(int16_t)(pos_sp_triplet->current.yaw * M_RAD_TO_DEG_F * 100.0f));
}
/* always send this message, even if it has not been updated */
pos_sp_triplet_sub->update(t);
mavlink_msg_global_position_setpoint_int_send(_channel,
MAV_FRAME_GLOBAL,
(int32_t)(pos_sp_triplet->current.lat * 1e7),
(int32_t)(pos_sp_triplet->current.lon * 1e7),
(int32_t)(pos_sp_triplet->current.alt * 1000),
(int16_t)(pos_sp_triplet->current.yaw * M_RAD_TO_DEG_F * 100.0f));
}
};