diff --git a/src/drivers/ocpoc_mmap_pwm_out/ocpoc_mmap_pwm_out.cpp b/src/drivers/ocpoc_mmap_pwm_out/ocpoc_mmap_pwm_out.cpp index 71e9c59401..179f48d49f 100644 --- a/src/drivers/ocpoc_mmap_pwm_out/ocpoc_mmap_pwm_out.cpp +++ b/src/drivers/ocpoc_mmap_pwm_out/ocpoc_mmap_pwm_out.cpp @@ -70,11 +70,11 @@ static bool _is_running = false; struct s_period_hi { uint32_t period; uint32_t hi; - }; +}; struct pwm_cmd { struct s_period_hi periodhi[MAX_ZYNQ_PWMS]; - }; +}; volatile struct pwm_cmd *sharedMem_cmd = nullptr; // jly static char _device[32] = "/dev/mem"; @@ -186,8 +186,8 @@ int initialize_mixer(const char *mixer_filename) float deadband = 0.13; _mixer = new MultirotorMixer(mixer_control_callback, (uintptr_t)&_controls, - MultirotorGeometry::QUAD_X, - roll_scale, pitch_scale, yaw_scale, deadband); + MultirotorGeometry::QUAD_X, + roll_scale, pitch_scale, yaw_scale, deadband); // TODO: temporary hack to make this compile (void)_config_index[0]; @@ -203,7 +203,7 @@ int initialize_mixer(const char *mixer_filename) unsigned long freq2tick(uint16_t freq_hz) { - unsigned long duty = TICK_PER_S/(unsigned long)freq_hz; + unsigned long duty = TICK_PER_S / (unsigned long)freq_hz; return duty; } @@ -214,9 +214,9 @@ int pwm_initialize(const char *device) int i; uint32_t mem_fd; //signal(SIGBUS,catch_sigbus); - mem_fd = open(device, O_RDWR|O_SYNC); - sharedMem_cmd = (struct pwm_cmd *) mmap(0, 0x1000, PROT_READ|PROT_WRITE, - MAP_SHARED, mem_fd, RCOUT_ZYNQ_PWM_BASE); + mem_fd = open(device, O_RDWR | O_SYNC); + sharedMem_cmd = (struct pwm_cmd *) mmap(0, 0x1000, PROT_READ | PROT_WRITE, + MAP_SHARED, mem_fd, RCOUT_ZYNQ_PWM_BASE); close(mem_fd); if (sharedMem_cmd == nullptr) { @@ -226,7 +226,7 @@ int pwm_initialize(const char *device) for (i = 0; i < NUM_PWM; ++i) { sharedMem_cmd->periodhi[i].period = freq2tick(FREQUENCY_PWM); - sharedMem_cmd->periodhi[i].hi = freq2tick(FREQUENCY_PWM)/2; // i prefer it is zero at the beginning + sharedMem_cmd->periodhi[i].hi = freq2tick(FREQUENCY_PWM) / 2; // i prefer it is zero at the beginning //PX4_ERR("initialize pwm pointer failed.%d, %d", sharedMem_cmd->periodhi[i].period, sharedMem_cmd->periodhi[i].hi); } @@ -252,7 +252,7 @@ void send_outputs_pwm(const uint16_t *pwm) for (unsigned i = 0; i < NUM_PWM; ++i) { //n = ::asprintf(&data, "%u", pwm[i] * 1000); //::write(_pwm_fd[i], data, n); - sharedMem_cmd->periodhi[i].hi = TICK_PER_US*pwm[i]; + sharedMem_cmd->periodhi[i].hi = TICK_PER_US * pwm[i]; //printf("ch:%d, val:%d*%d ", ch, period_us, TICK_PER_US); } } @@ -298,8 +298,9 @@ void task_main(int argc, char *argv[]) // Set up poll topic if (_armed.in_esc_calibration_mode == true) { fds[0].fd = _rc_channels_sub; - }else{ - fds[0].fd = _controls_sub; + + } else { + fds[0].fd = _controls_sub; } int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 10); @@ -324,7 +325,8 @@ void task_main(int argc, char *argv[]) _controls.control[1] = 0; _controls.control[2] = 0; _controls.control[3] = _rc.channels[_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE]]; - }else{ + + } else { orb_copy(ORB_ID(actuator_controls_0), _controls_sub, &_controls); } @@ -338,8 +340,8 @@ void task_main(int argc, char *argv[]) /* disable unused ports by setting their output to NaN */ for (size_t i = _outputs.noutputs; - i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); - i++) { + i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); + i++) { _outputs.output[i] = NAN; } @@ -358,33 +360,37 @@ void task_main(int argc, char *argv[]) // TODO FIXME: pre-armed seems broken pwm_limit_calc(_armed.armed, - false/*_armed.prearmed*/, - _outputs.noutputs, - reverse_mask, - disarmed_pwm, - min_pwm, - max_pwm, - _outputs.output, - pwm, - &_pwm_limit); + false/*_armed.prearmed*/, + _outputs.noutputs, + reverse_mask, + disarmed_pwm, + min_pwm, + max_pwm, + _outputs.output, + pwm, + &_pwm_limit); if (_armed.lockdown) { send_outputs_pwm(disarmed_pwm); - }else if (_armed.in_esc_calibration_mode) { - if (_controls.control[3]*1000 > 0.5f) { + + } else if (_armed.in_esc_calibration_mode) { + if (_controls.control[3] * 1000 > 0.5f) { pwm[0] = _pwm_max; pwm[1] = _pwm_max; pwm[2] = _pwm_max; pwm[3] = _pwm_max; - }else{ + + } else { pwm[0] = _pwm_min; pwm[1] = _pwm_min; pwm[2] = _pwm_min; pwm[3] = _pwm_min; } + send_outputs_pwm(pwm); - PX4_WARN("calib pwm %d:%d:%d:%d.", pwm[0],pwm[1],pwm[2],pwm[3]); + PX4_WARN("calib pwm %d:%d:%d:%d.", pwm[0], pwm[1], pwm[2], pwm[3]); + } else { send_outputs_pwm(pwm); } diff --git a/src/drivers/ocpoc_sbus_rc_in/ocpoc_sbus_rc_in.cpp b/src/drivers/ocpoc_sbus_rc_in/ocpoc_sbus_rc_in.cpp index 1372fd14ba..f1e0428ab7 100644 --- a/src/drivers/ocpoc_sbus_rc_in/ocpoc_sbus_rc_in.cpp +++ b/src/drivers/ocpoc_sbus_rc_in/ocpoc_sbus_rc_in.cpp @@ -115,7 +115,7 @@ int RcInput::ocpoc_subs_rc_init() _sbus_fd = sbus_init(RCINPUT_DEVICE_PATH, true); //jly - for (i=_channels; i < input_rc_s::RC_INPUT_MAX_CHANNELS; ++i) { + for (i = _channels; i < input_rc_s::RC_INPUT_MAX_CHANNELS; ++i) { _data.values[i] = UINT16_MAX; } @@ -179,11 +179,11 @@ void RcInput::_measure(void) * Gather R/C control inputs from sbus */ bool sbus_failsafe, sbus_frame_drop; - uint16_t values[SBUS_INPUT_CHANNELS*2]; + uint16_t values[SBUS_INPUT_CHANNELS * 2]; uint16_t num_values; bool sbus_updated = sbus_input(_sbus_fd, values, &num_values, &sbus_failsafe, &sbus_frame_drop, - _channels); + _channels); if (sbus_updated) { diff --git a/src/lib/rc/sbus.cpp b/src/lib/rc/sbus.cpp index 8ea00aeab3..5a0fcadaf1 100644 --- a/src/lib/rc/sbus.cpp +++ b/src/lib/rc/sbus.cpp @@ -159,11 +159,11 @@ sbus_config(int sbus_fd, bool singlewire) { #if defined(__PX4_POSIX_OCPOC) struct termios options; - + if (tcgetattr(sbus_fd, &options) != 0) { return -1; } - + tcflush(sbus_fd, TCIFLUSH); bzero(&options, sizeof(options)); @@ -182,24 +182,25 @@ sbus_config(int sbus_fd, bool singlewire) cfsetospeed(&options, B38400); tcflush(sbus_fd, TCIFLUSH); + if ((tcsetattr(sbus_fd, TCSANOW, &options)) != 0) { return -1; } int baud = 100000; struct serial_struct serials; - + if ((ioctl(sbus_fd, TIOCGSERIAL, &serials)) < 0) { return -1; } - + serials.flags = ASYNC_SPD_CUST; serials.custom_divisor = serials.baud_base / baud; - + if ((ioctl(sbus_fd, TIOCSSERIAL, &serials)) < 0) { return -1; } - + ioctl(sbus_fd, TIOCGSERIAL, &serials); tcflush(sbus_fd, TCIFLUSH); @@ -293,16 +294,17 @@ sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_fails ssize_t n; uint8_t buf[SBUS_FRAME_SIZE * 4]; - if ((n = read(sbus_fd, &buf[0], SBUS_FRAME_SIZE*4)) <= 0){ + + if ((n = read(sbus_fd, &buf[0], SBUS_FRAME_SIZE * 4)) <= 0) { return false; } - for (int i=0; i= 5) { + if (sbus_decode(now, sbus_frame, values, num_values, sbus_failsafe, sbus_frame_drop, max_channels) + && *num_values >= 5) { return true; } }