zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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

this generalises the gcs object usage
master
Andrew Tridgell 11 years ago
parent
ac24ff0b1e
commit
2fa5e6310d
6 changed files with 82 additions and 60 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 4
      ArduPlane/ArduPlane.pde
  2. 1
      ArduPlane/GCS.h
  3. 100
      ArduPlane/GCS_Mavlink.pde
  4. 5
      ArduPlane/Parameters.h
  5. 13
      ArduPlane/Parameters.pde
  6. 19
      ArduPlane/system.pde

4
ArduPlane/ArduPlane.pde
Unescape View File

@ -289,8 +289,8 @@ static bool guided_throttle_passthru; @@ -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
Unescape View File

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

100
ArduPlane/GCS_Mavlink.pde
Unescape View File

@ -542,7 +542,7 @@ static void NOINLINE send_current_waypoint(mavlink_channel_t chan) @@ -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, @@ -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 @@ -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) @@ -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,12 +2137,13 @@ mission_failed: @@ -2137,12 +2137,13 @@ 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
if (comm_get_txspace(out_chan) > ((uint16_t)msg->len) + MAVLINK_NUM_NON_PAYLOAD_BYTES) {
_mavlink_resend_uart(out_chan, msg);
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;
@ -2173,7 +2174,7 @@ GCS_MAVLINK::_count_parameters() @@ -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() @@ -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() { @@ -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() @@ -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) @@ -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) @@ -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) @@ -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, ...) @@ -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 >=
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 >=
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_1, vg);
airspeed.log_mavlink_send((mavlink_channel_t)i, vg);
}
}
}

5
ArduPlane/Parameters.h
Unescape View File

@ -101,13 +101,14 @@ public: @@ -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
Unescape View File

@ -9,6 +9,7 @@ @@ -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 = { @@ -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
Unescape View File

@ -105,7 +105,7 @@ static void init_ardupilot() @@ -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() @@ -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() @@ -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() @@ -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)

Write Preview
Loading…
Cancel
Save