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; }