Skip to content

Commit

Permalink
Remove unreachable code, and fix some bugs.
Browse files Browse the repository at this point in the history
  • Loading branch information
@lovettchris
lovettchris committed Feb 22, 2017
1 parent 67ee7b8 commit 9dd8d34
Show file tree
Hide file tree
Showing 2 changed files with 37 additions and 29 deletions.
46 changes: 25 additions & 21 deletions MavLinkCom/MavLinkTest/Commands.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,22 +37,21 @@ void Command::Execute(std::shared_ptr<MavLinkVehicle> com)
Close();
}
this->vehicle = com;
if (subscription == 0) {
if (subscription != 0) {
this->vehicle->getConnection()->unsubscribe(subscription);
}
subscription = com->getConnection()->subscribe([=](std::shared_ptr<MavLinkConnection> con, const MavLinkMessage& msg) {
try {
HandleMessage(msg);
}
catch (std::exception& e) {
Utils::logError("### Runtime Error: %s", e.what());
}
catch (...) {
Utils::logError("### unhandled exception");
}
});
}

if (subscription == 0)
{
subscription = com->getConnection()->subscribe([=](std::shared_ptr<MavLinkConnection> con, const MavLinkMessage& msg) {
try {
HandleMessage(msg);
}
catch (std::exception& e) {
Utils::logError("### Runtime Error: %s", e.what());
}
catch (...) {
Utils::logError("### unhandled exception");
}
});
}
}

void Command::Close() {
Expand Down Expand Up @@ -245,7 +244,7 @@ bool TakeOffCommand::Parse(std::vector<std::string>& args)
if (args.size() > 0) {
std::string cmd = args[0];
if (cmd == "takeoff") {

altitude = 5; // default
if (args.size() > 1)
{
altitude = static_cast<float>(atof(args[1].c_str()));
Expand Down Expand Up @@ -817,6 +816,7 @@ void IdleCommand::Close()
this->requested_control_ = false;
Command::Close();
}

void IdleCommand::Execute(std::shared_ptr<MavLinkVehicle> com)
{
Command::Execute(com);
Expand Down Expand Up @@ -863,6 +863,9 @@ bool GotoCommand::Parse(std::vector<std::string>& args) {
tx = static_cast<float>(atof(args[1].c_str()));
ty = static_cast<float>(atof(args[2].c_str()));
tz = static_cast<float>(atof(args[3].c_str()));
if (tz > 0) {
tz = -tz; // probably mean negative, since we are using NED coordinates.
}
return true;
}
else {
Expand All @@ -872,6 +875,7 @@ bool GotoCommand::Parse(std::vector<std::string>& args) {
}
return false;
}

void GotoCommand::Execute(std::shared_ptr<MavLinkVehicle> com) {
this->channel = com;
this->requestedControl = false;
Expand Down Expand Up @@ -1077,17 +1081,17 @@ void OrbitCommand::Execute(std::shared_ptr<MavLinkVehicle> com) {
throw std::runtime_error(Utils::stringf("Your drone does not support the MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED capability."));
}

GotoCommand::Execute(com);
printf("orbit current position at radius %f\n", static_cast<float>(radius));
orbiting = false;
flyingToRadius = false;
startAngle = 0;
startTime = 0;
orbits = 0;
halfWay = false;
halfWay = false;
previousAngle = 0;
orbitSpeed = 0;
printf("waiting for local position...\n");
GotoCommand::Execute(com);
}

void OrbitCommand::HasLocalPosition() {
Expand Down Expand Up @@ -1385,7 +1389,7 @@ void WiggleCommand::HandleMessage(const MavLinkMessage& message)
DebugOutput("start_thrust_=%f", start_thrust_);
// these PID values were calculated experimentally using AltHoldCommand, this provides the best
// control over thrust to achieve minimal over/under shoot in a reasonable amount of time.
thrust_controller_.setPoint(this->sz_, .05f, .005f, 0.09f);
thrust_controller_.setPoint(-this->sz_, .05f, .005f, 0.09f);
}
break;
}
Expand All @@ -1408,7 +1412,7 @@ void WiggleCommand::HandleMessage(const MavLinkMessage& message)
// the amount of roll should depend on our speed in that direction.
double speed = fabs(pos.vy);

float ctrl = thrust_controller_.control(static_cast<float>(z));
float ctrl = thrust_controller_.control(static_cast<float>(-z));

float thrust = start_thrust_ + ctrl;

Expand Down
20 changes: 12 additions & 8 deletions docs/sitl.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,32 +8,36 @@ the tiny flight controller on your drone, so you may not get the same flying per
But it's still useful when you don't have a flight controller handy. It is just a few more steps to set up as shown below.

# Software-In-Loop Simulation (SITL)
1. Install [BashOnWindows](https://msdn.microsoft.com/en-us/commandline/wsl/install_guide). You can also do this on an Ubuntu machine, but in that case
you will have to use real ip addresses instead of localhost.
2. Type `bash` at Windows command prompt. Follow [these steps for Linux](http://dev.px4.io/starting-installing-linux.html) and follow the instructions
under `NuttX based hardware` to install prerequisites.
3. Get the PX4 source code and build the SITL:
1. Install [BashOnWindows](https://msdn.microsoft.com/en-us/commandline/wsl/install_guide). You can also do this on an Ubuntu machine,
but in that case you will have to use real ip addresses instead of localhost (See Using VirtualBox Ubuntu below).
2. Type `bash` at Windows command prompt. Follow [these steps for Linux](http://dev.px4.io/starting-installing-linux.html)
and follow all the instructions under `NuttX based hardware` to install prerequisites. Technically you don't need the full NuttX
toolchain for cross-compiling ARM Cortex M4 code, but it won't hurt to get all that. The PX4 make system probably checks that you have it all,
even though you probably don't need that to build the posix-SITL version which will be running on your intel chipset.
3. Get the PX4 source code and build the posix SITL version of PX4:
```
git clone https://github.com/PX4/Firmware.git
cd Firmware
git submodule update --init --recursive
make posix_sitl_default
```
3. Use following command to start PX4 firmware:
3. Use following command to start PX4 firmware in SITL mode:
```
./build_posix_sitl_default/src/firmware/posix/px4 ./posix-configs/SITL/init/lpe/iris
```
You should see a message like this you `INFO [simulator] Waiting for initial data on UDP port 14560` which means the SITL PX4 app is
waiting for someone to connect.
4. Now edit [settings file](settings.md) with UdpIp address 127.0.0.1 and UdpPort 14560, set UseSerial to false
4. Now edit [settings file](settings.md) with `UdpIp` address 127.0.0.1 and `UdpPort` 14560, set `UseSerial` to false
5. Run Unreal environment and it should connect to SITL via UDP. You should see a bunch of messages from the SITL PX4 window from
things like local_position_estimator and "commander" and so on.
things like `local_position_estimator` and `commander` and so on.
6. You should also be able to use QGroundControl just like with actual [flight controller harware](prereq.md).
Note that as we don't have physical board, RC cannot be connected directly to it.
So the alternatives are either use XBox 360 Controller or connect your RC using USB port if it has it
(for example, in case of [FrSky Taranis X9D Plus](prereq.md)) or using trainer USB cable to PC.
This makes your RC look like joystick. You will need to do extra set up in QGroundControl to use RC control as below.

# Using VirtualBox Ubuntu

If you want to run the above posix_sitl in a `VirtualBox Ubuntu` machine then it will have a different ip address from localhost.
So in this case you need to set the [settings file](settings.md) with the UdpIp address set to the ip address of your virtual machine
and the LocalIpAddress is the address of your host machine running the Unreal engine.
Expand Down

0 comments on commit 9dd8d34

Please sign in to comment.