|
|
|
@ -194,6 +194,7 @@ PWMSim::~PWMSim()
@@ -194,6 +194,7 @@ PWMSim::~PWMSim()
|
|
|
|
|
_task_should_exit = true; |
|
|
|
|
|
|
|
|
|
unsigned i = 10; |
|
|
|
|
|
|
|
|
|
do { |
|
|
|
|
/* wait 50ms - it should wake every 100ms or so worst-case */ |
|
|
|
|
usleep(50000); |
|
|
|
@ -226,6 +227,7 @@ PWMSim::init()
@@ -226,6 +227,7 @@ PWMSim::init()
|
|
|
|
|
|
|
|
|
|
if (ret != OK) { |
|
|
|
|
return ret; |
|
|
|
|
|
|
|
|
|
} else { |
|
|
|
|
_primary_pwm_device = true; |
|
|
|
|
} |
|
|
|
@ -308,6 +310,7 @@ PWMSim::set_mode(Mode mode)
@@ -308,6 +310,7 @@ PWMSim::set_mode(Mode mode)
|
|
|
|
|
default: |
|
|
|
|
return -EINVAL; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
_mode = mode; |
|
|
|
|
return OK; |
|
|
|
|
} |
|
|
|
@ -315,8 +318,9 @@ PWMSim::set_mode(Mode mode)
@@ -315,8 +318,9 @@ PWMSim::set_mode(Mode mode)
|
|
|
|
|
int |
|
|
|
|
PWMSim::set_pwm_rate(unsigned rate) |
|
|
|
|
{ |
|
|
|
|
if ((rate > 500) || (rate < 10)) |
|
|
|
|
if ((rate > 500) || (rate < 10)) { |
|
|
|
|
return -EINVAL; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
_update_rate = rate; |
|
|
|
|
return OK; |
|
|
|
@ -383,8 +387,11 @@ PWMSim::task_main()
@@ -383,8 +387,11 @@ PWMSim::task_main()
|
|
|
|
|
/* handle update rate changes */ |
|
|
|
|
if (_current_update_rate != _update_rate) { |
|
|
|
|
int update_rate_in_ms = int(1000 / _update_rate); |
|
|
|
|
if (update_rate_in_ms < 2) |
|
|
|
|
|
|
|
|
|
if (update_rate_in_ms < 2) { |
|
|
|
|
update_rate_in_ms = 2; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
orb_set_interval(_t_actuators, update_rate_in_ms); |
|
|
|
|
// up_pwm_servo_set_rate(_update_rate);
|
|
|
|
|
_current_update_rate = _update_rate; |
|
|
|
@ -420,6 +427,7 @@ PWMSim::task_main()
@@ -420,6 +427,7 @@ PWMSim::task_main()
|
|
|
|
|
outputs.output[i] <= 1.0f) { |
|
|
|
|
/* scale for PWM output 900 - 2100us */ |
|
|
|
|
outputs.output[i] = 1500 + (600 * outputs.output[i]); |
|
|
|
|
|
|
|
|
|
} else { |
|
|
|
|
/*
|
|
|
|
|
* Value is NaN, INF or out of band - set to the minimum value. |
|
|
|
@ -468,10 +476,12 @@ PWMSim::control_callback(uintptr_t handle,
@@ -468,10 +476,12 @@ PWMSim::control_callback(uintptr_t handle,
|
|
|
|
|
|
|
|
|
|
if (_armed) { |
|
|
|
|
input = controls->control[control_index]; |
|
|
|
|
|
|
|
|
|
} else { |
|
|
|
|
/* clamp actuator to zero if not armed */ |
|
|
|
|
input = 0.0f; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
return 0; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -481,7 +491,7 @@ PWMSim::ioctl(device::file_t *filp, int cmd, unsigned long arg)
@@ -481,7 +491,7 @@ PWMSim::ioctl(device::file_t *filp, int cmd, unsigned long arg)
|
|
|
|
|
int ret; |
|
|
|
|
|
|
|
|
|
/* if we are in valid PWM mode, try it as a PWM ioctl as well */ |
|
|
|
|
switch(_mode) { |
|
|
|
|
switch (_mode) { |
|
|
|
|
case MODE_2PWM: |
|
|
|
|
case MODE_4PWM: |
|
|
|
|
case MODE_8PWM: |
|
|
|
@ -489,6 +499,7 @@ PWMSim::ioctl(device::file_t *filp, int cmd, unsigned long arg)
@@ -489,6 +499,7 @@ PWMSim::ioctl(device::file_t *filp, int cmd, unsigned long arg)
|
|
|
|
|
case MODE_16PWM: |
|
|
|
|
ret = PWMSim::pwm_ioctl(filp, cmd, arg); |
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
default: |
|
|
|
|
ret = -ENOTTY; |
|
|
|
|
DEVICE_DEBUG("not in a PWM mode"); |
|
|
|
@ -648,6 +659,7 @@ PWMSim::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
@@ -648,6 +659,7 @@ PWMSim::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
|
|
|
|
|
} else if (_mode == MODE_4PWM) { |
|
|
|
|
|
|
|
|
|
*(unsigned *)arg = 4; |
|
|
|
|
|
|
|
|
|
} else { |
|
|
|
|
|
|
|
|
|
*(unsigned *)arg = 2; |
|
|
|
@ -688,8 +700,9 @@ PWMSim::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
@@ -688,8 +700,9 @@ PWMSim::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
|
|
|
|
|
const char *buf = (const char *)arg; |
|
|
|
|
unsigned buflen = strnlen(buf, 1024); |
|
|
|
|
|
|
|
|
|
if (_mixers == nullptr) |
|
|
|
|
if (_mixers == nullptr) { |
|
|
|
|
_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if (_mixers == nullptr) { |
|
|
|
|
ret = -ENOMEM; |
|
|
|
@ -705,6 +718,7 @@ PWMSim::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
@@ -705,6 +718,7 @@ PWMSim::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
|
|
|
|
|
ret = -EINVAL; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -852,7 +866,8 @@ fake(int argc, char *argv[])
@@ -852,7 +866,8 @@ fake(int argc, char *argv[])
|
|
|
|
|
extern "C" __EXPORT int pwm_out_sim_main(int argc, char *argv[]); |
|
|
|
|
|
|
|
|
|
static void |
|
|
|
|
usage() { |
|
|
|
|
usage() |
|
|
|
|
{ |
|
|
|
|
PX4_WARN("pwm_out_sim: unrecognized command, try:"); |
|
|
|
|
PX4_WARN(" mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, mode_port2_pwm8, mode_port2_pwm12, mode_port2_pwm16"); |
|
|
|
|
} |
|
|
|
@ -868,10 +883,12 @@ pwm_out_sim_main(int argc, char *argv[])
@@ -868,10 +883,12 @@ pwm_out_sim_main(int argc, char *argv[])
|
|
|
|
|
usage(); |
|
|
|
|
return -EINVAL; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
verb = argv[1]; |
|
|
|
|
|
|
|
|
|
if (g_pwm_sim == nullptr) { |
|
|
|
|
g_pwm_sim = new PWMSim; |
|
|
|
|
|
|
|
|
|
if (g_pwm_sim == nullptr) { |
|
|
|
|
return -ENOMEM; |
|
|
|
|
} |
|
|
|
@ -905,13 +922,14 @@ pwm_out_sim_main(int argc, char *argv[])
@@ -905,13 +922,14 @@ pwm_out_sim_main(int argc, char *argv[])
|
|
|
|
|
if (new_mode != PORT_MODE_UNDEFINED) { |
|
|
|
|
|
|
|
|
|
/* yes but it's the same mode */ |
|
|
|
|
if (new_mode == g_port_mode) |
|
|
|
|
if (new_mode == g_port_mode) { |
|
|
|
|
return OK; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/* switch modes */ |
|
|
|
|
ret = hil_new_mode(new_mode); |
|
|
|
|
} |
|
|
|
|
else if (!strcmp(verb, "test")) { |
|
|
|
|
|
|
|
|
|
} else if (!strcmp(verb, "test")) { |
|
|
|
|
ret = test(); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -924,13 +942,15 @@ pwm_out_sim_main(int argc, char *argv[])
@@ -924,13 +942,15 @@ pwm_out_sim_main(int argc, char *argv[])
|
|
|
|
|
ret = -EINVAL; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if ( ret == OK && g_pwm_sim->_task == -1 ) { |
|
|
|
|
if (ret == OK && g_pwm_sim->_task == -1) { |
|
|
|
|
ret = g_pwm_sim->init(); |
|
|
|
|
|
|
|
|
|
if (ret != OK) { |
|
|
|
|
warnx("failed to start the pwm_out_sim driver"); |
|
|
|
|
delete g_pwm_sim; |
|
|
|
|
g_pwm_sim = nullptr; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
return ret; |
|
|
|
|
} |
|
|
|
|