mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-06-01 02:55:07 +08:00
pwm_out_sim: various fixes for tiltrotor simulation
- increased the number of pwm outputs - handle PWM_SERVO_SET_MIN_PWM and PWM_SERVO_SET_MAX_PWM Signed-off-by: Roman <bapstroman@gmail.com>
This commit is contained in:
@@ -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;
|
||||||
|
|||||||
Reference in New Issue
Block a user