Fixed syle issues

Signed-off-by: Mark Charlebois <charlebm@gmail.com>
This commit is contained in:
Mark Charlebois
2016-05-28 14:06:14 +02:00
committed by Lorenz Meier
parent af1e20a1d0
commit 86e470b1c2
3 changed files with 32 additions and 24 deletions
@@ -372,21 +372,24 @@ bool create_pubs()
memset(&_mag, 0, sizeof(struct mag_report)); memset(&_mag, 0, sizeof(struct mag_report));
_gyro_pub = orb_advertise_multi(ORB_ID(sensor_gyro), &_gyro, _gyro_pub = orb_advertise_multi(ORB_ID(sensor_gyro), &_gyro,
&_gyro_orb_class_instance, ORB_PRIO_HIGH - 1); &_gyro_orb_class_instance, ORB_PRIO_HIGH - 1);
if (_gyro_pub == nullptr) { if (_gyro_pub == nullptr) {
PX4_ERR("sensor_gyro advert fail"); PX4_ERR("sensor_gyro advert fail");
return false; return false;
} }
_accel_pub = orb_advertise_multi(ORB_ID(sensor_accel), &_accel, _accel_pub = orb_advertise_multi(ORB_ID(sensor_accel), &_accel,
&_accel_orb_class_instance, ORB_PRIO_HIGH - 1); &_accel_orb_class_instance, ORB_PRIO_HIGH - 1);
if (_accel_pub == nullptr) { if (_accel_pub == nullptr) {
PX4_ERR("sensor_accel advert fail"); PX4_ERR("sensor_accel advert fail");
return false; return false;
} }
_mag_pub = orb_advertise_multi(ORB_ID(sensor_mag), &_mag, _mag_pub = orb_advertise_multi(ORB_ID(sensor_mag), &_mag,
&_mag_orb_class_instance, ORB_PRIO_HIGH - 1); &_mag_orb_class_instance, ORB_PRIO_HIGH - 1);
if (_mag_pub == nullptr) { if (_mag_pub == nullptr) {
PX4_ERR("sensor_mag advert fail"); PX4_ERR("sensor_mag advert fail");
return false; return false;
@@ -266,7 +266,7 @@ void task_main(int argc, char *argv[])
// See modules/px4iofirmware/dsm.c for details // See modules/px4iofirmware/dsm.c for details
// NOTE: rc_receiver spektrum driver outputs the data in 10bit DSM // NOTE: rc_receiver spektrum driver outputs the data in 10bit DSM
// format, so we need to double the channel value before the scaling // format, so we need to double the channel value before the scaling
_rc_in.values[i] = ((((int)(rc_inputs[i]*2) - 1024) * 1000) / 1700) + 1500; _rc_in.values[i] = ((((int)(rc_inputs[i] * 2) - 1024) * 1000) / 1700) + 1500;
} }
orb_publish(ORB_ID(input_rc), _input_rc_pub, &_rc_in); orb_publish(ORB_ID(input_rc), _input_rc_pub, &_rc_in);
@@ -97,7 +97,7 @@ actuator_outputs_s _outputs;
static void usage(); static void usage();
/** uart_esc start */ /** uart_esc start */
static void start(const char *device) __attribute__ ((unused)); static void start(const char *device) __attribute__((unused));
/** uart_esc stop */ /** uart_esc stop */
static void stop(); static void stop();
@@ -118,8 +118,8 @@ static void parameters_update();
struct { struct {
int model; int model;
int baudrate; int baudrate;
int px4_motor_mapping[UART_ESC_MAX_MOTORS]; int px4_motor_mapping[UART_ESC_MAX_MOTORS];
} _parameters; } _parameters;
struct { struct {
@@ -163,7 +163,7 @@ void parameters_update()
for (unsigned int i = 0; i < UART_ESC_MAX_MOTORS; i++) { for (unsigned int i = 0; i < UART_ESC_MAX_MOTORS; i++) {
if (param_get(_parameter_handles.px4_motor_mapping[i], &v_int) == 0) { if (param_get(_parameter_handles.px4_motor_mapping[i], &v_int) == 0) {
_parameters.px4_motor_mapping[i] = v_int; _parameters.px4_motor_mapping[i] = v_int;
PX4_WARN("UART_ESC_PX4MOTOR%d %d", i+1, _parameters.px4_motor_mapping[i]); PX4_WARN("UART_ESC_PX4MOTOR%d %d", i + 1, _parameters.px4_motor_mapping[i]);
} }
} }
} }
@@ -194,34 +194,38 @@ int mixer_control_callback(uintptr_t handle,
int initialize_mixer(const char *mixer_filename) int initialize_mixer(const char *mixer_filename)
{ {
mixer = nullptr; mixer = nullptr;
int mixer_initialized = -1; int mixer_initialized = -1;
char buf[2048]; char buf[2048];
unsigned int buflen = sizeof(buf); unsigned int buflen = sizeof(buf);
PX4_INFO("Initializing mixer from config file in %s", mixer_filename); PX4_INFO("Initializing mixer from config file in %s", mixer_filename);
int fd_load = open(mixer_filename, O_RDONLY); int fd_load = open(mixer_filename, O_RDONLY);
if (fd_load != -1) { if (fd_load != -1) {
int nRead = read(fd_load, buf, buflen); int nRead = read(fd_load, buf, buflen);
close(fd_load); close(fd_load);
if (nRead > 0) { if (nRead > 0) {
mixer = MultirotorMixer::from_text(mixer_control_callback, (uintptr_t)&_controls, buf, buflen); mixer = MultirotorMixer::from_text(mixer_control_callback, (uintptr_t)&_controls, buf, buflen);
if (mixer != nullptr) { if (mixer != nullptr) {
PX4_INFO("Successfully initialized mixer from config file"); PX4_INFO("Successfully initialized mixer from config file");
mixer_initialized = 0; mixer_initialized = 0;
} else { } else {
PX4_WARN("Unable to parse from mixer config file"); PX4_WARN("Unable to parse from mixer config file");
} }
} else { } else {
PX4_WARN("Unable to read from mixer config file"); PX4_WARN("Unable to read from mixer config file");
} }
} else { } else {
PX4_WARN("Unable to open mixer config file"); PX4_WARN("Unable to open mixer config file");
} }
// mixer file loading failed, fall back to default mixer configuration for // mixer file loading failed, fall back to default mixer configuration for
@@ -233,8 +237,8 @@ int initialize_mixer(const char *mixer_filename)
float deadband = 0; float deadband = 0;
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);
if (mixer == nullptr) { if (mixer == nullptr) {
PX4_ERR("mixer initialization failed"); PX4_ERR("mixer initialization failed");
@@ -282,7 +286,7 @@ void task_main(int argc, char *argv[])
PX4_ERR("failed to new UartEsc instance"); PX4_ERR("failed to new UartEsc instance");
} else if (esc->initialize((enum esc_model_t)_parameters.model, } else if (esc->initialize((enum esc_model_t)_parameters.model,
_device, _parameters.baudrate) < 0) { _device, _parameters.baudrate) < 0) {
PX4_ERR("failed to initialize UartEsc"); PX4_ERR("failed to initialize UartEsc");
} else { } else {
@@ -334,13 +338,13 @@ void task_main(int argc, char *argv[])
if (_armed.armed) { if (_armed.armed) {
_outputs.noutputs = mixer->mix(&_outputs.output[0], _outputs.noutputs = mixer->mix(&_outputs.output[0],
actuator_controls_0_s::NUM_ACTUATOR_CONTROLS, actuator_controls_0_s::NUM_ACTUATOR_CONTROLS,
NULL); NULL);
// Make sure we support only up to UART_ESC_MAX_MOTORS motors // Make sure we support only up to UART_ESC_MAX_MOTORS motors
if (_outputs.noutputs > UART_ESC_MAX_MOTORS) { if (_outputs.noutputs > UART_ESC_MAX_MOTORS) {
PX4_ERR("Unsupported motors %d, up to %d motors supported", PX4_ERR("Unsupported motors %d, up to %d motors supported",
_outputs.noutputs, UART_ESC_MAX_MOTORS); _outputs.noutputs, UART_ESC_MAX_MOTORS);
continue; continue;
} }
@@ -348,13 +352,14 @@ void task_main(int argc, char *argv[])
for (unsigned outIdx = 0; outIdx < _outputs.noutputs; outIdx++) { for (unsigned outIdx = 0; outIdx < _outputs.noutputs; outIdx++) {
// map -1.0 - 1.0 outputs to RPMs // map -1.0 - 1.0 outputs to RPMs
motor_rpms[outIdx] = (int16_t)(((_outputs.output[outIdx] + 1.0) / 2.0) * motor_rpms[outIdx] = (int16_t)(((_outputs.output[outIdx] + 1.0) / 2.0) *
(esc->max_rpm() - esc->min_rpm()) + esc->min_rpm()); (esc->max_rpm() - esc->min_rpm()) + esc->min_rpm());
} }
uart_esc_rotate_motors(motor_rpms, _outputs.noutputs); uart_esc_rotate_motors(motor_rpms, _outputs.noutputs);
} else { } else {
_outputs.noutputs = UART_ESC_MAX_MOTORS; _outputs.noutputs = UART_ESC_MAX_MOTORS;
for (unsigned outIdx = 0; outIdx < _outputs.noutputs; outIdx++) { for (unsigned outIdx = 0; outIdx < _outputs.noutputs; outIdx++) {
motor_rpms[outIdx] = 0; motor_rpms[outIdx] = 0;
_outputs.output[outIdx] = -1.0; _outputs.output[outIdx] = -1.0;