aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorpx4dev <px4@purgatory.org>2012-08-14 08:47:04 -0700
committerpx4dev <px4@purgatory.org>2012-08-14 08:47:04 -0700
commit34118c72ef88d33d0074914c9bf0cda0232e4940 (patch)
tree95a797b6d8d5e7dbb82fc2c44c095de10491a1a8
parent92a1fab0fd9e95737258c30fc423e839652edb72 (diff)
downloadpx4-firmware-34118c72ef88d33d0074914c9bf0cda0232e4940.tar.gz
px4-firmware-34118c72ef88d33d0074914c9bf0cda0232e4940.tar.bz2
px4-firmware-34118c72ef88d33d0074914c9bf0cda0232e4940.zip
mechanical style fixups
-rw-r--r--apps/drivers/drv_gpio.h4
-rw-r--r--apps/drivers/drv_mixer.h17
-rw-r--r--apps/drivers/mpu6000/mpu6000.cpp10
-rw-r--r--apps/px4/fmu/fmu.cpp65
-rw-r--r--apps/systemlib/hx_stream.h12
-rw-r--r--apps/systemlib/mixer/mixer.cpp3
-rw-r--r--apps/systemlib/mixer/mixer.h17
-rw-r--r--apps/systemlib/mixer/mixer_group.cpp17
-rw-r--r--apps/systemlib/perf_counter.c2
-rw-r--r--apps/systemlib/visibility.h4
10 files changed, 84 insertions, 67 deletions
diff --git a/apps/drivers/drv_gpio.h b/apps/drivers/drv_gpio.h
index b9684bce1..361d731eb 100644
--- a/apps/drivers/drv_gpio.h
+++ b/apps/drivers/drv_gpio.h
@@ -40,10 +40,10 @@
#include <sys/ioctl.h>
-/*
+/*
* GPIO defines come from a board-specific header, as they are shared
* with board-specific logic.
- *
+ *
* The board-specific header must define:
* GPIO_DEVICE_PATH
* GPIO_RESET
diff --git a/apps/drivers/drv_mixer.h b/apps/drivers/drv_mixer.h
index 125b0d28a..fd54ed7fa 100644
--- a/apps/drivers/drv_mixer.h
+++ b/apps/drivers/drv_mixer.h
@@ -71,8 +71,7 @@
#define MIXERIOCRESET _MIXERIOC(1)
/** simple channel scaler */
-struct mixer_scaler_s
-{
+struct mixer_scaler_s {
float negative_scale;
float positive_scale;
float offset;
@@ -81,16 +80,14 @@ struct mixer_scaler_s
};
/** mixer input */
-struct mixer_control_s
-{
+struct mixer_control_s {
uint8_t control_group; /**< group from which the input reads */
uint8_t control_index; /**< index within the control group */
struct mixer_scaler_s scaler; /**< scaling applied to the input before use */
};
/** simple mixer */
-struct mixer_simple_s
-{
+struct mixer_simple_s {
uint8_t control_count; /**< number of inputs */
struct mixer_scaler_s output_scaler; /**< scaling for the output */
struct mixer_control_s controls[0]; /**< actual size of the array is set by control_count */
@@ -99,20 +96,18 @@ struct mixer_simple_s
#define MIXER_SIMPLE_SIZE(_icount) (sizeof(struct mixer_simple_s) + (_icount) * sizeof(struct mixer_control_s))
/**
- * add a simple mixer in (struct mixer_simple_s *)arg
+ * add a simple mixer in (struct mixer_simple_s *)arg
*/
#define MIXERIOCADDSIMPLE _MIXERIOC(2)
/** multirotor output definition */
-struct mixer_rotor_output_s
-{
+struct mixer_rotor_output_s {
float angle; /**< rotor angle clockwise from forward in radians */
float distance; /**< motor distance from centre in arbitrary units */
};
/** multirotor mixer */
-struct mixer_multirotor_s
-{
+struct mixer_multirotor_s {
uint8_t rotor_count;
struct mixer_control_s controls[4]; /**< controls are roll, pitch, yaw, thrust */
struct mixer_rotor_output_s rotors[0]; /**< actual size of the array is set by rotor_count */
diff --git a/apps/drivers/mpu6000/mpu6000.cpp b/apps/drivers/mpu6000/mpu6000.cpp
index 70e3e41c5..7576fc1d9 100644
--- a/apps/drivers/mpu6000/mpu6000.cpp
+++ b/apps/drivers/mpu6000/mpu6000.cpp
@@ -367,7 +367,7 @@ MPU6000::init()
// FS & DLPF FS=2000¼/s, DLPF = 98Hz (low pass filter)
write_reg(MPUREG_CONFIG, BITS_DLPF_CFG_98HZ);
usleep(1000);
- write_reg(MPUREG_GYRO_CONFIG,BITS_FS_2000DPS); // Gyro scale 2000¼/s
+ write_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS); // Gyro scale 2000¼/s
usleep(1000);
// product-specific scaling
@@ -390,15 +390,16 @@ MPU6000::init()
case MPU6000_REV_D9:
case MPU6000_REV_D10:
// Accel scale 8g (4096 LSB/g)
- write_reg(MPUREG_ACCEL_CONFIG,2<<3);
+ write_reg(MPUREG_ACCEL_CONFIG, 2 << 3);
break;
}
+
usleep(1000);
// INT CFG => Interrupt on Data Ready
- write_reg(MPUREG_INT_ENABLE,BIT_RAW_RDY_EN); // INT: Raw data ready
+ write_reg(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN); // INT: Raw data ready
usleep(1000);
- write_reg(MPUREG_INT_PIN_CFG,BIT_INT_ANYRD_2CLEAR); // INT: Clear on any read
+ write_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR); // INT: Clear on any read
usleep(1000);
// Oscillator set
@@ -898,6 +899,7 @@ mpu6000_main(int argc, char *argv[])
g_dev = nullptr;
return -EIO;
}
+
return OK;
}
diff --git a/apps/px4/fmu/fmu.cpp b/apps/px4/fmu/fmu.cpp
index bac9431eb..9d8847246 100644
--- a/apps/px4/fmu/fmu.cpp
+++ b/apps/px4/fmu/fmu.cpp
@@ -100,10 +100,10 @@ private:
static void task_main_trampoline(int argc, char *argv[]);
void task_main();
- static int control_callback_trampoline(uintptr_t handle,
- uint8_t control_group,
- uint8_t control_index,
- float &input);
+ static int control_callback_trampoline(uintptr_t handle,
+ uint8_t control_group,
+ uint8_t control_index,
+ float &input);
int control_callback(uint8_t control_group,
uint8_t control_index,
float &input);
@@ -285,7 +285,7 @@ FMUServo::task_main()
}
int
-FMUServo::control_callback_trampoline(uintptr_t handle,
+FMUServo::control_callback_trampoline(uintptr_t handle,
uint8_t control_group,
uint8_t control_index,
float &input)
@@ -363,6 +363,7 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
} else {
*(unsigned *)arg = 2;
}
+
break;
case MIXERIOCRESET:
@@ -370,22 +371,27 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
delete _mixers;
_mixers = nullptr;
}
+
break;
case MIXERIOCADDSIMPLE: {
- mixer_simple_s *mixinfo = (mixer_simple_s *)arg;
+ mixer_simple_s *mixinfo = (mixer_simple_s *)arg;
- SimpleMixer *mixer = new SimpleMixer(control_callback_trampoline, (uintptr_t)this, mixinfo);
- if (mixer->check()) {
- delete mixer;
- ret = -EINVAL;
- } else {
- if (_mixers == nullptr)
- _mixers = new MixerGroup(control_callback_trampoline, (uintptr_t)this);
- _mixers->add_mixer(mixer);
+ SimpleMixer *mixer = new SimpleMixer(control_callback_trampoline, (uintptr_t)this, mixinfo);
+
+ if (mixer->check()) {
+ delete mixer;
+ ret = -EINVAL;
+
+ } else {
+ if (_mixers == nullptr)
+ _mixers = new MixerGroup(control_callback_trampoline, (uintptr_t)this);
+
+ _mixers->add_mixer(mixer);
+ }
+
+ break;
}
- break;
- }
case MIXERIOCADDMULTIROTOR:
/* XXX not yet supported */
@@ -393,20 +399,23 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
break;
case MIXERIOCLOADFILE: {
- const char *path = (const char *)arg;
+ const char *path = (const char *)arg;
- if (_mixers != nullptr) {
- delete _mixers;
- _mixers = nullptr;
- }
- _mixers = new MixerGroup(control_callback_trampoline, (uintptr_t)this);
- if (_mixers->load_from_file(path) != 0) {
- delete _mixers;
- _mixers = nullptr;
- ret = -EINVAL;
+ if (_mixers != nullptr) {
+ delete _mixers;
+ _mixers = nullptr;
+ }
+
+ _mixers = new MixerGroup(control_callback_trampoline, (uintptr_t)this);
+
+ if (_mixers->load_from_file(path) != 0) {
+ delete _mixers;
+ _mixers = nullptr;
+ ret = -EINVAL;
+ }
+
+ break;
}
- break;
- }
default:
ret = -ENOTTY;
diff --git a/apps/systemlib/hx_stream.h b/apps/systemlib/hx_stream.h
index e33cfcd84..ac3b3e99d 100644
--- a/apps/systemlib/hx_stream.h
+++ b/apps/systemlib/hx_stream.h
@@ -33,7 +33,7 @@
/**
* @file hx_stream.h
- *
+ *
* A simple serial line framing protocol based on HDLC
* with 32-bit CRC protection.
*/
@@ -65,8 +65,8 @@ __BEGIN_DECLS
* not be allocated.
*/
__EXPORT extern hx_stream_t hx_stream_init(int fd,
- hx_stream_rx_callback callback,
- void *arg);
+ hx_stream_rx_callback callback,
+ void *arg);
/**
* Free a hx_stream object.
@@ -106,8 +106,8 @@ __EXPORT extern void hx_stream_set_counters(hx_stream_t stream,
* set on error.
*/
__EXPORT extern int hx_stream_send(hx_stream_t stream,
- const void *data,
- size_t count);
+ const void *data,
+ size_t count);
/**
* Handle a byte from the stream.
@@ -116,7 +116,7 @@ __EXPORT extern int hx_stream_send(hx_stream_t stream,
* @param c The character to process.
*/
__EXPORT extern void hx_stream_rx(hx_stream_t stream,
- uint8_t c);
+ uint8_t c);
__END_DECLS
diff --git a/apps/systemlib/mixer/mixer.cpp b/apps/systemlib/mixer/mixer.cpp
index eaa1d60ed..49c8ed75a 100644
--- a/apps/systemlib/mixer/mixer.cpp
+++ b/apps/systemlib/mixer/mixer.cpp
@@ -116,6 +116,7 @@ NullMixer::mix(float *outputs, unsigned space)
*outputs = 0.0f;
return 1;
}
+
return 0;
}
@@ -148,6 +149,7 @@ SimpleMixer::mix(float *outputs, unsigned space)
if (_info == nullptr)
return 0;
+
if (space < 1)
return 0;
@@ -161,6 +163,7 @@ SimpleMixer::mix(float *outputs, unsigned space)
sum += scale(_info->controls[i].scaler, input);
}
+
*outputs = scale(_info->output_scaler, sum);
return 1;
}
diff --git a/apps/systemlib/mixer/mixer.h b/apps/systemlib/mixer/mixer.h
index 5a3857a00..882acb33e 100644
--- a/apps/systemlib/mixer/mixer.h
+++ b/apps/systemlib/mixer/mixer.h
@@ -131,7 +131,7 @@
#include "drivers/drv_mixer.h"
/**
- * Abstract class defining a mixer mixing zero or more inputs to
+ * Abstract class defining a mixer mixing zero or more inputs to
* one or more outputs.
*/
class __EXPORT Mixer
@@ -149,10 +149,10 @@ public:
* @param control The returned control
* @return Zero if the value was fetched, nonzero otherwise.
*/
- typedef int (* ControlCallback)(uintptr_t handle,
- uint8_t control_group,
- uint8_t control_index,
- float &control);
+ typedef int (* ControlCallback)(uintptr_t handle,
+ uint8_t control_group,
+ uint8_t control_index,
+ float &control);
/**
* Constructor.
@@ -237,8 +237,8 @@ public:
*
* Z:
*
- * This mixer generates a constant zero output, and is normally used to
- * skip over outputs that are not in use.
+ * This mixer generates a constant zero output, and is normally used to
+ * skip over outputs that are not in use.
*
* Simple Mixer:
*
@@ -336,8 +336,7 @@ private:
class __EXPORT MultirotorMixer : public Mixer
{
public:
- enum Geometry
- {
+ enum Geometry {
MULTIROTOR_QUAD_PLUS,
MULTIROTOR_QUAD_X
/* XXX add more here */
diff --git a/apps/systemlib/mixer/mixer_group.cpp b/apps/systemlib/mixer/mixer_group.cpp
index 11894e02b..3f954c129 100644
--- a/apps/systemlib/mixer/mixer_group.cpp
+++ b/apps/systemlib/mixer/mixer_group.cpp
@@ -171,10 +171,12 @@ mixer_load_simple(Mixer::ControlCallback control_cb, uintptr_t cb_handle, int fd
/* first, get the output scaler */
ret = mixer_getline(fd, buf, sizeof(buf));
+
if (ret < 1) {
debug("failed reading for output scaler");
goto fail;
}
+
if (mixer_parse_output_scaler(buf, mixinfo->output_scaler)) {
debug("failed parsing output scaler");
goto fail;
@@ -183,17 +185,20 @@ mixer_load_simple(Mixer::ControlCallback control_cb, uintptr_t cb_handle, int fd
/* now get any inputs */
for (unsigned i = 0; i < inputs; i++) {
ret = mixer_getline(fd, buf, sizeof(buf));
+
if (ret < 1) {
debug("failed reading for control scaler");
goto fail;
}
- if (mixer_parse_control_scaler(buf,
- mixinfo->controls[i].scaler,
- mixinfo->controls[i].control_group,
- mixinfo->controls[i].control_index)) {
+
+ if (mixer_parse_control_scaler(buf,
+ mixinfo->controls[i].scaler,
+ mixinfo->controls[i].control_group,
+ mixinfo->controls[i].control_index)) {
debug("failed parsing control scaler");
goto fail;
}
+
debug("got control %d", i);
}
@@ -266,8 +271,10 @@ MixerGroup::add_mixer(Mixer *mixer)
Mixer **mpp;
mpp = &_first;
+
while (*mpp != nullptr)
mpp = &((*mpp)->_next);
+
*mpp = mixer;
mixer->_next = nullptr;
}
@@ -282,6 +289,7 @@ MixerGroup::mix(float *outputs, unsigned space)
index += mixer->mix(outputs + index, space - index);
mixer = mixer->_next;
}
+
return index;
}
@@ -303,6 +311,7 @@ MixerGroup::load_from_file(const char *path)
return -1;
int fd = open(path, O_RDONLY);
+
if (fd < 0) {
debug("failed to open %s", path);
return -1;
diff --git a/apps/systemlib/perf_counter.c b/apps/systemlib/perf_counter.c
index 14bd3c436..e25e548f0 100644
--- a/apps/systemlib/perf_counter.c
+++ b/apps/systemlib/perf_counter.c
@@ -33,7 +33,7 @@
/**
* @file perf_counter.c
- *
+ *
* @brief Performance measuring tools.
*/
diff --git a/apps/systemlib/visibility.h b/apps/systemlib/visibility.h
index 64e83bbad..2c6adc062 100644
--- a/apps/systemlib/visibility.h
+++ b/apps/systemlib/visibility.h
@@ -33,7 +33,7 @@
/**
* @file visibility.h
- *
+ *
* Definitions controlling symbol naming and visibility.
*
* This file is normally included automatically by the build system.
@@ -58,5 +58,5 @@
#else
# define __BEGIN_DECLS
# define __END_DECLS
-#endif
+#endif
#endif \ No newline at end of file