diff options
-rw-r--r-- | apps/px4io/comms.c | 66 | ||||
-rw-r--r-- | apps/px4io/protocol.h | 4 | ||||
-rw-r--r-- | apps/px4io/px4io.h | 28 |
3 files changed, 78 insertions, 20 deletions
diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index 83a006d43..eaaea7817 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -61,8 +61,7 @@ #define FMU_MIN_REPORT_INTERVAL 5000 /* 5ms */ #define FMU_MAX_REPORT_INTERVAL 100000 /* 100ms */ -int frame_rx; -int frame_bad; +#define FMU_STATUS_INTERVAL 1000000 /* 100ms */ static int fmu_fd; static hx_stream_t stream; @@ -87,6 +86,9 @@ comms_init(void) cfsetspeed(&t, 115200); t.c_cflag &= ~(CSTOPB | PARENB); tcsetattr(fmu_fd, TCSANOW, &t); + + /* init the ADC */ + adc_init(); } void @@ -135,9 +137,55 @@ comms_main(void) report.channel_count = system_state.rc_channels; report.armed = system_state.armed; + report.battery_mv = system_state.battery_mv; + report.adc_in = system_state.adc_in5; + report.overcurrent = system_state.overcurrent; + /* and send it */ hx_stream_send(stream, &report, sizeof(report)); } + + /* + * Fetch ADC values, check overcurrent flags, etc. + */ + static hrt_abstime last_status_time; + + if ((now - last_status_time) > FMU_STATUS_INTERVAL) { + + /* + * Coefficients here derived by measurement of the 5-16V + * range on one unit: + * + * V counts + * 5 1001 + * 6 1219 + * 7 1436 + * 8 1653 + * 9 1870 + * 10 2086 + * 11 2303 + * 12 2522 + * 13 2738 + * 14 2956 + * 15 3172 + * 16 3389 + * + * slope = 0.0046067 + * intercept = 0.3863 + * + * Intercept corrected for best results @ 12V. + */ + unsigned counts = adc_measure(ADC_VBATT); + system_state.battery_mv = (4150 + (counts * 46)) / 10; + + system_state.adc_in5 = adc_measure(ADC_IN5); + + system_state.overcurrent = + (OVERCURRENT_SERVO ? (1 << 0) : 0) | + (OVERCURRENT_ACC ? (1 << 1) : 0); + + last_status_time = now; + } } } @@ -146,12 +194,10 @@ comms_handle_config(const void *buffer, size_t length) { const struct px4io_config *cfg = (struct px4io_config *)buffer; - if (length != sizeof(*cfg)) { - frame_bad++; + if (length != sizeof(*cfg)) return; - } - frame_rx++; + /* XXX */ } static void @@ -159,12 +205,9 @@ comms_handle_command(const void *buffer, size_t length) { const struct px4io_command *cmd = (struct px4io_command *)buffer; - if (length != sizeof(*cmd)) { - frame_bad++; + if (length != sizeof(*cmd)) return; - } - - frame_rx++; + irqstate_t flags = irqsave(); /* fetch new PWM output values */ @@ -209,7 +252,6 @@ comms_handle_frame(void *arg, const void *buffer, size_t length) comms_handle_config(buffer, length); break; default: - frame_bad++; break; } } diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index c704b1201..a64e2f27d 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -73,6 +73,10 @@ struct px4io_report { uint16_t rc_channel[PX4IO_INPUT_CHANNELS]; bool armed; uint8_t channel_count; + + uint16_t battery_mv; + uint16_t adc_in; + uint8_t overcurrent; }; #pragma pack(pop)
\ No newline at end of file diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index 6221b08f2..af06c6ec1 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -105,17 +105,25 @@ struct sys_state_s bool fmu_data_received; /* - * Current serial interface mode, per the serial_rx_mode parameter - * in the config packet. + * Measured battery voltage in mV */ - uint8_t serial_rx_mode; + uint16_t battery_mv; + + /* + * ADC IN5 measurement + */ + uint16_t adc_in5; + + /* + * Power supply overcurrent status bits. + */ + uint8_t overcurrent; + }; extern struct sys_state_s system_state; -extern int frame_rx; -extern int frame_bad; - +#if 0 /* * Software countdown timers. * @@ -127,6 +135,7 @@ extern int frame_bad; #define TIMER_SANITY 7 #define TIMER_NUM_TIMERS 8 extern volatile int timers[TIMER_NUM_TIMERS]; +#endif /* * GPIO handling. @@ -141,10 +150,13 @@ extern volatile int timers[TIMER_NUM_TIMERS]; #define POWER_RELAY1(_s) stm32_gpiowrite(GPIO_RELAY1_EN, (_s)) #define POWER_RELAY2(_s) stm32_gpiowrite(GPIO_RELAY2_EN, (_s)) -#define OVERCURRENT_ACC stm32_gpioread(GPIO_ACC_OC_DETECT) -#define OVERCURRENT_SERVO stm32_gpioread(GPIO_SERVO_OC_DETECT +#define OVERCURRENT_ACC (!stm32_gpioread(GPIO_ACC_OC_DETECT)) +#define OVERCURRENT_SERVO (!stm32_gpioread(GPIO_SERVO_OC_DETECT)) #define BUTTON_SAFETY stm32_gpioread(GPIO_BTN_SAFETY) +#define ADC_VBATT 4 +#define ADC_IN5 5 + /* * Mixer */ |