Browse Source

fix code style

sbg
davidaroyer 8 years ago committed by Lorenz Meier
parent
commit
68983f7ee2
  1. 62
      src/drivers/ocpoc_mmap_pwm_out/ocpoc_mmap_pwm_out.cpp
  2. 6
      src/drivers/ocpoc_sbus_rc_in/ocpoc_sbus_rc_in.cpp
  3. 24
      src/lib/rc/sbus.cpp

62
src/drivers/ocpoc_mmap_pwm_out/ocpoc_mmap_pwm_out.cpp

@ -70,11 +70,11 @@ static bool _is_running = false;
struct s_period_hi { struct s_period_hi {
uint32_t period; uint32_t period;
uint32_t hi; uint32_t hi;
}; };
struct pwm_cmd { struct pwm_cmd {
struct s_period_hi periodhi[MAX_ZYNQ_PWMS]; struct s_period_hi periodhi[MAX_ZYNQ_PWMS];
}; };
volatile struct pwm_cmd *sharedMem_cmd = nullptr; // jly volatile struct pwm_cmd *sharedMem_cmd = nullptr; // jly
static char _device[32] = "/dev/mem"; static char _device[32] = "/dev/mem";
@ -186,8 +186,8 @@ int initialize_mixer(const char *mixer_filename)
float deadband = 0.13; float deadband = 0.13;
_mixer = new MultirotorMixer(mixer_control_callback, (uintptr_t)&_controls, _mixer = new MultirotorMixer(mixer_control_callback, (uintptr_t)&_controls,
MultirotorGeometry::QUAD_X, MultirotorGeometry::QUAD_X,
roll_scale, pitch_scale, yaw_scale, deadband); roll_scale, pitch_scale, yaw_scale, deadband);
// TODO: temporary hack to make this compile // TODO: temporary hack to make this compile
(void)_config_index[0]; (void)_config_index[0];
@ -203,7 +203,7 @@ int initialize_mixer(const char *mixer_filename)
unsigned long freq2tick(uint16_t freq_hz) 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; return duty;
} }
@ -214,9 +214,9 @@ int pwm_initialize(const char *device)
int i; int i;
uint32_t mem_fd; uint32_t mem_fd;
//signal(SIGBUS,catch_sigbus); //signal(SIGBUS,catch_sigbus);
mem_fd = open(device, O_RDWR|O_SYNC); mem_fd = open(device, O_RDWR | O_SYNC);
sharedMem_cmd = (struct pwm_cmd *) mmap(0, 0x1000, PROT_READ|PROT_WRITE, sharedMem_cmd = (struct pwm_cmd *) mmap(0, 0x1000, PROT_READ | PROT_WRITE,
MAP_SHARED, mem_fd, RCOUT_ZYNQ_PWM_BASE); MAP_SHARED, mem_fd, RCOUT_ZYNQ_PWM_BASE);
close(mem_fd); close(mem_fd);
if (sharedMem_cmd == nullptr) { if (sharedMem_cmd == nullptr) {
@ -226,7 +226,7 @@ int pwm_initialize(const char *device)
for (i = 0; i < NUM_PWM; ++i) { for (i = 0; i < NUM_PWM; ++i) {
sharedMem_cmd->periodhi[i].period = freq2tick(FREQUENCY_PWM); 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); //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) { for (unsigned i = 0; i < NUM_PWM; ++i) {
//n = ::asprintf(&data, "%u", pwm[i] * 1000); //n = ::asprintf(&data, "%u", pwm[i] * 1000);
//::write(_pwm_fd[i], data, n); //::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); //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 // Set up poll topic
if (_armed.in_esc_calibration_mode == true) { if (_armed.in_esc_calibration_mode == true) {
fds[0].fd = _rc_channels_sub; 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); 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[1] = 0;
_controls.control[2] = 0; _controls.control[2] = 0;
_controls.control[3] = _rc.channels[_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE]]; _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); 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 */ /* disable unused ports by setting their output to NaN */
for (size_t i = _outputs.noutputs; for (size_t i = _outputs.noutputs;
i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); i < sizeof(_outputs.output) / sizeof(_outputs.output[0]);
i++) { i++) {
_outputs.output[i] = NAN; _outputs.output[i] = NAN;
} }
@ -358,33 +360,37 @@ void task_main(int argc, char *argv[])
// TODO FIXME: pre-armed seems broken // TODO FIXME: pre-armed seems broken
pwm_limit_calc(_armed.armed, pwm_limit_calc(_armed.armed,
false/*_armed.prearmed*/, false/*_armed.prearmed*/,
_outputs.noutputs, _outputs.noutputs,
reverse_mask, reverse_mask,
disarmed_pwm, disarmed_pwm,
min_pwm, min_pwm,
max_pwm, max_pwm,
_outputs.output, _outputs.output,
pwm, pwm,
&_pwm_limit); &_pwm_limit);
if (_armed.lockdown) { if (_armed.lockdown) {
send_outputs_pwm(disarmed_pwm); 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[0] = _pwm_max;
pwm[1] = _pwm_max; pwm[1] = _pwm_max;
pwm[2] = _pwm_max; pwm[2] = _pwm_max;
pwm[3] = _pwm_max; pwm[3] = _pwm_max;
}else{
} else {
pwm[0] = _pwm_min; pwm[0] = _pwm_min;
pwm[1] = _pwm_min; pwm[1] = _pwm_min;
pwm[2] = _pwm_min; pwm[2] = _pwm_min;
pwm[3] = _pwm_min; pwm[3] = _pwm_min;
} }
send_outputs_pwm(pwm); 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 { } else {
send_outputs_pwm(pwm); send_outputs_pwm(pwm);
} }

6
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 _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; _data.values[i] = UINT16_MAX;
} }
@ -179,11 +179,11 @@ void RcInput::_measure(void)
* Gather R/C control inputs from sbus * Gather R/C control inputs from sbus
*/ */
bool sbus_failsafe, sbus_frame_drop; bool sbus_failsafe, sbus_frame_drop;
uint16_t values[SBUS_INPUT_CHANNELS*2]; uint16_t values[SBUS_INPUT_CHANNELS * 2];
uint16_t num_values; uint16_t num_values;
bool sbus_updated = sbus_input(_sbus_fd, values, &num_values, &sbus_failsafe, &sbus_frame_drop, bool sbus_updated = sbus_input(_sbus_fd, values, &num_values, &sbus_failsafe, &sbus_frame_drop,
_channels); _channels);
if (sbus_updated) { if (sbus_updated) {

24
src/lib/rc/sbus.cpp

@ -159,11 +159,11 @@ sbus_config(int sbus_fd, bool singlewire)
{ {
#if defined(__PX4_POSIX_OCPOC) #if defined(__PX4_POSIX_OCPOC)
struct termios options; struct termios options;
if (tcgetattr(sbus_fd, &options) != 0) { if (tcgetattr(sbus_fd, &options) != 0) {
return -1; return -1;
} }
tcflush(sbus_fd, TCIFLUSH); tcflush(sbus_fd, TCIFLUSH);
bzero(&options, sizeof(options)); bzero(&options, sizeof(options));
@ -182,24 +182,25 @@ sbus_config(int sbus_fd, bool singlewire)
cfsetospeed(&options, B38400); cfsetospeed(&options, B38400);
tcflush(sbus_fd, TCIFLUSH); tcflush(sbus_fd, TCIFLUSH);
if ((tcsetattr(sbus_fd, TCSANOW, &options)) != 0) { if ((tcsetattr(sbus_fd, TCSANOW, &options)) != 0) {
return -1; return -1;
} }
int baud = 100000; int baud = 100000;
struct serial_struct serials; struct serial_struct serials;
if ((ioctl(sbus_fd, TIOCGSERIAL, &serials)) < 0) { if ((ioctl(sbus_fd, TIOCGSERIAL, &serials)) < 0) {
return -1; return -1;
} }
serials.flags = ASYNC_SPD_CUST; serials.flags = ASYNC_SPD_CUST;
serials.custom_divisor = serials.baud_base / baud; serials.custom_divisor = serials.baud_base / baud;
if ((ioctl(sbus_fd, TIOCSSERIAL, &serials)) < 0) { if ((ioctl(sbus_fd, TIOCSSERIAL, &serials)) < 0) {
return -1; return -1;
} }
ioctl(sbus_fd, TIOCGSERIAL, &serials); ioctl(sbus_fd, TIOCGSERIAL, &serials);
tcflush(sbus_fd, TCIFLUSH); 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; ssize_t n;
uint8_t buf[SBUS_FRAME_SIZE * 4]; 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; return false;
} }
for (int i=0; i<n; i++) { for (int i = 0; i < n; i++) {
if (buf[i] == 0x0f && (i+24 < n) && buf[i+23] == 0x00) { if (buf[i] == 0x0f && (i + 24 < n) && buf[i + 23] == 0x00) {
memcpy(&sbus_frame[0], &buf[i], 25); memcpy(&sbus_frame[0], &buf[i], 25);
if (sbus_decode(now, sbus_frame, values, num_values, sbus_failsafe, sbus_frame_drop, max_channels) if (sbus_decode(now, sbus_frame, values, num_values, sbus_failsafe, sbus_frame_drop, max_channels)
&& *num_values >= 5) { && *num_values >= 5) {
return true; return true;
} }
} }

Loading…
Cancel
Save