mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
Refactor Iridiumsbd driver
- Reorder functions so that they have the same order in the .h and .cpp file - Update documentation for functions - Make variables and some functions private
This commit is contained in:
acfloria
Beat Küng
parent
634697946a
commit
b613ca051a
@ -62,6 +62,10 @@ IridiumSBD::IridiumSBD()
|
||||
{
|
||||
}
|
||||
|
||||
///////////////////////////////////////////////////////////////////////
|
||||
// public functions //
|
||||
///////////////////////////////////////////////////////////////////////
|
||||
|
||||
int IridiumSBD::start(int argc, char *argv[])
|
||||
{
|
||||
PX4_INFO("starting");
|
||||
@ -151,6 +155,35 @@ void IridiumSBD::test(int argc, char *argv[])
|
||||
instance->schedule_test();
|
||||
}
|
||||
|
||||
int IridiumSBD::ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||
{
|
||||
switch (cmd) {
|
||||
case FIONREAD: {
|
||||
int count = rx_msg_end_idx - rx_msg_read_idx;
|
||||
*(int *)arg = count;
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
case FIONWRITE: {
|
||||
int count = SATCOM_TX_BUF_LEN - tx_buf_write_idx;
|
||||
*(int *)arg = count;
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
default: {
|
||||
|
||||
/* see if the parent class can make any use of it */
|
||||
return CDev::ioctl(filp, cmd, arg);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
///////////////////////////////////////////////////////////////////////
|
||||
// private functions //
|
||||
///////////////////////////////////////////////////////////////////////
|
||||
|
||||
void IridiumSBD::main_loop_helper(int argc, char *argv[])
|
||||
{
|
||||
// start the main loop and stay in it
|
||||
@ -469,6 +502,73 @@ void IridiumSBD::test_loop(void)
|
||||
}
|
||||
}
|
||||
|
||||
void IridiumSBD::start_csq(void)
|
||||
{
|
||||
VERBOSE_INFO("UPDATING SIGNAL QUALITY");
|
||||
|
||||
if (!is_modem_ready()) {
|
||||
VERBOSE_INFO("UPDATE SIGNAL QUALITY: MODEM NOT READY!");
|
||||
return;
|
||||
}
|
||||
|
||||
write_at("AT+CSQ");
|
||||
new_state = SATCOM_STATE_CSQ;
|
||||
}
|
||||
|
||||
void IridiumSBD::start_sbd_session(void)
|
||||
{
|
||||
VERBOSE_INFO("STARTING SBD SESSION");
|
||||
|
||||
if (!is_modem_ready()) {
|
||||
VERBOSE_INFO("SBD SESSION: MODEM NOT READY!");
|
||||
return;
|
||||
}
|
||||
|
||||
if (ring_pending) {
|
||||
write_at("AT+SBDIXA");
|
||||
} else {
|
||||
write_at("AT+SBDIX");
|
||||
}
|
||||
|
||||
new_state = SATCOM_STATE_SBDSESSION;
|
||||
session_start_time = hrt_absolute_time();
|
||||
}
|
||||
|
||||
void IridiumSBD::start_test(void)
|
||||
{
|
||||
int res = read_at_command();
|
||||
|
||||
if (res != SATCOM_RESULT_NA) {
|
||||
PX4_WARN("SOMETHING WAS IN BUFFER");
|
||||
printf("TEST RESULT: %d, LENGTH %d\nDATA:\n%s\nRAW DATA:\n", res, rx_command_len, rx_command_buf);
|
||||
|
||||
for (int i = 0; i < rx_command_len; i++) {
|
||||
printf("%d ", rx_command_buf[i]);
|
||||
}
|
||||
|
||||
printf("\n");
|
||||
}
|
||||
|
||||
if (!is_modem_ready()) {
|
||||
PX4_WARN("MODEM NOT READY!");
|
||||
return;
|
||||
}
|
||||
|
||||
if (strlen(test_command) != 0) {
|
||||
if ((strstr(test_command, "AT") != nullptr) || (strstr(test_command, "at") != nullptr)) {
|
||||
PX4_INFO("TEST %s", test_command);
|
||||
write_at(test_command);
|
||||
new_state = SATCOM_STATE_TEST;
|
||||
} else {
|
||||
PX4_WARN("The test command does not include AT or at: %s, ignoring it.", test_command);
|
||||
new_state = SATCOM_STATE_STANDBY;
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_INFO("TEST DONE");
|
||||
}
|
||||
}
|
||||
|
||||
ssize_t IridiumSBD::write(struct file *filp, const char *buffer, size_t buflen)
|
||||
{
|
||||
VERBOSE_INFO("WRITE: LEN %d, TX WRITTEN: %d", buflen, tx_buf_write_idx);
|
||||
@ -511,46 +611,6 @@ ssize_t IridiumSBD::read(struct file *filp, char *buffer, size_t buflen)
|
||||
}
|
||||
}
|
||||
|
||||
int IridiumSBD::ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||
{
|
||||
switch (cmd) {
|
||||
case FIONREAD: {
|
||||
int count = rx_msg_end_idx - rx_msg_read_idx;
|
||||
*(int *)arg = count;
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
case FIONWRITE: {
|
||||
int count = SATCOM_TX_BUF_LEN - tx_buf_write_idx;
|
||||
*(int *)arg = count;
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
default: {
|
||||
|
||||
/* see if the parent class can make any use of it */
|
||||
return CDev::ioctl(filp, cmd, arg);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pollevent_t IridiumSBD::poll_state(struct file *filp)
|
||||
{
|
||||
pollevent_t pollstate = 0;
|
||||
|
||||
if (rx_msg_read_idx < rx_msg_end_idx) {
|
||||
pollstate |= POLLIN;
|
||||
}
|
||||
|
||||
if (SATCOM_TX_BUF_LEN - tx_buf_write_idx > 0) {
|
||||
pollstate |= POLLOUT;
|
||||
}
|
||||
|
||||
return pollstate;
|
||||
}
|
||||
|
||||
void IridiumSBD::write_tx_buf()
|
||||
{
|
||||
if (!is_modem_ready()) {
|
||||
@ -651,121 +711,6 @@ void IridiumSBD::read_rx_buf(void)
|
||||
VERBOSE_INFO("READ SBD: SUCCESS, LEN: %d", data_len);
|
||||
}
|
||||
|
||||
bool IridiumSBD::clear_mo_buffer()
|
||||
{
|
||||
write_at("AT+SBDD0");
|
||||
|
||||
if (read_at_command() != SATCOM_RESULT_OK || rx_command_buf[0] != '0') {
|
||||
VERBOSE_INFO("CLEAR MO BUFFER: ERROR");
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void IridiumSBD::start_csq(void)
|
||||
{
|
||||
VERBOSE_INFO("UPDATING SIGNAL QUALITY");
|
||||
|
||||
if (!is_modem_ready()) {
|
||||
VERBOSE_INFO("UPDATE SIGNAL QUALITY: MODEM NOT READY!");
|
||||
return;
|
||||
}
|
||||
|
||||
write_at("AT+CSQ");
|
||||
new_state = SATCOM_STATE_CSQ;
|
||||
}
|
||||
|
||||
void IridiumSBD::start_sbd_session(void)
|
||||
{
|
||||
VERBOSE_INFO("STARTING SBD SESSION");
|
||||
|
||||
if (!is_modem_ready()) {
|
||||
VERBOSE_INFO("SBD SESSION: MODEM NOT READY!");
|
||||
return;
|
||||
}
|
||||
|
||||
if (ring_pending) {
|
||||
write_at("AT+SBDIXA");
|
||||
|
||||
} else {
|
||||
write_at("AT+SBDIX");
|
||||
}
|
||||
|
||||
new_state = SATCOM_STATE_SBDSESSION;
|
||||
session_start_time = hrt_absolute_time();
|
||||
}
|
||||
|
||||
void IridiumSBD::start_test(void)
|
||||
{
|
||||
int res = read_at_command();
|
||||
|
||||
if (res != SATCOM_RESULT_NA) {
|
||||
PX4_WARN("SOMETHING WAS IN BUFFER");
|
||||
printf("TEST RESULT: %d, LENGTH %d\nDATA:\n%s\nRAW DATA:\n", res, rx_command_len, rx_command_buf);
|
||||
|
||||
for (int i = 0; i < rx_command_len; i++) {
|
||||
printf("%d ", rx_command_buf[i]);
|
||||
}
|
||||
|
||||
printf("\n");
|
||||
}
|
||||
|
||||
if (!is_modem_ready()) {
|
||||
PX4_WARN("MODEM NOT READY!");
|
||||
return;
|
||||
}
|
||||
|
||||
if (strlen(test_command) != 0) {
|
||||
if ((strstr(test_command, "AT") != nullptr) || (strstr(test_command, "at") != nullptr)) {
|
||||
PX4_INFO("TEST %s", test_command);
|
||||
write_at(test_command);
|
||||
new_state = SATCOM_STATE_TEST;
|
||||
|
||||
} else {
|
||||
PX4_WARN("The test command does not include AT or at: %s, ignoring it.", test_command);
|
||||
new_state = SATCOM_STATE_STANDBY;
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_INFO("TEST DONE");
|
||||
}
|
||||
}
|
||||
|
||||
satcom_uart_status IridiumSBD::open_uart(char *uart_name)
|
||||
{
|
||||
VERBOSE_INFO("opening Iridium SBD modem UART: %s", uart_name);
|
||||
|
||||
uart_fd = ::open(uart_name, O_RDWR | O_BINARY);
|
||||
|
||||
if (uart_fd < 0) {
|
||||
PX4_ERR("IridiumSBD: UART open failed!");
|
||||
return SATCOM_UART_OPEN_FAIL;
|
||||
}
|
||||
|
||||
// set the UART speed to 115200
|
||||
struct termios uart_config;
|
||||
tcgetattr(uart_fd, &uart_config);
|
||||
cfsetspeed(&uart_config, 115200);
|
||||
tcsetattr(uart_fd, TCSANOW, &uart_config);
|
||||
|
||||
VERBOSE_INFO("UART opened");
|
||||
|
||||
return SATCOM_UART_OK;
|
||||
}
|
||||
|
||||
bool IridiumSBD::is_modem_ready(void)
|
||||
{
|
||||
write_at("AT");
|
||||
|
||||
if (read_at_command() == SATCOM_RESULT_OK) {
|
||||
return true;
|
||||
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
void IridiumSBD::write_at(const char *command)
|
||||
{
|
||||
VERBOSE_INFO("WRITING AT COMMAND: %s", command);
|
||||
@ -855,8 +800,68 @@ void IridiumSBD::schedule_test(void)
|
||||
test_pending = true;
|
||||
}
|
||||
|
||||
void IridiumSBD::publish_telemetry_status()
|
||||
bool IridiumSBD::clear_mo_buffer()
|
||||
{
|
||||
write_at("AT+SBDD0");
|
||||
|
||||
if (read_at_command() != SATCOM_RESULT_OK || rx_command_buf[0] != '0') {
|
||||
VERBOSE_INFO("CLEAR MO BUFFER: ERROR");
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
satcom_uart_status IridiumSBD::open_uart(char *uart_name)
|
||||
{
|
||||
VERBOSE_INFO("opening Iridium SBD modem UART: %s", uart_name);
|
||||
|
||||
uart_fd = ::open(uart_name, O_RDWR | O_BINARY);
|
||||
|
||||
if (uart_fd < 0) {
|
||||
VERBOSE_INFO("UART open failed!");
|
||||
return SATCOM_UART_OPEN_FAIL;
|
||||
}
|
||||
|
||||
// set the UART speed to 115200
|
||||
struct termios uart_config;
|
||||
tcgetattr(uart_fd, &uart_config);
|
||||
cfsetspeed(&uart_config, 115200);
|
||||
tcsetattr(uart_fd, TCSANOW, &uart_config);
|
||||
|
||||
VERBOSE_INFO("UART opened");
|
||||
|
||||
return SATCOM_UART_OK;
|
||||
}
|
||||
|
||||
bool IridiumSBD::is_modem_ready(void)
|
||||
{
|
||||
write_at("AT");
|
||||
|
||||
if (read_at_command() == SATCOM_RESULT_OK) {
|
||||
return true;
|
||||
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
pollevent_t IridiumSBD::poll_state(struct file *filp)
|
||||
{
|
||||
pollevent_t pollstate = 0;
|
||||
|
||||
if (rx_msg_read_idx < rx_msg_end_idx) {
|
||||
pollstate |= POLLIN;
|
||||
}
|
||||
|
||||
if (SATCOM_TX_BUF_LEN - tx_buf_write_idx > 0) {
|
||||
pollstate |= POLLOUT;
|
||||
}
|
||||
|
||||
return pollstate;
|
||||
}
|
||||
|
||||
void IridiumSBD::publish_telemetry_status() {
|
||||
// publish telemetry status for logger
|
||||
struct telemetry_status_s tstatus = {};
|
||||
|
||||
|
||||
@ -91,6 +91,165 @@ extern "C" __EXPORT int iridiumsbd_main(int argc, char *argv[]);
|
||||
class IridiumSBD : public device::CDev
|
||||
{
|
||||
public:
|
||||
/*
|
||||
* Constructor
|
||||
*/
|
||||
IridiumSBD();
|
||||
|
||||
/*
|
||||
* Start the driver
|
||||
*/
|
||||
static int start(int argc, char *argv[]);
|
||||
|
||||
/*
|
||||
* Stop the driver
|
||||
*/
|
||||
static int stop();
|
||||
|
||||
/*
|
||||
* Display driver status
|
||||
*/
|
||||
static void status();
|
||||
|
||||
/*
|
||||
* Run a driver test based on the input
|
||||
* - `s`: Send a test string
|
||||
* - `read`: Start a sbd read session
|
||||
* - else: Is assumed to be a valid AT command and written to the modem
|
||||
*/
|
||||
static void test(int argc, char *argv[]);
|
||||
|
||||
/*
|
||||
* Passes everything to CDev
|
||||
*/
|
||||
int ioctl(struct file *filp, int cmd, unsigned long arg);
|
||||
|
||||
private:
|
||||
/*
|
||||
* Entry point of the task, has to be a static function
|
||||
*/
|
||||
static void main_loop_helper(int argc, char *argv[]);
|
||||
|
||||
/*
|
||||
* Main driver loop
|
||||
*/
|
||||
void main_loop(int argc, char *argv[]);
|
||||
|
||||
/*
|
||||
* Loop executed while in SATCOM_STATE_STANDBY
|
||||
*
|
||||
* Changes to SATCOM_STATE_TEST, SATCOM_STATE_SBDSESSION if required.
|
||||
* Periodically changes to SATCOM_STATE_CSQ for a signal quality check.
|
||||
*/
|
||||
void standby_loop(void);
|
||||
|
||||
/*
|
||||
* Loop executed while in SATCOM_STATE_CSQ
|
||||
*
|
||||
* Changes to SATCOM_STATE_STANDBY after finished signal quality check.
|
||||
*/
|
||||
void csq_loop(void);
|
||||
|
||||
/*
|
||||
* Loop executed while in SATCOM_STATE_SBDSESSION
|
||||
*
|
||||
* Changes to SATCOM_STATE_STANDBY after finished sbd session.
|
||||
*/
|
||||
void sbdsession_loop(void);
|
||||
|
||||
/*
|
||||
* Loop executed while in SATCOM_STATE_TEST
|
||||
*
|
||||
* Changes to SATCOM_STATE_STANDBY after finished test.
|
||||
*/
|
||||
void test_loop(void);
|
||||
|
||||
/*
|
||||
* Get the network signal strength
|
||||
*/
|
||||
void start_csq(void);
|
||||
|
||||
/*
|
||||
* Start a sbd session
|
||||
*/
|
||||
void start_sbd_session(void);
|
||||
|
||||
/*
|
||||
* Check if the test command is valid. If that is the case
|
||||
* change to SATCOM_STATE_TEST
|
||||
*/
|
||||
void start_test(void);
|
||||
|
||||
/*
|
||||
* Use to send mavlink messages directly
|
||||
*/
|
||||
ssize_t write(struct file *filp, const char *buffer, size_t buflen);
|
||||
|
||||
/*
|
||||
* Use to read received mavlink messages directly
|
||||
*/
|
||||
ssize_t read(struct file *filp, char *buffer, size_t buflen);
|
||||
|
||||
/*
|
||||
* Write the tx buffer to the modem
|
||||
*/
|
||||
void write_tx_buf();
|
||||
|
||||
/*
|
||||
* Read binary data from the modem
|
||||
*/
|
||||
void read_rx_buf();
|
||||
|
||||
/*
|
||||
* Send a AT command to the modem
|
||||
*/
|
||||
void write_at(const char *command);
|
||||
|
||||
/*
|
||||
* Read return from modem and store it in rx_command_buf
|
||||
*/
|
||||
satcom_result_code read_at_command(int16_t timeout = 100);
|
||||
|
||||
/*
|
||||
* Read return from modem and store it in rx_msg_buf
|
||||
*/
|
||||
satcom_result_code read_at_msg(int16_t timeout = 100);
|
||||
|
||||
/*
|
||||
* Read the return from the modem
|
||||
*/
|
||||
satcom_result_code read_at(uint8_t *rx_buf, int *rx_len, int16_t timeout = 100);
|
||||
|
||||
/*
|
||||
* Schedule a test (set test_pending to true)
|
||||
*/
|
||||
void schedule_test(void);
|
||||
|
||||
/*
|
||||
* Clear the MO message buffer
|
||||
*/
|
||||
bool clear_mo_buffer();
|
||||
|
||||
/*
|
||||
* Open and configure the given UART port
|
||||
*/
|
||||
satcom_uart_status open_uart(char *uart_name);
|
||||
|
||||
/*
|
||||
* Checks if the modem responds to the "AT" command
|
||||
*/
|
||||
bool is_modem_ready(void);
|
||||
|
||||
/*
|
||||
* Get the poll state
|
||||
*/
|
||||
pollevent_t poll_state(struct file *filp);
|
||||
|
||||
/*
|
||||
* Publish the up to date telemetry status
|
||||
*/
|
||||
void publish_telemetry_status(void);
|
||||
|
||||
static IridiumSBD *instance;
|
||||
static int task_handle;
|
||||
bool task_should_exit = false;
|
||||
@ -134,156 +293,4 @@ public:
|
||||
|
||||
pthread_mutex_t tx_buf_mutex = pthread_mutex_t();
|
||||
bool verbose = false;
|
||||
|
||||
/*
|
||||
* Constructor
|
||||
*/
|
||||
IridiumSBD();
|
||||
|
||||
/*
|
||||
* Start the driver
|
||||
*/
|
||||
static int start(int argc, char *argv[]);
|
||||
|
||||
/*
|
||||
* Stop the driver
|
||||
*/
|
||||
static int stop();
|
||||
|
||||
/*
|
||||
* Display driver status
|
||||
*/
|
||||
static void status();
|
||||
|
||||
/*
|
||||
* Run a basic driver test
|
||||
*/
|
||||
static void test(int argc, char *argv[]);
|
||||
|
||||
/*
|
||||
* Entry point of the task, has to be a static function
|
||||
*/
|
||||
static void main_loop_helper(int argc, char *argv[]);
|
||||
|
||||
/*
|
||||
* Main driver loop
|
||||
*/
|
||||
void main_loop(int argc, char *argv[]);
|
||||
|
||||
/*
|
||||
* Use to send mavlink messages directly
|
||||
*/
|
||||
ssize_t write(struct file *filp, const char *buffer, size_t buflen);
|
||||
|
||||
/*
|
||||
* Use to read received mavlink messages directly
|
||||
*/
|
||||
ssize_t read(struct file *filp, char *buffer, size_t buflen);
|
||||
|
||||
/*
|
||||
* Passes everything to CDev
|
||||
*/
|
||||
int ioctl(struct file *filp, int cmd, unsigned long arg);
|
||||
|
||||
/*
|
||||
* Get the poll state
|
||||
*/
|
||||
pollevent_t poll_state(struct file *filp);
|
||||
|
||||
/*
|
||||
* Open and configure the given UART port
|
||||
*/
|
||||
satcom_uart_status open_uart(char *uart_name);
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
void write_tx_buf();
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
void read_rx_buf();
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
bool clear_mo_buffer();
|
||||
|
||||
/*
|
||||
* Perform a SBD session, sending the message from the MO buffer (if previously written)
|
||||
* and retrieving a MT message from the Iridium system (if there is one waiting)
|
||||
* This will also update the registration needed for SBD RING
|
||||
*/
|
||||
int sbd_session(void);
|
||||
|
||||
/*
|
||||
* Get the network signal strength
|
||||
*/
|
||||
void start_csq(void);
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
satcom_result_code read_at_command(int16_t timeout = 100);
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
satcom_result_code read_at_msg(int16_t timeout = 100);
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
satcom_result_code read_at(uint8_t *rx_buf, int *rx_len, int16_t timeout = 100);
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
void schedule_test(void);
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
void standby_loop(void);
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
void csq_loop(void);
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
void sbdsession_loop(void);
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
void test_loop(void);
|
||||
|
||||
/*
|
||||
* TEST
|
||||
*/
|
||||
void start_test(void);
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
void start_sbd_session(void);
|
||||
|
||||
/*
|
||||
* Checks if the modem responds to the "AT" command
|
||||
*/
|
||||
bool is_modem_ready(void);
|
||||
|
||||
/*
|
||||
* Send a AT command to the modem
|
||||
*/
|
||||
void write_at(const char *command);
|
||||
|
||||
/*
|
||||
* Publish the up to date telemetry status
|
||||
*/
|
||||
void publish_telemetry_status();
|
||||
};
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user