Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

add / fix unintuitive lower level multirotor APIs #2297

Merged
merged 8 commits into from
Mar 25, 2020
Merged

add / fix unintuitive lower level multirotor APIs #2297

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
03c19a1
[simpleflight] add / fix unintuitive lower level APIs - RPYZ|Throttle…
madratman Nov 6, 2019
4db9e9b
[simpleflight] enable on the fly PID gain setting
madratman Nov 7, 2019
b56272b
[simpleflight] python API docstrings
madratman Nov 7, 2019
0fedc5a
[arducopterapi] add unimpl lowlevel apis impl in simpleflight
madratman Nov 7, 2019
c2f4789
[simpleflight] cleanup, fix minor errors
madratman Nov 9, 2019
52df395
[multirotorapi] make setControllerGains purevirtual; add non-impls in…
madratman Mar 24, 2020
3f2415a
[simpleflight] add moveByMotorPWMs
madratman Mar 24, 2020
06b0f06
[simpleflight] add gain types to types.py
madratman Mar 25, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
35 changes: 29 additions & 6 deletions AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,12 +24,20 @@ 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 commandMotorPWMs(float front_right_pwm, float rear_left_pwm, float front_left_pwm, float rear_right_pwm) = 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;

/************************* set Controller Gains APIs *********************************/
virtual void setControllerGains(uint8_t controllerType, const vector<float>& kp, const vector<float>& ki, const vector<float>& kd) = 0;

/************************* State APIs *********************************/
virtual Kinematics::State getKinematicsEstimated() const = 0;
virtual LandedState getLandedState() const = 0;
Expand Down Expand Up @@ -81,8 +89,13 @@ 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 moveByMotorPWMs(float front_right_pwm, float rear_left_pwm, float front_left_pwm, float rear_right_pwm, 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 All @@ -97,6 +110,12 @@ class MultirotorApiBase : public VehicleApiBase {
virtual bool hover();
virtual RCData estimateRCTrims(float trimduration = 1, float minCountForTrim = 10, float maxTrim = 100);

/************************* set angle gain APIs *********************************/
virtual void setAngleLevelControllerGains(const vector<float>& kp, const vector<float>& ki, const vector<float>& kd);
virtual void setAngleRateControllerGains(const vector<float>& kp, const vector<float>& ki, const vector<float>& kd);
virtual void setVelocityControllerGains(const vector<float>& kp, const vector<float>& ki, const vector<float>& kd);
virtual void setPositionControllerGains(const vector<float>& kp, const vector<float>& ki, const vector<float>& kd);

/************************* Safety APIs *********************************/
virtual void setSafetyEval(const shared_ptr<SafetyEval> safety_eval_ptr);
virtual bool setSafety(SafetyEval::SafetyViolationType enable_reasons, float obs_clearance, SafetyEval::ObsAvoidanceStrategy obs_startegy,
Expand Down Expand Up @@ -127,11 +146,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
14 changes: 11 additions & 3 deletions AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,8 +23,13 @@ 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* moveByMotorPWMsAsync(float front_right_pwm, float rear_left_pwm, float front_left_pwm, float rear_right_pwm, 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 All @@ -43,9 +48,12 @@ class MultirotorRpcLibClient : public RpcLibClientBase {
MultirotorRpcLibClient* rotateByYawRateAsync(float yaw_rate, float duration, const std::string& vehicle_name = "");
MultirotorRpcLibClient* hoverAsync(const std::string& vehicle_name = "");

void setAngleLevelControllerGains(const vector<float>& kp, const vector<float>& ki, const vector<float>& kd, const std::string& vehicle_name="");
void setAngleRateControllerGains(const vector<float>& kp, const vector<float>& ki, const vector<float>& kd, const std::string& vehicle_name="");
void setVelocityControllerGains(const vector<float>& kp, const vector<float>& ki, const vector<float>& kd, const std::string& vehicle_name="");
void setPositionControllerGains(const vector<float>& kp, const vector<float>& ki, const vector<float>& kd, const std::string& vehicle_name="");
void moveByRC(const RCData& rc_data, const std::string& vehicle_name = "");


MultirotorState getMultirotorState(const std::string& vehicle_name = "");

bool setSafety(SafetyEval::SafetyViolationType enable_reasons, float obs_clearance, SafetyEval::ObsAvoidanceStrategy obs_startegy,
Expand Down
68 changes: 61 additions & 7 deletions AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -180,22 +180,76 @@ class ArduCopterApi : 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 setControllerGains(uint8_t controllerType, const vector<float>& kp, const vector<float>& ki, const vector<float>& kd) override
{
unused(controllerType);
unused(kp);
unused(ki);
unused(kd);
Utils::log("Not Implemented: setControllerGains", Utils::kLogLevelInfo);
}

virtual void commandMotorPWMs(float front_right_pwm, float front_left_pwm, float rear_right_pwm, float rear_left_pwm) override
{
unused(front_right_pwm);
unused(front_left_pwm);
unused(rear_right_pwm);
unused(rear_left_pwm);
Utils::log("Not Implemented: commandMotorPWMs", Utils::kLogLevelInfo);
}

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

virtual void commandRollPitchZ(float pitch, float roll, float z, float yaw) override
virtual void commandRollPitchYawZ(float roll, float pitch, float yaw, float z) override
{
unused(pitch);
unused(roll);
unused(pitch);
unused(yaw);
unused(z);
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
57 changes: 54 additions & 3 deletions AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -473,7 +473,25 @@ class MavLinkMultirotorApi : public MultirotorApiBase
}

protected: //methods
virtual void commandRollPitchZ(float pitch, float roll, float z, float yaw) override
virtual void setControllerGains(uint8_t controllerType, const vector<float>& kp, const vector<float>& ki, const vector<float>& kd) override
{
unused(controllerType);
unused(kp);
unused(ki);
unused(kd);
Utils::log("Not Implemented: setControllerGains", Utils::kLogLevelInfo);
}

virtual void commandMotorPWMs(float front_right_pwm, float front_left_pwm, float rear_right_pwm, float rear_left_pwm) override
{
unused(front_right_pwm);
unused(front_left_pwm);
unused(rear_right_pwm);
unused(rear_left_pwm);
Utils::log("Not Implemented: commandMotorPWMs", Utils::kLogLevelInfo);
}

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 +505,44 @@ 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();
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();
mav_vehicle_->moveByAttitude(roll, pitch, yaw_rate, 0, 0, 0, throttle);
mav_vehicle_->moveByAttitude(roll, pitch, yaw, 0, 0, 0, throttle);
}
virtual void commandRollPitchYawrateThrottle(float roll, float pitch, float yaw_rate, float throttle) override
{
checkValidVehicle();
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();
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
Loading