diff options
| -rw-r--r-- | apps/px4io/comms.c | 12 | ||||
| -rw-r--r-- | apps/px4io/px4io.c | 10 | ||||
| -rw-r--r-- | apps/px4io/px4io.h | 1 |
3 files changed, 14 insertions, 9 deletions
diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index b24fba09d..375336730 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -47,6 +47,7 @@ #include <errno.h> #include <string.h> #include <poll.h> +#include <termios.h> #include <nuttx/clock.h> @@ -69,7 +70,7 @@ static struct px4io_report report; static void comms_handle_frame(void *arg, const void *buffer, size_t length); -void +static void comms_init(void) { /* initialise the FMU interface */ @@ -78,11 +79,20 @@ comms_init(void) /* default state in the report to FMU */ report.i2f_magic = I2F_MAGIC; + + struct termios t; + + /* 115200bps, no parity, one stop bit */ + tcgetattr(fmu_fd, &t); + cfsetspeed(&t, 115200); + t.c_cflag &= ~(CSTOPB | PARENB); + tcsetattr(fmu_fd, TCSANOW, &t); } void comms_main(void) { + comms_init(); struct pollfd fds; fds.fd = fmu_fd; diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index ba16ce247..bba236322 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -76,6 +76,9 @@ int user_start(int argc, char *argv[]) /* start the safety switch handler */ safety_init(); + /* configure the first 8 PWM outputs (i.e. all of them) */ + up_pwm_servo_init(0xff); + /* start the flight control signal handler */ task_create("FCon", SCHED_PRIORITY_DEFAULT, @@ -84,13 +87,6 @@ int user_start(int argc, char *argv[]) NULL); - /* initialise the FMU communications interface */ - comms_init(); - - /* configure the first 8 PWM outputs (i.e. all of them) */ - /* note, must do this after comms init to steal back PA0, which is CTS otherwise */ - up_pwm_servo_init(0xff); - struct mallinfo minfo = mallinfo(); lib_lowprintf("free %u largest %u\n", minfo.mxordblk, minfo.fordblks); diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index 2ecab966d..21c1482be 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -157,7 +157,6 @@ extern void safety_init(void); /* * FMU communications */ -extern void comms_init(void); extern void comms_main(void) __attribute__((noreturn)); /* |