added SERIAL3_BAUD parameter

this is the real baud rate divided by 1000. So 57600 is set as 57

git-svn-id: https://arducopter.googlecode.com/svn/trunk@2997 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
tridge60@gmail.com 2011-08-01 11:39:17 +00:00
parent 366b20bb9f
commit 9fa60889dd
2 changed files with 31 additions and 10 deletions

View File

@ -63,6 +63,7 @@ public:
k_param_streamrates_port3,
k_param_sysid_this_mav,
k_param_sysid_my_gcs,
k_param_serial3_baud,
//
// 140: Sensor parameters
@ -179,6 +180,7 @@ public:
//
AP_Int16 sysid_this_mav;
AP_Int16 sysid_my_gcs;
AP_Int8 serial3_baud;
// Crosstrack navigation
@ -282,6 +284,7 @@ public:
sysid_this_mav (MAV_SYSTEM_ID, k_param_sysid_this_mav, PSTR("SYSID_THISMAV")),
sysid_my_gcs (255, k_param_sysid_my_gcs, PSTR("SYSID_MYGCS")),
serial3_baud (SERIAL3_BAUD/1000, k_param_serial3_baud, PSTR("SERIAL3_BAUD")),
//crosstrack_gain (XTRACK_GAIN * 100, k_param_crosstrack_gain, PSTR("XTRK_GAIN_SC")),
crosstrack_entry_angle (XTRACK_ENTRY_ANGLE * 100, k_param_crosstrack_entry_angle, PSTR("XTRK_ANGLE_CD")),

View File

@ -73,16 +73,6 @@ static void init_ardupilot()
Serial1.begin(38400, 128, 16);
#endif
// Telemetry port.
//
// Not used if telemetry is going to the console.
//
// XXX for unidirectional protocols, we could (should) minimize
// the receive buffer, and the transmit buffer could also be
// shrunk for protocols that don't send large messages.
//
Serial3.begin(SERIAL3_BAUD, 128, 128);
Serial.printf_P(PSTR("\n\nInit ACM"
"\n\nRAM: %lu\n"),
freeRAM());
@ -150,6 +140,17 @@ static void init_ardupilot()
AP_Var::load_all();
}
// Telemetry port.
//
// Not used if telemetry is going to the console.
//
// XXX for unidirectional protocols, we could (should) minimize
// the receive buffer, and the transmit buffer could also be
// shrunk for protocols that don't send large messages.
//
Serial3.begin(map_baudrate(g.serial3_baud,SERIAL3_BAUD), 128, 128);
#ifdef RADIO_OVERRIDE_DEFAULTS
{
int16_t rc_override[8] = RADIO_OVERRIDE_DEFAULTS;
@ -529,3 +530,20 @@ check_startup_for_CLI()
#endif
/*
map from a 8 bit EEPROM baud rate to a real baud rate
*/
static uint32_t map_baudrate(int8_t rate, uint32_t default_baud)
{
switch (rate) {
case 9: return 9600;
case 19: return 19200;
case 38: return 38400;
case 57: return 57600;
case 111: return 111100;
case 115: return 115200;
}
Serial.println_P(PSTR("Invalid SERIAL3_BAUD"));
return default_baud;
}