|
|
@ -86,6 +86,8 @@ class PWMSim : public device::CDev |
|
|
|
{ |
|
|
|
{ |
|
|
|
const uint32_t PWM_SIM_DISARMED_MAGIC = 900; |
|
|
|
const uint32_t PWM_SIM_DISARMED_MAGIC = 900; |
|
|
|
const uint32_t PWM_SIM_FAILSAFE_MAGIC = 600; |
|
|
|
const uint32_t PWM_SIM_FAILSAFE_MAGIC = 600; |
|
|
|
|
|
|
|
const uint32_t PWM_SIM_PWM_MIN_MAGIC = 1000; |
|
|
|
|
|
|
|
const uint32_t PWM_SIM_PWM_MAX_MAGIC = 2000; |
|
|
|
public: |
|
|
|
public: |
|
|
|
enum Mode { |
|
|
|
enum Mode { |
|
|
|
MODE_2PWM, |
|
|
|
MODE_2PWM, |
|
|
@ -108,7 +110,7 @@ public: |
|
|
|
int _task; |
|
|
|
int _task; |
|
|
|
|
|
|
|
|
|
|
|
private: |
|
|
|
private: |
|
|
|
static const unsigned _max_actuators = 8; |
|
|
|
static const unsigned _max_actuators = 16; |
|
|
|
|
|
|
|
|
|
|
|
Mode _mode; |
|
|
|
Mode _mode; |
|
|
|
int _update_rate; |
|
|
|
int _update_rate; |
|
|
@ -120,6 +122,8 @@ private: |
|
|
|
orb_advert_t _outputs_pub; |
|
|
|
orb_advert_t _outputs_pub; |
|
|
|
unsigned _num_outputs; |
|
|
|
unsigned _num_outputs; |
|
|
|
bool _primary_pwm_device; |
|
|
|
bool _primary_pwm_device; |
|
|
|
|
|
|
|
unsigned _pwm_min[_max_actuators]; |
|
|
|
|
|
|
|
unsigned _pwm_max[_max_actuators]; |
|
|
|
|
|
|
|
|
|
|
|
uint32_t _groups_required; |
|
|
|
uint32_t _groups_required; |
|
|
|
uint32_t _groups_subscribed; |
|
|
|
uint32_t _groups_subscribed; |
|
|
@ -192,6 +196,11 @@ PWMSim::PWMSim() : |
|
|
|
{ |
|
|
|
{ |
|
|
|
memset(_controls, 0, sizeof(_controls)); |
|
|
|
memset(_controls, 0, sizeof(_controls)); |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
for(unsigned i = 0; i < _max_actuators; i++) { |
|
|
|
|
|
|
|
_pwm_min[i] = PWM_SIM_PWM_MIN_MAGIC; |
|
|
|
|
|
|
|
_pwm_max[i] = PWM_SIM_PWM_MAX_MAGIC; |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
_control_topics[0] = ORB_ID(actuator_controls_0); |
|
|
|
_control_topics[0] = ORB_ID(actuator_controls_0); |
|
|
|
_control_topics[1] = ORB_ID(actuator_controls_1); |
|
|
|
_control_topics[1] = ORB_ID(actuator_controls_1); |
|
|
|
_control_topics[2] = ORB_ID(actuator_controls_2); |
|
|
|
_control_topics[2] = ORB_ID(actuator_controls_2); |
|
|
@ -463,6 +472,10 @@ PWMSim::task_main() |
|
|
|
num_outputs = 8; |
|
|
|
num_outputs = 8; |
|
|
|
break; |
|
|
|
break; |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
case MODE_12PWM: |
|
|
|
|
|
|
|
num_outputs = 12; |
|
|
|
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
|
|
case MODE_16PWM: |
|
|
|
case MODE_16PWM: |
|
|
|
num_outputs = 16; |
|
|
|
num_outputs = 16; |
|
|
|
break; |
|
|
|
break; |
|
|
@ -494,6 +507,14 @@ PWMSim::task_main() |
|
|
|
/* scale for PWM output 1000 - 2000us */ |
|
|
|
/* scale for PWM output 1000 - 2000us */ |
|
|
|
outputs.output[i] = 1500 + (500 * outputs.output[i]); |
|
|
|
outputs.output[i] = 1500 + (500 * outputs.output[i]); |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if (outputs.output[i] > _pwm_max[i]) { |
|
|
|
|
|
|
|
outputs.output[i] = _pwm_max[i]; |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if (outputs.output[i] < _pwm_min[i]) { |
|
|
|
|
|
|
|
outputs.output[i] = _pwm_min[i]; |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
} else { |
|
|
|
} else { |
|
|
|
/*
|
|
|
|
/*
|
|
|
|
* Value is NaN, INF or out of band - set to the minimum value. |
|
|
|
* Value is NaN, INF or out of band - set to the minimum value. |
|
|
@ -618,6 +639,26 @@ PWMSim::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg) |
|
|
|
// up_pwm_servo_arm(false);
|
|
|
|
// up_pwm_servo_arm(false);
|
|
|
|
break; |
|
|
|
break; |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
case PWM_SERVO_SET_MIN_PWM: { |
|
|
|
|
|
|
|
struct pwm_output_values *pwm = (struct pwm_output_values *)arg; |
|
|
|
|
|
|
|
for(unsigned i = 0; i < pwm->channel_count; i++) { |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if (i <= _max_actuators) |
|
|
|
|
|
|
|
_pwm_min[i] = pwm->values[i]; |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
break; |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
case PWM_SERVO_SET_MAX_PWM: { |
|
|
|
|
|
|
|
struct pwm_output_values *pwm = (struct pwm_output_values *)arg; |
|
|
|
|
|
|
|
for(unsigned i = 0; i < pwm->channel_count; i++) { |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if (i <= _max_actuators) |
|
|
|
|
|
|
|
_pwm_max[i] = pwm->values[i]; |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
break; |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
case PWM_SERVO_SET_UPDATE_RATE: |
|
|
|
case PWM_SERVO_SET_UPDATE_RATE: |
|
|
|
// PWMSim always outputs at the alternate (usually faster) rate
|
|
|
|
// PWMSim always outputs at the alternate (usually faster) rate
|
|
|
|
g_pwm_sim->set_pwm_rate(arg); |
|
|
|
g_pwm_sim->set_pwm_rate(arg); |
|
|
@ -754,6 +795,10 @@ PWMSim::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg) |
|
|
|
if (_mode == MODE_16PWM) { |
|
|
|
if (_mode == MODE_16PWM) { |
|
|
|
*(unsigned *)arg = 16; |
|
|
|
*(unsigned *)arg = 16; |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
} else if (_mode == MODE_12PWM) { |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
*(unsigned *)arg = 12; |
|
|
|
|
|
|
|
|
|
|
|
} else if (_mode == MODE_8PWM) { |
|
|
|
} else if (_mode == MODE_8PWM) { |
|
|
|
|
|
|
|
|
|
|
|
*(unsigned *)arg = 8; |
|
|
|
*(unsigned *)arg = 8; |
|
|
@ -993,7 +1038,7 @@ pwm_out_sim_main(int argc, char *argv[]) |
|
|
|
|
|
|
|
|
|
|
|
// this was all cut-and-pasted from the FMU driver; it's junk
|
|
|
|
// this was all cut-and-pasted from the FMU driver; it's junk
|
|
|
|
if (!strcmp(verb, "mode_pwm")) { |
|
|
|
if (!strcmp(verb, "mode_pwm")) { |
|
|
|
new_mode = PORT1_FULL_PWM; |
|
|
|
new_mode = PORT2_12PWM; |
|
|
|
|
|
|
|
|
|
|
|
} else if (!strcmp(verb, "mode_port2_pwm8")) { |
|
|
|
} else if (!strcmp(verb, "mode_port2_pwm8")) { |
|
|
|
new_mode = PORT2_8PWM; |
|
|
|
new_mode = PORT2_8PWM; |
|
|
|