diff options
author | px4dev <px4@purgatory.org> | 2012-08-05 19:46:55 -0700 |
---|---|---|
committer | px4dev <px4@purgatory.org> | 2012-08-05 19:46:55 -0700 |
commit | 3860f7266525c09db6a1943066fff78aea79a289 (patch) | |
tree | f0183e6168af80eaa09181229d8642c3bd84924c | |
parent | 4f0875ab7314865ce88592ced778fda37c7a42f3 (diff) | |
download | px4-firmware-3860f7266525c09db6a1943066fff78aea79a289.tar.gz px4-firmware-3860f7266525c09db6a1943066fff78aea79a289.tar.bz2 px4-firmware-3860f7266525c09db6a1943066fff78aea79a289.zip |
Sketchy diagnostic commands useful for testing.
-rw-r--r-- | apps/px4/fmu/fmu.cpp | 76 |
1 files changed, 69 insertions, 7 deletions
diff --git a/apps/px4/fmu/fmu.cpp b/apps/px4/fmu/fmu.cpp index 57eb12f4e..3021321da 100644 --- a/apps/px4/fmu/fmu.cpp +++ b/apps/px4/fmu/fmu.cpp @@ -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 */ - ret = -EINVAL; - break; - } - /* 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 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; |