AP_SerialManager: add SBUS1 serial output support

This commit is contained in:
Mark Whitehorn 2017-08-21 14:31:51 -06:00 committed by Andrew Tridgell
parent cca251c135
commit 3587d7f253
2 changed files with 15 additions and 1 deletions

View File

@ -225,7 +225,6 @@ void AP_SerialManager::init()
AP_SERIALMANAGER_ULANDING_BUFSIZE_RX, AP_SERIALMANAGER_ULANDING_BUFSIZE_RX,
AP_SERIALMANAGER_ULANDING_BUFSIZE_TX); AP_SERIALMANAGER_ULANDING_BUFSIZE_TX);
break; break;
case SerialProtocol_Volz: case SerialProtocol_Volz:
// Note baudrate is hardcoded to 115200 // Note baudrate is hardcoded to 115200
state[i].baud = AP_SERIALMANAGER_VOLZ_BAUD; // update baud param in case user looks at it state[i].baud = AP_SERIALMANAGER_VOLZ_BAUD; // update baud param in case user looks at it
@ -233,6 +232,15 @@ void AP_SerialManager::init()
AP_SERIALMANAGER_VOLZ_BUFSIZE_RX, AP_SERIALMANAGER_VOLZ_BUFSIZE_RX,
AP_SERIALMANAGER_VOLZ_BUFSIZE_TX); AP_SERIALMANAGER_VOLZ_BUFSIZE_TX);
break; break;
case SerialProtocol_Sbus1:
state[i].baud = AP_SERIALMANAGER_SBUS1_BAUD / 1000; // update baud param in case user looks at it
state[i].uart->begin(map_baudrate(state[i].baud),
AP_SERIALMANAGER_SBUS1_BUFSIZE_RX,
AP_SERIALMANAGER_SBUS1_BUFSIZE_TX);
state[i].uart->configure_parity(2); // enable even parity
state[i].uart->set_stop_bits(2);
state[i].uart->set_unbuffered_writes(true);
break;
} }
} }
} }

View File

@ -73,6 +73,11 @@
#define AP_SERIALMANAGER_VOLZ_BUFSIZE_RX 128 #define AP_SERIALMANAGER_VOLZ_BUFSIZE_RX 128
#define AP_SERIALMANAGER_VOLZ_BUFSIZE_TX 128 #define AP_SERIALMANAGER_VOLZ_BUFSIZE_TX 128
// SBUS servo outputs
#define AP_SERIALMANAGER_SBUS1_BAUD 100000
#define AP_SERIALMANAGER_SBUS1_BUFSIZE_RX 16
#define AP_SERIALMANAGER_SBUS1_BUFSIZE_TX 32
class AP_SerialManager { class AP_SerialManager {
public: public:
@ -93,6 +98,7 @@ public:
SerialProtocol_Aerotenna_uLanding = 12, // Ulanding support SerialProtocol_Aerotenna_uLanding = 12, // Ulanding support
SerialProtocol_Beacon = 13, SerialProtocol_Beacon = 13,
SerialProtocol_Volz = 14, // Volz servo protocol SerialProtocol_Volz = 14, // Volz servo protocol
SerialProtocol_Sbus1 = 15
}; };
// get singleton instance // get singleton instance