dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Add more functionality to HIL driver

This commit is contained in:
Lorenz Meier 2015-06-12 13:30:44 +02:00
parent e7765d77f6
commit 0bfc727584
1 changed files with 97 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

126
src/drivers/hil/hil.cpp
View File

@ -264,36 +264,42 @@ HIL::set_mode(Mode mode)
debug("MODE_2PWM");
/* multi-port with flow control lines as PWM */
_update_rate = 50; /* default output rate */
_num_outputs = 2;
break;
case MODE_4PWM:
debug("MODE_4PWM");
/* multi-port as 4 PWM outs */
_update_rate = 50; /* default output rate */
_num_outputs = 4;
break;
case MODE_8PWM:
debug("MODE_8PWM");
/* multi-port as 8 PWM outs */
_update_rate = 50; /* default output rate */
break;
debug("MODE_8PWM");
/* multi-port as 8 PWM outs */
_update_rate = 50; /* default output rate */
_num_outputs = 8;
break;
case MODE_12PWM:
debug("MODE_12PWM");
/* multi-port as 12 PWM outs */
_update_rate = 50; /* default output rate */
break;
case MODE_12PWM:
debug("MODE_12PWM");
/* multi-port as 12 PWM outs */
_update_rate = 50; /* default output rate */
_num_outputs = 12;
break;
case MODE_16PWM:
debug("MODE_16PWM");
/* multi-port as 16 PWM outs */
_update_rate = 50; /* default output rate */
break;
case MODE_16PWM:
debug("MODE_16PWM");
/* multi-port as 16 PWM outs */
_update_rate = 50; /* default output rate */
_num_outputs = 16;
break;
case MODE_NONE:
debug("MODE_NONE");
/* disable servo outputs and set a very low update rate */
_update_rate = 10;
_num_outputs = 0;
break;
default:
@ -468,13 +474,6 @@ HIL::ioctl(file *filp, int cmd, unsigned long arg)
{
int ret;
debug("ioctl 0x%04x 0x%08x", cmd, arg);
// /* try it as a GPIO ioctl first */
// ret = HIL::gpio_ioctl(filp, cmd, arg);
// if (ret != -ENOTTY)
// return ret;
/* if we are in valid PWM mode, try it as a PWM ioctl as well */
switch(_mode) {
case MODE_2PWM:
@ -523,6 +522,62 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
// HIL always outputs at the alternate (usually faster) rate
break;
case PWM_SERVO_GET_DEFAULT_UPDATE_RATE:
*(uint32_t *)arg = 400;
break;
case PWM_SERVO_GET_UPDATE_RATE:
*(uint32_t *)arg = 400;
break;
case PWM_SERVO_GET_SELECT_UPDATE_RATE:
*(uint32_t *)arg = 0;
break;
case PWM_SERVO_GET_FAILSAFE_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < _num_outputs; i++) {
pwm->values[i] = 850;
}
pwm->channel_count = _num_outputs;
break;
}
case PWM_SERVO_GET_DISARMED_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < _num_outputs; i++) {
pwm->values[i] = 900;
}
pwm->channel_count = _num_outputs;
break;
}
case PWM_SERVO_GET_MIN_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < _num_outputs; i++) {
pwm->values[i] = 1000;
}
pwm->channel_count = _num_outputs;
break;
}
case PWM_SERVO_GET_MAX_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < _num_outputs; i++) {
pwm->values[i] = 2000;
}
pwm->channel_count = _num_outputs;
break;
}
case PWM_SERVO_SET(2):
case PWM_SERVO_SET(3):
if (_mode != MODE_4PWM) {
@ -543,18 +598,26 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
break;
case PWM_SERVO_GET(2):
case PWM_SERVO_GET(7):
case PWM_SERVO_GET(6):
case PWM_SERVO_GET(5):
case PWM_SERVO_GET(4):
if (_num_outputs < 8) {
ret = -EINVAL;
break;
}
case PWM_SERVO_GET(3):
if (_mode != MODE_4PWM) {
case PWM_SERVO_GET(2):
if (_num_outputs < 4) {
ret = -EINVAL;
break;
}
/* FALLTHROUGH */
case PWM_SERVO_GET(0):
case PWM_SERVO_GET(1): {
// channel = cmd - PWM_SERVO_SET(0);
// *(servo_position_t *)arg = up_pwm_servo_get(channel);
case PWM_SERVO_GET(1):
case PWM_SERVO_GET(0): {
*(servo_position_t *)arg = 1500;
break;
}
@ -566,11 +629,16 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
break;
}
case PWM_SERVO_GET_COUNT:
case MIXERIOCGETOUTPUTCOUNT:
if (_mode == MODE_4PWM) {
*(unsigned *)arg = 4;
if (_mode == MODE_8PWM) {
*(unsigned *)arg = 8;
} else if (_mode == MODE_4PWM) {
*(unsigned *)arg = 4;
} else {
*(unsigned *)arg = 2;
}