diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index 178b0bb431..932aab930f 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -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 */ diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index a2be4bfc4c..39ee4c0b1a 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -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)) diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index 2f411eebf7..0206e0db0e 100644 --- a/apps/px4io/registers.c +++ b/apps/px4io/registers.c @@ -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;