int
registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_values)
{
#define SELECT_PAGE(_page_name) \
do { \
*values = (uint16_t *)&_page_name[0]; \
*num_values = sizeof(_page_name) / sizeof(_page_name[0]); \
} while(0)
switch (page) {
/*
* Handle pages that are updated dynamically at read time.
*/
case PX4IO_PAGE_STATUS:
/* PX4IO_P_STATUS_FREEMEM */
{
struct mallinfo minfo = mallinfo();
r_page_status[PX4IO_P_STATUS_FREEMEM] = minfo.fordblks;
}
/* XXX PX4IO_P_STATUS_CPULOAD */
/* PX4IO_P_STATUS_FLAGS maintained externally */
/* PX4IO_P_STATUS_ALARMS maintained externally */
#ifdef ADC_VBATT
/* PX4IO_P_STATUS_VBATT */
{
/*
* Coefficients here derived by measurement of the 5-16V
* range on one unit, validated on sample points of another unit
*
* Data in Tools/tests-host/data folder.
*
* measured slope = 0.004585267878277 (int: 4585)
* nominal theoretic slope: 0.00459340659 (int: 4593)
* intercept = 0.016646394188076 (int: 16646)
* nominal theoretic intercept: 0.00 (int: 0)
*
*/
unsigned counts = adc_measure(ADC_VBATT);
if (counts != 0xffff) {
unsigned mV = (166460 + (counts * 45934)) / 10000;
unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000;
r_page_status[PX4IO_P_STATUS_VBATT] = corrected;
}
}
#endif
#ifdef ADC_IBATT
/* PX4IO_P_STATUS_IBATT */
{
/*
note that we have no idea what sort of
current sensor is attached, so we just
return the raw 12 bit ADC value and let the
FMU sort it out, with user selectable
configuration for their sensor
*/
unsigned counts = adc_measure(ADC_IBATT);
if (counts != 0xffff) {
r_page_status[PX4IO_P_STATUS_IBATT] = counts;
}
}
#endif
#ifdef ADC_VSERVO
/* PX4IO_P_STATUS_VSERVO */
{
unsigned counts = adc_measure(ADC_VSERVO);
if (counts != 0xffff) {
// use 3:1 scaling on 3.3V ADC input
unsigned mV = counts * 9900 / 4096;
r_page_status[PX4IO_P_STATUS_VSERVO] = mV;
}
}
#endif
#ifdef ADC_RSSI
/* PX4IO_P_STATUS_VRSSI */
{
unsigned counts = adc_measure(ADC_RSSI);
if (counts != 0xffff) {
// use 1:1 scaling on 3.3V ADC input
unsigned mV = counts * 3300 / 4096;
r_page_status[PX4IO_P_STATUS_VRSSI] = mV;
}
}
#endif
/* XXX PX4IO_P_STATUS_PRSSI */
SELECT_PAGE(r_page_status);
break;
case PX4IO_PAGE_RAW_ADC_INPUT:
memset(r_page_scratch, 0, sizeof(r_page_scratch));
#ifdef ADC_VBATT
r_page_scratch[0] = adc_measure(ADC_VBATT);
#endif
#ifdef ADC_IBATT
r_page_scratch[1] = adc_measure(ADC_IBATT);
#endif
#ifdef ADC_VSERVO
r_page_scratch[0] = adc_measure(ADC_VSERVO);
#endif
#ifdef ADC_RSSI
r_page_scratch[1] = adc_measure(ADC_RSSI);
#endif
SELECT_PAGE(r_page_scratch);
break;
case PX4IO_PAGE_PWM_INFO:
memset(r_page_scratch, 0, sizeof(r_page_scratch));
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
r_page_scratch[PX4IO_RATE_MAP_BASE + i] = up_pwm_servo_get_rate_group(i);
SELECT_PAGE(r_page_scratch);
break;
/*
* Pages that are just a straight read of the register state.
*/
/* status pages */
case PX4IO_PAGE_CONFIG:
SELECT_PAGE(r_page_config);
break;
case PX4IO_PAGE_ACTUATORS:
SELECT_PAGE(r_page_actuators);
break;
case PX4IO_PAGE_SERVOS:
SELECT_PAGE(r_page_servos);
break;
case PX4IO_PAGE_RAW_RC_INPUT:
SELECT_PAGE(r_page_raw_rc_input);
break;
case PX4IO_PAGE_RC_INPUT:
SELECT_PAGE(r_page_rc_input);
break;
/* readback of input pages */
case PX4IO_PAGE_SETUP:
SELECT_PAGE(r_page_setup);
break;
case PX4IO_PAGE_CONTROLS:
SELECT_PAGE(r_page_controls);
break;
case PX4IO_PAGE_RC_CONFIG:
SELECT_PAGE(r_page_rc_input_config);
break;
case PX4IO_PAGE_DIRECT_PWM:
SELECT_PAGE(r_page_servos);
break;
case PX4IO_PAGE_FAILSAFE_PWM:
SELECT_PAGE(r_page_servo_failsafe);
break;
case PX4IO_PAGE_CONTROL_MIN_PWM:
SELECT_PAGE(r_page_servo_control_min);
break;
case PX4IO_PAGE_CONTROL_MAX_PWM:
SELECT_PAGE(r_page_servo_control_max);
break;
case PX4IO_PAGE_DISARMED_PWM:
SELECT_PAGE(r_page_servo_disarmed);
break;
default:
return -1;
}
#undef SELECT_PAGE
#undef COPY_PAGE
last_page = page;
last_offset = offset;
/* if the offset is at or beyond the end of the page, we have no data */
if (offset >= *num_values)
return -1;
/* correct the data pointer and count for the offset */
*values += offset;
*num_values -= offset;
return 0;
}
int
registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_values)
{
#define SELECT_PAGE(_page_name) \
do { \
*values = &_page_name[0]; \
*num_values = sizeof(_page_name) / sizeof(_page_name[0]); \
} while(0)
switch (page) {
/*
* Handle pages that are updated dynamically at read time.
*/
case PX4IO_PAGE_STATUS:
/* PX4IO_P_STATUS_FREEMEM */
{
struct mallinfo minfo = mallinfo();
r_page_status[PX4IO_P_STATUS_FREEMEM] = minfo.fordblks;
}
/* XXX PX4IO_P_STATUS_CPULOAD */
/* PX4IO_P_STATUS_FLAGS maintained externally */
/* PX4IO_P_STATUS_ALARMS maintained externally */
/* PX4IO_P_STATUS_VBATT */
{
/*
* 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);
unsigned mV = (4150 + (counts * 46)) / 10 - 200;
unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000;
r_page_status[PX4IO_P_STATUS_VBATT] = corrected;
}
/* PX4IO_P_STATUS_IBATT */
{
unsigned counts = adc_measure(ADC_VBATT);
unsigned scaled = (counts * r_page_setup[PX4IO_P_SETUP_IBATT_SCALE]) / 10000;
int corrected = scaled + REG_TO_SIGNED(r_page_setup[PX4IO_P_SETUP_IBATT_BIAS]);
if (corrected < 0)
corrected = 0;
r_page_status[PX4IO_P_STATUS_IBATT] = corrected;
}
SELECT_PAGE(r_page_status);
break;
case PX4IO_PAGE_RAW_ADC_INPUT:
r_page_adc[0] = adc_measure(ADC_VBATT);
r_page_adc[1] = adc_measure(ADC_IN5);
SELECT_PAGE(r_page_adc);
break;
/*
* Pages that are just a straight read of the register state.
*/
/* status pages */
case PX4IO_PAGE_CONFIG:
SELECT_PAGE(r_page_config);
break;
case PX4IO_PAGE_ACTUATORS:
SELECT_PAGE(r_page_actuators);
break;
case PX4IO_PAGE_SERVOS:
SELECT_PAGE(r_page_servos);
break;
case PX4IO_PAGE_RAW_RC_INPUT:
SELECT_PAGE(r_page_raw_rc_input);
break;
case PX4IO_PAGE_RC_INPUT:
SELECT_PAGE(r_page_rc_input);
break;
/* readback of input pages */
case PX4IO_PAGE_SETUP:
SELECT_PAGE(r_page_setup);
break;
case PX4IO_PAGE_CONTROLS:
SELECT_PAGE(r_page_controls);
break;
case PX4IO_PAGE_RC_CONFIG:
SELECT_PAGE(r_page_rc_input_config);
break;
case PX4IO_PAGE_DIRECT_PWM:
SELECT_PAGE(r_page_servos);
break;
case PX4IO_PAGE_FAILSAFE_PWM:
SELECT_PAGE(r_page_servo_failsafe);
break;
default:
return -1;
}
#undef SELECT_PAGE
#undef COPY_PAGE
last_page = page;
last_offset = offset;
/* if the offset is at or beyond the end of the page, we have no data */
if (offset >= *num_values)
return -1;
/* correct the data pointer and count for the offset */
*values += offset;
*num_values -= offset;
return 0;
}
