Skip to content

Commit

Permalink
[simpleflight] cleanup, fix minor errors
Browse files Browse the repository at this point in the history
  • Loading branch information
@madratman
madratman committed Mar 24, 2020
1 parent 0fedc5a commit c2f4789
Show file tree
Hide file tree
Showing 4 changed files with 41 additions and 15 deletions.
35 changes: 32 additions & 3 deletions AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -185,7 +185,8 @@ class ArduCopterApi : public MultirotorApiBase {
unused(roll);
unused(pitch);
unused(yaw_rate);
Utils::log("Not Implemented: commandRollPitchThrottle", Utils::kLogLevelInfo);
unused(throttle);
Utils::log("Not Implemented: commandRollPitchYawrateThrottle", Utils::kLogLevelInfo);
}

virtual void commandRollPitchYawZ(float roll, float pitch, float yaw, float z) override
Expand All @@ -194,15 +195,43 @@ class ArduCopterApi : public MultirotorApiBase {
unused(pitch);
unused(yaw);
unused(z);
Utils::log("Not Implemented", Utils::kLogLevelInfo);
Utils::log("Not Implemented: commandRollPitchYawZ", Utils::kLogLevelInfo);
}

virtual void commandRollPitchYawThrottle(float roll, float pitch, float yaw, float throttle) override
{
unused(roll);
unused(pitch);
unused(yaw);
Utils::log("Not Implemented: commandRollPitchZ", Utils::kLogLevelInfo);
unused(throttle);
Utils::log("Not Implemented: commandRollPitchYawThrottle", Utils::kLogLevelInfo);
}

virtual void commandRollPitchYawrateZ(float roll, float pitch, float yaw_rate, float z) override
{
unused(roll);
unused(pitch);
unused(yaw_rate);
unused(z);
Utils::log("Not Implemented: commandRollPitchYawrateZ", Utils::kLogLevelInfo);
}

virtual void commandAngleRatesZ(float roll_rate, float pitch_rate, float yaw_rate, float z) override
{
unused(roll_rate);
unused(pitch_rate);
unused(yaw_rate);
unused(z);
Utils::log("Not Implemented: commandAngleRatesZ", Utils::kLogLevelInfo);
}

virtual void commandAngleRatesThrottle(float roll_rate, float pitch_rate, float yaw_rate, float throttle) override
{
unused(roll_rate);
unused(pitch_rate);
unused(yaw_rate);
unused(throttle);
Utils::log("Not Implemented: commandAngleRatesZ", Utils::kLogLevelInfo);
}

virtual void commandVelocity(float vx, float vy, float vz, const YawMode& yaw_mode) override
Expand Down
3 changes: 0 additions & 3 deletions AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -501,13 +501,11 @@ class MavLinkMultirotorApi : public MultirotorApiBase
virtual void commandRollPitchYawThrottle(float roll, float pitch, float yaw, float throttle) override
{
checkValidVehicle();
// todo in mavlinkvehicleimpl.cpp, thrush is supposed to be b/w -1 and +1. do we need to scale?
mav_vehicle_->moveByAttitude(roll, pitch, yaw, 0, 0, 0, throttle);
}
virtual void commandRollPitchYawrateThrottle(float roll, float pitch, float yaw_rate, float throttle) override
{
checkValidVehicle();
// todo in mavlinkvehicleimpl.cpp, thrush is supposed to be b/w -1 and +1. do we need to scale?
mav_vehicle_->moveByAttitude(roll, pitch, 0, 0, 0, yaw_rate, throttle);
}
virtual void commandAngleRatesZ(float roll_rate, float pitch_rate, float yaw_rate, float z) override
Expand All @@ -524,7 +522,6 @@ class MavLinkMultirotorApi : public MultirotorApiBase
virtual void commandAngleRatesThrottle(float roll_rate, float pitch_rate, float yaw_rate, float throttle) override
{
checkValidVehicle();
// todo in mavlinkvehicleimpl.cpp, thrush is supposed to be b/w -1 and +1. do we need to scale?
mav_vehicle_->moveByAttitude(0, 0, 0, roll_rate, pitch_rate, yaw_rate, throttle);
}

Expand Down
4 changes: 2 additions & 2 deletions AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/Params.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -70,7 +70,7 @@ struct Params {
const float kI = 0.0f;
const float kD = 0.0f;

//max_pitch/roll_angle > 5.5 would produce versicle thrust that is not enough to keep vehicle in air at extremities of controls
//max_pitch/roll_angle > 5.5 would produce verticle thrust that is not enough to keep vehicle in air at extremities of controls
Axis4r max_limit = Axis4r(pi / 5.5f, pi / 5.5f, pi, 1.0f); //roll, pitch, yaw - in radians/sec

Axis4r p = Axis4r(kP, kP, kP, 1.0f);
Expand All @@ -80,7 +80,7 @@ struct Params {

struct PositionPid {
const float kMaxLimit = 8.8E26f; //some big number like size of known universe
const float kP = 2.5f;
const float kP = 0.25f;
const float kI = 0.0f;
const float kD = 0.0f;

Expand Down
14 changes: 7 additions & 7 deletions DroneShell/src/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -505,7 +505,7 @@ class MoveByAngleZCommand : public DroneCommand {
CommandContext* context = params.context;

context->tasker.execute([=]() {
context->client.moveByRollPitchYawZAsync(pitch, roll, z, yaw, duration);
context->client.moveByRollPitchYawZAsync(roll, pitch, yaw, z, duration);
});

return false;
Expand Down Expand Up @@ -693,13 +693,13 @@ class BackForthByAngleCommand : public DroneCommand {
CommandContext* context = params.context;

context->tasker.execute([=]() {
context->client.moveByRollPitchYawZAsync(pitch, roll, z, yaw, duration);
context->client.moveByRollPitchYawZAsync(roll, pitch, yaw, z, duration);
if (!context->client.waitOnLastTask()) {
throw std::runtime_error("BackForthByAngleCommand canceled");
}
context->client.hoverAsync();
context->sleep_for(pause_time);
context->client.moveByRollPitchYawZAsync(-pitch, -roll, z, yaw, duration);
context->client.moveByRollPitchYawZAsync(-roll, -pitch, yaw, z, duration);
if (!context->client.waitOnLastTask()){
throw std::runtime_error("BackForthByAngleCommand canceled");
}
Expand Down Expand Up @@ -782,28 +782,28 @@ class SquareByAngleCommand : public DroneCommand {
CommandContext* context = params.context;

context->tasker.execute([=]() {
context->client.moveByRollPitchYawZAsync(pitch, -roll, z, yaw, 0);
context->client.moveByRollPitchYawZAsync(-roll, pitch, yaw, z, 0);
if (!context->client.waitOnLastTask()) {
throw std::runtime_error("SquareByAngleCommand canceled");
}
context->client.hoverAsync();
context->sleep_for(pause_time);

context->client.moveByRollPitchYawZAsync(-pitch, -roll, z, yaw, 0);
context->client.moveByRollPitchYawZAsync(-roll, -pitch, yaw, z, 0);
if (!context->client.waitOnLastTask()) {
throw std::runtime_error("SquareByAngleCommand canceled");
}
context->client.hoverAsync();
context->sleep_for(pause_time);

context->client.moveByRollPitchYawZAsync(-pitch, roll, z, yaw, 0);
context->client.moveByRollPitchYawZAsync(roll, -pitch, yaw, z, 0);
if (!context->client.waitOnLastTask()) {
throw std::runtime_error("SquareByAngleCommand canceled");
}
context->client.hoverAsync();
context->sleep_for(pause_time);

context->client.moveByRollPitchYawZAsync(-pitch, -roll, z, yaw, 0);
context->client.moveByRollPitchYawZAsync(-roll, -pitch, yaw, z, 0);
if (!context->client.waitOnLastTask()){
throw std::runtime_error("SquareByAngleCommand canceled");
}
Expand Down

0 comments on commit c2f4789

Please sign in to comment.