mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-06-02 20:16:48 +08:00
mechanical style fixups
This commit is contained in:
@@ -71,8 +71,7 @@
|
|||||||
#define MIXERIOCRESET _MIXERIOC(1)
|
#define MIXERIOCRESET _MIXERIOC(1)
|
||||||
|
|
||||||
/** simple channel scaler */
|
/** simple channel scaler */
|
||||||
struct mixer_scaler_s
|
struct mixer_scaler_s {
|
||||||
{
|
|
||||||
float negative_scale;
|
float negative_scale;
|
||||||
float positive_scale;
|
float positive_scale;
|
||||||
float offset;
|
float offset;
|
||||||
@@ -81,16 +80,14 @@ struct mixer_scaler_s
|
|||||||
};
|
};
|
||||||
|
|
||||||
/** mixer input */
|
/** mixer input */
|
||||||
struct mixer_control_s
|
struct mixer_control_s {
|
||||||
{
|
|
||||||
uint8_t control_group; /**< group from which the input reads */
|
uint8_t control_group; /**< group from which the input reads */
|
||||||
uint8_t control_index; /**< index within the control group */
|
uint8_t control_index; /**< index within the control group */
|
||||||
struct mixer_scaler_s scaler; /**< scaling applied to the input before use */
|
struct mixer_scaler_s scaler; /**< scaling applied to the input before use */
|
||||||
};
|
};
|
||||||
|
|
||||||
/** simple mixer */
|
/** simple mixer */
|
||||||
struct mixer_simple_s
|
struct mixer_simple_s {
|
||||||
{
|
|
||||||
uint8_t control_count; /**< number of inputs */
|
uint8_t control_count; /**< number of inputs */
|
||||||
struct mixer_scaler_s output_scaler; /**< scaling for the output */
|
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 */
|
struct mixer_control_s controls[0]; /**< actual size of the array is set by control_count */
|
||||||
@@ -104,15 +101,13 @@ struct mixer_simple_s
|
|||||||
#define MIXERIOCADDSIMPLE _MIXERIOC(2)
|
#define MIXERIOCADDSIMPLE _MIXERIOC(2)
|
||||||
|
|
||||||
/** multirotor output definition */
|
/** multirotor output definition */
|
||||||
struct mixer_rotor_output_s
|
struct mixer_rotor_output_s {
|
||||||
{
|
|
||||||
float angle; /**< rotor angle clockwise from forward in radians */
|
float angle; /**< rotor angle clockwise from forward in radians */
|
||||||
float distance; /**< motor distance from centre in arbitrary units */
|
float distance; /**< motor distance from centre in arbitrary units */
|
||||||
};
|
};
|
||||||
|
|
||||||
/** multirotor mixer */
|
/** multirotor mixer */
|
||||||
struct mixer_multirotor_s
|
struct mixer_multirotor_s {
|
||||||
{
|
|
||||||
uint8_t rotor_count;
|
uint8_t rotor_count;
|
||||||
struct mixer_control_s controls[4]; /**< controls are roll, pitch, yaw, thrust */
|
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 */
|
struct mixer_rotor_output_s rotors[0]; /**< actual size of the array is set by rotor_count */
|
||||||
|
|||||||
@@ -367,7 +367,7 @@ MPU6000::init()
|
|||||||
// FS & DLPF FS=2000¼/s, DLPF = 98Hz (low pass filter)
|
// FS & DLPF FS=2000¼/s, DLPF = 98Hz (low pass filter)
|
||||||
write_reg(MPUREG_CONFIG, BITS_DLPF_CFG_98HZ);
|
write_reg(MPUREG_CONFIG, BITS_DLPF_CFG_98HZ);
|
||||||
usleep(1000);
|
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);
|
usleep(1000);
|
||||||
|
|
||||||
// product-specific scaling
|
// product-specific scaling
|
||||||
@@ -390,15 +390,16 @@ MPU6000::init()
|
|||||||
case MPU6000_REV_D9:
|
case MPU6000_REV_D9:
|
||||||
case MPU6000_REV_D10:
|
case MPU6000_REV_D10:
|
||||||
// Accel scale 8g (4096 LSB/g)
|
// Accel scale 8g (4096 LSB/g)
|
||||||
write_reg(MPUREG_ACCEL_CONFIG,2<<3);
|
write_reg(MPUREG_ACCEL_CONFIG, 2 << 3);
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
usleep(1000);
|
usleep(1000);
|
||||||
|
|
||||||
// INT CFG => Interrupt on Data Ready
|
// 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);
|
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);
|
usleep(1000);
|
||||||
|
|
||||||
// Oscillator set
|
// Oscillator set
|
||||||
@@ -898,6 +899,7 @@ mpu6000_main(int argc, char *argv[])
|
|||||||
g_dev = nullptr;
|
g_dev = nullptr;
|
||||||
return -EIO;
|
return -EIO;
|
||||||
}
|
}
|
||||||
|
|
||||||
return OK;
|
return OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
+35
-26
@@ -101,9 +101,9 @@ private:
|
|||||||
void task_main();
|
void task_main();
|
||||||
|
|
||||||
static int control_callback_trampoline(uintptr_t handle,
|
static int control_callback_trampoline(uintptr_t handle,
|
||||||
uint8_t control_group,
|
uint8_t control_group,
|
||||||
uint8_t control_index,
|
uint8_t control_index,
|
||||||
float &input);
|
float &input);
|
||||||
int control_callback(uint8_t control_group,
|
int control_callback(uint8_t control_group,
|
||||||
uint8_t control_index,
|
uint8_t control_index,
|
||||||
float &input);
|
float &input);
|
||||||
@@ -363,6 +363,7 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
|
|||||||
} else {
|
} else {
|
||||||
*(unsigned *)arg = 2;
|
*(unsigned *)arg = 2;
|
||||||
}
|
}
|
||||||
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MIXERIOCRESET:
|
case MIXERIOCRESET:
|
||||||
@@ -370,22 +371,27 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
|
|||||||
delete _mixers;
|
delete _mixers;
|
||||||
_mixers = nullptr;
|
_mixers = nullptr;
|
||||||
}
|
}
|
||||||
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MIXERIOCADDSIMPLE: {
|
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);
|
SimpleMixer *mixer = new SimpleMixer(control_callback_trampoline, (uintptr_t)this, mixinfo);
|
||||||
if (mixer->check()) {
|
|
||||||
delete mixer;
|
if (mixer->check()) {
|
||||||
ret = -EINVAL;
|
delete mixer;
|
||||||
} else {
|
ret = -EINVAL;
|
||||||
if (_mixers == nullptr)
|
|
||||||
_mixers = new MixerGroup(control_callback_trampoline, (uintptr_t)this);
|
} else {
|
||||||
_mixers->add_mixer(mixer);
|
if (_mixers == nullptr)
|
||||||
|
_mixers = new MixerGroup(control_callback_trampoline, (uintptr_t)this);
|
||||||
|
|
||||||
|
_mixers->add_mixer(mixer);
|
||||||
|
}
|
||||||
|
|
||||||
|
break;
|
||||||
}
|
}
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
case MIXERIOCADDMULTIROTOR:
|
case MIXERIOCADDMULTIROTOR:
|
||||||
/* XXX not yet supported */
|
/* XXX not yet supported */
|
||||||
@@ -393,20 +399,23 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
|
|||||||
break;
|
break;
|
||||||
|
|
||||||
case MIXERIOCLOADFILE: {
|
case MIXERIOCLOADFILE: {
|
||||||
const char *path = (const char *)arg;
|
const char *path = (const char *)arg;
|
||||||
|
|
||||||
if (_mixers != nullptr) {
|
if (_mixers != nullptr) {
|
||||||
delete _mixers;
|
delete _mixers;
|
||||||
_mixers = nullptr;
|
_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;
|
||||||
}
|
}
|
||||||
_mixers = new MixerGroup(control_callback_trampoline, (uintptr_t)this);
|
|
||||||
if (_mixers->load_from_file(path) != 0) {
|
|
||||||
delete _mixers;
|
|
||||||
_mixers = nullptr;
|
|
||||||
ret = -EINVAL;
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
default:
|
default:
|
||||||
ret = -ENOTTY;
|
ret = -ENOTTY;
|
||||||
|
|||||||
@@ -65,8 +65,8 @@ __BEGIN_DECLS
|
|||||||
* not be allocated.
|
* not be allocated.
|
||||||
*/
|
*/
|
||||||
__EXPORT extern hx_stream_t hx_stream_init(int fd,
|
__EXPORT extern hx_stream_t hx_stream_init(int fd,
|
||||||
hx_stream_rx_callback callback,
|
hx_stream_rx_callback callback,
|
||||||
void *arg);
|
void *arg);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Free a hx_stream object.
|
* Free a hx_stream object.
|
||||||
@@ -106,8 +106,8 @@ __EXPORT extern void hx_stream_set_counters(hx_stream_t stream,
|
|||||||
* set on error.
|
* set on error.
|
||||||
*/
|
*/
|
||||||
__EXPORT extern int hx_stream_send(hx_stream_t stream,
|
__EXPORT extern int hx_stream_send(hx_stream_t stream,
|
||||||
const void *data,
|
const void *data,
|
||||||
size_t count);
|
size_t count);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Handle a byte from the stream.
|
* 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.
|
* @param c The character to process.
|
||||||
*/
|
*/
|
||||||
__EXPORT extern void hx_stream_rx(hx_stream_t stream,
|
__EXPORT extern void hx_stream_rx(hx_stream_t stream,
|
||||||
uint8_t c);
|
uint8_t c);
|
||||||
|
|
||||||
__END_DECLS
|
__END_DECLS
|
||||||
|
|
||||||
|
|||||||
@@ -116,6 +116,7 @@ NullMixer::mix(float *outputs, unsigned space)
|
|||||||
*outputs = 0.0f;
|
*outputs = 0.0f;
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -148,6 +149,7 @@ SimpleMixer::mix(float *outputs, unsigned space)
|
|||||||
|
|
||||||
if (_info == nullptr)
|
if (_info == nullptr)
|
||||||
return 0;
|
return 0;
|
||||||
|
|
||||||
if (space < 1)
|
if (space < 1)
|
||||||
return 0;
|
return 0;
|
||||||
|
|
||||||
@@ -161,6 +163,7 @@ SimpleMixer::mix(float *outputs, unsigned space)
|
|||||||
|
|
||||||
sum += scale(_info->controls[i].scaler, input);
|
sum += scale(_info->controls[i].scaler, input);
|
||||||
}
|
}
|
||||||
|
|
||||||
*outputs = scale(_info->output_scaler, sum);
|
*outputs = scale(_info->output_scaler, sum);
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -149,10 +149,10 @@ public:
|
|||||||
* @param control The returned control
|
* @param control The returned control
|
||||||
* @return Zero if the value was fetched, nonzero otherwise.
|
* @return Zero if the value was fetched, nonzero otherwise.
|
||||||
*/
|
*/
|
||||||
typedef int (* ControlCallback)(uintptr_t handle,
|
typedef int (* ControlCallback)(uintptr_t handle,
|
||||||
uint8_t control_group,
|
uint8_t control_group,
|
||||||
uint8_t control_index,
|
uint8_t control_index,
|
||||||
float &control);
|
float &control);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Constructor.
|
* Constructor.
|
||||||
@@ -336,8 +336,7 @@ private:
|
|||||||
class __EXPORT MultirotorMixer : public Mixer
|
class __EXPORT MultirotorMixer : public Mixer
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
enum Geometry
|
enum Geometry {
|
||||||
{
|
|
||||||
MULTIROTOR_QUAD_PLUS,
|
MULTIROTOR_QUAD_PLUS,
|
||||||
MULTIROTOR_QUAD_X
|
MULTIROTOR_QUAD_X
|
||||||
/* XXX add more here */
|
/* XXX add more here */
|
||||||
|
|||||||
@@ -171,10 +171,12 @@ mixer_load_simple(Mixer::ControlCallback control_cb, uintptr_t cb_handle, int fd
|
|||||||
|
|
||||||
/* first, get the output scaler */
|
/* first, get the output scaler */
|
||||||
ret = mixer_getline(fd, buf, sizeof(buf));
|
ret = mixer_getline(fd, buf, sizeof(buf));
|
||||||
|
|
||||||
if (ret < 1) {
|
if (ret < 1) {
|
||||||
debug("failed reading for output scaler");
|
debug("failed reading for output scaler");
|
||||||
goto fail;
|
goto fail;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (mixer_parse_output_scaler(buf, mixinfo->output_scaler)) {
|
if (mixer_parse_output_scaler(buf, mixinfo->output_scaler)) {
|
||||||
debug("failed parsing output scaler");
|
debug("failed parsing output scaler");
|
||||||
goto fail;
|
goto fail;
|
||||||
@@ -183,17 +185,20 @@ mixer_load_simple(Mixer::ControlCallback control_cb, uintptr_t cb_handle, int fd
|
|||||||
/* now get any inputs */
|
/* now get any inputs */
|
||||||
for (unsigned i = 0; i < inputs; i++) {
|
for (unsigned i = 0; i < inputs; i++) {
|
||||||
ret = mixer_getline(fd, buf, sizeof(buf));
|
ret = mixer_getline(fd, buf, sizeof(buf));
|
||||||
|
|
||||||
if (ret < 1) {
|
if (ret < 1) {
|
||||||
debug("failed reading for control scaler");
|
debug("failed reading for control scaler");
|
||||||
goto fail;
|
goto fail;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (mixer_parse_control_scaler(buf,
|
if (mixer_parse_control_scaler(buf,
|
||||||
mixinfo->controls[i].scaler,
|
mixinfo->controls[i].scaler,
|
||||||
mixinfo->controls[i].control_group,
|
mixinfo->controls[i].control_group,
|
||||||
mixinfo->controls[i].control_index)) {
|
mixinfo->controls[i].control_index)) {
|
||||||
debug("failed parsing control scaler");
|
debug("failed parsing control scaler");
|
||||||
goto fail;
|
goto fail;
|
||||||
}
|
}
|
||||||
|
|
||||||
debug("got control %d", i);
|
debug("got control %d", i);
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -266,8 +271,10 @@ MixerGroup::add_mixer(Mixer *mixer)
|
|||||||
Mixer **mpp;
|
Mixer **mpp;
|
||||||
|
|
||||||
mpp = &_first;
|
mpp = &_first;
|
||||||
|
|
||||||
while (*mpp != nullptr)
|
while (*mpp != nullptr)
|
||||||
mpp = &((*mpp)->_next);
|
mpp = &((*mpp)->_next);
|
||||||
|
|
||||||
*mpp = mixer;
|
*mpp = mixer;
|
||||||
mixer->_next = nullptr;
|
mixer->_next = nullptr;
|
||||||
}
|
}
|
||||||
@@ -282,6 +289,7 @@ MixerGroup::mix(float *outputs, unsigned space)
|
|||||||
index += mixer->mix(outputs + index, space - index);
|
index += mixer->mix(outputs + index, space - index);
|
||||||
mixer = mixer->_next;
|
mixer = mixer->_next;
|
||||||
}
|
}
|
||||||
|
|
||||||
return index;
|
return index;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -303,6 +311,7 @@ MixerGroup::load_from_file(const char *path)
|
|||||||
return -1;
|
return -1;
|
||||||
|
|
||||||
int fd = open(path, O_RDONLY);
|
int fd = open(path, O_RDONLY);
|
||||||
|
|
||||||
if (fd < 0) {
|
if (fd < 0) {
|
||||||
debug("failed to open %s", path);
|
debug("failed to open %s", path);
|
||||||
return -1;
|
return -1;
|
||||||
|
|||||||
Reference in New Issue
Block a user