|
|
|
@ -1040,7 +1040,7 @@ PX4IO::task_main()
@@ -1040,7 +1040,7 @@ PX4IO::task_main()
|
|
|
|
|
int pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RC_THR_FAILSAFE_US, &failsafe_thr, 1); |
|
|
|
|
|
|
|
|
|
if (pret != OK) { |
|
|
|
|
mavlink_log_critical(&_mavlink_log_pub, "failsafe upload failed, FS: %d us", (int)failsafe_thr); |
|
|
|
|
mavlink_log_critical(&_mavlink_log_pub, "failsafe upload failed, FS: %" PRId16 " us", failsafe_thr); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
@ -2224,7 +2224,7 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned
@@ -2224,7 +2224,7 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned
|
|
|
|
|
int ret = _interface->write((page << 8) | offset, (void *)values, num_values); |
|
|
|
|
|
|
|
|
|
if (ret != (int)num_values) { |
|
|
|
|
PX4_DEBUG("io_reg_set(%hhu,%hhu,%u): error %d", page, offset, num_values, ret); |
|
|
|
|
PX4_DEBUG("io_reg_set(%" PRIu8 ",%" PRIu8 ",%u): error %d", page, offset, num_values, ret); |
|
|
|
|
return -1; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -2249,7 +2249,7 @@ PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_v
@@ -2249,7 +2249,7 @@ PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_v
|
|
|
|
|
int ret = _interface->read((page << 8) | offset, reinterpret_cast<void *>(values), num_values); |
|
|
|
|
|
|
|
|
|
if (ret != (int)num_values) { |
|
|
|
|
PX4_DEBUG("io_reg_get(%hhu,%hhu,%u): data error %d", page, offset, num_values, ret); |
|
|
|
|
PX4_DEBUG("io_reg_get(%" PRIu8 ",%" PRIu8 ",%u): data error %d", page, offset, num_values, ret); |
|
|
|
|
return -1; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -2450,14 +2450,16 @@ void
@@ -2450,14 +2450,16 @@ void
|
|
|
|
|
PX4IO::print_status(bool extended_status) |
|
|
|
|
{ |
|
|
|
|
/* basic configuration */ |
|
|
|
|
printf("protocol %u hardware %u bootloader %u buffer %uB crc 0x%04x%04x\n", |
|
|
|
|
printf("protocol %" PRIu32 " hardware %" PRIu32 " bootloader %" PRIu32 " buffer %" PRIu32 "B crc 0x%04" PRIu32 "%04" |
|
|
|
|
PRIu32 "\n", |
|
|
|
|
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION), |
|
|
|
|
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_HARDWARE_VERSION), |
|
|
|
|
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_BOOTLOADER_VERSION), |
|
|
|
|
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER), |
|
|
|
|
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC), |
|
|
|
|
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC + 1)); |
|
|
|
|
printf("%u controls %u actuators %u R/C inputs %u analog inputs %u relays\n", |
|
|
|
|
printf("%" PRIu32 " controls %" PRIu32 " actuators %" PRIu32 " R/C inputs %" PRIu32 " analog inputs %" PRIu32 |
|
|
|
|
" relays\n", |
|
|
|
|
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT), |
|
|
|
|
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT), |
|
|
|
|
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT), |
|
|
|
@ -2492,17 +2494,17 @@ PX4IO::print_status(bool extended_status)
@@ -2492,17 +2494,17 @@ PX4IO::print_status(bool extended_status)
|
|
|
|
|
(double)trim_roll, (double)trim_pitch, (double)trim_yaw); |
|
|
|
|
|
|
|
|
|
uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT); |
|
|
|
|
printf("%hu raw R/C inputs", raw_inputs); |
|
|
|
|
printf("%" PRIu16 " raw R/C inputs", raw_inputs); |
|
|
|
|
|
|
|
|
|
for (unsigned i = 0; i < raw_inputs; i++) { |
|
|
|
|
printf(" %u", io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + i)); |
|
|
|
|
printf(" %" PRIu32, io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + i)); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
printf("\n"); |
|
|
|
|
|
|
|
|
|
uint16_t io_status_flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS); |
|
|
|
|
uint16_t flags = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_FLAGS); |
|
|
|
|
printf("R/C flags: 0x%04hx%s%s%s%s%s\n", flags, |
|
|
|
|
printf("R/C flags: 0x%04" PRIx16 "%s%s%s%s%s\n", flags, |
|
|
|
|
(((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11))) ? " DSM10" : ""), |
|
|
|
|
(((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11)) ? " DSM11" : ""), |
|
|
|
|
((flags & PX4IO_P_RAW_RC_FLAGS_FRAME_DROP) ? " FRAME_DROP" : ""), |
|
|
|
@ -2520,11 +2522,11 @@ PX4IO::print_status(bool extended_status)
@@ -2520,11 +2522,11 @@ PX4IO::print_status(bool extended_status)
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
uint16_t mapped_inputs = io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_VALID); |
|
|
|
|
printf("mapped R/C inputs 0x%04hx", mapped_inputs); |
|
|
|
|
printf("mapped R/C inputs 0x%04" PRIx16, mapped_inputs); |
|
|
|
|
|
|
|
|
|
for (unsigned i = 0; i < _max_rc_input; i++) { |
|
|
|
|
if (mapped_inputs & (1 << i)) { |
|
|
|
|
printf(" %u:%hd", i, REG_TO_SIGNED(io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_BASE + i))); |
|
|
|
|
printf(" %u:%" PRId16, i, REG_TO_SIGNED(io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_BASE + i))); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -2533,32 +2535,32 @@ PX4IO::print_status(bool extended_status)
@@ -2533,32 +2535,32 @@ PX4IO::print_status(bool extended_status)
|
|
|
|
|
printf("ADC inputs"); |
|
|
|
|
|
|
|
|
|
for (unsigned i = 0; i < adc_inputs; i++) { |
|
|
|
|
printf(" %u", io_reg_get(PX4IO_PAGE_RAW_ADC_INPUT, i)); |
|
|
|
|
printf(" %" PRIu32, io_reg_get(PX4IO_PAGE_RAW_ADC_INPUT, i)); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
printf("\n"); |
|
|
|
|
|
|
|
|
|
/* setup and state */ |
|
|
|
|
uint16_t features = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES); |
|
|
|
|
printf("features 0x%04hx%s%s%s%s\n", features, |
|
|
|
|
printf("features 0x%04" PRIx16 "%s%s%s%s\n", features, |
|
|
|
|
((features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) ? " S.BUS1_OUT" : ""), |
|
|
|
|
((features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) ? " S.BUS2_OUT" : ""), |
|
|
|
|
((features & PX4IO_P_SETUP_FEATURES_PWM_RSSI) ? " RSSI_PWM" : ""), |
|
|
|
|
((features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) ? " RSSI_ADC" : "") |
|
|
|
|
); |
|
|
|
|
|
|
|
|
|
printf("rates 0x%04x default %u alt %u sbus %u\n", |
|
|
|
|
printf("rates 0x%04" PRIx32 " default %" PRIu32 " alt %" PRIu32 " sbus %" PRIu32 "\n", |
|
|
|
|
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES), |
|
|
|
|
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE), |
|
|
|
|
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE), |
|
|
|
|
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SBUS_RATE)); |
|
|
|
|
printf("debuglevel %u\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG)); |
|
|
|
|
printf("debuglevel %" PRIu32 "\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG)); |
|
|
|
|
|
|
|
|
|
for (unsigned group = 0; group < 4; group++) { |
|
|
|
|
printf("controls %u:", group); |
|
|
|
|
|
|
|
|
|
for (unsigned i = 0; i < _max_controls; i++) { |
|
|
|
|
printf(" %hd", (int16_t) io_reg_get(PX4IO_PAGE_CONTROLS, group * PX4IO_PROTOCOL_MAX_CONTROL_COUNT + i)); |
|
|
|
|
printf(" %" PRIu16, (int16_t) io_reg_get(PX4IO_PAGE_CONTROLS, group * PX4IO_PROTOCOL_MAX_CONTROL_COUNT + i)); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
printf("\n"); |
|
|
|
@ -2568,7 +2570,8 @@ PX4IO::print_status(bool extended_status)
@@ -2568,7 +2570,8 @@ PX4IO::print_status(bool extended_status)
|
|
|
|
|
for (unsigned i = 0; i < _max_rc_input; i++) { |
|
|
|
|
unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i; |
|
|
|
|
uint16_t options = io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_OPTIONS); |
|
|
|
|
printf("input %u min %u center %u max %u deadzone %u assigned %u options 0x%04hx%s%s\n", |
|
|
|
|
printf("input %u min %" PRIu32 " center %" PRIu32 " max %" PRIu32 " deadzone %" PRIu32 " assigned %" PRIu32 |
|
|
|
|
" options 0x%04" PRIx16 "%s%s\n", |
|
|
|
|
i, |
|
|
|
|
io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MIN), |
|
|
|
|
io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_CENTER), |
|
|
|
@ -2584,13 +2587,13 @@ PX4IO::print_status(bool extended_status)
@@ -2584,13 +2587,13 @@ PX4IO::print_status(bool extended_status)
|
|
|
|
|
printf("failsafe"); |
|
|
|
|
|
|
|
|
|
for (unsigned i = 0; i < _max_actuators; i++) { |
|
|
|
|
printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i)); |
|
|
|
|
printf(" %" PRIu32, io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i)); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
printf("\ndisarmed values"); |
|
|
|
|
|
|
|
|
|
for (unsigned i = 0; i < _max_actuators; i++) { |
|
|
|
|
printf(" %u", io_reg_get(PX4IO_PAGE_DISARMED_PWM, i)); |
|
|
|
|
printf(" %" PRIu32, io_reg_get(PX4IO_PAGE_DISARMED_PWM, i)); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/* IMU heater (Pixhawk 2.1) */ |
|
|
|
@ -3001,7 +3004,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
@@ -3001,7 +3004,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if (io_crc != arg) { |
|
|
|
|
PX4_DEBUG("Firmware CRC mismatch 0x%08x 0x%08lx", io_crc, arg); |
|
|
|
|
PX4_DEBUG("Firmware CRC mismatch 0x%08" PRIx32 " 0x%08lx", io_crc, arg); |
|
|
|
|
return -EINVAL; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -3251,7 +3254,7 @@ checkcrc(int argc, char *argv[])
@@ -3251,7 +3254,7 @@ checkcrc(int argc, char *argv[])
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if (ret != OK) { |
|
|
|
|
PX4_ERR("check CRC failed: %d, CRC: %u", ret, fw_crc); |
|
|
|
|
PX4_ERR("check CRC failed: %d, CRC: %" PRIu32, ret, fw_crc); |
|
|
|
|
exit(1); |
|
|
|
|
|
|
|
|
|
} else { |
|
|
|
@ -3584,7 +3587,7 @@ px4io_main(int argc, char *argv[])
@@ -3584,7 +3587,7 @@ px4io_main(int argc, char *argv[])
|
|
|
|
|
exit(1); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
warnx("SET_DEBUG %hhu OK", level); |
|
|
|
|
warnx("SET_DEBUG %" PRIu8 " OK", level); |
|
|
|
|
exit(0); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|