Skip to content

Commit

Permalink
[simpleflight] add / fix unintuitive lower level APIs - RPYZ|Throttle…
Browse files Browse the repository at this point in the history 
…, AngleRateZ|Throttle
  • Loading branch information
@madratman
madratman committed Mar 24, 2020
1 parent 7a369b3 commit 03c19a1
Show file tree
Hide file tree
Showing 9 changed files with 460 additions and 46 deletions.
24 changes: 18 additions & 6 deletions AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,8 +24,12 @@ class MultirotorApiBase : public VehicleApiBase {
protected: //must be implemented

/************************* low level move APIs *********************************/
virtual void commandRollPitchZ(float pitch, float roll, float z, float yaw) = 0;
virtual void commandRollPitchThrottle(float pitch, float roll, float throttle, float yaw_rate) = 0;
virtual void commandRollPitchYawrateThrottle(float roll, float pitch, float yaw_rate, float throttle) = 0;
virtual void commandRollPitchYawZ(float roll, float pitch, float yaw, float z) = 0;
virtual void commandRollPitchYawThrottle(float roll, float pitch, float yaw, float throttle) = 0;
virtual void commandRollPitchYawrateZ(float roll, float pitch, float yaw_rate, float z) = 0;
virtual void commandAngleRatesZ(float roll_rate, float pitch_rate, float yaw_rate, float z) = 0;
virtual void commandAngleRatesThrottle(float roll_rate, float pitch_rate, float yaw_rate, float throttle) = 0;
virtual void commandVelocity(float vx, float vy, float vz, const YawMode& yaw_mode) = 0;
virtual void commandVelocityZ(float vx, float vy, float z, const YawMode& yaw_mode) = 0;
virtual void commandPosition(float x, float y, float z, const YawMode& yaw_mode) = 0;
Expand Down Expand Up @@ -81,8 +85,12 @@ class MultirotorApiBase : public VehicleApiBase {
virtual bool land(float timeout_sec);
virtual bool goHome(float timeout_sec);

virtual bool moveByAngleZ(float pitch, float roll, float z, float yaw, float duration);
virtual bool moveByAngleThrottle(float pitch, float roll, float throttle, float yaw_rate, float duration);
virtual bool moveByRollPitchYawZ(float roll, float pitch, float yaw, float z, float duration);
virtual bool moveByRollPitchYawThrottle(float roll, float pitch, float yaw, float throttle, float duration);
virtual bool moveByRollPitchYawrateThrottle(float roll, float pitch, float yaw_rate, float throttle, float duration);
virtual bool moveByRollPitchYawrateZ(float roll, float pitch, float yaw_rate, float z, float duration);
virtual bool moveByAngleRatesZ(float roll_rate, float pitch_rate, float yaw_rate, float z, float duration);
virtual bool moveByAngleRatesThrottle(float roll_rate, float pitch_rate, float yaw_rate, float throttle, float duration);
virtual bool moveByVelocity(float vx, float vy, float vz, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode);
virtual bool moveByVelocityZ(float vx, float vy, float z, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode);
virtual bool moveOnPath(const vector<Vector3r>& path, float velocity, float timeout_sec, DrivetrainType drivetrain, const YawMode& yaw_mode,
Expand Down Expand Up @@ -127,11 +135,15 @@ class MultirotorApiBase : public VehicleApiBase {
typedef std::function<bool()> WaitFunction;

//*********************************safe wrapper around low level commands***************************************************
virtual void moveByRollPitchYawZInternal(float roll, float pitch, float yaw, float z);
virtual void moveByRollPitchYawThrottleInternal(float roll, float pitch, float yaw, float throttle);
virtual void moveByRollPitchYawrateThrottleInternal(float roll, float pitch, float yaw_rate, float throttle);
virtual void moveByRollPitchYawrateZInternal(float roll, float pitch, float yaw_rate, float z);
virtual void moveByAngleRatesZInternal(float roll_rate, float pitch_rate, float yaw_rate, float z);
virtual void moveByAngleRatesThrottleInternal(float roll_rate, float pitch_rate, float yaw_rate, float throttle);
virtual void moveByVelocityInternal(float vx, float vy, float vz, const YawMode& yaw_mode);
virtual void moveByVelocityZInternal(float vx, float vy, float z, const YawMode& yaw_mode);
virtual void moveToPositionInternal(const Vector3r& dest, const YawMode& yaw_mode);
virtual void moveByRollPitchZInternal(float pitch, float roll, float z, float yaw);
virtual void moveByRollPitchThrottleInternal(float pitch, float roll, float throttle, float yaw_rate);

/************* safety checks & emergency maneuvers ************/
virtual bool emergencyManeuverIfUnsafe(const SafetyEval::EvalResult& result);
Expand Down
8 changes: 6 additions & 2 deletions AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,8 +23,12 @@ class MultirotorRpcLibClient : public RpcLibClientBase {
MultirotorRpcLibClient* landAsync(float timeout_sec = 60, const std::string& vehicle_name = "");
MultirotorRpcLibClient* goHomeAsync(float timeout_sec = Utils::max<float>(), const std::string& vehicle_name = "");

MultirotorRpcLibClient* moveByAngleZAsync(float pitch, float roll, float z, float yaw, float duration, const std::string& vehicle_name = "");
MultirotorRpcLibClient* moveByAngleThrottleAsync(float pitch, float roll, float throttle, float yaw_rate, float duration, const std::string& vehicle_name = "");
MultirotorRpcLibClient* moveByRollPitchYawZAsync(float roll, float pitch, float yaw, float z, float duration, const std::string& vehicle_name = "");
MultirotorRpcLibClient* moveByRollPitchYawThrottleAsync(float roll, float pitch, float yaw, float throttle, float duration, const std::string& vehicle_name = "");
MultirotorRpcLibClient* moveByRollPitchYawrateThrottleAsync(float roll, float pitch, float yaw_rate, float throttle, float duration, const std::string& vehicle_name = "");
MultirotorRpcLibClient* moveByRollPitchYawrateZAsync(float roll, float pitch, float yaw_rate, float z, float duration, const std::string& vehicle_name = "");
MultirotorRpcLibClient* moveByAngleRatesZAsync(float roll_rate, float pitch_rate, float yaw_rate, float z, float duration, const std::string& vehicle_name = "");
MultirotorRpcLibClient* moveByAngleRatesThrottleAsync(float roll_rate, float pitch_rate, float yaw_rate, float throttle, float duration, const std::string& vehicle_name = "");
MultirotorRpcLibClient* moveByVelocityAsync(float vx, float vy, float vz, float duration,
DrivetrainType drivetrain = DrivetrainType::MaxDegreeOfFreedom, const YawMode& yaw_mode = YawMode(), const std::string& vehicle_name = "");
MultirotorRpcLibClient* moveByVelocityZAsync(float vx, float vy, float z, float duration,
Expand Down
42 changes: 39 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 @@ -473,7 +473,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase
}

protected: //methods
virtual void commandRollPitchZ(float pitch, float roll, float z, float yaw) override
virtual void commandRollPitchYawZ(float roll, float pitch, float yaw, float z) override
{
if (target_height_ != -z) {
// these PID values were calculated experimentally using AltHoldCommand n MavLinkTest, this provides the best
Expand All @@ -487,11 +487,47 @@ class MavLinkMultirotorApi : public MultirotorApiBase
float thrust = 0.21f + thrust_controller_.control(-state.local_est.pos.z);
mav_vehicle_->moveByAttitude(roll, pitch, yaw, 0, 0, 0, thrust);
}
virtual void commandRollPitchThrottle(float pitch, float roll, float throttle, float yaw_rate) override
virtual void commandRollPitchYawrateZ(float roll, float pitch, float yaw_rate, float z) override
{
if (target_height_ != -z) {
thrust_controller_.setPoint(-z, .05f, .005f, 0.09f);
target_height_ = -z;
}
checkValidVehicle();
mav_vehicle_->moveByAttitude(roll, pitch, yaw_rate, 0, 0, 0, throttle);
auto state = mav_vehicle_->getVehicleState();
float thrust = 0.21f + thrust_controller_.control(-state.local_est.pos.z);
mav_vehicle_->moveByAttitude(roll, pitch, 0, 0, 0, yaw_rate, thrust);
}
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
{
if (target_height_ != -z) {
thrust_controller_.setPoint(-z, .05f, .005f, 0.09f);
target_height_ = -z;
}
checkValidVehicle();
auto state = mav_vehicle_->getVehicleState();
float thrust = 0.21f + thrust_controller_.control(-state.local_est.pos.z);
mav_vehicle_->moveByAttitude(0, 0, 0, roll_rate, pitch_rate, yaw_rate, thrust);
}
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);
}

virtual void commandVelocity(float vx, float vy, float vz, const YawMode& yaw_mode) override
{
checkValidVehicle();
Expand Down
64 changes: 58 additions & 6 deletions AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -195,9 +195,35 @@ class SimpleFlightApi : public MultirotorApiBase {
return 0.5f; //measured in simulator by firing commands "MoveToLocation -x 0 -y 0" multiple times and looking at distance traveled
}

virtual void commandRollPitchThrottle(float pitch, float roll, float throttle, float yaw_rate) override
virtual void commandRollPitchYawZ(float roll, float pitch, float yaw, float z) override
{
//Utils::log(Utils::stringf("commandRollPitchThrottle %f, %f, %f, %f", pitch, roll, throttle, yaw_rate));
//Utils::log(Utils::stringf("commandRollPitchYawZ %f, %f, %f, %f", pitch, roll, z, yaw));

typedef simple_flight::GoalModeType GoalModeType;
simple_flight::GoalMode mode(GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::PositionWorld);

simple_flight::Axis4r goal(roll, pitch, yaw, z);

std::string message;
firmware_->offboardApi().setGoalAndMode(&goal, &mode, message);
}

virtual void commandRollPitchYawThrottle(float roll, float pitch, float yaw, float throttle) override
{
//Utils::log(Utils::stringf("commandRollPitchYawThrottle %f, %f, %f, %f", roll, pitch, yaw, throttle));

typedef simple_flight::GoalModeType GoalModeType;
simple_flight::GoalMode mode(GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::Passthrough);

simple_flight::Axis4r goal(roll, pitch, yaw, throttle);

std::string message;
firmware_->offboardApi().setGoalAndMode(&goal, &mode, message);
}

virtual void commandRollPitchYawrateThrottle(float roll, float pitch, float yaw_rate, float throttle) override
{
//Utils::log(Utils::stringf("commandRollPitchYawThrottle %f, %f, %f, %f", roll, pitch, yaw, throttle));

typedef simple_flight::GoalModeType GoalModeType;
simple_flight::GoalMode mode(GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::AngleRate, GoalModeType::Passthrough);
Expand All @@ -208,14 +234,40 @@ class SimpleFlightApi : public MultirotorApiBase {
firmware_->offboardApi().setGoalAndMode(&goal, &mode, message);
}

virtual void commandRollPitchZ(float pitch, float roll, float z, float yaw) override
virtual void commandRollPitchYawrateZ(float roll, float pitch, float yaw_rate, float z) override
{
//Utils::log(Utils::stringf("commandRollPitchZ %f, %f, %f, %f", pitch, roll, z, yaw));
//Utils::log(Utils::stringf("commandRollPitchYawThrottle %f, %f, %f, %f", roll, pitch, yaw_rate, throttle));

typedef simple_flight::GoalModeType GoalModeType;
simple_flight::GoalMode mode(GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::PositionWorld);
simple_flight::GoalMode mode(GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::AngleRate, GoalModeType::PositionWorld);

simple_flight::Axis4r goal(roll, pitch, yaw, z);
simple_flight::Axis4r goal(roll, pitch, yaw_rate, z);

std::string message;
firmware_->offboardApi().setGoalAndMode(&goal, &mode, message);
}

virtual void commandAngleRatesZ(float roll_rate, float pitch_rate, float yaw_rate, float z) override
{
//Utils::log(Utils::stringf("commandRollPitchYawThrottle %f, %f, %f, %f", roll, pitch, yaw_rate, throttle));

typedef simple_flight::GoalModeType GoalModeType;
simple_flight::GoalMode mode(GoalModeType::AngleRate, GoalModeType::AngleRate, GoalModeType::AngleRate, GoalModeType::PositionWorld);

simple_flight::Axis4r goal(roll_rate, pitch_rate, yaw_rate, z);

std::string message;
firmware_->offboardApi().setGoalAndMode(&goal, &mode, message);
}

virtual void commandAngleRatesThrottle(float roll_rate, float pitch_rate, float yaw_rate, float throttle) override
{
//Utils::log(Utils::stringf("commandRollPitchYawThrottle %f, %f, %f, %f", roll, pitch, yaw_rate, throttle));

typedef simple_flight::GoalModeType GoalModeType;
simple_flight::GoalMode mode(GoalModeType::AngleRate, GoalModeType::AngleRate, GoalModeType::AngleRate, GoalModeType::Passthrough);

simple_flight::Axis4r goal(roll_rate, pitch_rate, yaw_rate, throttle);

std::string message;
firmware_->offboardApi().setGoalAndMode(&goal, &mode, message);
Expand Down
92 changes: 84 additions & 8 deletions AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -72,28 +72,80 @@ bool MultirotorApiBase::goHome(float timeout_sec)
return moveToPosition(0, 0, 0, 0.5f, timeout_sec, DrivetrainType::MaxDegreeOfFreedom, YawMode::Zero(), -1, 1);
}

bool MultirotorApiBase::moveByAngleZ(float pitch, float roll, float z, float yaw, float duration)
bool MultirotorApiBase::moveByRollPitchYawZ(float roll, float pitch, float yaw, float z, float duration)
{
SingleTaskCall lock(this);

if (duration <= 0)
return true;

return waitForFunction([&]() {
moveByRollPitchZInternal(pitch, roll, z, yaw);
moveByRollPitchYawZInternal(roll, pitch, yaw, z);
return false; //keep moving until timeout
}, duration).isTimeout();
}

bool MultirotorApiBase::moveByAngleThrottle(float pitch, float roll, float throttle, float yaw_rate, float duration)
bool MultirotorApiBase::moveByRollPitchYawThrottle(float roll, float pitch, float yaw, float throttle, float duration)
{
SingleTaskCall lock(this);

if (duration <= 0)
return true;

return waitForFunction([&]() {
moveByRollPitchThrottleInternal(pitch, roll, throttle, yaw_rate);
moveByRollPitchYawThrottleInternal(roll, pitch, yaw, throttle);
return false; //keep moving until timeout
}, duration).isTimeout();
}

bool MultirotorApiBase::moveByRollPitchYawrateThrottle(float roll, float pitch, float yaw_rate, float throttle, float duration)
{
SingleTaskCall lock(this);

if (duration <= 0)
return true;

return waitForFunction([&]() {
moveByRollPitchYawrateThrottleInternal(roll, pitch, yaw_rate, throttle);
return false; //keep moving until timeout
}, duration).isTimeout();
}

bool MultirotorApiBase::moveByRollPitchYawrateZ(float roll, float pitch, float yaw_rate, float z, float duration)
{
SingleTaskCall lock(this);

if (duration <= 0)
return true;

return waitForFunction([&]() {
moveByRollPitchYawrateZInternal(roll, pitch, yaw_rate, z);
return false; //keep moving until timeout
}, duration).isTimeout();
}

bool MultirotorApiBase::moveByAngleRatesZ(float roll_rate, float pitch_rate, float yaw_rate, float z, float duration)
{
SingleTaskCall lock(this);

if (duration <= 0)
return true;

return waitForFunction([&]() {
moveByAngleRatesZInternal(roll_rate, pitch_rate, yaw_rate, z);
return false; //keep moving until timeout
}, duration).isTimeout();
}

bool MultirotorApiBase::moveByAngleRatesThrottle(float roll_rate, float pitch_rate, float yaw_rate, float throttle, float duration)
{
SingleTaskCall lock(this);

if (duration <= 0)
return true;

return waitForFunction([&]() {
moveByAngleRatesThrottleInternal(roll_rate, pitch_rate, yaw_rate, throttle);
return false; //keep moving until timeout
}, duration).isTimeout();
}
Expand Down Expand Up @@ -451,16 +503,40 @@ void MultirotorApiBase::moveToPositionInternal(const Vector3r& dest, const YawMo
commandPosition(dest.x(), dest.y(), dest.z(), yaw_mode);
}

void MultirotorApiBase::moveByRollPitchThrottleInternal(float pitch, float roll, float throttle, float yaw_rate)
void MultirotorApiBase::moveByRollPitchYawZInternal(float roll, float pitch, float yaw, float z)
{
if (safetyCheckVelocity(getVelocity()))
commandRollPitchYawZ(roll, pitch, yaw, z);
}

void MultirotorApiBase::moveByRollPitchYawThrottleInternal(float roll, float pitch, float yaw, float throttle)
{
if (safetyCheckVelocity(getVelocity()))
commandRollPitchYawThrottle(roll, pitch, yaw, throttle);
}

void MultirotorApiBase::moveByRollPitchYawrateThrottleInternal(float roll, float pitch, float yaw_rate, float throttle)
{
if (safetyCheckVelocity(getVelocity()))
commandRollPitchYawrateThrottle(roll, pitch, yaw_rate, throttle);
}

void MultirotorApiBase::moveByRollPitchYawrateZInternal(float roll, float pitch, float yaw_rate, float z)
{
if (safetyCheckVelocity(getVelocity()))
commandRollPitchYawrateZ(roll, pitch, yaw_rate, z);
}

void MultirotorApiBase::moveByAngleRatesZInternal(float roll_rate, float pitch_rate, float yaw_rate, float z)
{
if (safetyCheckVelocity(getVelocity()))
commandRollPitchThrottle(pitch, roll, throttle, yaw_rate);
commandAngleRatesZ(roll_rate, pitch_rate, yaw_rate, z);
}

void MultirotorApiBase::moveByRollPitchZInternal(float pitch, float roll, float z, float yaw)
void MultirotorApiBase::moveByAngleRatesThrottleInternal(float roll_rate, float pitch_rate, float yaw_rate, float throttle)
{
if (safetyCheckVelocity(getVelocity()))
commandRollPitchZ(pitch, roll, z, yaw);
commandAngleRatesThrottle(roll_rate, pitch_rate, yaw_rate, throttle);
}

//executes a given function until it returns true. Each execution is spaced apart at command period.
Expand Down
Loading

0 comments on commit 03c19a1

Please sign in to comment.