forked from Archive/PX4-Autopilot
px4io: add support for test_motor
And fix some IO bugs: - PX4IO_P_STATUS_FLAGS_RAW_PWM was never cleared. This meant that after a 'pwm test' command, normal mixing was not possible anymore. Fixed by remembering when we are in test mode and not sending PX4IO_PAGE_CONTROLS during that time. PX4IO_P_STATUS_FLAGS_RAW_PWM is cleared when PX4IO_PAGE_CONTROLS are received. - when entering test mode w/o specifying all channels, the other channels were not set at all, which means they could still be set to values from a previous test call. This is fixed by setting all channels to disarmed when entering/leaving test mode.
This commit is contained in:
parent
0d91d4e702
commit
02833c6075
|
@ -91,6 +91,7 @@
|
||||||
#include <uORB/topics/servorail_status.h>
|
#include <uORB/topics/servorail_status.h>
|
||||||
#include <uORB/topics/parameter_update.h>
|
#include <uORB/topics/parameter_update.h>
|
||||||
#include <uORB/topics/multirotor_motor_limits.h>
|
#include <uORB/topics/multirotor_motor_limits.h>
|
||||||
|
#include <uORB/topics/test_motor.h>
|
||||||
|
|
||||||
#include <debug.h>
|
#include <debug.h>
|
||||||
|
|
||||||
|
@ -113,6 +114,8 @@ static constexpr unsigned UPDATE_INTERVAL_MAX{100}; // 100 ms -> 10 Hz
|
||||||
#define ORB_CHECK_INTERVAL 200000 // 200 ms -> 5 Hz
|
#define ORB_CHECK_INTERVAL 200000 // 200 ms -> 5 Hz
|
||||||
#define IO_POLL_INTERVAL 20000 // 20 ms -> 50 Hz
|
#define IO_POLL_INTERVAL 20000 // 20 ms -> 50 Hz
|
||||||
|
|
||||||
|
using namespace time_literals;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* The PX4IO class.
|
* The PX4IO class.
|
||||||
*
|
*
|
||||||
|
@ -223,7 +226,6 @@ public:
|
||||||
private:
|
private:
|
||||||
device::Device *_interface;
|
device::Device *_interface;
|
||||||
|
|
||||||
// XXX
|
|
||||||
unsigned _hardware; ///< Hardware revision
|
unsigned _hardware; ///< Hardware revision
|
||||||
unsigned _max_actuators; ///< Maximum # of actuators supported by PX4IO
|
unsigned _max_actuators; ///< Maximum # of actuators supported by PX4IO
|
||||||
unsigned _max_controls; ///< Maximum # of controls supported by PX4IO
|
unsigned _max_controls; ///< Maximum # of controls supported by PX4IO
|
||||||
|
@ -288,6 +290,13 @@ private:
|
||||||
|
|
||||||
bool _test_fmu_fail; ///< To test what happens if IO looses FMU
|
bool _test_fmu_fail; ///< To test what happens if IO looses FMU
|
||||||
|
|
||||||
|
struct MotorTest {
|
||||||
|
uORB::Subscription test_motor_sub{ORB_ID(test_motor)};
|
||||||
|
bool in_test_mode{false};
|
||||||
|
hrt_abstime timeout{0};
|
||||||
|
};
|
||||||
|
MotorTest _motor_test;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Trampoline to the worker task
|
* Trampoline to the worker task
|
||||||
*/
|
*/
|
||||||
|
@ -440,6 +449,11 @@ private:
|
||||||
*/
|
*/
|
||||||
void io_handle_vservo(uint16_t vservo, uint16_t vrssi);
|
void io_handle_vservo(uint16_t vservo, uint16_t vrssi);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* check and handle test_motor topic updates
|
||||||
|
*/
|
||||||
|
void handle_motor_test();
|
||||||
|
|
||||||
/* do not allow to copy this class due to ptr data members */
|
/* do not allow to copy this class due to ptr data members */
|
||||||
PX4IO(const PX4IO &);
|
PX4IO(const PX4IO &);
|
||||||
PX4IO operator=(const PX4IO &);
|
PX4IO operator=(const PX4IO &);
|
||||||
|
@ -947,6 +961,13 @@ PX4IO::task_main()
|
||||||
(void)io_set_control_groups();
|
(void)io_set_control_groups();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if (!_armed && !_lockdown_override) {
|
||||||
|
handle_motor_test();
|
||||||
|
|
||||||
|
} else {
|
||||||
|
_motor_test.in_test_mode = false;
|
||||||
|
}
|
||||||
|
|
||||||
if (now >= poll_last + IO_POLL_INTERVAL) {
|
if (now >= poll_last + IO_POLL_INTERVAL) {
|
||||||
/* run at 50-250Hz */
|
/* run at 50-250Hz */
|
||||||
poll_last = now;
|
poll_last = now;
|
||||||
|
@ -1296,7 +1317,7 @@ PX4IO::io_set_control_state(unsigned group)
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!_test_fmu_fail) {
|
if (!_test_fmu_fail && !_motor_test.in_test_mode) {
|
||||||
/* copy values to registers in IO */
|
/* copy values to registers in IO */
|
||||||
return io_reg_set(PX4IO_PAGE_CONTROLS, group * PX4IO_PROTOCOL_MAX_CONTROL_COUNT, regs, _max_controls);
|
return io_reg_set(PX4IO_PAGE_CONTROLS, group * PX4IO_PROTOCOL_MAX_CONTROL_COUNT, regs, _max_controls);
|
||||||
|
|
||||||
|
@ -1305,6 +1326,74 @@ PX4IO::io_set_control_state(unsigned group)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void
|
||||||
|
PX4IO::handle_motor_test()
|
||||||
|
{
|
||||||
|
test_motor_s test_motor;
|
||||||
|
|
||||||
|
while (_motor_test.test_motor_sub.update(&test_motor)) {
|
||||||
|
if (test_motor.driver_instance != 0 ||
|
||||||
|
hrt_elapsed_time(&test_motor.timestamp) > 100_ms) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool in_test_mode = test_motor.action == test_motor_s::ACTION_RUN;
|
||||||
|
|
||||||
|
if (in_test_mode != _motor_test.in_test_mode) {
|
||||||
|
// reset all outputs to disarmed on state change
|
||||||
|
pwm_output_values pwm_disarmed;
|
||||||
|
|
||||||
|
if (io_reg_get(PX4IO_PAGE_DISARMED_PWM, 0, pwm_disarmed.values, _max_actuators) == 0) {
|
||||||
|
for (unsigned i = 0; i < _max_actuators; ++i) {
|
||||||
|
io_reg_set(PX4IO_PAGE_DIRECT_PWM, i, pwm_disarmed.values[i]);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (in_test_mode) {
|
||||||
|
unsigned idx = test_motor.motor_number;
|
||||||
|
|
||||||
|
if (idx < _max_actuators) {
|
||||||
|
if (test_motor.value < 0.f) {
|
||||||
|
pwm_output_values pwm_disarmed;
|
||||||
|
|
||||||
|
if (io_reg_get(PX4IO_PAGE_DISARMED_PWM, 0, pwm_disarmed.values, _max_actuators) == 0) {
|
||||||
|
io_reg_set(PX4IO_PAGE_DIRECT_PWM, idx, pwm_disarmed.values[idx]);
|
||||||
|
}
|
||||||
|
|
||||||
|
} else {
|
||||||
|
pwm_output_values pwm_min;
|
||||||
|
pwm_output_values pwm_max;
|
||||||
|
|
||||||
|
if (io_reg_get(PX4IO_PAGE_CONTROL_MIN_PWM, 0, pwm_min.values, _max_actuators) == 0 &&
|
||||||
|
io_reg_get(PX4IO_PAGE_CONTROL_MAX_PWM, 0, pwm_max.values, _max_actuators) == 0) {
|
||||||
|
|
||||||
|
uint16_t value = math::constrain<uint16_t>(pwm_min.values[idx] +
|
||||||
|
(uint16_t)((pwm_max.values[idx] - pwm_min.values[idx]) * test_motor.value),
|
||||||
|
pwm_min.values[idx], pwm_max.values[idx]);
|
||||||
|
io_reg_set(PX4IO_PAGE_DIRECT_PWM, idx, value);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (test_motor.timeout_ms > 0) {
|
||||||
|
_motor_test.timeout = test_motor.timestamp + test_motor.timeout_ms * 1000;
|
||||||
|
|
||||||
|
} else {
|
||||||
|
_motor_test.timeout = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
_motor_test.in_test_mode = in_test_mode;
|
||||||
|
}
|
||||||
|
|
||||||
|
// check for timeouts
|
||||||
|
if (_motor_test.timeout != 0 && hrt_absolute_time() > _motor_test.timeout) {
|
||||||
|
_motor_test.in_test_mode = false;
|
||||||
|
_motor_test.timeout = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
int
|
int
|
||||||
PX4IO::io_set_arming_state()
|
PX4IO::io_set_arming_state()
|
||||||
{
|
{
|
||||||
|
@ -2643,7 +2732,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
|
||||||
ret = -EINVAL;
|
ret = -EINVAL;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
if (!_test_fmu_fail) {
|
if (!_test_fmu_fail && _motor_test.in_test_mode) {
|
||||||
/* send a direct PWM value */
|
/* send a direct PWM value */
|
||||||
ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, channel, arg);
|
ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, channel, arg);
|
||||||
|
|
||||||
|
@ -2692,8 +2781,20 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
case PWM_SERVO_SET_MODE:
|
case PWM_SERVO_SET_MODE: {
|
||||||
|
// reset all channels to disarmed when entering/leaving test mode, so that we don't
|
||||||
|
// accidentially use values from previous tests
|
||||||
|
pwm_output_values pwm_disarmed;
|
||||||
|
|
||||||
|
if (io_reg_get(PX4IO_PAGE_DISARMED_PWM, 0, pwm_disarmed.values, _max_actuators) == 0) {
|
||||||
|
for (unsigned i = 0; i < _max_actuators; ++i) {
|
||||||
|
io_reg_set(PX4IO_PAGE_DIRECT_PWM, i, pwm_disarmed.values[i]);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
_motor_test.in_test_mode = (arg == PWM_SERVO_ENTER_TEST_MODE);
|
||||||
ret = (arg == PWM_SERVO_ENTER_TEST_MODE || PWM_SERVO_EXIT_TEST_MODE) ? 0 : -EINVAL;
|
ret = (arg == PWM_SERVO_ENTER_TEST_MODE || PWM_SERVO_EXIT_TEST_MODE) ? 0 : -EINVAL;
|
||||||
|
}
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MIXERIOCGETOUTPUTCOUNT:
|
case MIXERIOCGETOUTPUTCOUNT:
|
||||||
|
|
|
@ -291,6 +291,8 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
|
||||||
values++;
|
values++;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RAW_PWM;
|
||||||
|
|
||||||
system_state.fmu_data_received_time = hrt_absolute_time();
|
system_state.fmu_data_received_time = hrt_absolute_time();
|
||||||
|
|
||||||
break;
|
break;
|
||||||
|
|
Loading…
Reference in New Issue