Here’s an improved version of your message:
Hello everyone,
I’m currently facing challenges with setting up serial communication in PX4 through a custom task module. My goal right now is simply to send the number “three” over serial. I have a Cube Orange Plus connected via the TELEM2 port (ttyS1) to an FTDI USB adapter, which in turn connects to MATLAB, where I’m monitoring the serial data. Although PX4 indicates a successful write operation, MATLAB isn’t receiving any data.
I Have tried setting it up as closely to a serial driver as possible but it still doesn’t want to write though the port. I was getting errors with the buffer being too full before so now when it stops writing I clear the buffer.
I’ve attached my files below, which outline the code for the custom serial communication. Has anyone encountered a similar issue, or is there something additional I might need to configure in PX4 to get this working?
Any guidance would be greatly appreciated—thank you in advance!
Output from nsh terminal
INFO [strat_serial_task] we wrote something
INFO [strat_serial_task] we wrote something
INFO [strat_serial_task] write error Try again
ERROR [strat_serial_task] Failed to get Tx buffer status, number of iterations: 375
ERROR [strat_serial_task] Tx buffer check error: Not a typewriter
INFO [strat_serial_task] we wrote something
INFO [strat_serial_task] we wrote something
INFO [strat_serial_task] we wrote something
CMakeLists.txt
set(MODULE_NAME strat_serial_task)
Add definitions specific to PX4
add_definitions(
-DNUMST=1
-DNCSTATES=0
-DHAVESTDIO
-DMODEL_HAS_DYNAMICALLY_LOADED_SFCNS=0
-DCLASSIC_INTERFACE=0
-DALLOCATIONFCN=0
-DTID01EQ=0
-DTERMFCN=1
-DONESTEPFCN=1
-DMAT_FILE=0
-DMULTI_INSTANCE_CODE=0
-DINTEGER_CODE=0
-DMT=0
-DPX4
-DNULL=0
-DMW_PX4_NUTTX_BUILD
-DEXTMODE_DISABLETESTING
-DEXTMODE_DISABLEPRINTF
-DEXTMODE_DISABLE_ARGS_PROCESSING=1
-D__linux__
)
px4_add_module(
MODULE modules__strat_serial_task
MAIN strat_serial_task
COMPILE_FLAGS
SRCS
SCI_ST.hpp
SCI_ST.cpp
strat_st.cpp
strat_st.hpp
MODULE_CONFIG
module.yaml
COMPILE_FLAGS
-fpermissive
-Wno-narrowing
-Wno-address-of-packed-member
-Wno-cast-align
INCLUDES
${PX4_SOURCE_DIR}/src/modules/${MODULE_NAME}
)
Remove -Werror from compile options
get_target_property(MODULE_COMPILE_FLAGS modules__${MODULE_NAME} COMPILE_OPTIONS)
list(REMOVE_ITEM MODULE_COMPILE_FLAGS -Werror)
set_target_properties(modules__${MODULE_NAME} PROPERTIES COMPILE_OPTIONS "${MODULE_COMPILE_FLAGS}")
module.yaml
module_name: Strat Serial
serial_config:
- command: strat_serial_task start -d ${SERIAL_DEV}
port_config_param:
name: STRAT_ST_CONFIG
group: Communication
SCI_ST.cpp
#include "SCI_ST.hpp"
#include <stdio.h> /* Standard input/output definitions */
#include <string.h> /* String function definitions */
#include <unistd.h> /* UNIX standard function definitions */
#include <fcntl.h> /* File control definitions */
#include <errno.h> /* Error number definitions */
#include <termios.h> /* POSIX terminal control definitions */
#include <stdbool.h>
// time-out using select( )
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/select.h>
#include <sys/ioctl.h>
#include <sys/time.h>
/* Local defines */
#define NUM_MAX_SCI_DEV (10) /*a random choice - no limit on serial devices*/
#define MAX_DEV_NAME (64)
#define MICROS 1000000L
#define NANOS 1000000000L
#define DEBUG 0
static SCI_dev_t sciDev[NUM_MAX_SCI_DEV];
static uint32_T AllBaudrates[] = {50, 75, 110, 134, 150, 200, 300,
600, 1200, 1800, 2400, 4800, 9600, 19200,
38400, 57600, 115200, 230400, 460800, 500000, 576000,
921600, 1000000, 1152000, 1500000, 2000000, 2500000, 3000000};
/* Open SERIAL channel*/
static int SERIAL_open(const char* port) {
int fd;
int retries = 0;
/* O_NDELAY: disregard DCD signal line state
* O_NOCTTY: we don't want to be the controlling terminal */
fflush(stdout);
while (retries < 15) {
fd = open(port, O_RDWR | O_NOCTTY | O_NONBLOCK);
if (fd != -1) {
break;
}
// sleep a bit and retry. There seems to be a NuttX bug
// that can cause ttyACM0 to not be available immediately,
// but a small delay can fix it
usleep(50000);
retries++;
}
if (fd == -1) {
perror("SERIAL_open/open");
}
/*flush both data received but not read, and data written but not transmitted.*/
tcflush(fd, TCIOFLUSH);
return fd;
}
/* Return device ID given serial port name */
int getCurrSciDev(const char* name) {
int i;
for (i = NUM_MAX_SCI_DEV - 1; i > -1; i--) {
if ((name != NULL) && (sciDev[i].portname != NULL) &&
(strncmp(name, sciDev[i].portname, MAX_DEV_NAME) == 0)) {
break;
}
}
return i;
}
/* Allocate device*/
int Serial_alloc(const char* name) {
int i;
for (i = 0; i < NUM_MAX_SCI_DEV; i++) {
if (sciDev[i].portname == NULL) {
break;
}
}
if (i >= NUM_MAX_SCI_DEV) {
fprintf(stderr, "Cannot allocate a new device for %s: [%d]\n", name, i);
return -1;
}
sciDev[i].portname = strndup(name, MAX_DEV_NAME);
return i;
}
MW_Handle_Type MW_SCI_Open(void* SCIModule,
uint8_T isString,
uint32_T RxPin, /* Not used */
uint32_T TxPin) /*Not Used*/
{
MW_Handle_Type SCIHandle = (MW_Handle_Type)NULL;
char* port;
int currSciDev;
/* Check parameters */
if (0 == isString) {
fprintf(stderr, "Only string as SCI Module name is supported.\n");
exit(-1);
} else {
/* Initialize the SCI Module*/
port = (char*)SCIModule;
#ifdef PX4_CONNECTEDIO
/* For IO, the port name will be sent from host PC. IO wrapper does not use the length of
* port name and pass it as the last part in the packet. While reading port name in target,
* everything remaining in the buffer is read as port name. Because of this, sometimes extra
* junk bytes get added to the port name. Since all the supported serial ports are of length
* 10 (eg: /dev/ttyS1,/dev/ttyS6) manually ending the port name after 10 characters. note:
* /dev/ttyACM0 is not supported for serial blocks as it is conflicting with Connected I/O.
*/
port[10] = '\0';
#endif
#if DEBUG
printf("In MW_SCI_Open. Port = %s\n", port);
#endif
currSciDev = getCurrSciDev(port);
fprintf(stdout, "INIT: sciDevNo = %d\n", currSciDev);
if (currSciDev == -1) {
currSciDev = Serial_alloc(port);
fprintf(stdout, "ALLOC: devNo = %d\n", currSciDev);
if (currSciDev == -1) {
fprintf(stderr, "Error opening Serial bus (SERIAL_init/Alloc).\n");
exit(-1);
}
}
if (sciDev[currSciDev].fd < 0) {
sciDev[currSciDev].fd = SERIAL_open(port);
if (sciDev[currSciDev].fd < 0) {
fprintf(stderr, "Error opening Serial bus (SERIAL_init/Open).\n");
exit(-1);
}
}
SCIHandle = (MW_Handle_Type)&sciDev[currSciDev];
}
return SCIHandle;
}
/* Set SCI frame format */
MW_SCI_Status_Type MW_SCI_SetFrameFormat(MW_Handle_Type SCIModuleHandle,
uint8_T DataBitsLength,
MW_SCI_Parity_Type Parity,
MW_SCI_StopBits_Type StopBits) {
SCI_dev_t* sci;
struct termios options;
if (NULL != (void*)SCIModuleHandle) {
sci = (SCI_dev_t*)SCIModuleHandle;
if (tcgetattr(sci->fd, &options) < 0) {
perror("SERIAL_GetAttribute");
return MW_SCI_BUS_ERROR;
}
/* Set data bits*/
options.c_cflag &= ~CSIZE;
switch (DataBitsLength) {
case 5:
options.c_cflag |= CS5;
break;
case 6:
options.c_cflag |= CS6;
break;
case 7:
options.c_cflag |= CS7;
break;
case 8:
options.c_cflag |= CS8;
break;
default:
perror("SERIAL_SetFrameFormat/DataBitsLength");
return MW_SCI_BUS_ERROR;
}
#ifndef PX4_CONNECTEDIO
if (strcmp(sci->portname, "/dev/ttyACM0") == 0) {
Parity = static_cast<MW_SCI_Parity_Type>(MW_TTYACM0_PARITY);
StopBits = static_cast<MW_SCI_StopBits_Type>((2 * MW_TTYACM0_STOPBIT) + 1);
} else if (strcmp(sci->portname, "/dev/ttyS0") == 0) {
Parity = static_cast<MW_SCI_Parity_Type>(MW_TTYS0_PARITY);
StopBits = static_cast<MW_SCI_StopBits_Type>((2 * MW_TTYS0_STOPBIT) + 1);
} else if (strcmp(sci->portname, "/dev/ttyS1") == 0) {
Parity = static_cast<MW_SCI_Parity_Type>(MW_TTYS1_PARITY);
StopBits = static_cast<MW_SCI_StopBits_Type>((2 * MW_TTYS1_STOPBIT) + 1);
} else if (strcmp(sci->portname, "/dev/ttyS2") == 0) {
Parity = static_cast<MW_SCI_Parity_Type>(MW_TTYS2_PARITY);
StopBits = static_cast<MW_SCI_StopBits_Type>((2 * MW_TTYS2_STOPBIT) + 1);
} else if (strcmp(sci->portname, "/dev/ttyS3") == 0) {
Parity = static_cast<MW_SCI_Parity_Type>(MW_TTYS3_PARITY);
StopBits = static_cast<MW_SCI_StopBits_Type>((2 * MW_TTYS3_STOPBIT) + 1);
} else if (strcmp(sci->portname, "/dev/ttyS4") == 0) {
Parity = static_cast<MW_SCI_Parity_Type>(MW_TTYS4_PARITY);
StopBits = static_cast<MW_SCI_StopBits_Type>((2 * MW_TTYS4_STOPBIT) + 1);
} else if (strcmp(sci->portname, "/dev/ttyS5") == 0) {
Parity = static_cast<MW_SCI_Parity_Type>(MW_TTYS5_PARITY);
StopBits = static_cast<MW_SCI_StopBits_Type>((2 * MW_TTYS5_STOPBIT) + 1);
} else if (strcmp(sci->portname, "/dev/ttyS6") == 0) {
Parity = static_cast<MW_SCI_Parity_Type>(MW_TTYS6_PARITY);
StopBits = static_cast<MW_SCI_StopBits_Type>((2 * MW_TTYS6_STOPBIT) + 1);
} else {
perror("SERIAL_SetFrameFormat/Parity");
return MW_SCI_BUS_ERROR;
}
#endif
/* Set parity*/
switch (Parity) {
case MW_SCI_PARITY_NONE:
options.c_cflag &= ~PARENB;
break;
case MW_SCI_PARITY_EVEN:
options.c_cflag |= PARENB;
options.c_cflag &= ~PARODD;
break;
case MW_SCI_PARITY_ODD:
options.c_cflag |= PARENB;
options.c_cflag |= PARODD;
break;
default:
perror("SERIAL_SetFrameFormat/Parity");
return MW_SCI_BUS_ERROR;
}
/* Set stop bits (1 or 2)*/
switch (StopBits) {
case MW_SCI_STOPBITS_1:
options.c_cflag &= ~CSTOPB;
break;
case MW_SCI_STOPBITS_2:
options.c_cflag |= CSTOPB;
break;
default:
perror("SERIAL_SetFrameFormat/StopBits");
return MW_SCI_BUS_ERROR;
}
/* Set attributes*/
if (tcsetattr(sci->fd, TCSANOW, &options) < 0) {
perror("SERIAL_SetAttribute");
return MW_SCI_BUS_ERROR;
}
/*flush both data received but not read, and data written but not transmitted.*/
tcflush(sci->fd, TCIOFLUSH);
}
return MW_SCI_SUCCESS;
}
/* Set the SCI bus speed */
MW_SCI_Status_Type MW_SCI_SetBaudrate(MW_Handle_Type SCIModuleHandle, uint32_T Baudrate) {
SCI_dev_t* sci;
struct termios options;
speed_t optBaud;
if (NULL != (void*)SCIModuleHandle) {
sci = (SCI_dev_t*)SCIModuleHandle;
// #ifndef PX4_CONNECTEDIO
// if (strcmp(sci->portname, "/dev/ttyACM0") == 0) {
// Baudrate = AllBaudrates[MW_TTYACM0_BAUDRATE];
// } else if (strcmp(sci->portname, "/dev/ttyS0") == 0) {
// Baudrate = AllBaudrates[MW_TTYS0_BAUDRATE];
// } else if (strcmp(sci->portname, "/dev/ttyS1") == 0) {
// Baudrate = AllBaudrates[MW_TTYS1_BAUDRATE];
// } else if (strcmp(sci->portname, "/dev/ttyS2") == 0) {
// Baudrate = AllBaudrates[MW_TTYS2_BAUDRATE];
// } else if (strcmp(sci->portname, "/dev/ttyS3") == 0) {
// Baudrate = AllBaudrates[MW_TTYS3_BAUDRATE];
// } else if (strcmp(sci->portname, "/dev/ttyS4") == 0) {
// Baudrate = AllBaudrates[MW_TTYS4_BAUDRATE];
// } else if (strcmp(sci->portname, "/dev/ttyS5") == 0) {
// Baudrate = AllBaudrates[MW_TTYS5_BAUDRATE];
// } else if (strcmp(sci->portname, "/dev/ttyS6") == 0) {
// Baudrate = AllBaudrates[MW_TTYS6_BAUDRATE];
// } else if (strcmp(sci->portname, "/dev/ttyS7") == 0) {
// Baudrate = AllBaudrates[MW_TTYS7_BAUDRATE];
// } else {
// perror("SERIAL_SetBaudrate");
// return MW_SCI_BUS_ERROR;
// }
// #endif
/* Set parameters of the serial connection*/
// optBaud = Baudrate;
switch (Baudrate) {
case 50:
optBaud = B50;
break;
case 75:
optBaud = B75;
break;
case 110:
optBaud = B110;
break;
case 134:
optBaud = B134;
break;
case 150:
optBaud = B150;
break;
case 200:
optBaud = B200;
break;
case 300:
optBaud = B300;
break;
case 600:
optBaud = B600;
break;
case 1200:
optBaud = B1200;
break;
case 1800:
optBaud = B1800;
break;
case 2400:
optBaud = B2400;
break;
case 4800:
optBaud = B4800;
break;
case 9600:
optBaud = B9600;
break;
case 19200:
optBaud = B19200;
break;
case 38400:
optBaud = B38400;
break;
case 57600:
optBaud = B57600;
break;
case 115200:
optBaud = B115200;
break;
case 230400:
optBaud = B230400;
break;
case 460800:
optBaud = B460800;
break;
case 500000:
optBaud = B500000;
break;
case 576000:
optBaud = B576000;
break;
case 921600:
optBaud = B921600;
break;
case 1000000:
optBaud = B1000000;
break;
case 1152000:
optBaud = B1152000;
break;
case 1500000:
optBaud = B1500000;
break;
case 2000000:
optBaud = B2000000;
break;
case 2500000:
optBaud = B2500000;
break;
case 3000000:
optBaud = B3000000;
break;
default:
perror("SERIAL_SetBaudrate");
return MW_SCI_BUS_ERROR;
}
if (tcgetattr(sci->fd, &options) < 0) {
perror("SERIAL_GetAttribute");
return MW_SCI_BUS_ERROR;
}
/* Enable the receiver and set local mode*/
options.c_cflag |= (CLOCAL /*To not become port 'owner'*/
| CREAD); /*Allow reading of incoming data*/
options.c_iflag &= ~(IXON /*Disable Software Flow Control*/
| IXOFF | IXANY);
options.c_lflag &= ~(ICANON /*To have raw output*/
| ECHO /*Disable input character echo*/
| ECHOE /*Disable echo character erase*/
| ISIG); /*Disable SIGINTR, SIGSUSP, SIGDSUSP,
* and SIGQUIT signals*/
/* Set character read options*/
options.c_cc[VMIN] = 0;
options.c_cc[VTIME] = 0; /*This is a completely non-blocking read -*/
/* Local options. Configure for RAW input*/
options.c_oflag &= ~OPOST; /*Disable Post processing of output*/
/* Set baud rate*/
if (cfsetispeed(&options, optBaud) < 0) {
PX4_INFO("SERIAL_Set_ip_Speed");
return MW_SCI_BUS_ERROR;
}
if (cfsetospeed(&options, optBaud) < 0) {
perror("SERIAL_Set_op_Speed");
return MW_SCI_BUS_ERROR;
}
/* Set attributes*/
if (tcsetattr(sci->fd, TCSANOW, &options) < 0) {
perror("SERIAL_SetAttribute");
return MW_SCI_BUS_ERROR;
} else {
printf("Serial Port Opened. fd = %d. BaudRate = %lu\n", sci->fd, optBaud);
}
/*flush both data received but not read, and data written but not transmitted.*/
tcflush(sci->fd, TCIOFLUSH);
}
struct termios current_options;
if (tcgetattr(sci->fd, ¤t_options) == 0) {
speed_t actual_baud = cfgetispeed(¤t_options);
PX4_INFO("Current baud rate for /dev/ttyS1 is: %lu", (unsigned long)actual_baud);
} else {
PX4_ERR("Failed to get baud rate for /dev/ttyS1");
}
return MW_SCI_SUCCESS;
}
MW_SCI_Status_Type MW_SCI_ConfigureHardwareFlowControl(
MW_Handle_Type SCIModuleHandle,
MW_SCI_HardwareFlowControl_Type HardwareFlowControl,
uint32_T RtsDtrPin,
uint32_T CtsDtsPin) {
uint8_T hardwareFCEnable = 0;
SCI_dev_t* sci;
struct termios options;
if (NULL != (void*)SCIModuleHandle) {
sci = (SCI_dev_t*)SCIModuleHandle;
if (tcgetattr(sci->fd, &options) < 0) {
perror("SERIAL_GetAttribute");
return MW_SCI_BUS_ERROR;
}
#ifndef PX4_CONNECTEDIO
if (strcmp(sci->portname, "/dev/ttyACM0") == 0) {
hardwareFCEnable = MW_TTYACM0HWFLOWCONTROL_CHECKBOX;
} else if (strcmp(sci->portname, "/dev/ttyS0") == 0) {
hardwareFCEnable = MW_TTYS0HWFLOWCONTROL_CHECKBOX;
} else if (strcmp(sci->portname, "/dev/ttyS1") == 0) {
hardwareFCEnable = MW_TTYS1HWFLOWCONTROL_CHECKBOX;
} else if (strcmp(sci->portname, "/dev/ttyS2") == 0) {
hardwareFCEnable = MW_TTYS2HWFLOWCONTROL_CHECKBOX;
} else if (strcmp(sci->portname, "/dev/ttyS3") == 0) {
hardwareFCEnable = MW_TTYS3HWFLOWCONTROL_CHECKBOX;
} else if (strcmp(sci->portname, "/dev/ttyS4") == 0) {
hardwareFCEnable = MW_TTYS4HWFLOWCONTROL_CHECKBOX;
} else if (strcmp(sci->portname, "/dev/ttyS5") == 0) {
hardwareFCEnable = MW_TTYS5HWFLOWCONTROL_CHECKBOX;
} else if (strcmp(sci->portname, "/dev/ttyS6") == 0) {
hardwareFCEnable = MW_TTYS6HWFLOWCONTROL_CHECKBOX;
} else {
perror("SERIAL_SetFrameFormat/Parity");
return MW_SCI_BUS_ERROR;
}
#endif
/* Set data bits for Hardware Flow Control*/
if (1 == hardwareFCEnable) {
options.c_cflag |= CRTSCTS;
} else if (0 == hardwareFCEnable) {
options.c_cflag &= ~CRTSCTS;
}
}
return MW_SCI_SUCCESS;
}
/* Transmit the data over SCI */
MW_SCI_Status_Type MW_SCI_Transmit(MW_Handle_Type SCIModuleHandle,
uint8_T* TxDataPtr,
uint32_T TxDataLength) {
SCI_dev_t* sci;
int ret;
if (NULL != (void*)SCIModuleHandle) {
sci = (SCI_dev_t*)SCIModuleHandle;
int currSciDev;
currSciDev = getCurrSciDev(sci->portname);
PX4_INFO("Inside Transmit. SCIModule = %d. | ", currSciDev);
PX4_INFO("port name = %s | fd = %d | ", sci->portname, sci->fd);
PX4_INFO("data : ");
uint32_T i;
for (i = 0; i < TxDataLength; i++) {
PX4_INFO("[%d]", TxDataPtr[i]);
}
PX4_INFO("\n");
// FAR struct file *filep;
// ssize_t ret_test;
// ret_test = (ssize_t)fs_getfilep(sci->fd, &filep);
// PX4_INFO("%p", filep->f_inode);
if (0 == sci->busy) {
sci->busy = 2; /* Set the busy flag */
ret = write(sci->fd, TxDataPtr, TxDataLength);
if (ret < 0) {
PX4_ERR("SERIAL_write/write: %s", strerror(errno)); /* Log the exact error */
sci->busy = 0; /* Reset the busy flag */
return MW_SCI_BUS_ERROR;
}
sci->busy = 0; /* Reset the busy flag */
} else {
PX4_ERR("Serial Port busy. Unable to perform write operation.");
return MW_SCI_TX_BUSY;
}
if (ret < 0) {
PX4_ERR("SERIAL_write/write");
return MW_SCI_BUS_ERROR;
}
}
return MW_SCI_SUCCESS;
}
/* Close SERIAL channel*/
static void SERIAL_close(int fd) {
int ret;
ret = close(fd);
if (ret < 0) {
/* EBADF, EINTR, EIO: In all cases, descriptor is torn down*/
perror("SERIAL_close/close");
}
}
/* Release SCI module */
void MW_SCI_Close(MW_Handle_Type SCIModuleHandle) {
SCI_dev_t* sci;
if (NULL != (void*)SCIModuleHandle) {
sci = (SCI_dev_t*)SCIModuleHandle;
if (sci->fd > 0) {
#if DEBUG
printf("Inside SCI_Close. port name = %s | fd = %d \n", sci->portname, sci->fd);
#endif
SERIAL_close(sci->fd);
sci->fd = -1;
}
}
}
strat_st.cpp file
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "strat_st.hpp"
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/log.h>
#include <px4_platform_common/posix.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_combined.h>
StratSerialTask::StratSerialTask()
: ModuleParams(nullptr)
{
}
int StratSerialTask::print_status()
{
PX4_INFO("Running");
// TODO: print additional runtime information about the state of the module
return 0;
}
int StratSerialTask::custom_command(int argc, char *argv[])
{
/*
if (!is_running()) {
print_usage("not running");
return 1;
}
// additional custom commands can be handled like this:
if (!strcmp(argv[0], "do-something")) {
get_instance()->do_something();
return 0;
}
*/
return print_usage("unknown command");
}
int StratSerialTask::task_spawn(int argc, char *argv[])
{
_task_id = px4_task_spawn_cmd("strat_serial_task",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
1024,
(px4_main_t)&run_trampoline,
(char *const *)argv);
if (_task_id < 0) {
_task_id = -1;
return -errno;
}
return 0;
}
StratSerialTask *StratSerialTask::instantiate(int argc, char *argv[])
{
// int example_param = 0;
// bool example_flag = false;
//bool error_flag = false;
//int myoptind = 1;
//int ch;
//const char *myoptarg = nullptr;
// // parse CLI arguments
// while ((ch = px4_getopt(argc, argv, "p:f", &myoptind, &myoptarg)) != EOF) {
// switch (ch) {
// // case 'p':
// // example_param = (int)strtol(myoptarg, nullptr, 10);
// // break;
// // case 'f':
// // example_flag = true;
// // break;
// case '?':
// error_flag = true;
// PX4_ERR("Error flag set true");
// break;
// default:
// PX4_WARN("unrecognized flag");
// error_flag = true;
// break;
// }
// }
// if (error_flag) {
// return nullptr;
// }
StratSerialTask *instance = new StratSerialTask();
if (instance == nullptr) {
PX4_ERR("alloc failed");
}
if(!instance->start_serial(57600)){
PX4_ERR("Serial failed to start");
return nullptr;
}
return instance;
}
bool StratSerialTask::start_serial(int baud)
{
PX4_INFO("Initializing StratSerialTask...");
static const char* serial_device = "/dev/ttyS1";
void* SCIModuleVoidPtr = (void*)serial_device;
serial_handle = MW_SCI_Open(SCIModuleVoidPtr, true, MW_UNDEFINED_VALUE, MW_UNDEFINED_VALUE);
if (serial_handle == nullptr) {
PX4_ERR("Failed to open serial port.");
return false;
}else{
PX4_INFO("Serial port open %p", serial_handle);
}
// Set the baud rate, frame format, and hardware flow control
if(MW_SCI_SetBaudrate(serial_handle, 57600) == 0){
PX4_INFO("Baudrate set");
}else{
PX4_ERR("Baudrate NOT set");
return false;
}
/* Set SCI frame format */
if(MW_SCI_SetFrameFormat(serial_handle, 8, MW_SCI_PARITY_NONE, MW_SCI_STOPBITS_1) == 0){
PX4_INFO("Frame format set");
}else{
PX4_ERR("Frame format NOT set");
return false;
}
if( MW_SCI_ConfigureHardwareFlowControl(serial_handle, MW_SCI_FLOWCONTROL_NONE, MW_UNDEFINED_VALUE, MW_UNDEFINED_VALUE) == 0){
PX4_INFO("Flow control set");
}else{
PX4_ERR("Flow control NOT set");
return false;
}
sci = (SCI_dev_t*)serial_handle;
PX4_INFO("%d", sci->fd);
return true;
}
void StratSerialTask::run()
{
// Example: run the loop synchronized to the sensor_combined topic publication
int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
px4_pollfd_struct_t fds[1];
fds[0].fd = sensor_combined_sub;
fds[0].events = POLLIN;
// initialize parameters
parameters_update(true);
usleep(20000000);
int tx_buffer_available;
int count = 0;
while (!should_exit()) {
usleep(100000);
// wait for up to 1000ms for data
int pret = px4_poll(fds, (sizeof(fds) / sizeof(fds[0])), 1000);
if (pret == 0) {
// Timeout: let the loop run anyway, don't do `continue` here
} else if (pret < 0) {
// this is undesirable but not much we can do
PX4_ERR("poll error %d, %d", pret, errno);
px4_usleep(50000);
continue;
} else if (fds[0].events & POLLIN) {
struct sensor_combined_s sensor_combined;
orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor_combined);
//TODO: do something with the data...
//uint8_t number_to_send = 42; // Your number
//int bytes_written = dprintf(sci->fd, "%d\n", number_to_send);
uint8_t integer_value[8] = {3};
uint8_t data_to_send[sizeof(integer_value)/sizeof(uint8_t)];
memcpy(data_to_send, &integer_value, sizeof(integer_value)/sizeof(uint8_t));
ssize_t write_size = write(sci->fd, &data_to_send, sizeof(data_to_send));
if (write_size>0){
PX4_INFO("we wrote something");
count++;
}else{
PX4_INFO("write error %s",strerror(errno));
// Flush the Tx bufferif
if(ioctl(sci->fd, TCFLSH, TCOFLUSH) < 0) {
PX4_ERR("Failed to flush Tx buffer");
//return;
}
if (ioctl(sci->fd, TIOCOUTQ, &tx_buffer_available) < 0) {
PX4_ERR("Failed to get Tx buffer status, number of iterations: %d", count);
//close(fd);
PX4_ERR("Tx buffer check error: %s",strerror(errno));
}
}
// PX4_INFO("byte %d",data_to_send[0]);
// if (bytes_written > 0) {
// PX4_INFO("Message successfully sent: %d bytes", bytes_written);
// } else {
// PX4_ERR("Failed to send message over serial");
// }
}
}
PX4_INFO("module exited");
parameters_update();
MW_SCI_Close(serial_handle);
orb_unsubscribe(sensor_combined_sub);
}
void StratSerialTask::parameters_update(bool force)
{
// check for parameter updates
if (_parameter_update_sub.updated() || force) {
// clear update
parameter_update_s update;
_parameter_update_sub.copy(&update);
// update parameters from storage
updateParams();
}
}
int StratSerialTask::print_usage(const char *reason)
{
if (reason) {
PX4_WARN("%s\n", reason);
}
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
Section that describes the provided module functionality.
This is a template for a module running as a task in the background with start/stop/status functionality.
### Implementation
Section describing the high-level implementation of this module.
### Examples
CLI usage example:
$ module start -f -p 42
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("strat_serial_task","module");
PRINT_MODULE_USAGE_COMMAND("start");
// PRINT_MODULE_USAGE_PARAM_FLAG('f', "Optional example flag", true);
// PRINT_MODULE_USAGE_PARAM_INT('p', 0, 0, 1000, "Optional example parameter", true);
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
}
int strat_serial_task_main(int argc, char *argv[])
{
return StratSerialTask::main(argc, argv);
}