mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-22 06:14:14 +08:00
fix code style
This commit is contained in:
committed by
Lorenz Meier
parent
0de70af78d
commit
68983f7ee2
@@ -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);
|
||||
}
|
||||
|
||||
@@ -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) {
|
||||
|
||||
|
||||
+13
-11
@@ -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<n; i++) {
|
||||
if (buf[i] == 0x0f && (i+24 < n) && buf[i+23] == 0x00) {
|
||||
for (int i = 0; i < n; i++) {
|
||||
if (buf[i] == 0x0f && (i + 24 < n) && buf[i + 23] == 0x00) {
|
||||
memcpy(&sbus_frame[0], &buf[i], 25);
|
||||
|
||||
if (sbus_decode(now, sbus_frame, values, num_values, sbus_failsafe, sbus_frame_drop, max_channels)
|
||||
&& *num_values >= 5) {
|
||||
if (sbus_decode(now, sbus_frame, values, num_values, sbus_failsafe, sbus_frame_drop, max_channels)
|
||||
&& *num_values >= 5) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user