int
user_start(int argc, char *argv[])
{
/* run C++ ctors before we go any further */
up_cxxinitialize();
/* reset all to zero */
memset(&system_state, 0, sizeof(system_state));
/* configure the high-resolution time/callout interface */
hrt_init();
/* calculate our fw CRC so FMU can decide if we need to update */
calculate_fw_crc();
/*
* Poll at 1ms intervals for received bytes that have not triggered
* a DMA event.
*/
#ifdef CONFIG_ARCH_DMA
hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL);
#endif
/* print some startup info */
lowsyslog("\nPX4IO: starting\n");
/* default all the LEDs to off while we start */
LED_AMBER(false);
LED_BLUE(false);
LED_SAFETY(false);
#ifdef GPIO_LED4
LED_RING(false);
#endif
/* turn on servo power (if supported) */
#ifdef POWER_SERVO
POWER_SERVO(true);
#endif
/* turn off S.Bus out (if supported) */
#ifdef ENABLE_SBUS_OUT
ENABLE_SBUS_OUT(false);
#endif
/* start the safety switch handler */
safety_init();
/* configure the first 8 PWM outputs (i.e. all of them) */
up_pwm_servo_init(0xff);
/* initialise the control inputs */
controls_init();
/* set up the ADC */
adc_init();
/* start the FMU interface */
interface_init();
/* add a performance counter for mixing */
perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix");
/* add a performance counter for controls */
perf_counter_t controls_perf = perf_alloc(PC_ELAPSED, "controls");
/* and one for measuring the loop rate */
perf_counter_t loop_perf = perf_alloc(PC_INTERVAL, "loop");
struct mallinfo minfo = mallinfo();
lowsyslog("MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks);
/* initialize PWM limit lib */
pwm_limit_init(&pwm_limit);
/*
* P O L I C E L I G H T S
*
* Not enough memory, lock down.
*
* We might need to allocate mixers later, and this will
* ensure that a developer doing a change will notice
* that he just burned the remaining RAM with static
* allocations. We don't want him to be able to
* get past that point. This needs to be clearly
* documented in the dev guide.
*
*/
if (minfo.mxordblk < 600) {
lowsyslog("ERR: not enough MEM");
bool phase = false;
while (true) {
if (phase) {
LED_AMBER(true);
LED_BLUE(false);
} else {
LED_AMBER(false);
LED_BLUE(true);
}
up_udelay(250000);
phase = !phase;
}
}
/* Start the failsafe led init */
failsafe_led_init();
/*
* Run everything in a tight loop.
*/
uint64_t last_debug_time = 0;
uint64_t last_heartbeat_time = 0;
for (;;) {
/* track the rate at which the loop is running */
perf_count(loop_perf);
/* kick the mixer */
perf_begin(mixer_perf);
mixer_tick();
perf_end(mixer_perf);
/* kick the control inputs */
perf_begin(controls_perf);
controls_tick();
perf_end(controls_perf);
if ((hrt_absolute_time() - last_heartbeat_time) > 250 * 1000) {
last_heartbeat_time = hrt_absolute_time();
heartbeat_blink();
}
ring_blink();
check_reboot();
/* check for debug activity (default: none) */
show_debug_messages();
/* post debug state at ~1Hz - this is via an auxiliary serial port
* DEFAULTS TO OFF!
*/
if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) {
isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x m=%u",
(unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG],
(unsigned)r_status_flags,
(unsigned)r_setup_arming,
(unsigned)r_setup_features,
(unsigned)mallinfo().mxordblk);
last_debug_time = hrt_absolute_time();
}
}
}
int
user_start(int argc, char *argv[])
{
/* run C++ ctors before we go any further */
up_cxxinitialize();
/* reset all to zero */
memset(&system_state, 0, sizeof(system_state));
/* configure the high-resolution time/callout interface */
hrt_init();
/*
* Poll at 1ms intervals for received bytes that have not triggered
* a DMA event.
*/
#ifdef CONFIG_ARCH_DMA
hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL);
#endif
/* print some startup info */
lowsyslog("\nPX4IO: starting\n");
/* default all the LEDs to off while we start */
LED_AMBER(false);
LED_BLUE(false);
LED_SAFETY(false);
/* turn on servo power */
POWER_SERVO(true);
/* start the safety switch handler */
safety_init();
/* configure the first 8 PWM outputs (i.e. all of them) */
up_pwm_servo_init(0xff);
/* initialise the control inputs */
controls_init();
#ifdef CONFIG_STM32_I2C1
/* start the i2c handler */
i2c_init();
#endif
/* add a performance counter for mixing */
perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix");
/* add a performance counter for controls */
perf_counter_t controls_perf = perf_alloc(PC_ELAPSED, "controls");
/* and one for measuring the loop rate */
perf_counter_t loop_perf = perf_alloc(PC_INTERVAL, "loop");
struct mallinfo minfo = mallinfo();
lowsyslog("MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks);
#if 0
/* not enough memory, lock down */
if (minfo.mxordblk < 500) {
lowsyslog("ERR: not enough MEM");
bool phase = false;
if (phase) {
LED_AMBER(true);
LED_BLUE(false);
} else {
LED_AMBER(false);
LED_BLUE(true);
}
phase = !phase;
usleep(300000);
}
#endif
/*
* Run everything in a tight loop.
*/
uint64_t last_debug_time = 0;
for (;;) {
/* track the rate at which the loop is running */
perf_count(loop_perf);
/* kick the mixer */
perf_begin(mixer_perf);
mixer_tick();
perf_end(mixer_perf);
/* kick the control inputs */
perf_begin(controls_perf);
controls_tick();
perf_end(controls_perf);
/* check for debug activity */
show_debug_messages();
/* post debug state at ~1Hz */
if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) {
struct mallinfo minfo = mallinfo();
isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x r=%u m=%u",
(unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG],
(unsigned)r_status_flags,
(unsigned)r_setup_arming,
(unsigned)r_setup_features,
(unsigned)i2c_loop_resets,
(unsigned)minfo.mxordblk);
last_debug_time = hrt_absolute_time();
}
}
}
void
controls_tick() {
/*
* Gather R/C control inputs from supported sources.
*
* Note that if you're silly enough to connect more than
* one control input source, they're going to fight each
* other. Don't do that.
*/
/* receive signal strenght indicator (RSSI). 0 = no connection, 255: perfect connection */
uint16_t rssi = 0;
#ifdef ADC_RSSI
if (r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) {
unsigned counts = adc_measure(ADC_RSSI);
if (counts != 0xffff) {
/* use 1:1 scaling on 3.3V ADC input */
unsigned mV = counts * 3300 / 4096;
/* scale to 0..253 */
rssi = mV / 13;
}
}
#endif
perf_begin(c_gather_dsm);
uint16_t temp_count = r_raw_rc_count;
bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count);
if (dsm_updated) {
r_raw_rc_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
r_raw_rc_count = temp_count & 0x7fff;
if (temp_count & 0x8000)
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
else
r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
}
perf_end(c_gather_dsm);
perf_begin(c_gather_sbus);
bool sbus_status = (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS);
bool sbus_failsafe, sbus_frame_drop;
bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, PX4IO_RC_INPUT_CHANNELS);
if (sbus_updated) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS;
rssi = 255;
if (sbus_frame_drop) {
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP;
rssi = 100;
} else {
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
}
if (sbus_failsafe) {
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE;
rssi = 0;
} else {
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
}
}
perf_end(c_gather_sbus);
/*
* XXX each S.bus frame will cause a PPM decoder interrupt
* storm (lots of edges). It might be sensible to actually
* disable the PPM decoder completely if we have S.bus signal.
*/
perf_begin(c_gather_ppm);
bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_raw_rc_input[PX4IO_P_RAW_RC_DATA]);
if (ppm_updated) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM;
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
}
perf_end(c_gather_ppm);
/* limit number of channels to allowable data size */
if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS)
r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS;
/* store RSSI */
r_page_raw_rc_input[PX4IO_P_RAW_RC_NRSSI] = rssi;
/*
* In some cases we may have received a frame, but input has still
* been lost.
*/
bool rc_input_lost = false;
/*
* If we received a new frame from any of the RC sources, process it.
*/
if (dsm_updated || sbus_updated || ppm_updated) {
/* record a bitmask of channels assigned */
unsigned assigned_channels = 0;
/* update RC-received timestamp */
system_state.rc_channels_timestamp_received = hrt_absolute_time();
/* do not command anything in failsafe, kick in the RC loss counter */
if (!(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
/* update RC-received timestamp */
system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received;
/* map raw inputs to mapped inputs */
/* XXX mapping should be atomic relative to protocol */
for (unsigned i = 0; i < r_raw_rc_count; i++) {
/* map the input channel */
uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE];
if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
uint16_t raw = r_raw_rc_values[i];
int16_t scaled;
/*
* 1) Constrain to min/max values, as later processing depends on bounds.
*/
if (raw < conf[PX4IO_P_RC_CONFIG_MIN])
raw = conf[PX4IO_P_RC_CONFIG_MIN];
if (raw > conf[PX4IO_P_RC_CONFIG_MAX])
raw = conf[PX4IO_P_RC_CONFIG_MAX];
/*
* 2) Scale around the mid point differently for lower and upper range.
*
* This is necessary as they don't share the same endpoints and slope.
*
* First normalize to 0..1 range with correct sign (below or above center),
* then scale to 20000 range (if center is an actual center, -10000..10000,
* if parameters only support half range, scale to 10000 range, e.g. if
* center == min 0..10000, if center == max -10000..0).
*
* As the min and max bounds were enforced in step 1), division by zero
* cannot occur, as for the case of center == min or center == max the if
* statement is mutually exclusive with the arithmetic NaN case.
*
* DO NOT REMOVE OR ALTER STEP 1!
*/
if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]));
} else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));
} else {
/* in the configured dead zone, output zero */
scaled = 0;
}
/* invert channel if requested */
if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE)
scaled = -scaled;
/* and update the scaled/mapped version */
unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
if (mapped < PX4IO_CONTROL_CHANNELS) {
/* invert channel if pitch - pulling the lever down means pitching up by convention */
if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */
scaled = -scaled;
r_rc_values[mapped] = SIGNED_TO_REG(scaled);
assigned_channels |= (1 << mapped);
}
}
}
/* set un-assigned controls to zero */
for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
if (!(assigned_channels & (1 << i)))
r_rc_values[i] = 0;
}
/* set RC OK flag, as we got an update */
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK;
/* if we have enough channels (5) to control the vehicle, the mapping is ok */
if (assigned_channels > 4) {
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK;
} else {
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK);
}
}
/*
* Export the valid channel bitmap
*/
r_rc_valid = assigned_channels;
}
/*
* If we haven't seen any new control data in 200ms, assume we
* have lost input.
*/
if (hrt_elapsed_time(&system_state.rc_channels_timestamp_received) > 200000) {
rc_input_lost = true;
/* clear the input-kind flags here */
r_status_flags &= ~(
PX4IO_P_STATUS_FLAGS_RC_PPM |
PX4IO_P_STATUS_FLAGS_RC_DSM |
PX4IO_P_STATUS_FLAGS_RC_SBUS);
}
/*
* Handle losing RC input
*/
/* this kicks in if the receiver is gone or the system went to failsafe */
if (rc_input_lost || (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
/* Clear the RC input status flag, clear manual override flag */
r_status_flags &= ~(
PX4IO_P_STATUS_FLAGS_OVERRIDE |
PX4IO_P_STATUS_FLAGS_RC_OK);
/* Mark all channels as invalid, as we just lost the RX */
r_rc_valid = 0;
/* Set the RC_LOST alarm */
r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST;
}
/* this kicks in if the receiver is completely gone */
if (rc_input_lost) {
/* Set channel count to zero */
r_raw_rc_count = 0;
}
/*
* Check for manual override.
*
* The PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK flag must be set, and we
* must have R/C input.
* Override is enabled if either the hardcoded channel / value combination
* is selected, or the AP has requested it.
*/
if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) {
bool override = false;
/*
* Check mapped channel 5 (can be any remote channel,
* depends on RC_MAP_OVER parameter);
* If the value is 'high' then the pilot has
* requested override.
*
*/
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(r_rc_values[4]) < RC_CHANNEL_LOW_THRESH))
override = true;
if (override) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE;
/* mix new RC input control values to servos */
if (dsm_updated || sbus_updated || ppm_updated)
mixer_tick();
} else {
void
controls_tick() {
/*
* Gather R/C control inputs from supported sources.
*
* Note that if you're silly enough to connect more than
* one control input source, they're going to fight each
* other. Don't do that.
*/
perf_begin(c_gather_dsm);
bool dsm_updated = dsm_input(r_raw_rc_values, &r_raw_rc_count);
if (dsm_updated)
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
perf_end(c_gather_dsm);
perf_begin(c_gather_sbus);
bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count);
if (sbus_updated)
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS;
perf_end(c_gather_sbus);
/*
* XXX each S.bus frame will cause a PPM decoder interrupt
* storm (lots of edges). It might be sensible to actually
* disable the PPM decoder completely if we have S.bus signal.
*/
perf_begin(c_gather_ppm);
bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count);
if (ppm_updated)
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM;
perf_end(c_gather_ppm);
ASSERT(r_raw_rc_count <= MAX_CONTROL_CHANNELS);
/*
* In some cases we may have received a frame, but input has still
* been lost.
*/
bool rc_input_lost = false;
/*
* If we received a new frame from any of the RC sources, process it.
*/
if (dsm_updated || sbus_updated || ppm_updated) {
/* update RC-received timestamp */
system_state.rc_channels_timestamp = hrt_absolute_time();
/* record a bitmask of channels assigned */
unsigned assigned_channels = 0;
/* map raw inputs to mapped inputs */
/* XXX mapping should be atomic relative to protocol */
for (unsigned i = 0; i < r_raw_rc_count; i++) {
/* map the input channel */
uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE];
if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
uint16_t raw = r_raw_rc_values[i];
int16_t scaled;
/*
* 1) Constrain to min/max values, as later processing depends on bounds.
*/
if (raw < conf[PX4IO_P_RC_CONFIG_MIN])
raw = conf[PX4IO_P_RC_CONFIG_MIN];
if (raw > conf[PX4IO_P_RC_CONFIG_MAX])
raw = conf[PX4IO_P_RC_CONFIG_MAX];
/*
* 2) Scale around the mid point differently for lower and upper range.
*
* This is necessary as they don't share the same endpoints and slope.
*
* First normalize to 0..1 range with correct sign (below or above center),
* then scale to 20000 range (if center is an actual center, -10000..10000,
* if parameters only support half range, scale to 10000 range, e.g. if
* center == min 0..10000, if center == max -10000..0).
*
* As the min and max bounds were enforced in step 1), division by zero
* cannot occur, as for the case of center == min or center == max the if
* statement is mutually exclusive with the arithmetic NaN case.
*
* DO NOT REMOVE OR ALTER STEP 1!
*/
if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]));
} else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));
} else {
/* in the configured dead zone, output zero */
scaled = 0;
}
/* invert channel if requested */
if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE)
scaled = -scaled;
/* and update the scaled/mapped version */
unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
ASSERT(mapped < MAX_CONTROL_CHANNELS);
/* invert channel if pitch - pulling the lever down means pitching up by convention */
if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */
scaled = -scaled;
r_rc_values[mapped] = SIGNED_TO_REG(scaled);
assigned_channels |= (1 << mapped);
}
}
/* set un-assigned controls to zero */
for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) {
if (!(assigned_channels & (1 << i)))
r_rc_values[i] = 0;
}
/*
* If we got an update with zero channels, treat it as
* a loss of input.
*
* This might happen if a protocol-based receiver returns an update
* that contains no channels that we have mapped.
*/
if (assigned_channels == 0) {
rc_input_lost = true;
} else {
/* set RC OK flag and clear RC lost alarm */
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK;
r_status_alarms &= ~PX4IO_P_STATUS_ALARMS_RC_LOST;
}
/*
* Export the valid channel bitmap
*/
r_rc_valid = assigned_channels;
}
/*
* If we haven't seen any new control data in 200ms, assume we
* have lost input.
*/
if (hrt_elapsed_time(&system_state.rc_channels_timestamp) > 200000) {
rc_input_lost = true;
/* clear the input-kind flags here */
r_status_flags &= ~(
PX4IO_P_STATUS_FLAGS_RC_PPM |
PX4IO_P_STATUS_FLAGS_RC_DSM |
PX4IO_P_STATUS_FLAGS_RC_SBUS);
}
/*
* Handle losing RC input
*/
if (rc_input_lost) {
/* Clear the RC input status flag, clear manual override flag */
r_status_flags &= ~(
PX4IO_P_STATUS_FLAGS_OVERRIDE |
PX4IO_P_STATUS_FLAGS_RC_OK);
/* Set the RC_LOST alarm */
r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST;
/* Mark the arrays as empty */
r_raw_rc_count = 0;
r_rc_valid = 0;
}
/*
* Check for manual override.
*
* The PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK flag must be set, and we
* must have R/C input.
* Override is enabled if either the hardcoded channel / value combination
* is selected, or the AP has requested it.
*/
if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) {
bool override = false;
/*
* Check mapped channel 5 (can be any remote channel,
* depends on RC_MAP_OVER parameter);
* If the value is 'high' then the pilot has
* requested override.
*
*/
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(r_rc_values[4]) > RC_CHANNEL_HIGH_THRESH))
override = true;
if (override) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE;
/* mix new RC input control values to servos */
if (dsm_updated || sbus_updated || ppm_updated)
mixer_tick();
} else {
int
user_start(int argc, char *argv[])
{
/* configure the first 8 PWM outputs (i.e. all of them) */
up_pwm_servo_init(0xff);
#if defined(CONFIG_HAVE_CXX) && defined(CONFIG_HAVE_CXXINITIALIZE)
/* run C++ ctors before we go any further */
up_cxxinitialize();
# if defined(CONFIG_EXAMPLES_NSH_CXXINITIALIZE)
# error CONFIG_EXAMPLES_NSH_CXXINITIALIZE Must not be defined! Use CONFIG_HAVE_CXX and CONFIG_HAVE_CXXINITIALIZE.
# endif
#else
# error platform is dependent on c++ both CONFIG_HAVE_CXX and CONFIG_HAVE_CXXINITIALIZE must be defined.
#endif
/* reset all to zero */
memset(&system_state, 0, sizeof(system_state));
/* configure the high-resolution time/callout interface */
hrt_init();
/* calculate our fw CRC so FMU can decide if we need to update */
calculate_fw_crc();
/*
* Poll at 1ms intervals for received bytes that have not triggered
* a DMA event.
*/
#ifdef CONFIG_ARCH_DMA
hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL);
#endif
/* print some startup info */
syslog(LOG_INFO, "\nPX4IO: starting\n");
/* default all the LEDs to off while we start */
LED_AMBER(false);
LED_BLUE(false);
LED_SAFETY(false);
#ifdef GPIO_LED4
LED_RING(false);
#endif
/* turn on servo power (if supported) */
#ifdef POWER_SERVO
POWER_SERVO(true);
#endif
/* turn off S.Bus out (if supported) */
#ifdef ENABLE_SBUS_OUT
ENABLE_SBUS_OUT(false);
#endif
/* start the safety switch handler */
safety_init();
/* initialise the control inputs */
controls_init();
/* set up the ADC */
adc_init();
/* start the FMU interface */
interface_init();
/* add a performance counter for mixing */
perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix");
/* add a performance counter for controls */
perf_counter_t controls_perf = perf_alloc(PC_ELAPSED, "controls");
/* and one for measuring the loop rate */
perf_counter_t loop_perf = perf_alloc(PC_INTERVAL, "loop");
struct mallinfo minfo = mallinfo();
r_page_status[PX4IO_P_STATUS_FREEMEM] = minfo.mxordblk;
syslog(LOG_INFO, "MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks);
/* initialize PWM limit lib */
pwm_limit_init(&pwm_limit);
/*
* P O L I C E L I G H T S
*
* Not enough memory, lock down.
*
* We might need to allocate mixers later, and this will
* ensure that a developer doing a change will notice
* that he just burned the remaining RAM with static
* allocations. We don't want him to be able to
* get past that point. This needs to be clearly
* documented in the dev guide.
*
*/
if (minfo.mxordblk < 600) {
syslog(LOG_ERR, "ERR: not enough MEM");
bool phase = false;
while (true) {
if (phase) {
LED_AMBER(true);
LED_BLUE(false);
} else {
LED_AMBER(false);
LED_BLUE(true);
}
up_udelay(250000);
phase = !phase;
}
}
/* Start the failsafe led init */
failsafe_led_init();
/*
* Run everything in a tight loop.
*/
uint64_t last_debug_time = 0;
uint64_t last_heartbeat_time = 0;
uint64_t last_loop_time = 0;
for (;;) {
dt = (hrt_absolute_time() - last_loop_time) / 1000000.0f;
last_loop_time = hrt_absolute_time();
if (dt < 0.0001f) {
dt = 0.0001f;
} else if (dt > 0.02f) {
dt = 0.02f;
}
/* track the rate at which the loop is running */
perf_count(loop_perf);
/* kick the mixer */
perf_begin(mixer_perf);
mixer_tick();
perf_end(mixer_perf);
/* kick the control inputs */
perf_begin(controls_perf);
controls_tick();
perf_end(controls_perf);
/* some boards such as Pixhawk 2.1 made
the unfortunate choice to combine the blue led channel with
the IMU heater. We need a software hack to fix the hardware hack
by allowing to disable the LED / heater.
*/
if (r_page_setup[PX4IO_P_SETUP_THERMAL] == PX4IO_THERMAL_IGNORE) {
/*
blink blue LED at 4Hz in normal operation. When in
override blink 4x faster so the user can clearly see
that override is happening. This helps when
pre-flight testing the override system
*/
uint32_t heartbeat_period_us = 250 * 1000UL;
if (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) {
heartbeat_period_us /= 4;
}
if ((hrt_absolute_time() - last_heartbeat_time) > heartbeat_period_us) {
last_heartbeat_time = hrt_absolute_time();
heartbeat_blink();
}
} else if (r_page_setup[PX4IO_P_SETUP_THERMAL] < PX4IO_THERMAL_FULL) {
/* switch resistive heater off */
LED_BLUE(false);
} else {
/* switch resistive heater hard on */
LED_BLUE(true);
}
update_mem_usage();
ring_blink();
check_reboot();
/* check for debug activity (default: none) */
show_debug_messages();
/* post debug state at ~1Hz - this is via an auxiliary serial port
* DEFAULTS TO OFF!
*/
if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) {
isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x m=%u",
(unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG],
(unsigned)r_status_flags,
(unsigned)r_setup_arming,
(unsigned)r_setup_features,
(unsigned)mallinfo().mxordblk);
last_debug_time = hrt_absolute_time();
}
}
}
int
registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
{
switch (page) {
/* handle bulk controls input */
case PX4IO_PAGE_CONTROLS:
/* copy channel data */
while ((offset < PX4IO_CONTROL_GROUPS * PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
/* XXX range-check value? */
r_page_controls[offset] = *values;
offset++;
num_values--;
values++;
}
system_state.fmu_data_received_time = hrt_absolute_time();
r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK;
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RAW_PWM;
break;
/* handle raw PWM input */
case PX4IO_PAGE_DIRECT_PWM: {
/* copy channel data */
uint16_t widest_pulse = 0;
while ((offset < PX4IO_ACTUATOR_COUNT) && (num_values > 0)) {
/* XXX range-check value? */
if (*values != PWM_IGNORE_THIS_CHANNEL) {
r_page_direct_pwm[offset] = *values;
if (*values > widest_pulse) {
widest_pulse = *values;
}
}
offset++;
num_values--;
values++;
}
if (widest_pulse > 2300) {
// don't allow extreme pulses to cause issues with oneshot delays
widest_pulse = 2300;
}
system_state.fmu_data_received_time = hrt_absolute_time();
r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM;
if (r_setup_features & PX4IO_P_SETUP_FEATURES_ONESHOT) {
/*
in oneshot we run the mixer as soon as we
get new data from the FMU
*/
mixer_tick();
/*
we now need to trigger the output pulses,
ensuring that the new pulses don't interrupt
the previous pulses. We add a 50 usec safety
margin to ensure the ESC has registered the
end of the previous pulses.
*/
static hrt_abstime oneshot_delay_till;
hrt_abstime now = hrt_absolute_time();
if (now < oneshot_delay_till) {
up_udelay(oneshot_delay_till - now);
}
up_pwm_servo_trigger(r_setup_pwm_rates);
oneshot_delay_till = hrt_absolute_time() + widest_pulse + 50;
}
break;
}
/* handle setup for servo failsafe values */
case PX4IO_PAGE_FAILSAFE_PWM:
/* copy channel data */
while ((offset < PX4IO_ACTUATOR_COUNT) && (num_values > 0)) {
if (*values == 0) {
/* ignore 0 */
} else if (*values < PWM_LOWEST_MIN) {
r_page_servo_failsafe[offset] = PWM_LOWEST_MIN;
} else if (*values > PWM_HIGHEST_MAX) {
r_page_servo_failsafe[offset] = PWM_HIGHEST_MAX;
} else {
r_page_servo_failsafe[offset] = *values;
}
/* flag the failsafe values as custom */
r_setup_arming |= PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM;
offset++;
num_values--;
values++;
}
break;
case PX4IO_PAGE_CONTROL_MIN_PWM:
/* copy channel data */
while ((offset < PX4IO_ACTUATOR_COUNT) && (num_values > 0)) {
if (*values == 0) {
/* ignore 0 */
} else if (*values > PWM_HIGHEST_MIN) {
r_page_servo_control_min[offset] = PWM_HIGHEST_MIN;
} else if (*values < PWM_LOWEST_MIN) {
r_page_servo_control_min[offset] = PWM_LOWEST_MIN;
} else {
r_page_servo_control_min[offset] = *values;
}
offset++;
num_values--;
values++;
}
break;
case PX4IO_PAGE_CONTROL_MAX_PWM:
/* copy channel data */
while ((offset < PX4IO_ACTUATOR_COUNT) && (num_values > 0)) {
if (*values == 0) {
/* ignore 0 */
} else if (*values > PWM_HIGHEST_MAX) {
r_page_servo_control_max[offset] = PWM_HIGHEST_MAX;
} else if (*values < PWM_LOWEST_MAX) {
r_page_servo_control_max[offset] = PWM_LOWEST_MAX;
} else {
r_page_servo_control_max[offset] = *values;
}
offset++;
num_values--;
values++;
}
break;
case PX4IO_PAGE_DISARMED_PWM: {
/* flag for all outputs */
bool all_disarmed_off = true;
/* copy channel data */
while ((offset < PX4IO_ACTUATOR_COUNT) && (num_values > 0)) {
if (*values == 0) {
/* 0 means disabling always PWM */
r_page_servo_disarmed[offset] = 0;
} else if (*values < PWM_LOWEST_MIN) {
r_page_servo_disarmed[offset] = PWM_LOWEST_MIN;
all_disarmed_off = false;
} else if (*values > PWM_HIGHEST_MAX) {
r_page_servo_disarmed[offset] = PWM_HIGHEST_MAX;
all_disarmed_off = false;
} else {
r_page_servo_disarmed[offset] = *values;
all_disarmed_off = false;
}
offset++;
num_values--;
values++;
}
if (all_disarmed_off) {
/* disable PWM output if disarmed */
r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE);
} else {
/* enable PWM output always */
r_setup_arming |= PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE;
}
}
break;
/* handle text going to the mixer parser */
case PX4IO_PAGE_MIXERLOAD:
/* do not change the mixer if FMU is armed and IO's safety is off
* this state defines an active system. This check is done in the
* text handling function.
*/
return mixer_handle_text(values, num_values * sizeof(*values));
default:
/* avoid offset wrap */
if ((offset + num_values) > 255) {
num_values = 255 - offset;
}
/* iterate individual registers, set each in turn */
while (num_values--) {
if (registers_set_one(page, offset, *values)) {
return -1;
}
offset++;
values++;
}
break;
}
return 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);
}
void
controls_main(void)
{
struct pollfd fds[2];
/* DSM input */
fds[0].fd = dsm_init("/dev/ttyS0");
fds[0].events = POLLIN;
/* S.bus input */
fds[1].fd = sbus_init("/dev/ttyS2");
fds[1].events = POLLIN;
for (;;) {
/* run this loop at ~100Hz */
poll(fds, 2, 10);
/*
* Gather R/C control inputs from supported sources.
*
* Note that if you're silly enough to connect more than
* one control input source, they're going to fight each
* other. Don't do that.
*/
bool locked = false;
if (fds[0].revents & POLLIN)
locked |= dsm_input();
if (fds[1].revents & POLLIN)
locked |= sbus_input();
/*
* If we don't have lock from one of the serial receivers,
* look for PPM. It shares an input with S.bus, so there's
* a possibility it will mis-parse an S.bus frame.
*
* XXX each S.bus frame will cause a PPM decoder interrupt
* storm (lots of edges). It might be sensible to actually
* disable the PPM decoder completely if we have an alternate
* receiver lock.
*/
if (!locked)
ppm_input();
/*
* If we haven't seen any new control data in 200ms, assume we
* have lost input and tell FMU.
*/
if ((hrt_absolute_time() - system_state.rc_channels_timestamp) > 200000) {
/* set the number of channels to zero - no inputs */
system_state.rc_channels = 0;
/* trigger an immediate report to the FMU */
system_state.fmu_report_due = true;
}
/* XXX do bypass mode, etc. here */
/* do PWM output updates */
mixer_tick();
}
}
