Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Rover: added support for uartD

This commit is contained in:
Andrew Tridgell 2013-11-23 21:57:26 +11:00
parent 2c840547fa
commit 14a0f8f46d
6 changed files with 80 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
APMrover2/APMrover2.pde
View File

@ -270,8 +270,8 @@ SITL sitl;
// GCS selection
////////////////////////////////////////////////////////////////////////////////
//
GCS_MAVLINK gcs0;
GCS_MAVLINK gcs3;
static const uint8_t num_gcs = MAVLINK_COMM_NUM_BUFFERS;
static GCS_MAVLINK gcs[MAVLINK_COMM_NUM_BUFFERS];
// a pin for reading the receiver RSSI voltage. The scaling by 0.25
// is to take the 0 to 1024 range down to an 8 bit range for MAVLink

1
APMrover2/GCS.h
View File

@ -41,7 +41,6 @@ public:
///
void init(AP_HAL::UARTDriver *port) {
_port = port;
initialised = true;
}
/// Update GCS state.

94
APMrover2/GCS_Mavlink.pde
View File

@ -452,7 +452,7 @@ static void NOINLINE send_current_waypoint(mavlink_channel_t chan)
static void NOINLINE send_statustext(mavlink_channel_t chan)
{
mavlink_statustext_t *s = (chan == MAVLINK_COMM_0?&gcs0.pending_status:&gcs3.pending_status);
mavlink_statustext_t *s = &gcs[chan-MAVLINK_COMM_0].pending_status;
mavlink_msg_statustext_send(
chan,
s->severity,
@ -575,20 +575,12 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id,
case MSG_NEXT_PARAM:
CHECK_PAYLOAD_SIZE(PARAM_VALUE);
if (chan == MAVLINK_COMM_0) {
gcs0.queued_param_send();
} else if (gcs3.initialised) {
gcs3.queued_param_send();
}
gcs[chan-MAVLINK_COMM_0].queued_param_send();
break;
case MSG_NEXT_WAYPOINT:
CHECK_PAYLOAD_SIZE(MISSION_REQUEST);
if (chan == MAVLINK_COMM_0) {
gcs0.queued_waypoint_send();
} else if (gcs3.initialised) {
gcs3.queued_waypoint_send();
}
gcs[chan-MAVLINK_COMM_0].queued_waypoint_send();
break;
case MSG_STATUSTEXT:
@ -628,7 +620,7 @@ static struct mavlink_queue {
enum ap_message deferred_messages[MAX_DEFERRED_MESSAGES];
uint8_t next_deferred_message;
uint8_t num_deferred_messages;
} mavlink_queue[2];
} mavlink_queue[MAVLINK_COMM_NUM_BUFFERS];
// send a message using mavlink
static void mavlink_send_message(mavlink_channel_t chan, enum ap_message id, uint16_t packet_drops)
@ -690,7 +682,7 @@ void mavlink_send_text(mavlink_channel_t chan, gcs_severity severity, const char
if (severity == SEVERITY_LOW) {
// send via the deferred queuing system
mavlink_statustext_t *s = (chan == MAVLINK_COMM_0?&gcs0.pending_status:&gcs3.pending_status);
mavlink_statustext_t *s = &gcs[chan-MAVLINK_COMM_0].pending_status;
s->severity = (uint8_t)severity;
strncpy((char *)s->text, str, sizeof(s->text));
mavlink_send_message(chan, MSG_STATUSTEXT, 0);
@ -803,9 +795,17 @@ GCS_MAVLINK::init(AP_HAL::UARTDriver *port)
if (port == (AP_HAL::BetterStream*)hal.uartA) {
mavlink_comm_0_port = port;
chan = MAVLINK_COMM_0;
}else{
initialised = true;
} else if (port == (AP_HAL::BetterStream*)hal.uartC) {
mavlink_comm_1_port = port;
chan = MAVLINK_COMM_1;
initialised = true;
#if MAVLINK_COMM_NUM_BUFFERS > 2
} else if (port == (AP_HAL::BetterStream*)hal.uartD) {
mavlink_comm_2_port = port;
chan = MAVLINK_COMM_2;
initialised = true;
#endif
}
_queued_parameter = NULL;
reset_cli_timeout();
@ -1691,7 +1691,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
} else if (var_type == AP_PARAM_INT32) {
if (packet.param_value < 0) rounding_addition = -rounding_addition;
float v = packet.param_value+rounding_addition;
v = constrain_float(v, -2147483648.0f, 2147483647.0f);
v = constrain_float(v, -2147483648.0, 2147483647.0);
((AP_Int32 *)vp)->set_and_save(v);
} else if (var_type == AP_PARAM_INT16) {
if (packet.param_value < 0) rounding_addition = -rounding_addition;
@ -1857,14 +1857,15 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
default:
// forward unknown messages to the other link if there is one
if ((chan == MAVLINK_COMM_1 && gcs0.initialised) ||
(chan == MAVLINK_COMM_0 && gcs3.initialised)) {
mavlink_channel_t out_chan = (mavlink_channel_t)(((uint8_t)chan)^1);
// only forward if it would fit in our transmit buffer
for (uint8_t i=0; i<num_gcs; i++) {
if (gcs[i].initialised && i != (uint8_t)chan) {
mavlink_channel_t out_chan = (mavlink_channel_t)i;
// only forward if it would fit in the transmit buffer
if (comm_get_txspace(out_chan) > ((uint16_t)msg->len) + MAVLINK_NUM_NON_PAYLOAD_BYTES) {
_mavlink_resend_uart(out_chan, msg);
}
}
}
break;
} // end switch
@ -1893,7 +1894,7 @@ GCS_MAVLINK::_count_parameters()
void
GCS_MAVLINK::queued_param_send()
{
if (_queued_parameter == NULL) {
if (!initialised || _queued_parameter == NULL) {
return;
}
@ -1943,7 +1944,8 @@ GCS_MAVLINK::queued_param_send()
void
GCS_MAVLINK::queued_waypoint_send()
{
if (waypoint_receiving &&
if (initialised &&
waypoint_receiving &&
waypoint_request_i <= waypoint_request_last) {
mavlink_msg_mission_request_send(
chan,
@ -1965,7 +1967,7 @@ void GCS_MAVLINK::reset_cli_timeout() {
static void mavlink_delay_cb()
{
static uint32_t last_1hz, last_50hz, last_5s;
if (!gcs0.initialised) return;
if (!gcs[0].initialised) return;
in_mavlink_delay = true;
@ -1995,9 +1997,10 @@ static void mavlink_delay_cb()
*/
static void gcs_send_message(enum ap_message id)
{
gcs0.send_message(id);
if (gcs3.initialised) {
gcs3.send_message(id);
for (uint8_t i=0; i<num_gcs; i++) {
if (gcs[i].initialised) {
gcs[i].send_message(id);
}
}
}
@ -2006,9 +2009,10 @@ static void gcs_send_message(enum ap_message id)
*/
static void gcs_data_stream_send(void)
{
gcs0.data_stream_send();
if (gcs3.initialised) {
gcs3.data_stream_send();
for (uint8_t i=0; i<num_gcs; i++) {
if (gcs[i].initialised) {
gcs[i].data_stream_send();
}
}
}
@ -2017,19 +2021,23 @@ static void gcs_data_stream_send(void)
*/
static void gcs_update(void)
{
gcs0.update();
if (gcs3.initialised) {
gcs3.update();
for (uint8_t i=0; i<num_gcs; i++) {
if (gcs[i].initialised) {
gcs[i].update();
}
}
}
static void gcs_send_text_P(gcs_severity severity, const prog_char_t *str)
{
gcs0.send_text_P(severity, str);
if (gcs3.initialised) {
gcs3.send_text_P(severity, str);
for (uint8_t i=0; i<num_gcs; i++) {
if (gcs[i].initialised) {
gcs[i].send_text_P(severity, str);
}
}
#if LOGGING_ENABLED == ENABLED
DataFlash.Log_Write_Message_P(str);
#endif
}
/*
@ -2040,16 +2048,20 @@ static void gcs_send_text_P(gcs_severity severity, const prog_char_t *str)
void gcs_send_text_fmt(const prog_char_t *fmt, ...)
{
va_list arg_list;
gcs0.pending_status.severity = (uint8_t)SEVERITY_LOW;
gcs[0].pending_status.severity = (uint8_t)SEVERITY_LOW;
va_start(arg_list, fmt);
hal.util->vsnprintf_P((char *)gcs0.pending_status.text,
sizeof(gcs0.pending_status.text), fmt, arg_list);
hal.util->vsnprintf_P((char *)gcs[0].pending_status.text,
sizeof(gcs[0].pending_status.text), fmt, arg_list);
va_end(arg_list);
DataFlash.Log_Write_Message(gcs0.pending_status.text);
gcs3.pending_status = gcs0.pending_status;
#if LOGGING_ENABLED == ENABLED
DataFlash.Log_Write_Message(gcs[0].pending_status.text);
#endif
mavlink_send_message(MAVLINK_COMM_0, MSG_STATUSTEXT, 0);
if (gcs3.initialised) {
mavlink_send_message(MAVLINK_COMM_1, MSG_STATUSTEXT, 0);
for (uint8_t i=1; i<num_gcs; i++) {
if (gcs[i].initialised) {
gcs[i].pending_status = gcs[0].pending_status;
mavlink_send_message((mavlink_channel_t)i, MSG_STATUSTEXT, 0);
}
}
}

5
APMrover2/Parameters.h
View File

@ -43,14 +43,15 @@ public:
// 110: Telemetry control
//
k_param_gcs0 = 110, // stream rates for port0
k_param_gcs3, // stream rates for port3
k_param_gcs0 = 110, // stream rates for uartA
k_param_gcs1, // stream rates for uartC
k_param_sysid_this_mav,
k_param_sysid_my_gcs,
k_param_serial0_baud,
k_param_serial3_baud,
k_param_telem_delay,
k_param_skip_gyro_cal,
k_param_gcs2, // stream rates for uartD
//
// 130: Sensor parameters

13
APMrover2/Parameters.pde
View File

@ -7,6 +7,7 @@
#define GSCALAR(v, name, def) { g.v.vtype, name, Parameters::k_param_ ## v, &g.v, {def_value:def} }
#define GGROUP(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &g.v, {group_info:class::var_info} }
#define GOBJECT(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &v, {group_info:class::var_info} }
#define GOBJECTN(v, pname, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## pname, &v, {group_info : class::var_info} }
const AP_Param::Info var_info[] PROGMEM = {
GSCALAR(format_version, "FORMAT_VERSION", 1),
@ -416,11 +417,17 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Group: SR0_
// @Path: GCS_Mavlink.pde
GOBJECT(gcs0, "SR0_", GCS_MAVLINK),
GOBJECTN(gcs[0], gcs0, "SR0_", GCS_MAVLINK),
// @Group: SR3_
// @Group: SR1_
// @Path: GCS_Mavlink.pde
GOBJECT(gcs3, "SR3_", GCS_MAVLINK),
GOBJECTN(gcs[1], gcs1, "SR1_", GCS_MAVLINK),
#if MAVLINK_COMM_NUM_BUFFERS > 2
// @Group: SR2_
// @Path: GCS_Mavlink.pde
GOBJECTN(gcs[2], gcs2, "SR2_", GCS_MAVLINK),
#endif
// @Group: NAVL1_
// @Path: ../libraries/AP_L1_Control/AP_L1_Control.cpp

15
APMrover2/system.pde
View File

@ -114,7 +114,7 @@ static void init_ardupilot()
g.num_resets.set_and_save(g.num_resets+1);
// init the GCS
gcs0.init(hal.uartA);
gcs[0].init(hal.uartA);
// Register mavlink_delay_cb, which will run anytime you have
// more than 5ms remaining in your call to hal.scheduler->delay
@ -127,7 +127,13 @@ static void init_ardupilot()
// we have a 2nd serial port for telemetry
hal.uartC->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128);
gcs3.init(hal.uartC);
gcs[1].init(hal.uartC);
// we may have a 3rd serial port for telemetry
if (hal.uartD != NULL) {
hal.uartD->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128);
gcs[2].init(hal.uartD);
}
mavlink_system.sysid = g.sysid_this_mav;
@ -192,9 +198,12 @@ static void init_ardupilot()
//
const prog_char_t *msg = PSTR("\nPress ENTER 3 times to start interactive setup\n");
cliSerial->println_P(msg);
if (gcs3.initialised) {
if (gcs[1].initialised) {
hal.uartC->println_P(msg);
}
if (num_gcs > 2 && gcs[2].initialised) {
hal.uartD->println_P(msg);
}
startup_ground();