Teach 'fake' to set the arming state as well.

Whitespace.
This commit is contained in:
px4dev 2012-12-18 00:33:33 -08:00
parent b8044d9786
commit 8d716dea45
1 changed files with 42 additions and 13 deletions

View File

@ -178,6 +178,7 @@ PX4FMU::~PX4FMU()
_task_should_exit = true; _task_should_exit = true;
unsigned i = 10; unsigned i = 10;
do { do {
/* wait 50ms - it should wake every 100ms or so worst-case */ /* wait 50ms - it should wake every 100ms or so worst-case */
usleep(50000); usleep(50000);
@ -213,6 +214,7 @@ PX4FMU::init()
/* try to claim the generic PWM output device node as well - it's OK if we fail at this */ /* try to claim the generic PWM output device node as well - it's OK if we fail at this */
ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this); ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this);
if (ret == OK) { if (ret == OK) {
log("default PWM output device"); log("default PWM output device");
_primary_pwm_device = true; _primary_pwm_device = true;
@ -280,6 +282,7 @@ PX4FMU::set_mode(Mode mode)
default: default:
return -EINVAL; return -EINVAL;
} }
_mode = mode; _mode = mode;
return OK; return OK;
} }
@ -332,8 +335,10 @@ PX4FMU::task_main()
/* handle update rate changes */ /* handle update rate changes */
if (_current_update_rate != _update_rate) { if (_current_update_rate != _update_rate) {
int update_rate_in_ms = int(1000 / _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; update_rate_in_ms = 2;
orb_set_interval(_t_actuators, update_rate_in_ms); orb_set_interval(_t_actuators, update_rate_in_ms);
up_pwm_servo_set_rate(_update_rate); up_pwm_servo_set_rate(_update_rate);
_current_update_rate = _update_rate; _current_update_rate = _update_rate;
@ -425,6 +430,7 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg)
/* try it as a GPIO ioctl first */ /* try it as a GPIO ioctl first */
ret = gpio_ioctl(filp, cmd, arg); ret = gpio_ioctl(filp, cmd, arg);
if (ret != -ENOTTY) if (ret != -ENOTTY)
return ret; return ret;
@ -434,6 +440,7 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg)
case MODE_4PWM: case MODE_4PWM:
ret = pwm_ioctl(filp, cmd, arg); ret = pwm_ioctl(filp, cmd, arg);
break; break;
default: default:
debug("not in a PWM mode"); debug("not in a PWM mode");
break; break;
@ -551,8 +558,10 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
} }
_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
if (_mixers == nullptr) { if (_mixers == nullptr) {
ret = -ENOMEM; ret = -ENOMEM;
} else { } else {
debug("loading mixers from %s", path); debug("loading mixers from %s", path);
@ -615,12 +624,15 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function)
case GPIO_SET_INPUT: case GPIO_SET_INPUT:
stm32_configgpio(_gpio_tab[i].input); stm32_configgpio(_gpio_tab[i].input);
break; break;
case GPIO_SET_OUTPUT: case GPIO_SET_OUTPUT:
stm32_configgpio(_gpio_tab[i].output); stm32_configgpio(_gpio_tab[i].output);
break; break;
case GPIO_SET_ALT_1: case GPIO_SET_ALT_1:
if (_gpio_tab[i].alt != 0) if (_gpio_tab[i].alt != 0)
stm32_configgpio(_gpio_tab[i].alt); stm32_configgpio(_gpio_tab[i].alt);
break; break;
} }
} }
@ -763,6 +775,7 @@ fmu_new_mode(PortMode new_mode, int update_rate)
/* (re)set the PWM output mode */ /* (re)set the PWM output mode */
g_fmu->set_mode(servo_mode); g_fmu->set_mode(servo_mode);
if ((servo_mode != PX4FMU::MODE_NONE) && (update_rate != 0)) if ((servo_mode != PX4FMU::MODE_NONE) && (update_rate != 0))
g_fmu->set_pwm_rate(update_rate); g_fmu->set_pwm_rate(update_rate);
@ -805,9 +818,13 @@ test(void)
errx(1, "open fail"); errx(1, "open fail");
if (ioctl(fd, PWM_SERVO_ARM, 0) < 0) err(1, "servo arm failed"); if (ioctl(fd, PWM_SERVO_ARM, 0) < 0) err(1, "servo arm failed");
if (ioctl(fd, PWM_SERVO_SET(0), 1000) < 0) err(1, "servo 1 set failed"); if (ioctl(fd, PWM_SERVO_SET(0), 1000) < 0) err(1, "servo 1 set failed");
if (ioctl(fd, PWM_SERVO_SET(1), 1200) < 0) err(1, "servo 2 set failed"); if (ioctl(fd, PWM_SERVO_SET(1), 1200) < 0) err(1, "servo 2 set failed");
if (ioctl(fd, PWM_SERVO_SET(2), 1400) < 0) err(1, "servo 3 set failed"); if (ioctl(fd, PWM_SERVO_SET(2), 1400) < 0) err(1, "servo 3 set failed");
if (ioctl(fd, PWM_SERVO_SET(3), 1600) < 0) err(1, "servo 4 set failed"); if (ioctl(fd, PWM_SERVO_SET(3), 1600) < 0) err(1, "servo 4 set failed");
close(fd); close(fd);
@ -836,6 +853,16 @@ fake(int argc, char *argv[])
if (handle < 0) if (handle < 0)
errx(1, "advertise failed"); errx(1, "advertise failed");
actuator_armed_s aa;
aa.armed = true;
aa.lockdown = false;
handle = orb_advertise(ORB_ID(actuator_armed), &aa);
if (handle < 0)
errx(1, "advertise failed 2");
exit(0); exit(0);
} }
@ -888,10 +915,12 @@ fmu_main(int argc, char *argv[])
if (new_mode == PORT_FULL_PWM || new_mode == PORT_PWM_AND_GPIO) { if (new_mode == PORT_FULL_PWM || new_mode == PORT_PWM_AND_GPIO) {
if (argc > i + 1) { if (argc > i + 1) {
pwm_update_rate_in_hz = atoi(argv[i + 1]); pwm_update_rate_in_hz = atoi(argv[i + 1]);
} else { } else {
errx(1, "missing argument for pwm update rate (-u)"); errx(1, "missing argument for pwm update rate (-u)");
return 1; return 1;
} }
} else { } else {
errx(1, "pwm update rate currently only supported for mode_pwm, mode_pwm_gpio"); errx(1, "pwm update rate currently only supported for mode_pwm, mode_pwm_gpio");
} }