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

AntennaTracker: use gcs[] array instead of old idiom gcs0, gcs3

This commit is contained in:
Mike McCauley 2014-03-05 16:38:22 +10:00 committed by Andrew Tridgell
parent 05646904de
commit 7c2bba169e
6 changed files with 78 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
Tools/AntennaTracker/AntennaTracker.pde
View File

@ -183,8 +183,8 @@ static RC_Channel channel_pitch(CH_2);
////////////////////////////////////////////////////////////////////////////////
// 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];
////////////////////////////////////////////////////////////////////////////////
// 3D Location vectors

61
Tools/AntennaTracker/GCS_Mavlink.pde
View File

@ -212,15 +212,12 @@ static void NOINLINE send_hwstatus(mavlink_channel_t chan)
static void NOINLINE send_waypoint_request(mavlink_channel_t chan)
{
if (chan == MAVLINK_COMM_0)
gcs0.queued_waypoint_send();
else
gcs3.queued_waypoint_send();
gcs[chan-MAVLINK_COMM_0].queued_waypoint_send();
}
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,
@ -294,11 +291,7 @@ 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:
@ -403,7 +396,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);
@ -1042,7 +1035,7 @@ mission_failed:
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;
@ -1070,9 +1063,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);
}
}
}
@ -1081,9 +1075,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();
}
}
}
@ -1092,17 +1087,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);
@ -1117,18 +1114,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);
}
}
}

13
Tools/AntennaTracker/Parameters.h
View File

@ -51,12 +51,12 @@ public:
k_param_format_version = 0,
k_param_software_type,
k_param_gcs0 = 100, // stream rates for port0
k_param_gcs3, // stream rates for port3
k_param_gcs0 = 100, // 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_serial1_baud,
k_param_imu,
k_param_compass_enabled,
k_param_compass,
@ -67,6 +67,8 @@ public:
k_param_sitl,
k_param_pidPitch2Srv,
k_param_pidYaw2Srv,
k_param_gcs2, // stream rates for uartD
k_param_serial2_baud,
k_param_channel_yaw = 200,
k_param_channel_pitch,
@ -87,7 +89,10 @@ public:
AP_Int16 sysid_this_mav;
AP_Int16 sysid_my_gcs;
AP_Int8 serial0_baud;
AP_Int8 serial3_baud;
AP_Int8 serial1_baud;
#if MAVLINK_COMM_NUM_BUFFERS > 2
AP_Int8 serial2_baud;
#endif
AP_Int8 compass_enabled;

30
Tools/AntennaTracker/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),
@ -30,19 +31,26 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Param: SERIAL0_BAUD
// @DisplayName: USB Console Baud Rate
// @Description: The baud rate used on the main uart
// @Description: The baud rate used on the USB console
// @Values: 1:1200,2:2400,4:4800,9:9600,19:19200,38:38400,57:57600,111:111100,115:115200
// @User: Standard
GSCALAR(serial0_baud, "SERIAL0_BAUD", SERIAL0_BAUD/1000),
// @Param: SERIAL3_BAUD
// @Param: SERIAL1_BAUD
// @DisplayName: Telemetry Baud Rate
// @Description: The baud rate used on the telemetry port
// @Description: The baud rate used on the first telemetry port
// @Values: 1:1200,2:2400,4:4800,9:9600,19:19200,38:38400,57:57600,111:111100,115:115200
// @User: Standard
GSCALAR(serial3_baud, "SERIAL3_BAUD", SERIAL3_BAUD/1000),
GSCALAR(serial1_baud, "SERIAL1_BAUD", SERIAL1_BAUD/1000),
#if MAVLINK_COMM_NUM_BUFFERS > 2
// @Param: SERIAL2_BAUD
// @DisplayName: Telemetry Baud Rate
// @Description: The baud rate used on the second telemetry port
// @Values: 1:1200,2:2400,4:4800,9:9600,19:19200,38:38400,57:57600,111:111100,115:115200
// @User: Standard
GSCALAR(serial2_baud, "SERIAL2_BAUD", SERIAL2_BAUD/1000),
#endif
// @Param: MAG_ENABLE
// @DisplayName: Enable Compass
@ -67,11 +75,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

12
Tools/AntennaTracker/config.h
View File

@ -69,15 +69,21 @@
#ifndef SERIAL0_BAUD
# define SERIAL0_BAUD 115200
#endif
#ifndef SERIAL3_BAUD
# define SERIAL3_BAUD 57600
#ifndef SERIAL1_BAUD
# define SERIAL1_BAUD 57600
#endif
#ifndef SERIAL2_BAUD
# define SERIAL2_BAUD 57600
#endif
#ifndef SERIAL_BUFSIZE
# define SERIAL_BUFSIZE 512
#endif
#ifndef SERIAL1_BUFSIZE
# define SERIAL1_BUFSIZE 256
#endif
#ifndef SERIAL2_BUFSIZE
# define SERIAL2_BUFSIZE 256
#endif

11
Tools/AntennaTracker/system.pde
View File

@ -21,15 +21,16 @@ static void init_tracker()
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);
// 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);
hal.uartC->begin(map_baudrate(g.serial1_baud, SERIAL1_BAUD),
128, SERIAL1_BUFSIZE);
gcs[1].init(hal.uartC);
mavlink_system.sysid = g.sysid_this_mav;
@ -117,7 +118,7 @@ static uint32_t map_baudrate(int8_t rate, uint32_t default_baud)
case 111: return 111100;
case 115: return 115200;
}
cliSerial->println_P(PSTR("Invalid SERIAL3_BAUD"));
cliSerial->println_P(PSTR("Invalid baudrate"));
return default_baud;
}