void
onTerminate(void)
{
setThrust(0);
for (unsigned i = 0; i < 4; ++i)
setFin(i, 0);
}
/**
* Check if received value seems to be realistic. Set if is.
* @param battery Value to check
* @return True if setThrust(_) was called.
*/
bool QuadcopterThrust::checkAndSetBatteryValue(float battery)
{
if (battery > BATTERY_MAX) {
ROS_ERROR("checkAndSet +");
return false;
} else if (battery < BATTERY_MIN) {
ROS_ERROR("checkAndSet -");
return false;
}
setThrust(battery);
this->setThrustCalled = true;
return true;
}
int
main(int argc, char** argv)
{
OptionParser options;
options.executable("dune-test-tail")
.program(DUNE_SHORT_NAME)
.copyright(DUNE_COPYRIGHT)
.email(DUNE_CONTACT)
.version(getFullVersion())
.date(getCompileDate())
.arch(DUNE_SYSTEM_NAME)
.add("-i", "--address",
"Vehicle's IP address", "ADDRESS")
.add("-w", "--wait",
"Wait DELAY seconds before starting test", "DELAY")
.add("-d", "--duration",
"Test duration in seconds", "DURATION")
.add("-s", "--speed",
"Speed in percentage", "SPEED")
.add("-t", "--angle",
"Angle in degrees", "ANGLE");
// Parse command line arguments.
if (!options.parse(argc, argv))
{
if (options.bad())
std::cerr << "ERROR: " << options.error() << std::endl;
options.usage();
return 1;
}
// Set destination address.
if (options.value("--address") == "")
g_addr = "127.0.0.1";
else
g_addr = options.value("--address").c_str();
// Set start delay.
double sdelay = 0;
if (options.value("--wait") == "")
sdelay = 0;
else
sdelay = castLexical<double>(options.value("--wait"));
// Set duration.
double duration = 0;
if (options.value("--duration") == "")
duration = 0;
else
duration = castLexical<double>(options.value("--duration"));
// Set speed.
double speed = 0;
if (options.value("--speed") == "")
speed = 0;
else
{
speed = castLexical<double>(options.value("--speed"));
speed /= 100.0;
}
// Set Angle
double angle = 0;
if (options.value("--angle") == "")
angle = 0;
else
angle = castLexical<double>(options.value("--angle"));
// POSIX implementation.
#if defined(DUNE_SYS_HAS_SIGACTION)
struct sigaction actions;
std::memset(&actions, 0, sizeof(actions));
sigemptyset(&actions.sa_mask);
actions.sa_flags = 0;
actions.sa_handler = handleTerminate;
sigaction(SIGALRM, &actions, 0);
sigaction(SIGHUP, &actions, 0);
sigaction(SIGINT, &actions, 0);
sigaction(SIGQUIT, &actions, 0);
sigaction(SIGTERM, &actions, 0);
sigaction(SIGCHLD, &actions, 0);
sigaction(SIGCONT, &actions, 0);
#endif
setThrust(0);
Delay::wait(sdelay);
setLog("mcrt_endurance");
Delay::wait(2.0);
double deadline = Clock::get() + duration;
setThrust(speed);
while ((Clock::get() < deadline) && !g_stop)
{
setFin(0, -angle);
setFin(1, -angle);
setFin(2, -angle);
setFin(3, -angle);
if (!g_stop)
Delay::wait(1.0);
if (!g_stop)
Delay::wait(1.0);
if (!g_stop)
Delay::wait(1.0);
if (!g_stop)
Delay::wait(1.0);
setFin(0, angle);
setFin(1, angle);
setFin(2, angle);
setFin(3, angle);
if (!g_stop)
Delay::wait(1.0);
if (!g_stop)
Delay::wait(1.0);
if (!g_stop)
Delay::wait(1.0);
if (!g_stop)
Delay::wait(1.0);
}
// Change log.
Delay::wait(2.0);
setLog("idle");
onTerminate();
return 0;
}
