forked from Archive/PX4-Autopilot
Add support for battery current scaling. Add feedback for mixer load operations.
This commit is contained in:
parent
0eb5a070f1
commit
b4dcdae03d
|
@ -70,6 +70,7 @@ static bool mixer_servos_armed = false;
|
||||||
|
|
||||||
/* selected control values and count for mixing */
|
/* selected control values and count for mixing */
|
||||||
enum mixer_source {
|
enum mixer_source {
|
||||||
|
MIX_NONE,
|
||||||
MIX_FMU,
|
MIX_FMU,
|
||||||
MIX_OVERRIDE,
|
MIX_OVERRIDE,
|
||||||
MIX_FAILSAFE
|
MIX_FAILSAFE
|
||||||
|
@ -81,7 +82,6 @@ static int mixer_callback(uintptr_t handle,
|
||||||
uint8_t control_index,
|
uint8_t control_index,
|
||||||
float &control);
|
float &control);
|
||||||
|
|
||||||
static void mix();
|
|
||||||
static MixerGroup mixer_group(mixer_callback, 0);
|
static MixerGroup mixer_group(mixer_callback, 0);
|
||||||
|
|
||||||
void
|
void
|
||||||
|
@ -97,30 +97,56 @@ mixer_tick(void)
|
||||||
debug("AP RX timeout");
|
debug("AP RX timeout");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
source = MIX_FAILSAFE;
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Decide which set of controls we're using.
|
* 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;
|
|
||||||
|
|
||||||
/* mix from the override controls */
|
|
||||||
mix();
|
|
||||||
|
|
||||||
} else if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) {
|
|
||||||
|
|
||||||
if (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PPM) {
|
if (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PPM) {
|
||||||
/* FMU has already provided PWM values */
|
|
||||||
|
/* don't actually mix anything - we already have raw PWM values */
|
||||||
|
source = MIX_NONE;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
|
|
||||||
|
if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
|
||||||
|
(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
|
||||||
|
|
||||||
/* mix from FMU controls */
|
/* mix from FMU controls */
|
||||||
source = MIX_FMU;
|
source = MIX_FMU;
|
||||||
mix();
|
|
||||||
}
|
}
|
||||||
} else {
|
|
||||||
source = MIX_FAILSAFE;
|
if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
|
||||||
/* XXX actually, have no idea what to do here... load hardcoded failsafe controls? */
|
(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
|
#if 0
|
||||||
|
@ -196,9 +222,12 @@ mixer_tick(void)
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Decide whether the servos should be armed right now.
|
* 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) {
|
if (should_arm && !mixer_servos_armed) {
|
||||||
/* need to arm, but not 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
|
static int
|
||||||
mixer_callback(uintptr_t handle,
|
mixer_callback(uintptr_t handle,
|
||||||
uint8_t control_group,
|
uint8_t control_group,
|
||||||
|
@ -264,6 +266,7 @@ mixer_callback(uintptr_t handle,
|
||||||
return -1;
|
return -1;
|
||||||
|
|
||||||
case MIX_FAILSAFE:
|
case MIX_FAILSAFE:
|
||||||
|
case MIX_NONE:
|
||||||
/* XXX we could allow for configuration of per-output failsafe values */
|
/* XXX we could allow for configuration of per-output failsafe values */
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
|
@ -292,6 +295,7 @@ mixer_handle_text(const void *buffer, size_t length)
|
||||||
debug("reset");
|
debug("reset");
|
||||||
mixer_group.reset();
|
mixer_group.reset();
|
||||||
mixer_text_length = 0;
|
mixer_text_length = 0;
|
||||||
|
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
|
||||||
|
|
||||||
/* FALLTHROUGH */
|
/* FALLTHROUGH */
|
||||||
case F2I_MIXER_ACTION_APPEND:
|
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 */
|
/* check for overflow - this is really fatal */
|
||||||
/* XXX could add just what will fit & try to parse, then repeat... */
|
/* 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;
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
/* append mixer text and nul-terminate */
|
/* append mixer text and nul-terminate */
|
||||||
memcpy(&mixer_text[mixer_text_length], msg->text, text_length);
|
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 anything was parsed */
|
||||||
if (resid != mixer_text_length) {
|
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);
|
debug("used %u", mixer_text_length - resid);
|
||||||
|
|
||||||
/* copy any leftover text to the base of the buffer for re-use */
|
/* copy any leftover text to the base of the buffer for re-use */
|
||||||
|
|
|
@ -101,6 +101,7 @@
|
||||||
#define PX4IO_P_STATUS_FLAGS_RC_SBUS (1 << 5) /* SBUS input is valid */
|
#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_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_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 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 */
|
#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_ALARMS_RC_LOST (1 << 5)
|
||||||
|
|
||||||
#define PX4IO_P_STATUS_VBATT 4 /* battery voltage in mV */
|
#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 */
|
/* array of post-mix actuator outputs, -10000..10000 */
|
||||||
#define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */
|
#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_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_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_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_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_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 */
|
/* autopilot control values, -10000..10000 */
|
||||||
#define PX4IO_PAGE_CONTROLS 101 /* 0..CONFIG_CONTROL_COUNT */
|
#define PX4IO_PAGE_CONTROLS 101 /* 0..CONFIG_CONTROL_COUNT */
|
||||||
|
@ -187,5 +189,3 @@ struct px4io_mixdata {
|
||||||
char text[0]; /* actual text size may vary */
|
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))
|
|
||||||
|
|
|
@ -60,6 +60,9 @@ volatile uint16_t r_page_setup[] =
|
||||||
[PX4IO_P_SETUP_PWM_LOWRATE] = 50,
|
[PX4IO_P_SETUP_PWM_LOWRATE] = 50,
|
||||||
[PX4IO_P_SETUP_PWM_HIGHRATE] = 200,
|
[PX4IO_P_SETUP_PWM_HIGHRATE] = 200,
|
||||||
[PX4IO_P_SETUP_RELAYS] = 0,
|
[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)
|
#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_PROTOCOL_VERSION] = 0,
|
||||||
[PX4IO_P_CONFIG_SOFTWARE_VERSION] = 0,
|
[PX4IO_P_CONFIG_SOFTWARE_VERSION] = 0,
|
||||||
[PX4IO_P_CONFIG_BOOTLOADER_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_CONTROL_COUNT] = PX4IO_CONTROL_CHANNELS,
|
||||||
[PX4IO_P_CONFIG_ACTUATOR_COUNT] = IO_SERVO_COUNT,
|
[PX4IO_P_CONFIG_ACTUATOR_COUNT] = IO_SERVO_COUNT,
|
||||||
[PX4IO_P_CONFIG_RC_INPUT_COUNT] = MAX_CONTROL_CHANNELS,
|
[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_FLAGS] = 0,
|
||||||
[PX4IO_P_STATUS_ALARMS] = 0,
|
[PX4IO_P_STATUS_ALARMS] = 0,
|
||||||
[PX4IO_P_STATUS_VBATT] = 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.
|
* Intercept corrected for best results @ 12V.
|
||||||
*/
|
*/
|
||||||
unsigned counts = adc_measure(ADC_VBATT);
|
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);
|
SELECT_PAGE(r_page_status);
|
||||||
break;
|
break;
|
||||||
|
|
Loading…
Reference in New Issue