dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Add support for battery current scaling. Add feedback for mixer load operations.

This commit is contained in:
px4dev 2013-01-15 22:22:15 -08:00
parent 0eb5a070f1
commit b4dcdae03d
3 changed files with 79 additions and 55 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

102
apps/px4io/mixer.cpp
View File

@ -70,6 +70,7 @@ static bool mixer_servos_armed = false;
/* selected control values and count for mixing */
enum mixer_source {
MIX_NONE,
MIX_FMU,
MIX_OVERRIDE,
MIX_FAILSAFE
@ -81,7 +82,6 @@ static int mixer_callback(uintptr_t handle,
uint8_t control_index,
float &control);
static void mix();
static MixerGroup mixer_group(mixer_callback, 0);
void
@ -97,30 +97,56 @@ mixer_tick(void)
debug("AP RX timeout");
}
source = MIX_FAILSAFE;
/*
* Decide which set of controls we're using.
*/
if ((r_setup_features & PX4IO_P_FEAT_ARMING_MANUAL_OVERRIDE_OK) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) {
/* this is for planes, where manual override makes sense */
source = MIX_OVERRIDE;
if (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PPM) {
/* mix from the override controls */
mix();
/* don't actually mix anything - we already have raw PWM values */
source = MIX_NONE;
} else if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) {
} else {
if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
if (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PPM) {
/* FMU has already provided PWM values */
} else {
/* mix from FMU controls */
source = MIX_FMU;
mix();
}
} else {
source = MIX_FAILSAFE;
/* XXX actually, have no idea what to do here... load hardcoded failsafe controls? */
if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) {
/* if allowed, mix from RC inputs directly */
source = MIX_OVERRIDE;
}
}
/*
* Run the mixers.
*/
if (source != MIX_NONE) {
float outputs[IO_SERVO_COUNT];
unsigned mixed;
/* mix */
mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT);
/* scale to PWM and update the servo outputs as required */
for (unsigned i = 0; i < mixed; i++) {
/* save actuator values for FMU readback */
r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
/* scale to servo output */
r_page_servos[i] = (outputs[i] * 500.0f) + 1500;
}
for (unsigned i = mixed; i < IO_SERVO_COUNT; i++)
r_page_servos[i] = 0;
}
#if 0
@ -196,9 +222,12 @@ mixer_tick(void)
/*
* Decide whether the servos should be armed right now.
*
* We must be armed, and we must have a PWM source; either raw from
* FMU or from the mixer.
*/
bool should_arm = (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED);
bool should_arm = ((r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
(r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PPM | PX4IO_P_STATUS_FLAGS_MIXER_OK)));
if (should_arm && !mixer_servos_armed) {
/* need to arm, but not armed */
@ -212,33 +241,6 @@ mixer_tick(void)
}
}
static void
mix()
{
/*
* Run the mixers.
*/
float outputs[IO_SERVO_COUNT];
unsigned mixed;
/* mix */
mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT);
/* scale to PWM and update the servo outputs as required */
for (unsigned i = 0; i < mixed; i++) {
/* save actuator values for FMU readback */
r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
/* scale to servo output */
r_page_servos[i] = (outputs[i] * 500.0f) + 1500;
}
for (unsigned i = mixed; i < IO_SERVO_COUNT; i++)
r_page_servos[i] = 0;
}
static int
mixer_callback(uintptr_t handle,
uint8_t control_group,
@ -264,6 +266,7 @@ mixer_callback(uintptr_t handle,
return -1;
case MIX_FAILSAFE:
case MIX_NONE:
/* XXX we could allow for configuration of per-output failsafe values */
return -1;
}
@ -292,6 +295,7 @@ mixer_handle_text(const void *buffer, size_t length)
debug("reset");
mixer_group.reset();
mixer_text_length = 0;
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
/* FALLTHROUGH */
case F2I_MIXER_ACTION_APPEND:
@ -299,8 +303,10 @@ mixer_handle_text(const void *buffer, size_t length)
/* check for overflow - this is really fatal */
/* XXX could add just what will fit & try to parse, then repeat... */
if ((mixer_text_length + text_length + 1) > sizeof(mixer_text))
if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) {
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
return;
}
/* append mixer text and nul-terminate */
memcpy(&mixer_text[mixer_text_length], msg->text, text_length);
@ -314,6 +320,10 @@ mixer_handle_text(const void *buffer, size_t length)
/* if anything was parsed */
if (resid != mixer_text_length) {
/* ideally, this should test resid == 0 ? */
r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK;
debug("used %u", mixer_text_length - resid);
/* copy any leftover text to the base of the buffer for re-use */

10
apps/px4io/protocol.h
View File

@ -101,6 +101,7 @@
#define PX4IO_P_STATUS_FLAGS_RC_SBUS (1 << 5) /* SBUS input is valid */
#define PX4IO_P_STATUS_FLAGS_FMU_OK (1 << 6) /* controls from FMU are valid */
#define PX4IO_P_STATUS_FLAGS_RAW_PPM (1 << 7) /* raw PPM from FMU is bypassing the mixer */
#define PX4IO_P_STATUS_FLAGS_MIXER_OK (1 << 8) /* mixer is OK */
#define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */
#define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* VBatt is very close to regulator dropout */
@ -111,7 +112,7 @@
#define PX4IO_P_STATUS_ALARMS_RC_LOST (1 << 5)
#define PX4IO_P_STATUS_VBATT 4 /* battery voltage in mV */
#define PX4IO_P_STATUS_TEMPERATURE 5 /* temperature in (units tbd) */
#define PX4IO_P_STATUS_IBATT 5 /* battery current in cA */
/* array of post-mix actuator outputs, -10000..10000 */
#define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */
@ -143,11 +144,12 @@
#define PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK (1 << 3)
#define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */
#define PX4IO_P_SETUP_PWM_LOWRATE 3 /* 'low' PWM frame output rate in Hz */
#define PX4IO_P_SETUP_PWM_HIGHRATE 4 /* 'high' PWM frame output rate in Hz */
#define PX4IO_P_SETUP_RELAYS 5 /* bitmask of relay/switch outputs, 0 = off, 1 = on */
#define PX4IO_P_SETUP_VBATT_SCALE 6 /* battery voltage correction factor (float) */
#define PX4IO_P_SETUP_IBATT_SCALE 7 /* battery current scaling factor (float) */
#define PX4IO_P_SETUP_IBATT_BIAS 8 /* battery current bias value */
/* autopilot control values, -10000..10000 */
#define PX4IO_PAGE_CONTROLS 101 /* 0..CONFIG_CONTROL_COUNT */
@ -187,5 +189,3 @@ struct px4io_mixdata {
char text[0]; /* actual text size may vary */
};
/* maximum size is limited by the link frame size XXX use config values to set this */
#define F2I_MIXER_MAX_TEXT (64 - sizeof(struct px4io_mixdata))

22
apps/px4io/registers.c
View File

@ -60,6 +60,9 @@ volatile uint16_t r_page_setup[] =
[PX4IO_P_SETUP_PWM_LOWRATE] = 50,
[PX4IO_P_SETUP_PWM_HIGHRATE] = 200,
[PX4IO_P_SETUP_RELAYS] = 0,
[PX4IO_P_SETUP_VBATT_SCALE] = 10000,
[PX4IO_P_SETUP_IBATT_SCALE] = 0,
[PX4IO_P_SETUP_IBATT_BIAS] = 0
};
#define PX4IO_P_SETUP_FEATURES_VALID (PX4IO_P_FEAT_ARMING_MANUAL_OVERRIDE_OK)
@ -80,7 +83,7 @@ static const uint16_t r_page_config[] = {
[PX4IO_P_CONFIG_PROTOCOL_VERSION] = 0,
[PX4IO_P_CONFIG_SOFTWARE_VERSION] = 0,
[PX4IO_P_CONFIG_BOOTLOADER_VERSION] = 0,
[PX4IO_P_CONFIG_MAX_TRANSFER] = 64,
[PX4IO_P_CONFIG_MAX_TRANSFER] = 64, /* XXX hardcoded magic number */
[PX4IO_P_CONFIG_CONTROL_COUNT] = PX4IO_CONTROL_CHANNELS,
[PX4IO_P_CONFIG_ACTUATOR_COUNT] = IO_SERVO_COUNT,
[PX4IO_P_CONFIG_RC_INPUT_COUNT] = MAX_CONTROL_CHANNELS,
@ -97,7 +100,7 @@ uint16_t r_page_status[] = {
[PX4IO_P_STATUS_FLAGS] = 0,
[PX4IO_P_STATUS_ALARMS] = 0,
[PX4IO_P_STATUS_VBATT] = 0,
[PX4IO_P_STATUS_TEMPERATURE] = 0
[PX4IO_P_STATUS_IBATT] = 0
};
/**
@ -396,10 +399,21 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
* Intercept corrected for best results @ 12V.
*/
unsigned counts = adc_measure(ADC_VBATT);
r_page_status[PX4IO_P_STATUS_VBATT] = (4150 + (counts * 46)) / 10;
unsigned mV = (4150 + (counts * 46)) / 10;
unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000;
r_page_status[PX4IO_P_STATUS_VBATT] = corrected;
}
/* XXX PX4IO_P_STATUS_TEMPERATURE */
/* PX4IO_P_STATUS_IBATT */
{
unsigned counts = adc_measure(ADC_VBATT);
unsigned scaled = (counts * r_page_setup[PX4IO_P_SETUP_IBATT_SCALE]) / 10000;
int corrected = scaled + REG_TO_SIGNED(r_page_setup[PX4IO_P_SETUP_IBATT_BIAS]);
if (corrected < 0)
corrected = 0;
r_page_status[PX4IO_P_STATUS_IBATT] = corrected;
}
SELECT_PAGE(r_page_status);
break;