diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 671651ed60..de6c5dd903 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -171,14 +171,15 @@ private: device::Device *const _interface; - unsigned _hardware{0}; ///< Hardware revision - unsigned _max_actuators{0}; ///< Maximum # of actuators supported by PX4IO - unsigned _max_controls{0}; ///< Maximum # of controls supported by PX4IO - unsigned _max_rc_input{0}; ///< Maximum receiver channels supported by PX4IO - unsigned _max_transfer{16}; ///< Maximum number of I2C transfers supported by PX4IO + uint16_t _hardware{0}; ///< Hardware revision + uint16_t _max_actuators{0}; ///< Maximum # of actuators supported by PX4IO + uint16_t _max_controls{0}; ///< Maximum # of controls supported by PX4IO + uint16_t _max_rc_input{0}; ///< Maximum receiver channels supported by PX4IO + uint16_t _max_transfer{16}; ///< Maximum number of I2C transfers supported by PX4IO + + uint16_t _group_channels[PX4IO_P_SETUP_PWM_RATE_GROUP3 - PX4IO_P_SETUP_PWM_RATE_GROUP0 + 1] {}; bool _first_update_cycle{true}; - uint32_t _group_channels[PX4IO_P_SETUP_PWM_RATE_GROUP3 - PX4IO_P_SETUP_PWM_RATE_GROUP0 + 1] {}; hrt_abstime _poll_last{0}; @@ -254,7 +255,7 @@ private: int io_publish_raw_rc(); /** - * write register(s) + * write registers * * @param page Register page to write to. * @param offset Register offset to start writing at. @@ -262,7 +263,7 @@ private: * @param num_values The number of values to write. * @return OK if all values were successfully written. */ - int io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values); + int io_regs_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values); /** * write a register @@ -272,10 +273,10 @@ private: * @param value Value to write. * @return OK if the value was written successfully. */ - int io_reg_set(uint8_t page, uint8_t offset, const uint16_t value); + int io_reg_set(uint8_t page, uint8_t offset, uint16_t value); /** - * read register(s) + * read registers * * @param page Register page to read from. * @param offset Register offset to start reading from. @@ -283,17 +284,17 @@ private: * @param num_values The number of values to read. * @return OK if all values were successfully read. */ - int io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values); + int io_regs_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values); /** * read a register * * @param page Register page to read from. * @param offset Register offset to start reading from. - * @return Register value that was read, or _io_reg_get_error on error. + * @param value Read register value. + * @return Ok if read was successful. */ - uint32_t io_reg_get(uint8_t page, uint8_t offset); - static const uint32_t _io_reg_get_error = 0x80000000; + int io_reg_get(uint8_t page, uint8_t offset, uint16_t &value); /** * modify a register @@ -378,7 +379,7 @@ bool PX4IO::updateOutputs(float outputs[MAX_ACTUATORS], unsigned num_outputs, un if (!_test_fmu_fail) { /* output to the servos */ - io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, hw_outputs, num_outputs); + io_regs_set(PX4IO_PAGE_DIRECT_PWM, 0, hw_outputs, num_outputs); } return true; @@ -397,16 +398,16 @@ int PX4IO::init() } /* get some parameters */ - unsigned protocol; + uint16_t protocol; hrt_abstime start_try_time = hrt_absolute_time(); do { px4_usleep(2000); - protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION); - } while (protocol == _io_reg_get_error && (hrt_elapsed_time(&start_try_time) < 700U * 1000U)); + ret = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION, protocol); + } while (ret != OK && (hrt_elapsed_time(&start_try_time) < 700U * 1000U)); /* if the error still persists after timing out, we give up */ - if (protocol == _io_reg_get_error) { + if (ret != OK) { mavlink_log_emergency(&_mavlink_log_pub, "Failed to communicate with IO, abort.\t"); events::send(events::ID("px4io_comm_failed"), events::Log::Emergency, "Failed to communicate with IO, aborting initialization"); @@ -420,11 +421,12 @@ int PX4IO::init() return -1; } - _hardware = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_HARDWARE_VERSION); - _max_actuators = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT); - _max_controls = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT); - _max_transfer = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER) - 2; - _max_rc_input = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT); + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_HARDWARE_VERSION, _hardware); + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT, _max_actuators); + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT, _max_controls); + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER, _max_transfer); + _max_transfer -= 2; + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT, _max_rc_input); if ((_max_actuators < 1) || (_max_actuators > PX4IO_MAX_ACTUATORS) || (_max_transfer < 16) || (_max_transfer > 255) || @@ -445,8 +447,13 @@ int PX4IO::init() return -1; } + /* Set safety_off to false when FMU boot*/ - io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SAFETY_OFF, 0); + ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SAFETY_OFF, 0); + + if (ret != OK) { + return -1; + } if (_max_rc_input > input_rc_s::RC_INPUT_MAX_CHANNELS) { _max_rc_input = input_rc_s::RC_INPUT_MAX_CHANNELS; @@ -455,15 +462,16 @@ int PX4IO::init() uint16_t reg = 0; /* get IO's last seen FMU state */ - ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, ®, sizeof(reg)); + ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, reg); if (ret != OK) { return ret; } /* dis-arm IO before touching anything */ - io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FMU_ARMED | PX4IO_P_SETUP_ARMING_LOCKDOWN, - 0); + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, + PX4IO_P_SETUP_ARMING_FMU_ARMED | PX4IO_P_SETUP_ARMING_LOCKDOWN, + 0); if (ret != OK) { mavlink_log_critical(&_mavlink_log_pub, "IO RC config upload fail\t"); @@ -475,7 +483,11 @@ int PX4IO::init() /* initialize _group_channels */ for (uint8_t group = PX4IO_P_SETUP_PWM_RATE_GROUP0; group <= PX4IO_P_SETUP_PWM_RATE_GROUP3; ++group) { unsigned group_idx = group - PX4IO_P_SETUP_PWM_RATE_GROUP0; - _group_channels[group_idx] = io_reg_get(PX4IO_PAGE_PWM_INFO, PX4IO_RATE_MAP_BASE + group_idx); + ret = io_reg_get(PX4IO_PAGE_PWM_INFO, PX4IO_RATE_MAP_BASE + group_idx, _group_channels[group_idx]); + + if (ret != OK) { + return -1; + } } /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ @@ -498,7 +510,7 @@ void PX4IO::updateDisarmed() values[i] = _mixing_output.disarmedValue(i); } - io_reg_set(PX4IO_PAGE_DISARMED_PWM, 0, values, _max_actuators); + io_regs_set(PX4IO_PAGE_DISARMED_PWM, 0, values, _max_actuators); } void PX4IO::updateFailsafe() @@ -509,7 +521,7 @@ void PX4IO::updateFailsafe() values[i] = static_cast(lroundf(_mixing_output.actualFailsafeValue(i))); } - io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, values, _max_actuators); + io_regs_set(PX4IO_PAGE_FAILSAFE_PWM, 0, values, _max_actuators); } void PX4IO::Run() @@ -682,7 +694,7 @@ void PX4IO::updateTimerRateGroups() ++timer; } - int ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATE_GROUP0, timer_rates, timer); + int ret = io_regs_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATE_GROUP0, timer_rates, timer); if (ret != 0) { PX4_ERR("io_reg_set failed (%i)", ret); @@ -928,7 +940,7 @@ int PX4IO::io_get_status() * STATUS_VSERVO, STATUS_VRSSI * in that order */ uint16_t regs[6] {}; - int ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, ®s[0], sizeof(regs) / sizeof(regs[0])); + int ret = io_regs_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, ®s[0], sizeof(regs) / sizeof(regs[0])); if (ret != OK) { return ret; @@ -953,7 +965,8 @@ int PX4IO::io_get_status() _analog_rc_rssi_stable = true; } - const uint16_t SETUP_ARMING = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING); + uint16_t SETUP_ARMING; + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, SETUP_ARMING); if ((hrt_elapsed_time(&_last_status_publish) >= 1_s) || (_status != STATUS_FLAGS) @@ -966,7 +979,7 @@ int PX4IO::io_get_status() status.voltage_v = STATUS_VSERVO * 0.001f; // voltage is scaled to mV status.rssi_v = rssi_v; - status.free_memory_bytes = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM); + io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM, status.free_memory_bytes); // PX4IO_P_STATUS_FLAGS status.status_outputs_armed = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED; @@ -998,9 +1011,9 @@ int PX4IO::io_get_status() status.arming_termination_failsafe = SETUP_ARMING & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE; for (unsigned i = 0; i < _max_actuators; i++) { - status.pwm[i] = io_reg_get(PX4IO_PAGE_SERVOS, i); - status.pwm_disarmed[i] = io_reg_get(PX4IO_PAGE_DISARMED_PWM, i); - status.pwm_failsafe[i] = io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i); + io_reg_get(PX4IO_PAGE_SERVOS, i, status.pwm[i]); + io_reg_get(PX4IO_PAGE_DISARMED_PWM, i, status.pwm_disarmed[i]); + io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i, status.pwm_failsafe[i]); } { @@ -1008,14 +1021,15 @@ int PX4IO::io_get_status() for (uint8_t offset = PX4IO_P_SETUP_PWM_RATE_GROUP0; offset <= PX4IO_P_SETUP_PWM_RATE_GROUP3; ++offset) { // This is a bit different than below, setting the groups, not the channels - status.pwm_rate_hz[i++] = io_reg_get(PX4IO_PAGE_SETUP, offset); + io_reg_get(PX4IO_PAGE_SETUP, offset, status.pwm_rate_hz[i++]); } } - uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT); + uint16_t raw_inputs = 0; + io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, raw_inputs); for (unsigned i = 0; (i < raw_inputs) && (i < _max_rc_input); i++) { - status.raw_inputs[i] = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + i); + io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + i, status.raw_inputs[i]); } status.timestamp = hrt_absolute_time(); @@ -1032,7 +1046,8 @@ int PX4IO::io_get_status() int PX4IO::io_publish_raw_rc() { - const uint16_t rc_valid_update_count = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_FRAME_COUNT); + uint16_t rc_valid_update_count = 0; + io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_FRAME_COUNT, rc_valid_update_count); const bool rc_updated = (rc_valid_update_count != _rc_valid_update_count); _rc_valid_update_count = rc_valid_update_count; @@ -1060,7 +1075,7 @@ int PX4IO::io_publish_raw_rc() * * This should be the common case (9 channel R/C control being a reasonable upper bound). */ - int ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, ®s[0], prolog + 9); + int ret = io_regs_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, ®s[0], prolog + 9); if (ret != OK) { return ret; @@ -1109,7 +1124,7 @@ int PX4IO::io_publish_raw_rc() /* input_rc.input_source is set after this call XXX we might want to mirror the flags in the RC struct */ if (channel_count > 9) { - ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + 9, ®s[prolog + 9], channel_count - 9); + ret = io_regs_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + 9, ®s[prolog + 9], channel_count - 9); if (ret != OK) { return ret; @@ -1162,11 +1177,11 @@ int PX4IO::io_publish_raw_rc() return ret; } -int PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) +int PX4IO::io_regs_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) { /* range check the transfer */ if (num_values > ((_max_transfer) / sizeof(*values))) { - PX4_DEBUG("io_reg_set: too many registers (%u, max %u)", num_values, _max_transfer / 2); + PX4_DEBUG("io_regs_set: too many registers (%u, max %u)", num_values, _max_transfer / 2); return -EINVAL; } @@ -1175,7 +1190,7 @@ int PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsi perf_end(_interface_write_perf); if (ret != (int)num_values) { - PX4_DEBUG("io_reg_set(%" PRIu8 ",%" PRIu8 ",%u): error %d", page, offset, num_values, ret); + PX4_DEBUG("io_regs_set(%" PRIu8 ",%" PRIu8 ",%u): error %d", page, offset, num_values, ret); return -1; } @@ -1184,14 +1199,13 @@ int PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsi int PX4IO::io_reg_set(uint8_t page, uint8_t offset, uint16_t value) { - return io_reg_set(page, offset, &value, 1); + return io_regs_set(page, offset, &value, 1); } -int PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values) +int PX4IO::io_regs_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values) { - /* range check the transfer */ if (num_values > ((_max_transfer) / sizeof(*values))) { - PX4_DEBUG("io_reg_get: too many registers (%u, max %u)", num_values, _max_transfer / 2); + PX4_DEBUG("io_regs_get: too many registers (%u, max %u)", num_values, _max_transfer / 2); return -EINVAL; } @@ -1207,21 +1221,15 @@ int PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned n return OK; } -uint32_t PX4IO::io_reg_get(uint8_t page, uint8_t offset) +int PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t &value) { - uint16_t value; - - if (io_reg_get(page, offset, &value, 1) != OK) { - return _io_reg_get_error; - } - - return value; + return io_regs_get(page, offset, &value, 1); } int PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t setbits) { uint16_t value = 0; - int ret = io_reg_get(page, offset, &value, 1); + int ret = io_regs_get(page, offset, &value, 1); if (ret != OK) { return ret; @@ -1235,21 +1243,25 @@ int PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint1 int PX4IO::print_status() { - /* basic configuration */ - printf("protocol %" PRIu32 " hardware %" PRIu32 " bootloader %" PRIu32 " buffer %" PRIu32 "B crc 0x%04" PRIx32 "%04" - PRIx32 "\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)); + uint16_t protocol, hardware, bootloader, buffer, crc1, crc2; + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION, protocol); + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_HARDWARE_VERSION, hardware); + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_BOOTLOADER_VERSION, bootloader); + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER, buffer); + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC, crc1); + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC + 1, crc2); - printf("%" PRIu32 " controls %" PRIu32 " actuators %" PRIu32 " R/C inputs %" PRIu32 " analog inputs\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), - io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ADC_INPUT_COUNT)); + /* basic configuration */ + printf("protocol %" PRIu16 " hardware %" PRIu16 " bootloader %" PRIu16 " buffer %" PRIu16 "B crc 0x%04" PRIx16 "%04" PRIx16 "\n", + protocol, hardware, bootloader, buffer, crc1, crc2); + + uint16_t controls, actuators, rc_inputs, adc_input_count; + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT, controls); + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT, actuators); + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT, rc_inputs); + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ADC_INPUT_COUNT, adc_input_count); + printf("%" PRIu16 " controls %" PRIu16 " actuators %" PRIu16 " R/C inputs %" PRIu16 " analog inputs\n", + controls, actuators, rc_inputs, adc_input_count); /* status */ uORB::SubscriptionData status_sub{ORB_ID(px4io_status)}; @@ -1262,37 +1274,49 @@ int PX4IO::print_status() printf("\n"); - uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT); + uint16_t raw_inputs; + io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, raw_inputs); printf("%" PRIu16 " raw R/C inputs", raw_inputs); for (unsigned i = 0; i < raw_inputs; i++) { - printf(" %" PRIu32, io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + i)); + uint16_t value = 0; + io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + i, value); + printf(" %" PRIu16, value); } printf("\n"); - uint16_t adc_inputs = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ADC_INPUT_COUNT); + uint16_t adc_inputs; + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ADC_INPUT_COUNT, adc_inputs); printf("ADC inputs"); for (unsigned i = 0; i < adc_inputs; i++) { - printf(" %" PRIu32, io_reg_get(PX4IO_PAGE_RAW_ADC_INPUT, i)); + uint16_t value = 0; + io_reg_get(PX4IO_PAGE_RAW_ADC_INPUT, i, value); + printf(" %" PRIu16, value); } printf("\n"); /* setup and state */ - uint16_t features = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES); + uint16_t features; + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, features); printf("features 0x%04" PRIx16 "%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_ADC_RSSI) ? " RSSI_ADC" : "") ); - printf("sbus rate %" PRIu32 "\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SBUS_RATE)); + uint16_t sbus_rate; + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SBUS_RATE, sbus_rate); + printf("sbus rate %" PRIu16 "\n", sbus_rate); - printf("debuglevel %" PRIu32 "\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG)); + uint16_t debug_level; + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG, debug_level); + printf("debuglevel %" PRIu16 "\n", debug_level); /* IMU heater (Pixhawk 2.1) */ - uint16_t heater_level = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_THERMAL); + uint16_t heater_level; + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_THERMAL, heater_level); if (heater_level != UINT16_MAX) { if (heater_level == PX4IO_THERMAL_OFF) { @@ -1355,7 +1379,7 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg) /* check IO firmware CRC against passed value */ uint32_t io_crc = 0; - ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC, (uint16_t *)&io_crc, 2); + ret = io_regs_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC, (uint16_t *)&io_crc, 2); if (ret != OK) { return ret; diff --git a/src/drivers/px4io/px4io_serial.cpp b/src/drivers/px4io/px4io_serial.cpp index efb7065c35..736ca2661c 100644 --- a/src/drivers/px4io/px4io_serial.cpp +++ b/src/drivers/px4io/px4io_serial.cpp @@ -168,6 +168,9 @@ PX4IO_serial::read(unsigned address, void *data, unsigned count) for (unsigned retries = 0; retries < 3; retries++) { + /* Clear the entire packet to ensure no stale data */ + memset(_io_buffer_ptr, 0, sizeof(IOPacket)); + _io_buffer_ptr->count_code = count | PKT_CODE_READ; _io_buffer_ptr->page = page; _io_buffer_ptr->offset = offset;