aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Plane: added 3rd telemetry stream on platforms that have it

Browse Source 
this generalises the gcs object usage
This commit is contained in:
Andrew Tridgell 2013-11-22 19:18:19 +11:00
parent ac24ff0b1e
commit 2fa5e6310d
6 changed files with 82 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduPlane/ArduPlane.pde
View File

@ -289,8 +289,8 @@ static bool guided_throttle_passthru;
////////////////////////////////////////////////////////////////////////////////
// GCS selection
////////////////////////////////////////////////////////////////////////////////
static GCS_MAVLINK gcs0;
static GCS_MAVLINK gcs3;
static const uint8_t num_gcs = MAVLINK_COMM_NUM_BUFFERS;
static GCS_MAVLINK gcs[MAVLINK_COMM_NUM_BUFFERS];
// selected navigation controller
static AP_Navigation *nav_controller = &L1_controller;

1
ArduPlane/GCS.h
View File

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

96
ArduPlane/GCS_Mavlink.pde
View File

@ -542,7 +542,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,
@ -670,20 +670,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:
@ -792,7 +784,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);
@ -905,9 +897,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();
@ -2137,14 +2137,15 @@ mission_failed:
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
@ -2173,7 +2174,7 @@ GCS_MAVLINK::_count_parameters()
void
GCS_MAVLINK::queued_param_send()
{
if (_queued_parameter == NULL) {
if (!initialised || _queued_parameter == NULL) {
return;
}
@ -2223,7 +2224,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,
@ -2245,7 +2247,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;
@ -2275,9 +2277,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);
}
}
}
@ -2286,9 +2289,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();
}
}
}
@ -2297,17 +2301,19 @@ 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);
@ -2322,18 +2328,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);
#if LOGGING_ENABLED == ENABLED
DataFlash.Log_Write_Message(gcs0.pending_status.text);
DataFlash.Log_Write_Message(gcs[0].pending_status.text);
#endif
gcs3.pending_status = gcs0.pending_status;
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);
}
}
}
@ -2342,14 +2350,10 @@ void gcs_send_text_fmt(const prog_char_t *fmt, ...)
*/
static void gcs_send_airspeed_calibration(const Vector3f &vg)
{
if (comm_get_txspace(MAVLINK_COMM_0) - MAVLINK_NUM_NON_PAYLOAD_BYTES >=
for (uint8_t i=0; i<num_gcs; i++) {
if (comm_get_txspace((mavlink_channel_t)i) - MAVLINK_NUM_NON_PAYLOAD_BYTES >=
MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN) {
airspeed.log_mavlink_send(MAVLINK_COMM_0, vg);
}
if (gcs3.initialised) {
if (comm_get_txspace(MAVLINK_COMM_1) - MAVLINK_NUM_NON_PAYLOAD_BYTES >=
MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN) {
airspeed.log_mavlink_send(MAVLINK_COMM_1, vg);
airspeed.log_mavlink_send((mavlink_channel_t)i, vg);
}
}
}

5
ArduPlane/Parameters.h
View File

@ -101,13 +101,14 @@ 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_serial3_baud,
k_param_telem_delay,
k_param_serial0_baud,
k_param_gcs2, // stream rates for uartD
// 120: Fly-by-wire control
//

13
ArduPlane/Parameters.pde
View File

@ -9,6 +9,7 @@
#define ASCALAR(v, name, def) { aparm.v.vtype, name, Parameters::k_param_ ## v, &aparm.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", 0),
@ -854,11 +855,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: INS_
// @Path: ../libraries/AP_InertialSensor/AP_InertialSensor.cpp

19
ArduPlane/system.pde
View File

@ -105,7 +105,7 @@ static void init_ardupilot()
barometer.init();
// 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
hal.scheduler->register_delay_callback(mavlink_delay_cb, 5);
@ -118,7 +118,13 @@ static void init_ardupilot()
// we have a 2nd serial port for telemetry
hal.uartC->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD),
128, SERIAL2_BUFSIZE);
gcs3.init(hal.uartC);
gcs[1].init(hal.uartC);
if (num_gcs > 2) {
hal.uartD->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD),
128, SERIAL2_BUFSIZE);
gcs[2].init(hal.uartD);
}
mavlink_system.sysid = g.sysid_this_mav;
@ -130,7 +136,9 @@ static void init_ardupilot()
} else if (DataFlash.NeedErase()) {
gcs_send_text_P(SEVERITY_LOW, PSTR("ERASING LOGS"));
do_erase_logs();
gcs0.reset_cli_timeout();
for (uint8_t i=0; i<num_gcs; i++) {
gcs[i].reset_cli_timeout();
}
}
if (g.log_bitmask != 0) {
start_logging();
@ -183,9 +191,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();
if (g.log_bitmask & MASK_LOG_CMD)