diff --git a/apps/px4io/adc.c b/apps/px4io/adc.c index 62ff0b1f19..e06b269dc0 100644 --- a/apps/px4io/adc.c +++ b/apps/px4io/adc.c @@ -87,12 +87,16 @@ adc_init(void) #ifdef ADC_CR2_CAL rCR2 |= ADC_CR2_RSTCAL; up_udelay(1); + if (rCR2 & ADC_CR2_RSTCAL) return -1; + rCR2 |= ADC_CR2_CAL; up_udelay(100); + if (rCR2 & ADC_CR2_CAL) return -1; + #endif /* arbitrarily configure all channels for 55 cycle sample time */ @@ -103,7 +107,7 @@ adc_init(void) rCR1 = 0; /* enable the temperature sensor / Vrefint channel if supported*/ - rCR2 = + rCR2 = #ifdef ADC_CR2_TSVREFE /* enable the temperature sensor in CR2 */ ADC_CR2_TSVREFE | @@ -118,7 +122,7 @@ adc_init(void) /* configure for a single-channel sequence */ rSQR1 = 0; rSQR2 = 0; - rSQR3 = 0; /* will be updated with the channel each tick */ + rSQR3 = 0; /* will be updated with the channel each tick */ /* power-cycle the ADC and turn it on */ rCR2 &= ~ADC_CR2_ADON; @@ -146,6 +150,7 @@ adc_measure(unsigned channel) /* wait for the conversion to complete */ hrt_abstime now = hrt_absolute_time(); + while (!(rSR & ADC_SR_EOC)) { /* never spin forever - this will give a bogus result though */ @@ -159,5 +164,5 @@ adc_measure(unsigned channel) uint16_t result = rDR; perf_end(adc_perf); - return result; -} \ No newline at end of file + return result; +} \ No newline at end of file diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index adedea74cc..e51c2771af 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -190,7 +190,7 @@ comms_main(void) system_state.adc_in5 = adc_measure(ADC_IN5); - system_state.overcurrent = + system_state.overcurrent = (OVERCURRENT_SERVO ? (1 << 0) : 0) | (OVERCURRENT_ACC ? (1 << 1) : 0); @@ -212,7 +212,7 @@ comms_handle_config(const void *buffer, size_t length) system_state.rc_map[i] = cfg->rc_map[i]; } - /* fetch the rc channel attributes */ + /* fetch the rc channel attributes */ for (unsigned i = 0; i < 4; i++) { system_state.rc_min[i] = cfg->rc_min[i]; system_state.rc_trim[i] = cfg->rc_trim[i]; @@ -229,7 +229,7 @@ comms_handle_command(const void *buffer, size_t length) if (length != sizeof(*cmd)) return; - + irqstate_t flags = irqsave(); /* fetch new PWM output values */ @@ -253,10 +253,12 @@ comms_handle_command(const void *buffer, size_t length) 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; @@ -270,24 +272,28 @@ comms_handle_command(const void *buffer, size_t length) */ mixer_tick(); - /* handle relay state changes here */ + /* 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]; } diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index da445e3b41..169d5bb817 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -121,6 +121,7 @@ controls_main(void) if (system_state.rc_channel_data[4] > RC_CHANNEL_HIGH_THRESH) { /* force manual input override */ system_state.mixer_manual_override = true; + } else { /* override not engaged, use FMU */ system_state.mixer_manual_override = false; @@ -139,7 +140,7 @@ controls_main(void) */ system_state.fmu_report_due = true; } - + /* set the number of channels to zero - no inputs */ system_state.rc_channels = 0; } diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index 139bbb3217..e388f65e30 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -78,7 +78,7 @@ struct sys_state_s { uint8_t rc_map[4]; /** - * Remote control channel attributes + * Remote control channel attributes */ uint16_t rc_min[4]; uint16_t rc_trim[4]; diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c index 3e02b717d4..0cae29ac98 100644 --- a/apps/px4io/safety.c +++ b/apps/px4io/safety.c @@ -159,6 +159,7 @@ safety_check_button(void *arg) } else if (system_state.arm_ok) { pattern = LED_PATTERN_FMU_ARMED; + } else if (system_state.vector_flight_mode_ok) { pattern = LED_PATTERN_VECTOR_FLIGHT_MODE_OK; } diff --git a/apps/px4io/sbus.c b/apps/px4io/sbus.c index f84e5df8ac..568ef8091c 100644 --- a/apps/px4io/sbus.c +++ b/apps/px4io/sbus.c @@ -88,6 +88,7 @@ sbus_init(const char *device) last_rx_time = hrt_absolute_time(); debug("S.Bus: ready"); + } else { debug("S.Bus: open failed"); } @@ -109,7 +110,7 @@ sbus_input(void) * frame transmission time is ~2ms. * * We expect to only be called when bytes arrive for processing, - * and if an interval of more than 3ms passes between calls, + * and if an interval of more than 3ms passes between calls, * the first byte we read will be the first byte of a frame. * * In the case where byte(s) are dropped from a frame, this also @@ -117,6 +118,7 @@ sbus_input(void) * if we didn't drop bytes... */ now = hrt_absolute_time(); + if ((now - last_rx_time) > 3000) { if (partial_frame_count > 0) { sbus_frame_drops++; @@ -133,6 +135,7 @@ sbus_input(void) /* if the read failed for any reason, just give up here */ if (ret < 1) goto out; + last_rx_time = now; /* @@ -144,7 +147,7 @@ sbus_input(void) * If we don't have a full frame, return */ if (partial_frame_count < SBUS_FRAME_SIZE) - goto out; + goto out; /* * Great, it looks like we might have a frame. Go ahead and @@ -155,7 +158,7 @@ sbus_input(void) out: /* - * If we have seen a frame in the last 200ms, we consider ourselves 'locked' + * If we have seen a frame in the last 200ms, we consider ourselves 'locked' */ return (now - last_frame_time) < 200000; } @@ -177,23 +180,23 @@ struct sbus_bit_pick { uint8_t mask; uint8_t lshift; }; -static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = { -/* 0 */ { { 0, 0, 0xff, 0},{ 1, 0, 0x07, 8},{ 0, 0, 0x00, 0} }, -/* 1 */ { { 1, 3, 0x1f, 0},{ 2, 0, 0x3f, 5},{ 0, 0, 0x00, 0} }, -/* 2 */ { { 2, 6, 0x03, 0},{ 3, 0, 0xff, 2},{ 4, 0, 0x01, 10} }, -/* 3 */ { { 4, 1, 0x7f, 0},{ 5, 0, 0x0f, 7},{ 0, 0, 0x00, 0} }, -/* 4 */ { { 5, 4, 0x0f, 0},{ 6, 0, 0x7f, 4},{ 0, 0, 0x00, 0} }, -/* 5 */ { { 6, 7, 0x01, 0},{ 7, 0, 0xff, 1},{ 8, 0, 0x03, 9} }, -/* 6 */ { { 8, 2, 0x3f, 0},{ 9, 0, 0x1f, 6},{ 0, 0, 0x00, 0} }, -/* 7 */ { { 9, 5, 0x07, 0},{10, 0, 0xff, 3},{ 0, 0, 0x00, 0} }, -/* 8 */ { {11, 0, 0xff, 0},{12, 0, 0x07, 8},{ 0, 0, 0x00, 0} }, -/* 9 */ { {12, 3, 0x1f, 0},{13, 0, 0x3f, 5},{ 0, 0, 0x00, 0} }, -/* 10 */ { {13, 6, 0x03, 0},{14, 0, 0xff, 2},{15, 0, 0x01, 10} }, -/* 11 */ { {15, 1, 0x7f, 0},{16, 0, 0x0f, 7},{ 0, 0, 0x00, 0} }, -/* 12 */ { {16, 4, 0x0f, 0},{17, 0, 0x7f, 4},{ 0, 0, 0x00, 0} }, -/* 13 */ { {17, 7, 0x01, 0},{18, 0, 0xff, 1},{19, 0, 0x03, 9} }, -/* 14 */ { {19, 2, 0x3f, 0},{20, 0, 0x1f, 6},{ 0, 0, 0x00, 0} }, -/* 15 */ { {20, 5, 0x07, 0},{21, 0, 0xff, 3},{ 0, 0, 0x00, 0} } +static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = { + /* 0 */ { { 0, 0, 0xff, 0}, { 1, 0, 0x07, 8}, { 0, 0, 0x00, 0} }, + /* 1 */ { { 1, 3, 0x1f, 0}, { 2, 0, 0x3f, 5}, { 0, 0, 0x00, 0} }, + /* 2 */ { { 2, 6, 0x03, 0}, { 3, 0, 0xff, 2}, { 4, 0, 0x01, 10} }, + /* 3 */ { { 4, 1, 0x7f, 0}, { 5, 0, 0x0f, 7}, { 0, 0, 0x00, 0} }, + /* 4 */ { { 5, 4, 0x0f, 0}, { 6, 0, 0x7f, 4}, { 0, 0, 0x00, 0} }, + /* 5 */ { { 6, 7, 0x01, 0}, { 7, 0, 0xff, 1}, { 8, 0, 0x03, 9} }, + /* 6 */ { { 8, 2, 0x3f, 0}, { 9, 0, 0x1f, 6}, { 0, 0, 0x00, 0} }, + /* 7 */ { { 9, 5, 0x07, 0}, {10, 0, 0xff, 3}, { 0, 0, 0x00, 0} }, + /* 8 */ { {11, 0, 0xff, 0}, {12, 0, 0x07, 8}, { 0, 0, 0x00, 0} }, + /* 9 */ { {12, 3, 0x1f, 0}, {13, 0, 0x3f, 5}, { 0, 0, 0x00, 0} }, + /* 10 */ { {13, 6, 0x03, 0}, {14, 0, 0xff, 2}, {15, 0, 0x01, 10} }, + /* 11 */ { {15, 1, 0x7f, 0}, {16, 0, 0x0f, 7}, { 0, 0, 0x00, 0} }, + /* 12 */ { {16, 4, 0x0f, 0}, {17, 0, 0x7f, 4}, { 0, 0, 0x00, 0} }, + /* 13 */ { {17, 7, 0x01, 0}, {18, 0, 0xff, 1}, {19, 0, 0x03, 9} }, + /* 14 */ { {19, 2, 0x3f, 0}, {20, 0, 0x1f, 6}, { 0, 0, 0x00, 0} }, + /* 15 */ { {20, 5, 0x07, 0}, {21, 0, 0xff, 3}, { 0, 0, 0x00, 0} } }; static void @@ -207,7 +210,7 @@ sbus_decode(hrt_abstime frame_time) /* if the failsafe or connection lost bit is set, we consider the frame invalid */ if ((frame[23] & (1 << 2)) && /* signal lost */ - (frame[23] & (1 << 3))) { /* failsafe */ + (frame[23] & (1 << 3))) { /* failsafe */ /* actively announce signal loss */ system_state.rc_channels = 0; @@ -217,8 +220,8 @@ sbus_decode(hrt_abstime frame_time) /* we have received something we think is a frame */ last_frame_time = frame_time; - unsigned chancount = (PX4IO_INPUT_CHANNELS > SBUS_INPUT_CHANNELS) ? - SBUS_INPUT_CHANNELS : PX4IO_INPUT_CHANNELS; + unsigned chancount = (PX4IO_INPUT_CHANNELS > SBUS_INPUT_CHANNELS) ? + SBUS_INPUT_CHANNELS : PX4IO_INPUT_CHANNELS; /* use the decoder matrix to extract channel data */ for (unsigned channel = 0; channel < chancount; channel++) { @@ -236,6 +239,7 @@ sbus_decode(hrt_abstime frame_time) value |= piece; } } + /* convert 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */ system_state.rc_channel_data[channel] = (value / 2) + 998; } @@ -252,7 +256,7 @@ sbus_decode(hrt_abstime frame_time) system_state.rc_channels = chancount; /* and note that we have received data from the R/C controller */ - system_state.rc_channels_timestamp = frame_time; + system_state.rc_channels_timestamp = frame_time; /* trigger an immediate report to the FMU */ system_state.fmu_report_due = true;