Пример #1
0
static int
registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
{
	switch (page) {

	case PX4IO_PAGE_STATUS:
		switch (offset) {
		case PX4IO_P_STATUS_ALARMS:
			/* clear bits being written */
			r_status_alarms &= ~value;
			break;

		case PX4IO_P_STATUS_FLAGS:
			/* 
			 * Allow FMU override of arming state (to allow in-air restores),
			 * but only if the arming state is not in sync on the IO side.
			 */
			if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
				r_status_flags = value;
			}
			break;

		default:
			/* just ignore writes to other registers in this page */
			break;
		}
		break;

	case PX4IO_PAGE_SETUP:
		switch (offset) {
		case PX4IO_P_SETUP_FEATURES:

			value &= PX4IO_P_SETUP_FEATURES_VALID;

			/* some of the options conflict - give S.BUS out precedence, then ADC RSSI, then PWM RSSI */

			/* switch S.Bus output pin as needed */
			#ifdef ENABLE_SBUS_OUT
			ENABLE_SBUS_OUT(value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT));

			/* disable the conflicting options with SBUS 1 */
			if (value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT)) {
				value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI |
					PX4IO_P_SETUP_FEATURES_ADC_RSSI |
					PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
			}

			/* disable the conflicting options with SBUS 2 */
			if (value & (PX4IO_P_SETUP_FEATURES_SBUS2_OUT)) {
				value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI |
					PX4IO_P_SETUP_FEATURES_ADC_RSSI |
					PX4IO_P_SETUP_FEATURES_SBUS1_OUT);
			}
			#endif

			/* disable the conflicting options with ADC RSSI */
			if (value & (PX4IO_P_SETUP_FEATURES_ADC_RSSI)) {
				value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI |
					PX4IO_P_SETUP_FEATURES_SBUS1_OUT |
					PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
			}

			/* disable the conflicting options with PWM RSSI (without effect here, but for completeness) */
			if (value & (PX4IO_P_SETUP_FEATURES_PWM_RSSI)) {
				value &= ~(PX4IO_P_SETUP_FEATURES_ADC_RSSI |
					PX4IO_P_SETUP_FEATURES_SBUS1_OUT |
					PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
			}

			/* apply changes */
			r_setup_features = value;

			break;

		case PX4IO_P_SETUP_ARMING:

			value &= PX4IO_P_SETUP_ARMING_VALID;

			/*
			 * Update arming state - disarm if no longer OK.
			 * This builds on the requirement that the FMU driver
			 * asks about the FMU arming state on initialization,
			 * so that an in-air reset of FMU can not lead to a
			 * lockup of the IO arming state.
			 */

			if (value & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) {
				r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;
			}

			/*
			 * If the failsafe termination flag is set, do not allow the autopilot to unset it
			 */
			value |= (r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE);

			/*
			 * If failsafe termination is enabled and force failsafe bit is set, do not allow
			 * the autopilot to clear it.
			 */
			if (r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) {
				value |= (r_setup_arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE);
			}

			r_setup_arming = value;

			break;

		case PX4IO_P_SETUP_PWM_RATES:
			value &= PX4IO_P_SETUP_RATES_VALID;
			pwm_configure_rates(value, r_setup_pwm_defaultrate, r_setup_pwm_altrate);
			break;

		case PX4IO_P_SETUP_PWM_DEFAULTRATE:
			if (value < 50) {
				value = 50;
			}
			if (value > 400) {
				value = 400;
			}
			pwm_configure_rates(r_setup_pwm_rates, value, r_setup_pwm_altrate);
			break;

		case PX4IO_P_SETUP_PWM_ALTRATE:
			if (value < 50) {
				value = 50;
			}
			if (value > 400) {
				value = 400;
			}
			pwm_configure_rates(r_setup_pwm_rates, r_setup_pwm_defaultrate, value);
			break;

#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
		case PX4IO_P_SETUP_RELAYS:
			value &= PX4IO_P_SETUP_RELAYS_VALID;
			r_setup_relays = value;
			POWER_RELAY1((value & PX4IO_P_SETUP_RELAYS_POWER1) ? 1 : 0);
			POWER_RELAY2((value & PX4IO_P_SETUP_RELAYS_POWER2) ? 1 : 0);
			POWER_ACC1((value & PX4IO_P_SETUP_RELAYS_ACC1) ? 1 : 0);
			POWER_ACC2((value & PX4IO_P_SETUP_RELAYS_ACC2) ? 1 : 0);
			break;
#endif

		case PX4IO_P_SETUP_VBATT_SCALE:
			r_page_setup[PX4IO_P_SETUP_VBATT_SCALE] = value;
			break;

		case PX4IO_P_SETUP_SET_DEBUG:
			r_page_setup[PX4IO_P_SETUP_SET_DEBUG] = value;
			isr_debug(0, "set debug %u\n", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG]);
			break;

		case PX4IO_P_SETUP_REBOOT_BL:
			if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) {
				// don't allow reboot while armed
				break;
			}

			// check the magic value
			if (value != PX4IO_REBOOT_BL_MAGIC) {
				break;
			}

                        // we schedule a reboot rather than rebooting
                        // immediately to allow the IO board to ACK
                        // the reboot command
                        schedule_reboot(100000);
			break;

		case PX4IO_P_SETUP_DSM:
			dsm_bind(value & 0x0f, (value >> 4) & 0xF);
			break;

		case PX4IO_P_SETUP_FORCE_SAFETY_ON:
			if (value == PX4IO_FORCE_SAFETY_MAGIC) {
				r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
            } else {
                return -1;
            }
			break;

		case PX4IO_P_SETUP_FORCE_SAFETY_OFF:
			if (value == PX4IO_FORCE_SAFETY_MAGIC) {
				r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
			} else {
                return -1;
            }
			break;

		case PX4IO_P_SETUP_RC_THR_FAILSAFE_US:
			if (value > 650 && value < 2350) {
				r_page_setup[PX4IO_P_SETUP_RC_THR_FAILSAFE_US] = value;
			}
			break;

		default:
			return -1;
		}
		break;

	case PX4IO_PAGE_RC_CONFIG: {

		/**
		 * do not allow a RC config change while safety is off
		 */
		if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) {
			break;
		}

		unsigned channel = offset / PX4IO_P_RC_CONFIG_STRIDE;
		unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE;
		uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE];

		if (channel >= PX4IO_RC_INPUT_CHANNELS)
			return -1;

		/* disable the channel until we have a chance to sanity-check it */
		conf[PX4IO_P_RC_CONFIG_OPTIONS] &= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;

		switch (index) {

		case PX4IO_P_RC_CONFIG_MIN:
		case PX4IO_P_RC_CONFIG_CENTER:
		case PX4IO_P_RC_CONFIG_MAX:
		case PX4IO_P_RC_CONFIG_DEADZONE:
		case PX4IO_P_RC_CONFIG_ASSIGNMENT:
			conf[index] = value;
			break;

		case PX4IO_P_RC_CONFIG_OPTIONS:
			value &= PX4IO_P_RC_CONFIG_OPTIONS_VALID;
			r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;

			/* clear any existing RC disabled flag */
			r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED);

			/* set all options except the enabled option */
			conf[index] = value & ~PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;

			/* should the channel be enabled? */
			/* this option is normally set last */
			if (value & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
				uint8_t count = 0;
				bool disabled = false;

				/* assert min..center..max ordering */
				if (conf[PX4IO_P_RC_CONFIG_MIN] < 500) {
					count++;
				}
				if (conf[PX4IO_P_RC_CONFIG_MAX] > 2500) {
					count++;
				}
				if (conf[PX4IO_P_RC_CONFIG_CENTER] < conf[PX4IO_P_RC_CONFIG_MIN]) {
					count++;
				}
				if (conf[PX4IO_P_RC_CONFIG_CENTER] > conf[PX4IO_P_RC_CONFIG_MAX]) {
					count++;
				}
				/* assert deadzone is sane */
				if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) {
					count++;
				}

				if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) {
					disabled = true;
				} else if ((conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) &&
					   (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] != PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH)) {
					count++;
				}

				/* sanity checks pass, enable channel */
				if (count) {
					isr_debug(0, "ERROR: %d config error(s) for RC%d.\n", count, (channel + 1));
					r_status_flags &= ~PX4IO_P_STATUS_FLAGS_INIT_OK;
				} else if (!disabled) {
					conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
				}
			}
			break;
			/* inner switch: case PX4IO_P_RC_CONFIG_OPTIONS */

		}
		break;
		/* case PX4IO_RC_PAGE_CONFIG */
	}

	case PX4IO_PAGE_TEST:
		switch (offset) {
		case PX4IO_P_TEST_LED:
			LED_AMBER(value & 1);
			break;
		}
		break;

	default:
		return -1;
	}
	return 0;
}
Пример #2
0
static int
registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
{
	switch (page) {

	case PX4IO_PAGE_STATUS:
		switch (offset) {
		case PX4IO_P_STATUS_ALARMS:
			/* clear bits being written */
			r_status_alarms &= ~value;
			break;

		case PX4IO_P_STATUS_FLAGS:
			/* 
			 * Allow FMU override of arming state (to allow in-air restores),
			 * but only if the arming state is not in sync on the IO side.
			 */
			if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
				r_status_flags = value;
			}
			break;

		default:
			/* just ignore writes to other registers in this page */
			break;
		}
		break;

	case PX4IO_PAGE_SETUP:
		switch (offset) {
		case PX4IO_P_SETUP_FEATURES:

			value &= PX4IO_P_SETUP_FEATURES_VALID;
			r_setup_features = value;

			/* no implemented feature selection at this point */

			break;

		case PX4IO_P_SETUP_ARMING:

			value &= PX4IO_P_SETUP_ARMING_VALID;

			/*
			 * Update arming state - disarm if no longer OK.
			 * This builds on the requirement that the FMU driver
			 * asks about the FMU arming state on initialization,
			 * so that an in-air reset of FMU can not lead to a
			 * lockup of the IO arming state.
			 */
			if ((r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) && !(value & PX4IO_P_SETUP_ARMING_ARM_OK)) {
				r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
			}

			r_setup_arming = value;

			break;

		case PX4IO_P_SETUP_PWM_RATES:
			value &= PX4IO_P_SETUP_RATES_VALID;
			r_setup_pwm_rates = value;
			/* XXX re-configure timers */
			break;

		case PX4IO_P_SETUP_PWM_LOWRATE:
			if (value < 50)
				value = 50;
			if (value > 400)
				value = 400;
			r_setup_pwm_lowrate = value;
			/* XXX re-configure timers */
			break;

		case PX4IO_P_SETUP_PWM_HIGHRATE:
			if (value < 50)
				value = 50;
			if (value > 400)
				value = 400;
			r_setup_pwm_highrate = value;
			/* XXX re-configure timers */
			break;

		case PX4IO_P_SETUP_RELAYS:
			value &= PX4IO_P_SETUP_RELAYS_VALID;
			r_setup_relays = value;
			POWER_RELAY1(value & (1 << 0) ? 1 : 0);
			POWER_RELAY2(value & (1 << 1) ? 1 : 0);
			POWER_ACC1(value & (1 << 2) ? 1 : 0);
			POWER_ACC2(value & (1 << 3) ? 1 : 0);
			break;

		case PX4IO_P_SETUP_SET_DEBUG:
			debug_level = value;
			isr_debug(0, "set debug %u\n", (unsigned)debug_level);
			break;

		default:
			return -1;
		}
		break;

	case PX4IO_PAGE_RC_CONFIG: {
		unsigned channel = offset / PX4IO_P_RC_CONFIG_STRIDE;
		unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE;
		uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE];

		if (channel >= MAX_CONTROL_CHANNELS)
			return -1;

		/* disable the channel until we have a chance to sanity-check it */
		conf[PX4IO_P_RC_CONFIG_OPTIONS] &= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;

		switch (index) {

		case PX4IO_P_RC_CONFIG_MIN:
		case PX4IO_P_RC_CONFIG_CENTER:
		case PX4IO_P_RC_CONFIG_MAX:
		case PX4IO_P_RC_CONFIG_DEADZONE:
		case PX4IO_P_RC_CONFIG_ASSIGNMENT:
			conf[index] = value;
			break;

		case PX4IO_P_RC_CONFIG_OPTIONS:
			value &= PX4IO_P_RC_CONFIG_OPTIONS_VALID;

			/* set all options except the enabled option */
			conf[index] = value & ~PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;

			/* should the channel be enabled? */
			/* this option is normally set last */
			if (value & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
				/* assert min..center..max ordering */
				if (conf[PX4IO_P_RC_CONFIG_MIN] < 500)
					break;
				if (conf[PX4IO_P_RC_CONFIG_MAX] > 2500)
					break;
				if (conf[PX4IO_P_RC_CONFIG_CENTER] < conf[PX4IO_P_RC_CONFIG_MIN])
					break;
				if (conf[PX4IO_P_RC_CONFIG_CENTER] > conf[PX4IO_P_RC_CONFIG_MAX])
					break;
				/* assert deadzone is sane */
				if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500)
					break;
				if (conf[PX4IO_P_RC_CONFIG_MIN] > (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]))
					break;
				if (conf[PX4IO_P_RC_CONFIG_MAX] < (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]))
					break;
				if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS)
					break;

				/* sanity checks pass, enable channel */
				conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
			}
			break;
			/* inner switch: case PX4IO_P_RC_CONFIG_OPTIONS */

		}
		break;
		/* case PX4IO_RC_PAGE_CONFIG */
	}

	default:
		return -1;
	}
	return 0;
}
Пример #3
0
static void
comms_handle_command(const void *buffer, size_t length)
{
	const struct px4io_command *cmd = (struct px4io_command *)buffer;

	if (length != sizeof(*cmd))
		return;

	irqstate_t flags = irqsave();

	/* fetch new PWM output values */
	for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++)
		system_state.fmu_channel_data[i] = cmd->output_control[i];

	/* if the IO is armed and the FMU gets disarmed, the IO must also disarm */
	if (system_state.arm_ok && !cmd->arm_ok)
		system_state.armed = false;

	system_state.arm_ok = cmd->arm_ok;
	system_state.vector_flight_mode_ok = cmd->vector_flight_mode_ok;
	system_state.manual_override_ok = cmd->manual_override_ok;
	system_state.mixer_fmu_available = true;
	system_state.fmu_data_received_time = hrt_absolute_time();

	/* set PWM update rate if changed (after limiting) */
	uint16_t new_servo_rate = cmd->servo_rate;

	/* reject faster than 500 Hz updates */
	if (new_servo_rate > 500) {
		new_servo_rate = 500;
	}

	/* reject slower than 50 Hz updates */
	if (new_servo_rate < 50) {
		new_servo_rate = 50;
	}

	if (system_state.servo_rate != new_servo_rate) {
		up_pwm_servo_set_rate(new_servo_rate);
		system_state.servo_rate = new_servo_rate;
	}

	/*
	 * update servo values immediately.
	 * the updates are done in addition also
	 * in the mainloop, since this function will only
	 * update with a connected FMU.
	 */
	mixer_tick();

	/* handle relay state changes here */
	for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) {
		if (system_state.relays[i] != cmd->relay_state[i]) {
			switch (i) {
			case 0:
				POWER_ACC1(cmd->relay_state[i]);
				break;

			case 1:
				POWER_ACC2(cmd->relay_state[i]);
				break;

			case 2:
				POWER_RELAY1(cmd->relay_state[i]);
				break;

			case 3:
				POWER_RELAY2(cmd->relay_state[i]);
				break;
			}
		}

		system_state.relays[i] = cmd->relay_state[i];
	}

	irqrestore(flags);
}
Пример #4
0
static int
registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
{
	switch (page) {

	case PX4IO_PAGE_STATUS:
		switch (offset) {
		case PX4IO_P_STATUS_ALARMS:
			/* clear bits being written */
			r_status_alarms &= ~value;
			break;

		case PX4IO_P_STATUS_FLAGS:
			/* 
			 * Allow FMU override of arming state (to allow in-air restores),
			 * but only if the arming state is not in sync on the IO side.
			 */
			if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
				r_status_flags = value;
			}
			break;

		default:
			/* just ignore writes to other registers in this page */
			break;
		}
		break;

	case PX4IO_PAGE_SETUP:
		switch (offset) {
		case PX4IO_P_SETUP_FEATURES:

			value &= PX4IO_P_SETUP_FEATURES_VALID;
			r_setup_features = value;

			/* no implemented feature selection at this point */

			break;

		case PX4IO_P_SETUP_ARMING:

			value &= PX4IO_P_SETUP_ARMING_VALID;

			/*
			 * Update arming state - disarm if no longer OK.
			 * This builds on the requirement that the FMU driver
			 * asks about the FMU arming state on initialization,
			 * so that an in-air reset of FMU can not lead to a
			 * lockup of the IO arming state.
			 */
			if ((r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) && !(value & PX4IO_P_SETUP_ARMING_ARM_OK)) {
				r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
			}

			r_setup_arming = value;

			break;

		case PX4IO_P_SETUP_PWM_RATES:
			value &= PX4IO_P_SETUP_RATES_VALID;
			pwm_configure_rates(value, r_setup_pwm_defaultrate, r_setup_pwm_altrate);
			break;

		case PX4IO_P_SETUP_PWM_DEFAULTRATE:
			if (value < 50)
				value = 50;
			if (value > 400)
				value = 400;
			pwm_configure_rates(r_setup_pwm_rates, value, r_setup_pwm_altrate);
			break;

		case PX4IO_P_SETUP_PWM_ALTRATE:
			if (value < 50)
				value = 50;
			if (value > 400)
				value = 400;
			pwm_configure_rates(r_setup_pwm_rates, r_setup_pwm_defaultrate, value);
			break;

		case PX4IO_P_SETUP_RELAYS:
			value &= PX4IO_P_SETUP_RELAYS_VALID;
			r_setup_relays = value;
			POWER_RELAY1(value & (1 << 0) ? 1 : 0);
			POWER_RELAY2(value & (1 << 1) ? 1 : 0);
			POWER_ACC1(value & (1 << 2) ? 1 : 0);
			POWER_ACC2(value & (1 << 3) ? 1 : 0);
			break;

		case PX4IO_P_SETUP_SET_DEBUG:
			r_page_setup[PX4IO_P_SETUP_SET_DEBUG] = value;
			isr_debug(0, "set debug %u\n", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG]);
			break;

		default:
			return -1;
		}
		break;

	case PX4IO_PAGE_RC_CONFIG: {

		/* do not allow a RC config change while fully armed */
		if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) &&
		    /* IO is armed */  (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
			break;
		}

		unsigned channel = offset / PX4IO_P_RC_CONFIG_STRIDE;
		unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE;
		uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE];

		if (channel >= MAX_CONTROL_CHANNELS)
			return -1;

		/* disable the channel until we have a chance to sanity-check it */
		conf[PX4IO_P_RC_CONFIG_OPTIONS] &= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;

		switch (index) {

		case PX4IO_P_RC_CONFIG_MIN:
		case PX4IO_P_RC_CONFIG_CENTER:
		case PX4IO_P_RC_CONFIG_MAX:
		case PX4IO_P_RC_CONFIG_DEADZONE:
		case PX4IO_P_RC_CONFIG_ASSIGNMENT:
			conf[index] = value;
			break;

		case PX4IO_P_RC_CONFIG_OPTIONS:
			value &= PX4IO_P_RC_CONFIG_OPTIONS_VALID;
			r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;

			/* set all options except the enabled option */
			conf[index] = value & ~PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;

			/* should the channel be enabled? */
			/* this option is normally set last */
			if (value & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
				uint8_t count = 0;

				/* assert min..center..max ordering */
				if (conf[PX4IO_P_RC_CONFIG_MIN] < 500) {
					count++;
				}
				if (conf[PX4IO_P_RC_CONFIG_MAX] > 2500) {
					count++;
				}
				if (conf[PX4IO_P_RC_CONFIG_CENTER] < conf[PX4IO_P_RC_CONFIG_MIN]) {
					count++;
				}
				if (conf[PX4IO_P_RC_CONFIG_CENTER] > conf[PX4IO_P_RC_CONFIG_MAX]) {
					count++;
				}

				/* assert deadzone is sane */
				if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) {
					count++;
				}
				// The following check isn't needed as constraint checks in controls.c will catch this.
				//if (conf[PX4IO_P_RC_CONFIG_MIN] > (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
				//	count++;
				//}
				//if (conf[PX4IO_P_RC_CONFIG_MAX] < (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
				//	count++;
				//}
				if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS) {
					count++;
				}

				/* sanity checks pass, enable channel */
				if (count) {
					isr_debug(0, "ERROR: %d config error(s) for RC%d.\n", count, (channel + 1));
					r_status_flags &= ~PX4IO_P_STATUS_FLAGS_INIT_OK;
				} else {
					conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
				}
			}
			break;
			/* inner switch: case PX4IO_P_RC_CONFIG_OPTIONS */

		}
		break;
		/* case PX4IO_RC_PAGE_CONFIG */
	}

	default:
		return -1;
	}
	return 0;
}
Пример #5
0
static int
registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
{
    switch (page) {

    case PX4IO_PAGE_STATUS:
        switch (offset) {
        case PX4IO_P_STATUS_ALARMS:
            /* clear bits being written */
            r_status_alarms &= ~value;
            break;

        case PX4IO_P_STATUS_FLAGS:
            /*
             * Allow FMU override of arming state (to allow in-air restores),
             * but only if the arming state is not in sync on the IO side.
             */
            if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
                r_status_flags = value;
            }
            break;

        default:
            /* just ignore writes to other registers in this page */
            break;
        }
        break;

    case PX4IO_PAGE_SETUP:
        switch (offset) {
        case PX4IO_P_SETUP_FEATURES:

            value &= PX4IO_P_SETUP_FEATURES_VALID;
            r_setup_features = value;

            /* no implemented feature selection at this point */

            break;

        case PX4IO_P_SETUP_ARMING:

            value &= PX4IO_P_SETUP_ARMING_VALID;

            /*
             * Update arming state - disarm if no longer OK.
             * This builds on the requirement that the FMU driver
             * asks about the FMU arming state on initialization,
             * so that an in-air reset of FMU can not lead to a
             * lockup of the IO arming state.
             */

            // XXX do not reset IO's safety state by FMU for now
            // if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
            // 	r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
            // }

            if (value & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) {
                r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;
            }

            r_setup_arming = value;

            break;

        case PX4IO_P_SETUP_PWM_RATES:
            value &= PX4IO_P_SETUP_RATES_VALID;
            pwm_configure_rates(value, r_setup_pwm_defaultrate, r_setup_pwm_altrate);
            break;

        case PX4IO_P_SETUP_PWM_DEFAULTRATE:
            if (value < 50)
                value = 50;
            if (value > 400)
                value = 400;
            pwm_configure_rates(r_setup_pwm_rates, value, r_setup_pwm_altrate);
            break;

        case PX4IO_P_SETUP_PWM_ALTRATE:
            if (value < 50)
                value = 50;
            if (value > 400)
                value = 400;
            pwm_configure_rates(r_setup_pwm_rates, r_setup_pwm_defaultrate, value);
            break;

#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
        case PX4IO_P_SETUP_RELAYS:
            value &= PX4IO_P_SETUP_RELAYS_VALID;
            r_setup_relays = value;
            POWER_RELAY1((value & PX4IO_P_SETUP_RELAYS_POWER1) ? 1 : 0);
            POWER_RELAY2((value & PX4IO_P_SETUP_RELAYS_POWER2) ? 1 : 0);
            POWER_ACC1((value & PX4IO_P_SETUP_RELAYS_ACC1) ? 1 : 0);
            POWER_ACC2((value & PX4IO_P_SETUP_RELAYS_ACC2) ? 1 : 0);
            break;
#endif

        case PX4IO_P_SETUP_VBATT_SCALE:
            r_page_setup[PX4IO_P_SETUP_VBATT_SCALE] = value;
            break;

        case PX4IO_P_SETUP_SET_DEBUG:
            r_page_setup[PX4IO_P_SETUP_SET_DEBUG] = value;
            isr_debug(0, "set debug %u\n", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG]);
            break;

        case PX4IO_P_SETUP_DSM:
            dsm_bind(value & 0x0f, (value >> 4) & 7);
            break;

        default:
            return -1;
        }
        break;

    case PX4IO_PAGE_RC_CONFIG: {

        /**
         * do not allow a RC config change while outputs armed
         */
        if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
                (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ||
                (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
            break;
        }

        unsigned channel = offset / PX4IO_P_RC_CONFIG_STRIDE;
        unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE;
        uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE];

        if (channel >= PX4IO_CONTROL_CHANNELS)
            return -1;

        /* disable the channel until we have a chance to sanity-check it */
        conf[PX4IO_P_RC_CONFIG_OPTIONS] &= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;

        switch (index) {

        case PX4IO_P_RC_CONFIG_MIN:
        case PX4IO_P_RC_CONFIG_CENTER:
        case PX4IO_P_RC_CONFIG_MAX:
        case PX4IO_P_RC_CONFIG_DEADZONE:
        case PX4IO_P_RC_CONFIG_ASSIGNMENT:
            conf[index] = value;
            break;

        case PX4IO_P_RC_CONFIG_OPTIONS:
            value &= PX4IO_P_RC_CONFIG_OPTIONS_VALID;
            r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;

            /* clear any existing RC disabled flag */
            r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED);

            /* set all options except the enabled option */
            conf[index] = value & ~PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;

            /* should the channel be enabled? */
            /* this option is normally set last */
            if (value & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
                uint8_t count = 0;

                /* assert min..center..max ordering */
                if (conf[PX4IO_P_RC_CONFIG_MIN] < 500) {
                    count++;
                }
                if (conf[PX4IO_P_RC_CONFIG_MAX] > 2500) {
                    count++;
                }
                if (conf[PX4IO_P_RC_CONFIG_CENTER] < conf[PX4IO_P_RC_CONFIG_MIN]) {
                    count++;
                }
                if (conf[PX4IO_P_RC_CONFIG_CENTER] > conf[PX4IO_P_RC_CONFIG_MAX]) {
                    count++;
                }
                /* assert deadzone is sane */
                if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) {
                    count++;
                }
                if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_CONTROL_CHANNELS) {
                    count++;
                }

                /* sanity checks pass, enable channel */
                if (count) {
                    isr_debug(0, "ERROR: %d config error(s) for RC%d.\n", count, (channel + 1));
                    r_status_flags &= ~PX4IO_P_STATUS_FLAGS_INIT_OK;
                } else {
                    conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
                }
            }
            break;
            /* inner switch: case PX4IO_P_RC_CONFIG_OPTIONS */

        }
        break;
        /* case PX4IO_RC_PAGE_CONFIG */
    }

    case PX4IO_PAGE_TEST:
        switch (offset) {
        case PX4IO_P_TEST_LED:
            LED_AMBER(value & 1);
            break;
        }
        break;

    default:
        return -1;
    }
    return 0;
}