Skip to content

Commit

Permalink
Merge pull request microsoft#2347 from rajat2004/arducopter-fixes
Browse files Browse the repository at this point in the history 
ArduCopterApi: Improvements & Fixes
  • Loading branch information
@madratman
madratman authored Mar 24, 2020
2 parents 88c72e5 + 6c34970 commit 7a369b3
Show file tree
Hide file tree
Showing 2 changed files with 43 additions and 38 deletions.
49 changes: 24 additions & 25 deletions AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -68,26 +68,25 @@ class ArduCopterApi : public MultirotorApiBase {
// TODO:VehicleApiBase implementation
virtual bool isApiControlEnabled() const override
{
Utils::log("Not Implemented", Utils::kLogLevelInfo);
Utils::log("Not Implemented: isApiControlEnabled", Utils::kLogLevelInfo);
return false;
}
virtual void enableApiControl(bool) override
{
Utils::log("Not Implemented", Utils::kLogLevelInfo);
Utils::log("Not Implemented: enableApiControl", Utils::kLogLevelInfo);
}
virtual bool armDisarm(bool) override
{
Utils::log("Not Implemented", Utils::kLogLevelInfo);
Utils::log("Not Implemented: armDisarm", Utils::kLogLevelInfo);
return false;
}
virtual GeoPoint getHomeGeoPoint() const override
{
Utils::log("Not Implemented", Utils::kLogLevelInfo);
Utils::log("Not Implemented: getHomeGeoPoint", Utils::kLogLevelInfo);
return GeoPoint(Utils::nan<double>(), Utils::nan<double>(), Utils::nan<float>());
}
virtual void getStatusMessages(std::vector<std::string>&) override
{
Utils::log("Not Implemented", Utils::kLogLevelInfo);
}

virtual const SensorCollection& getSensors() const override
Expand Down Expand Up @@ -122,32 +121,32 @@ class ArduCopterApi : public MultirotorApiBase {
protected:
virtual Kinematics::State getKinematicsEstimated() const override
{
Utils::log("Not Implemented", Utils::kLogLevelInfo);
Utils::log("Not Implemented: getKinematicsEstimated", Utils::kLogLevelInfo);
Kinematics::State state;
return state;
}

virtual Vector3r getPosition() const override
{
Utils::log("Not Implemented", Utils::kLogLevelInfo);
Utils::log("Not Implemented: getPosition", Utils::kLogLevelInfo);
return Vector3r(Utils::nan<float>(), Utils::nan<float>(), Utils::nan<float>());
}

virtual Vector3r getVelocity() const override
{
Utils::log("Not Implemented", Utils::kLogLevelInfo);
Utils::log("Not Implemented: getVelocity", Utils::kLogLevelInfo);
return Vector3r(Utils::nan<float>(), Utils::nan<float>(), Utils::nan<float>());
}

virtual Quaternionr getOrientation() const override
{
Utils::log("Not Implemented", Utils::kLogLevelInfo);
Utils::log("Not Implemented: getOrientation", Utils::kLogLevelInfo);
return Quaternionr(Utils::nan<float>(), Utils::nan<float>(), Utils::nan<float>(), Utils::nan<float>());
}

virtual LandedState getLandedState() const override
{
Utils::log("Not Implemented", Utils::kLogLevelInfo);
Utils::log("Not Implemented: getLandedState", Utils::kLogLevelInfo);
return LandedState::Landed;
}

Expand All @@ -159,7 +158,7 @@ class ArduCopterApi : public MultirotorApiBase {

virtual GeoPoint getGpsLocation() const override
{
Utils::log("Not Implemented", Utils::kLogLevelInfo);
Utils::log("Not Implemented: getGpsLocation", Utils::kLogLevelInfo);
return GeoPoint(Utils::nan<double>(), Utils::nan<double>(), Utils::nan<float>());
}

Expand Down Expand Up @@ -187,7 +186,7 @@ class ArduCopterApi : public MultirotorApiBase {
unused(roll);
unused(throttle);
unused(yaw_rate);
Utils::log("Not Implemented", Utils::kLogLevelInfo);
Utils::log("Not Implemented: commandRollPitchThrottle", Utils::kLogLevelInfo);
}

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

virtual void commandVelocity(float vx, float vy, float vz, const YawMode& yaw_mode) override
Expand All @@ -205,7 +204,7 @@ class ArduCopterApi : public MultirotorApiBase {
unused(vy);
unused(vz);
unused(yaw_mode);
Utils::log("Not Implemented", Utils::kLogLevelInfo);
Utils::log("Not Implemented: commandVelocity", Utils::kLogLevelInfo);
}

virtual void commandVelocityZ(float vx, float vy, float z, const YawMode& yaw_mode) override
Expand All @@ -214,7 +213,7 @@ class ArduCopterApi : public MultirotorApiBase {
unused(vy);
unused(z);
unused(yaw_mode);
Utils::log("Not Implemented", Utils::kLogLevelInfo);
Utils::log("Not Implemented: commandVelocityZ", Utils::kLogLevelInfo);
}

virtual void commandPosition(float x, float y, float z, const YawMode& yaw_mode) override
Expand All @@ -223,7 +222,7 @@ class ArduCopterApi : public MultirotorApiBase {
unused(y);
unused(z);
unused(yaw_mode);
Utils::log("Not Implemented", Utils::kLogLevelInfo);
Utils::log("Not Implemented: commandPosition", Utils::kLogLevelInfo);
}

virtual const MultirotorApiParams& getMultirotorApiParams() const override
Expand Down Expand Up @@ -346,21 +345,21 @@ class ArduCopterApi : public MultirotorApiBase {
"{"
"\"timestamp\": %" PRIu64 ","
"\"imu\": {"
"\"angular_velocity\": [%lf, %lf, %lf],"
"\"linear_acceleration\": [%lf, %lf, %lf]"
"\"angular_velocity\": [%.12f, %.12f, %.12f],"
"\"linear_acceleration\": [%.12f, %.12f, %.12f]"
"},"
"\"gps\": {"
"\"lat\": %lf,"
"\"lon\": %lf,"
"\"alt\": %lf"
"\"lat\": %.7f,"
"\"lon\": %.7f,"
"\"alt\": %.3f"
"},"
"\"velocity\": {"
"\"world_linear_velocity\": [%lf, %lf, %lf]"
"\"world_linear_velocity\": [%.12f, %.12f, %.12f]"
"},"
"\"pose\": {"
"\"roll\": %lf,"
"\"pitch\": %lf,"
"\"yaw\": %lf"
"\"roll\": %.12f,"
"\"pitch\": %.12f,"
"\"yaw\": %.12f"
"}"
"%s"
"}\n",
Expand Down
32 changes: 19 additions & 13 deletions AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterParams.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,12 +28,29 @@ class ArduCopterParams : public MultiRotorParams {
}

protected:
// TODO: Change the below function to setupGenericQuadX() which can be called by setupParams() based on connection_info.model
// This will be needed for different frames
virtual void setupParams() override
{
auto& params = getParams();

// Use connection_info_.model for the model name, see Px4MultiRotorParams for example

// Only Generic for now
setupFrameGenericQuad(params);
}

virtual const SensorFactory* getSensorFactory() const override
{
return sensor_factory_.get();
}

static const AirSimSettings::MavLinkConnectionInfo& getConnectionInfo(const AirSimSettings::MavLinkVehicleSetting& vehicle_setting)
{
return vehicle_setting.connection_info;
}

private:
void setupFrameGenericQuad(Params& params)
{
/******* Below is same config as PX4 generic model ********/

//set up arm lengths
Expand Down Expand Up @@ -63,17 +80,6 @@ class ArduCopterParams : public MultiRotorParams {
//leave everything else to defaults
}

virtual const SensorFactory* getSensorFactory() const override
{
return sensor_factory_.get();
}

static const AirSimSettings::MavLinkConnectionInfo& getConnectionInfo(const AirSimSettings::MavLinkVehicleSetting& vehicle_setting)
{
return vehicle_setting.connection_info;
}

private:
AirSimSettings::MavLinkConnectionInfo connection_info_;
vector<unique_ptr<SensorBase>> sensor_storage_;
// const AirSimSettings::MavlinkVehicleSetting* vehicle_setting_; //store as pointer because of derived classes
Expand Down

0 comments on commit 7a369b3

Please sign in to comment.