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

Sketchy diagnostic commands useful for testing.

This commit is contained in:
px4dev 2012-08-05 19:46:55 -07:00
parent 4f0875ab73
commit 3860f72665
1 changed files with 69 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

72
apps/px4/fmu/fmu.cpp
View File

@ -209,6 +209,8 @@ FMUServo::task_main()
unsigned num_outputs = (_mode == MODE_2PWM) ? 2 : 4;
log("starting");
/* loop until killed */
while (!_task_should_exit) {
@ -235,7 +237,6 @@ FMUServo::task_main()
/* if the actuator is configured */
if (_mixer[i] != nullptr) {
/* mix controls to the actuator */
float output = mixer_mix(_mixer[i], &controls[0]);
@ -263,6 +264,8 @@ FMUServo::task_main()
/* make sure servos are off */
up_pwm_servo_deinit();
log("stopping");
/* note - someone else is responsible for restoring the GPIO config */
/* tell the dtor that we are exiting */
@ -379,13 +382,16 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
/* get the caller-supplied mixer and check */
mm = (struct mixer_s *)arg;
if (mixer_check(mm, 1, NUM_ACTUATOR_CONTROLS)) { /* only the attitude group is supported */
/* allocate local storage and copy from the caller*/
if (mm != nullptr) {
if (mixer_check(mm, 1, NUM_ACTUATOR_CONTROLS)) {
/* only the attitude group is supported */
ret = -EINVAL;
break;
}
/* allocate local storage and copy from the caller*/
if (mm != nullptr) {
/* allocate a new mixer struct */
tmm = (struct mixer_s *)malloc(MIXER_SIZE(mm->control_count));
memcpy(tmm, mm, MIXER_SIZE(mm->control_count));
@ -512,6 +518,56 @@ fmu_new_mode(PortMode new_mode)
return ret;
}
void
test(void)
{
int fd;
fd = open(PWM_OUTPUT_DEVICE_PATH, 0);
if (fd < 0) {
puts("open fail");
exit(1);
}
ioctl(fd, PWM_SERVO_ARM, 0);
ioctl(fd, PWM_SERVO_SET(0), 1000);
close(fd);
exit(0);
}
void
fake(int argc, char *argv[])
{
if (argc < 5) {
puts("fmu fake <roll> <pitch> <yaw> <thrust> (values -100 - 100)");
exit(1);
}
struct actuator_controls_s ac;
ac.control[0] = strtol(argv[1], 0, 0) / 100.0f;
ac.control[1] = strtol(argv[2], 0, 0) / 100.0f;
ac.control[2] = strtol(argv[3], 0, 0) / 100.0f;
ac.control[3] = strtol(argv[4], 0, 0) / 100.0f;
int handle = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &ac);
if (handle < 0) {
puts("advertise failed");
exit(1);
}
close(handle);
exit(0);
}
} // namespace
extern "C" __EXPORT int fmu_main(int argc, char *argv[]);
@ -521,10 +577,16 @@ fmu_main(int argc, char *argv[])
{
PortMode new_mode = PORT_MODE_UNSET;
if (!strcmp(argv[1], "test"))
test();
if (!strcmp(argv[1], "fake"))
fake(argc - 1, argv + 1);
/*
* Mode switches.
*
* XXX use getopt
* XXX use getopt?
*/
if (!strcmp(argv[1], "mode_gpio")) {
new_mode = PORT_FULL_GPIO;