forked from Archive/PX4-Autopilot
px4io code style
This commit is contained in:
Lorenz Meier
parent
dc100f2020
commit
cf563eda86
|
|
@ -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;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
|
@ -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];
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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];
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
Loading…
Reference in New Issue