Skip to content

MultiRotorParams: Consolidate frame types #2495

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

Merged
madratman merged 2 commits into from
Jul 22, 2020
Merged

MultiRotorParams: Consolidate frame types #2495

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
cbfc1ca
Consolidate frame types in MultiRotorParams
rajat2004 Mar 30, 2020
588b473
Remove extra RotorControlsCount variable
rajat2004 Mar 30, 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
222 changes: 205 additions & 17 deletions AirLib/include/vehicles/multirotor/MultiRotorParams.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,12 +36,12 @@ class MultiRotorParams {
Vector3r body_box;

/*********** optional parameters with defaults ***********/
real_T linear_drag_coefficient = 1.3f / 4.0f;
real_T linear_drag_coefficient = 1.3f / 4.0f;
//sample value 1.3 from http://klsin.bpmsg.com/how-fast-can-a-quadcopter-fly/, but divided by 4 to account
// for nice streamlined frame design and allow higher top speed which is more fun.
//angular coefficient is usually 10X smaller than linear, however we should replace this with exact number
//http://physics.stackexchange.com/q/304742/14061
real_T angular_drag_coefficient = linear_drag_coefficient;
real_T angular_drag_coefficient = linear_drag_coefficient;
real_T restitution = 0.55f; // value of 1 would result in perfectly elastic collisions, 0 would be completely inelastic.
real_T friction = 0.5f;
RotorParams rotor_params;
Expand Down Expand Up @@ -100,37 +100,37 @@ class MultiRotorParams {
/// which shows that given an array of 4 motors, the first is placed top right and flies counter clockwise (CCW) and
/// the second is placed bottom left, and also flies CCW. The third in the array is placed top left and flies clockwise (CW)
/// while the last is placed bottom right and also flies clockwise. This is how the 4 items in the arm_lengths and
/// arm_angles arrays will be used. So arm_lengths is 4 numbers (in meters) where four arm lengths, 0 is top right,
/// 1 is bottom left, 2 is top left and 3 is bottom right. arm_angles is 4 numbers (in degrees) relative to forward vector (0,1),
/// provided in the same order where 0 is top right, 1 is bottom left, 2 is top left and 3 is bottom right, so for example,
/// arm_angles arrays will be used. So arm_lengths is 4 numbers (in meters) where four arm lengths, 0 is top right,
/// 1 is bottom left, 2 is top left and 3 is bottom right. arm_angles is 4 numbers (in degrees) relative to forward vector (0,1),
/// provided in the same order where 0 is top right, 1 is bottom left, 2 is top left and 3 is bottom right, so for example,
/// the angles for a regular symmetric X pattern would be 45, 225, 315, 135. The rotor_z is the offset of each motor upwards
/// relative to the center of mass (in meters).
/// relative to the center of mass (in meters).
static void initializeRotorQuadX(vector<RotorPose>& rotor_poses /* the result we are building */,
uint rotor_count /* must be 4 */,
real_T arm_lengths[],
uint rotor_count /* must be 4 */,
real_T arm_lengths[],
real_T rotor_z /* z relative to center of gravity */)
{
Vector3r unit_z(0, 0, -1); //NED frame
if (rotor_count == 4) {
rotor_poses.clear();

/* Note: rotor_poses are built in this order:
/* Note: rotor_poses are built in this order:
x-axis
(2) | (0)
|
-------------- y-axis
|
(1) | (3)
(1) | (3)
*/
// vectors below are rotated according to NED left hand rule (so the vectors are rotated counter clockwise).
Quaternionr quadx_rot(AngleAxisr(M_PIf / 4, unit_z));
rotor_poses.emplace_back(VectorMath::rotateVector(Vector3r(0, arm_lengths[0], rotor_z), quadx_rot, true),
unit_z, RotorTurningDirection::RotorTurningDirectionCCW);
rotor_poses.emplace_back(VectorMath::rotateVector(Vector3r(0, -arm_lengths[1], rotor_z), quadx_rot, true),
unit_z, RotorTurningDirection::RotorTurningDirectionCCW);
rotor_poses.emplace_back(VectorMath::rotateVector(Vector3r(arm_lengths[2], 0, rotor_z), quadx_rot, true),
rotor_poses.emplace_back(VectorMath::rotateVector(Vector3r(arm_lengths[2], 0, rotor_z), quadx_rot, true),
unit_z, RotorTurningDirection::RotorTurningDirectionCW);
rotor_poses.emplace_back(VectorMath::rotateVector(Vector3r(-arm_lengths[3], 0, rotor_z), quadx_rot, true),
rotor_poses.emplace_back(VectorMath::rotateVector(Vector3r(-arm_lengths[3], 0, rotor_z), quadx_rot, true),
unit_z, RotorTurningDirection::RotorTurningDirectionCW);
}
else
Expand All @@ -153,8 +153,8 @@ class MultiRotorParams {
\ /
\/
(1)-------(0) y-axis
/\
/ \
/\
/ \
(5) (3)

*/
Expand Down Expand Up @@ -200,9 +200,9 @@ class MultiRotorParams {
inertia = Matrix3x3r::Zero();

//http://farside.ph.utexas.edu/teaching/336k/Newtonhtml/node64.html
inertia(0, 0) = box_mass / 12.0f * (body_box.y()*body_box.y() + body_box.z()*body_box.z());
inertia(1, 1) = box_mass / 12.0f * (body_box.x()*body_box.x() + body_box.z()*body_box.z());
inertia(2, 2) = box_mass / 12.0f * (body_box.x()*body_box.x() + body_box.y()*body_box.y());
inertia(0, 0) = box_mass / 12.0f * (body_box.y()*body_box.y() + body_box.z()*body_box.z());
inertia(1, 1) = box_mass / 12.0f * (body_box.x()*body_box.x() + body_box.z()*body_box.z());
inertia(2, 2) = box_mass / 12.0f * (body_box.x()*body_box.x() + body_box.y()*body_box.y());

for (size_t i = 0; i < rotor_poses.size(); ++i) {
const auto& pos = rotor_poses.at(i).position;
Expand All @@ -212,6 +212,194 @@ class MultiRotorParams {
}
}

// Some Frame types which can be used by different firmwares
// Specific frame configurations, modifications can be done in the Firmware Params

void setupFrameGenericQuad(Params& params)
{
//set up arm lengths
//dimensions are for F450 frame: http://artofcircuits.com/product/quadcopter-frame-hj450-with-power-distribution
params.rotor_count = 4;
std::vector<real_T> arm_lengths(params.rotor_count, 0.2275f);

//set up mass
params.mass = 1.0f; //can be varied from 0.800 to 1.600
real_T motor_assembly_weight = 0.055f; //weight for MT2212 motor for F450 frame
real_T box_mass = params.mass - params.rotor_count * motor_assembly_weight;

// using rotor_param default, but if you want to change any of the rotor_params, call calculateMaxThrust() to recompute the max_thrust
// given new thrust coefficients, motor max_rpm and propeller diameter.
params.rotor_params.calculateMaxThrust();

//set up dimensions of core body box or abdomen (not including arms).
params.body_box.x() = 0.180f; params.body_box.y() = 0.11f; params.body_box.z() = 0.040f;
real_T rotor_z = 2.5f / 100;

//computer rotor poses
initializeRotorQuadX(params.rotor_poses, params.rotor_count, arm_lengths.data(), rotor_z);

//compute inertia matrix
computeInertiaMatrix(params.inertia, params.body_box, params.rotor_poses, box_mass, motor_assembly_weight);
}

void setupFrameGenericHex(Params& params)
{
//set up arm lengths
//dimensions are for F450 frame: http://artofcircuits.com/product/quadcopter-frame-hj450-with-power-distribution
params.rotor_count = 6;
std::vector<real_T> arm_lengths(params.rotor_count, 0.2275f);

//set up mass
params.mass = 1.0f; //can be varied from 0.800 to 1.600
real_T motor_assembly_weight = 0.055f; //weight for MT2212 motor for F450 frame
real_T box_mass = params.mass - params.rotor_count * motor_assembly_weight;

// using rotor_param default, but if you want to change any of the rotor_params, call calculateMaxThrust() to recompute the max_thrust
// given new thrust coefficients, motor max_rpm and propeller diameter.
params.rotor_params.calculateMaxThrust();

//set up dimensions of core body box or abdomen (not including arms).
params.body_box.x() = 0.180f; params.body_box.y() = 0.11f; params.body_box.z() = 0.040f;
real_T rotor_z = 2.5f / 100;

//computer rotor poses
initializeRotorHexX(params.rotor_poses, params.rotor_count, arm_lengths.data(), rotor_z);

//compute inertia matrix
computeInertiaMatrix(params.inertia, params.body_box, params.rotor_poses, box_mass, motor_assembly_weight);
}

void setupFrameFlamewheel(Params& params)
{
//set up arm lengths
//dimensions are for F450 frame: http://artofcircuits.com/product/quadcopter-frame-hj450-with-power-distribution
params.rotor_count = 4;
std::vector<real_T> arm_lengths(params.rotor_count, 0.225f);

//set up mass
params.mass = 1.635f;
real_T motor_assembly_weight = 0.052f;
real_T box_mass = params.mass - params.rotor_count * motor_assembly_weight;

params.rotor_params.C_T = 0.11f;
params.rotor_params.C_P = 0.047f;
params.rotor_params.max_rpm = 9500;
params.rotor_params.calculateMaxThrust();
params.linear_drag_coefficient *= 4; // make top speed more real.

//set up dimensions of core body box or abdomen (not including arms).
params.body_box.x() = 0.16f; params.body_box.y() = 0.10f; params.body_box.z() = 0.14f;
real_T rotor_z = 0.15f;

//computer rotor poses
initializeRotorQuadX(params.rotor_poses, params.rotor_count, arm_lengths.data(), rotor_z);

//compute inertia matrix
computeInertiaMatrix(params.inertia, params.body_box, params.rotor_poses, box_mass, motor_assembly_weight);
}

void setupFrameFlamewheelFLA(Params& params)
{
//set up arm lengths
//dimensions are for F450 frame: http://artofcircuits.com/product/quadcopter-frame-hj450-with-power-distribution
params.rotor_count = 4;
std::vector<real_T> arm_lengths(params.rotor_count, 0.225f);

//set up mass
params.mass = 2.25f;
real_T motor_assembly_weight = 0.1f;
real_T box_mass = params.mass - params.rotor_count * motor_assembly_weight;

params.rotor_params.C_T = 0.2f;
params.rotor_params.C_P = 0.1f;
params.rotor_params.max_rpm = 9324;
params.rotor_params.calculateMaxThrust();
params.linear_drag_coefficient *= 4; // make top speed more real.

//set up dimensions of core body box or abdomen (not including arms).
params.body_box.x() = 0.16f; params.body_box.y() = 0.10f; params.body_box.z() = 0.14f;
real_T rotor_z = 0.15f;

//computer rotor poses
initializeRotorQuadX(params.rotor_poses, params.rotor_count, arm_lengths.data(), rotor_z);

//compute inertia matrix
computeInertiaMatrix(params.inertia, params.body_box, params.rotor_poses, box_mass, motor_assembly_weight);
}

void setupFrameBlacksheep(Params& params)
{
/*
Motor placement:
x
(2) | (0)
|
------------ y
|
(1) | (3)
|

*/
//set up arm lengths
//dimensions are for Team Blacksheep Discovery (http://team-blacksheep.com/products/product:98)
params.rotor_count = 4;
std::vector<real_T> arm_lengths;

Vector3r unit_z(0, 0, -1); //NED frame

// relative to Forward vector in the order (0,3,1,2) required by quad X pattern
// http://ardupilot.org/copter/_images/MOTORS_QuadX_QuadPlus.jpg
arm_lengths.push_back(0.22f);
arm_lengths.push_back(0.255f);
arm_lengths.push_back(0.22f);
arm_lengths.push_back(0.255f);

// note: the Forward vector is actually the "x" axis, and the AngleAxisr rotation is pointing down and is left handed, so this means the rotation
// is counter clockwise, so the vector (arm_lengths[i], 0) is the X-axis, so the CCW rotations to position each arm correctly are listed below:
// See measurements here: http://diydrones.com/profiles/blogs/arducopter-tbs-discovery-style (angles reversed because we are doing CCW rotation)
std::vector<real_T> arm_angles;
arm_angles.push_back(-55.0f);
arm_angles.push_back(125.0f);
arm_angles.push_back(55.0f);
arm_angles.push_back(-125.0f);

// quad X pattern
std::vector<RotorTurningDirection> rotor_directions;
rotor_directions.push_back(RotorTurningDirection::RotorTurningDirectionCCW);
rotor_directions.push_back(RotorTurningDirection::RotorTurningDirectionCCW);
rotor_directions.push_back(RotorTurningDirection::RotorTurningDirectionCW);
rotor_directions.push_back(RotorTurningDirection::RotorTurningDirectionCW);

// data from
// http://dronesvision.net/team-blacksheep-750kv-motor-esc-set-for-tbs-discovery-fpv-quadcopter/
//set up mass
params.mass = 2.0f; //can be varied from 0.800 to 1.600
real_T motor_assembly_weight = 0.052f; // weight for TBS motors
real_T box_mass = params.mass - params.rotor_count * motor_assembly_weight;

// the props we are using a E-Prop, which I didn't find in UIUC database, but this one is close:
// http://m-selig.ae.illinois.edu/props/volume-2/plots/ef_130x70_static_ctcp.png
params.rotor_params.C_T = 0.11f;
params.rotor_params.C_P = 0.047f;
params.rotor_params.max_rpm = 9500;
params.rotor_params.calculateMaxThrust();

//set up dimensions of core body box or abdomen (not including arms).
params.body_box.x() = 0.20f; params.body_box.y() = 0.12f; params.body_box.z() = 0.04f;
real_T rotor_z = 2.5f / 100;

//computer rotor poses
params.rotor_poses.clear();
for (uint i = 0; i < 4; i++)
{
Quaternionr angle(AngleAxisr(arm_angles[i] * M_PIf / 180, unit_z));
params.rotor_poses.emplace_back(VectorMath::rotateVector(Vector3r(arm_lengths[i], 0, rotor_z), angle, true), unit_z, rotor_directions[i]);
};

//compute inertia matrix
computeInertiaMatrix(params.inertia, params.body_box, params.rotor_poses, box_mass, motor_assembly_weight);
}

private:
Params params_;
SensorCollection sensors_; //maintains sensor type indexed collection of sensors
Expand Down
7 changes: 2 additions & 5 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 @@ -462,7 +462,7 @@ class ArduCopterApi : public MultirotorApiBase {
recv_ret = udpSocket_->recv(&pkt, sizeof(pkt), 100);
}

for (auto i = 0; i < RotorControlCount && i < kArduCopterRotorControlCount; ++i) {
for (auto i = 0; i < kArduCopterRotorControlCount; ++i) {
rotor_controls_[i] = pkt.pwm[i];
}

Expand All @@ -489,10 +489,7 @@ class ArduCopterApi : public MultirotorApiBase {
RCData last_rcData_;
bool is_rc_connected;

// TODO: Increase to 6 or 8 for hexa or larger frames, 11 was used in SoloAPI
static const int RotorControlCount = 4;

float rotor_controls_[RotorControlCount];
float rotor_controls_[kArduCopterRotorControlCount];
};

}} //namespace
Expand Down
31 changes: 0 additions & 31 deletions AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterParams.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -49,37 +49,6 @@ class ArduCopterParams : public MultiRotorParams {
}

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

//set up arm lengths
//dimensions are for F450 frame: http://artofcircuits.com/product/quadcopter-frame-hj450-with-power-distribution
params.rotor_count = 4;
std::vector<real_T> arm_lengths(params.rotor_count, 0.2275f);

//set up mass
params.mass = 1.0f; //can be varied from 0.800 to 1.600
real_T motor_assembly_weight = 0.055f; //weight for MT2212 motor for F450 frame
real_T box_mass = params.mass - params.rotor_count * motor_assembly_weight;

// using rotor_param default, but if you want to change any of the rotor_params, call calculateMaxThrust() to recompute the max_thrust
// given new thrust coefficients, motor max_rpm and propeller diameter.
params.rotor_params.calculateMaxThrust();

//set up dimensions of core body box or abdomen (not including arms).
params.body_box.x() = 0.180f; params.body_box.y() = 0.11f; params.body_box.z() = 0.040f;
real_T rotor_z = 2.5f / 100;

//computer rotor poses
initializeRotorQuadX(params.rotor_poses, params.rotor_count, arm_lengths.data(), rotor_z);

//compute inertia matrix
computeInertiaMatrix(params.inertia, params.body_box, params.rotor_poses, box_mass, motor_assembly_weight);

//leave everything else to defaults
}

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