diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index b24fba09d9..375336730a 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -47,6 +47,7 @@ #include #include #include +#include #include @@ -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 ba16ce2472..bba236322d 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 2ecab966d0..21c1482bed 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)); /*