fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Working bottle Drop test

Browse Source
This commit is contained in:
Juchli D 2013-11-15 10:30:48 +01:00
parent af3a56f17f
commit adee479782
2 changed files with 86 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
ROMFS/px4fmu_common/init.d/900_bottle_drop_test
View File

@ -28,6 +28,32 @@ set EXIT_ON_END no
mavlink start
usleep 5000
#
# Start the sensors and test them.
#
sh /etc/init.d/rc.sensors
#
# Start logging (depends on sensors)
#
#sh /etc/init.d/rc.logging
sdlog2 start -r 200 -e -b 16
#
# Start GPS interface (depends on orb)
#
gps start
#
# Start the attitude estimator
#
attitude_estimator_ekf start
#
# Start position estimator
#
position_estimator_inav start
sh /etc/init.d/rc.io
@ -35,6 +61,9 @@ fmu mode_pwm
mixer load /dev/px4fmu /etc/mixers/FMU_pass.mix
pwm min -d /dev/px4fmu -c 123 -p 900
pwm max -d /dev/px4fmu -c 123 -p 2100
pwm arm -d /dev/px4fmu
bottle_drop start

78
src/modules/bottle_drop/bottle_drop.c
View File

@ -42,13 +42,16 @@
#include <nuttx/sched.h>
#include <unistd.h>
#include <stdio.h>
#include <poll.h>
#include <systemlib/systemlib.h>
#include <systemlib/err.h>
#include <uORB/uORB.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_attitude.h>
#include <drivers/drv_rgbled.h>
#include <drivers/drv_hrt.h>
static bool thread_should_exit = false; /**< daemon exit flag */
static bool thread_running = false; /**< daemon status flag */
@ -101,7 +104,7 @@ int bottle_drop_main(int argc, char *argv[])
}
thread_should_exit = false;
daemon_task = task_spawn_cmd("daemon",
daemon_task = task_spawn_cmd("bottle_drop",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
4096,
@ -143,38 +146,71 @@ int bottle_drop_thread_main(int argc, char *argv[]) {
int rgbleds = open(RGBLED_DEVICE_PATH, 0);
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
orb_set_interval(vehicle_attitude_sub, 20);
struct actuator_controls_s actuators;
memset(&actuators, 0, sizeof(actuators));
orb_advert_t actuator_pub = orb_advertise(ORB_ID(actuator_controls_1), &actuators);
struct pollfd fds[1] = {
{ .fd = vehicle_attitude_sub, .events = POLLIN },
};
uint64_t drop_start = 0;
bool open_now = false;
while (!thread_should_exit) {
// warnx("in while!\n");
// values from -1 to 1
if (drop) {
// drop here
drop = false;
int ret = poll(fds, 1, 500);
ioctl(rgbleds, RGBLED_SET_MODE, RGBLED_MODE_ON);
ioctl(rgbleds, RGBLED_SET_COLOR, RGBLED_COLOR_RED);
if (ret < 0) {
/* poll error, count it in perf */
warnx("poll error");
} else if (ret > 0) {
/* attitude */
orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
if (drop) {
// drop here
open_now = true;
drop = false;
drop_start = hrt_absolute_time();
}
if (open_now && (drop_start + 2e6 > hrt_absolute_time())) { // open door
actuators.control[0] = -1.0f;
actuators.control[1] = 1.0f;
} else if (open_now) { // unlock bottle
ioctl(rgbleds, RGBLED_SET_MODE, RGBLED_MODE_ON);
ioctl(rgbleds, RGBLED_SET_COLOR, RGBLED_COLOR_RED);
actuators.control[2] = 0.5f;
} else {
// leave closed
// warnx( "%4.4f", att.pitch);
actuators.control[0] = 0.5f;
actuators.control[1] = -0.5f;
actuators.control[2] = -0.5f;
}
actuators.timestamp = hrt_absolute_time();
orb_publish(ORB_ID(actuator_controls_1), actuator_pub, &actuators);
i++;
}
actuators.control[0] = 0.5f;
actuators.control[1] = 0.5f;
actuators.control[2] = 0.5f;
actuators.control[3] = 0.5f;
actuators.timestamp = hrt_absolute_time();
orb_publish(ORB_ID(actuator_controls_1), actuator_pub, &actuators);
usleep(50);
i++;
}
warnx("exiting.\n");