in MavLinkCom/MavLinkTest/main.cpp [1278:1490]
int console(std::stringstream& script)
{
std::string line;
std::shared_ptr<MavLinkNode> logViewer = nullptr;
Command* currentCommand = nullptr;
OrbitCommand* orbit = new OrbitCommand();
SendImageCommand* sendImage = nullptr;
NshCommand* nshCommand = new NshCommand();
std::vector<Command*> cmdTable;
Command::setAllCommand(&cmdTable);
cmdTable.push_back(new ArmDisarmCommand());
cmdTable.push_back(new TakeOffCommand());
cmdTable.push_back(new LandCommand());
cmdTable.push_back(new MissionCommand());
cmdTable.push_back(new LoiterCommand());
cmdTable.push_back(new CapabilitiesCommand());
cmdTable.push_back(new RtlCommand());
cmdTable.push_back(new GetParamsCommand());
cmdTable.push_back(new GetSetParamCommand());
cmdTable.push_back(new StatusCommand());
cmdTable.push_back(new PositionCommand());
cmdTable.push_back(new HilCommand());
cmdTable.push_back(new FakeGpsCommand());
cmdTable.push_back(new RequestImageCommand());
cmdTable.push_back(new FtpCommand());
cmdTable.push_back(new PlayLogCommand());
cmdTable.push_back(new DumpLogCommandsCommand());
cmdTable.push_back(nshCommand);
// this is advanced command that can get us into trouble on real drone, so remove it for now.
//cmdTable.push_back(new AltHoldCommand());
cmdTable.push_back(sendImage = new SendImageCommand());
cmdTable.push_back(new SetMessageIntervalCommand());
cmdTable.push_back(new BatteryCommand());
cmdTable.push_back(new WaitForAltitudeCommand());
droneConnection->subscribe([=](std::shared_ptr<MavLinkConnection> connection, const MavLinkMessage& message) {
MavLinkStatustext statustext;
if (inLogFile != nullptr && inLogFile->isOpen()) {
std::lock_guard<std::mutex> lock(logLock);
inLogFile->write(message);
}
switch (message.msgid) {
case MavLinkHeartbeat::kMessageId:
CheckHeartbeat(message);
break;
case MavLinkAttitudeTarget::kMessageId:
/*
MavLinkAttitudeTarget target;
target.decode(message);
float pitch, roll, yaw;
mavlink_quaternion_to_euler(target.q, &roll, &pitch, &yaw);
float q2[4];
mavlink_euler_to_quaternion(roll, pitch, yaw, q2);*/
//DebugOutput("q1 : %f\t%f\t%f\t%g", target.q[0], target.q[1], target.q[2], target.q[3]);
//DebugOutput("q2 : %f\t%f\t%f\t%g", q2[0], q2[1], q2[2], q2[3]);
//DebugOutput("target roll: %f\tpitch: %f\tyaw:%f\tthrust: %f", roll, pitch, yaw, target.thrust);
break;
case MavLinkStatustext::kMessageId: // MAVLINK_MSG_ID_STATUSTEXT:
statustext.decode(message);
handleStatus(statustext);
break;
default:
break;
}
});
if (logConnection != nullptr) {
logViewer = std::make_shared<MavLinkNode>(LocalLogViewerSystemId, LocalComponentId);
logViewer->connect(logConnection);
orbit->setLogViewer(logViewer);
sendImage->setLogViewer(logViewer);
}
// this stops us from being able to connect to SITL mode PX4.
//checkPulse();
int retries = 0;
while (retries++ < 5) {
try {
if (mavLinkVehicle->isLocalControlSupported()) {
cmdTable.push_back(new GotoCommand());
cmdTable.push_back(new RotateCommand());
cmdTable.push_back(orbit);
cmdTable.push_back(new SquareCommand());
cmdTable.push_back(new WiggleCommand());
}
break;
}
catch (std::exception& e) {
printf("isLocalControlSupported failed: %s\n", e.what());
}
}
if (noRadio) {
MavLinkParameter p = mavLinkVehicle->getCachedParameter("NAV_RCL_ACT");
if (p.value != 0) {
p.value = 0;
mavLinkVehicle->setParameter(p);
}
}
if (nsh) {
currentCommand = nshCommand;
currentCommand->Execute(mavLinkVehicle);
}
else {
if (!noparams) {
printf("Downloading drone parameters so we know how to control it properly...\n");
try {
mavLinkVehicle->getParamList();
}
catch (std::exception& e) {
printf("%s\n", e.what());
}
}
mavLinkVehicle->setStabilizedFlightMode();
}
printf("Ready...\n");
script << "status\n";
while (!std::cin.eof()) {
if (!script.eof()) {
std::getline(script, line);
}
else {
printf("mavlink> ");
std::getline(std::cin, line);
}
line = mavlink_utils::Utils::trim(line, ' ');
if (line.length() > 0) {
if (line.length() == 0) {
continue;
}
std::vector<std::string> args = Command::parseArgs(line);
std::string cmd = args[0];
if (cmd == "x") {
break;
}
else if (cmd == "disconnect") {
mavLinkVehicle->close();
}
else if (cmd == "connect") {
connect();
}
else if (cmd == "?" || cmd == "help") {
for (size_t i = 0; i < cmdTable.size(); i++) {
Command* command = cmdTable[i];
if (args.size() > 1 && args[1] == command->Name) {
command->PrintHelp();
break;
}
else {
printf("%s\n", command->Name.c_str());
}
}
}
else {
Command* selected = Command::create(args);
//add command text in log
if (selected != nullptr && inLogFile != nullptr && inLogFile->isOpen()) {
auto str = std::string(Command::kCommandLogPrefix) + line;
MavLinkStatustext st;
strncpy(st.text, str.c_str(), 50);
MavLinkMessage m;
st.encode(m);
droneConnection->prepareForSending(m);
std::lock_guard<std::mutex> lock(logLock);
inLogFile->write(m);
}
if (currentCommand != nullptr && currentCommand != selected) {
// close previous command.
currentCommand->Close();
}
currentCommand = selected;
if (currentCommand != NULL) {
try {
currentCommand->Execute(mavLinkVehicle);
}
catch (const std::exception& e) {
const char* reason = e.what();
if (reason == nullptr) {
reason = "(unknown)";
}
printf("Error: %s\n", reason);
}
}
else {
printf("Unknown command. Type '?' to get list of commands\n");
}
}
}
}
stopTelemetry();
logViewer = nullptr;
droneConnection = nullptr;
logConnection = nullptr;
mavLinkVehicle = nullptr;
CloseLogFiles();
return 0;
}