Skip to content

Commit 1517a00

Browse files
srinath7979srinath7979
committed
Merged in umd/utilities-axis-corrected/umd/quatostoolcc-edited-to-make-coordinate-s-1424332129863 (pull request #2)
 quatosTool updated to match with axis convention and ensure correct powerDistro, MM for asymmetric frames
2 parents e01cc1b + a86638a commit 1517a00

File tree

3 files changed

+90
-86
lines changed

3 files changed

+90
-86
lines changed

‎mot_mixes.xml

+58-58
Original file line numberDiff line numberDiff line change
@@ -21,10 +21,10 @@
2121
<!-- quads -->
2222
<craft id="quad_plus" config="custom" motors="4" configId="4">
2323
<geometry>
24-
<motor rotation="+1" port="1">0,1</motor>
25-
<motor rotation="-1" port="2">1,0</motor>
26-
<motor rotation="+1" port="3">0,-1</motor>
27-
<motor rotation="-1" port="4">-1,0</motor>
24+
<motor rotation="+1" port="1">1,0</motor>
25+
<motor rotation="-1" port="2">0,1</motor>
26+
<motor rotation="+1" port="3">-1,0</motor>
27+
<motor rotation="-1" port="4">0,-1</motor>
2828
</geometry>
2929
</craft>
3030
<craft id="quad_x" config="quad_x" configId="5">
@@ -61,35 +61,35 @@
6161
<!-- The YAW values on hexa_h come out wrong... The other values are the same as for hex_x. -->
6262
<craft id="hexa_h" config="custom" motors="6" configId="12">
6363
<geometry>
64-
<motor rotation="+1" port="1">-0.57,1</motor>
65-
<motor rotation="-1" port="2">0.57,1</motor>
66-
<motor rotation="+1" port="3">0.57,0</motor>
67-
<motor rotation="-1" port="4">0.57,-1</motor>
68-
<motor rotation="+1" port="5">-0.57,-1</motor>
69-
<motor rotation="-1" port="6">-0.57,0</motor>
64+
<motor rotation="+1" port="1">1,-0.57</motor>//Note that x axis runs from back to front and y axis from left to right
65+
<motor rotation="-1" port="2">1,0.57</motor>
66+
<motor rotation="+1" port="3">0,0.57</motor>
67+
<motor rotation="-1" port="4">-1,0.57</motor>
68+
<motor rotation="+1" port="5">-1,-0.57</motor>
69+
<motor rotation="-1" port="6">0,-0.57</motor>
7070
</geometry>
7171
</craft>
7272

7373
<!-- The generated Pitch values for Y6 are 50% too high... need to adjust the output manually. -->
7474
<craft id="hexa_coax" config="custom" motors="6" configId="13">
7575
<geometry>
76-
<motor rotation="+1" port="1">-0.5,0.5</motor>
77-
<motor rotation="-1" port="2">-0.5,0.5</motor>
78-
<motor rotation="+1" port="3">0.5,0.5</motor>
79-
<motor rotation="-1" port="4">0.5,0.5</motor>
80-
<motor rotation="-1" port="5">0,-1</motor>
81-
<motor rotation="+1" port="6">0,-1</motor>
76+
<motor rotation="+1" port="1">0.5,-.866</motor>//Note how x-y axis are defined above!
77+
<motor rotation="-1" port="2">0.5,-.866</motor>
78+
<motor rotation="+1" port="3">0.5,.866</motor>
79+
<motor rotation="-1" port="4">0.5,.866</motor>
80+
<motor rotation="-1" port="5">-1,0</motor>
81+
<motor rotation="+1" port="6">-1,0</motor>
8282
</geometry>
8383
</craft>
8484
<!-- "Reversed" Y6, with the single arm leading. -->
8585
<craft id="hexa_coax_rev" config="custom" motors="6" configId="14">
8686
<geometry>
87-
<motor rotation="+1" port="1">0,1</motor>
88-
<motor rotation="-1" port="2">0,1</motor>
89-
<motor rotation="+1" port="3">0.5,-0.5</motor>
90-
<motor rotation="-1" port="4">0.5,-0.5</motor>
91-
<motor rotation="-1" port="5">-0.5,-0.5</motor>
92-
<motor rotation="+1" port="6">-0.5,-0.5</motor>
87+
<motor rotation="+1" port="1">1,0</motor>
88+
<motor rotation="-1" port="2">1,0</motor>
89+
<motor rotation="+1" port="3">-0.5,.866</motor>
90+
<motor rotation="-1" port="4">-0.5,.866</motor>
91+
<motor rotation="-1" port="5">-0.5,-.866</motor>
92+
<motor rotation="+1" port="6">-0.5,-.866</motor>
9393
</geometry>
9494
</craft>
9595

@@ -121,49 +121,49 @@
121121
</craft>
122122
<craft id="octo_coax_plus" config="custom" motors="8" configId="32">
123123
<geometry>
124-
<motor rotation="+1" port="1">0,1</motor>
125-
<motor rotation="-1" port="2">0,1</motor>
126-
<motor rotation="-1" port="3">1,0</motor>
127-
<motor rotation="+1" port="4">1,0</motor>
128-
<motor rotation="+1" port="5">0,-1</motor>
129-
<motor rotation="-1" port="6">0,-1</motor>
130-
<motor rotation="-1" port="7">-1,0</motor>
131-
<motor rotation="+1" port="8">-1,0</motor>
124+
<motor rotation="+1" port="1">1,0</motor>
125+
<motor rotation="-1" port="2">1,0</motor>
126+
<motor rotation="-1" port="3">0,1</motor>
127+
<motor rotation="+1" port="4">0,1</motor>
128+
<motor rotation="+1" port="5">-1,0</motor>
129+
<motor rotation="-1" port="6">-1,0</motor>
130+
<motor rotation="-1" port="7">0,-1</motor>
131+
<motor rotation="+1" port="8">0,-1</motor>
132132
</geometry>
133133
</craft>
134134
<craft id="octo_coax_x" config="custom" motors="8" configId="33">
135135
<geometry>
136-
<motor rotation="+1" port="1">-1,1</motor>
137-
<motor rotation="-1" port="2">-1,1</motor>
136+
<motor rotation="+1" port="1">1,-1</motor>
137+
<motor rotation="-1" port="2">1,-1</motor>
138138
<motor rotation="-1" port="3">1,1</motor>
139139
<motor rotation="+1" port="4">1,1</motor>
140-
<motor rotation="+1" port="5">1,-1</motor>
141-
<motor rotation="-1" port="6">1,-1</motor>
140+
<motor rotation="+1" port="5">-1,1</motor>
141+
<motor rotation="-1" port="6">-1,1</motor>
142142
<motor rotation="-1" port="7">-1,-1</motor>
143143
<motor rotation="+1" port="8">-1,-1</motor>
144144
</geometry>
145145
</craft>
146146
<!-- "colinear octo" meaning 4 arms, two motors per arm, one set of motors is half-way between center of 'copter and the outer set of motors -->
147147
<craft id="octo_colin_plus" config="custom" motors="8" configId="34">
148148
<geometry>
149-
<motor rotation="+1" port="1">0,1</motor>
150-
<motor rotation="-1" port="2">0,0.5</motor>
151-
<motor rotation="+1" port="3">1,0</motor>
152-
<motor rotation="-1" port="4">0.5,0</motor>
153-
<motor rotation="+1" port="5">0,-1</motor>
154-
<motor rotation="-1" port="6">0,-0.5</motor>
155-
<motor rotation="+1" port="7">-1,0</motor>
156-
<motor rotation="-1" port="8">-0.5,0</motor>
149+
<motor rotation="+1" port="1">1,0</motor>
150+
<motor rotation="-1" port="2">0.5,0</motor>
151+
<motor rotation="+1" port="3">0,1</motor>
152+
<motor rotation="-1" port="4">0,0.5</motor>
153+
<motor rotation="+1" port="5">-1,0</motor>
154+
<motor rotation="-1" port="6">-0.5,0</motor>
155+
<motor rotation="+1" port="7">0,-1</motor>
156+
<motor rotation="-1" port="8">0,-0.5</motor>
157157
</geometry>
158158
</craft>
159159
<craft id="octo_colin_x" config="custom" motors="8" configId="35">
160160
<geometry>
161-
<motor rotation="+1" port="1">-1,1</motor>
162-
<motor rotation="-1" port="2">-0.5,0.5</motor>
161+
<motor rotation="+1" port="1">1,-1</motor>
162+
<motor rotation="-1" port="2">0.5,-0.5</motor>
163163
<motor rotation="+1" port="3">1,1</motor>
164164
<motor rotation="-1" port="4">0.5,0.5</motor>
165-
<motor rotation="+1" port="5">1,-1</motor>
166-
<motor rotation="-1" port="6">0.5,-0.5</motor>
165+
<motor rotation="+1" port="5">-1,1</motor>
166+
<motor rotation="-1" port="6">-0.5,0.5</motor>
167167
<motor rotation="+1" port="7">-1,-1</motor>
168168
<motor rotation="-1" port="8">-0.5,-0.5</motor>
169169
</geometry>
@@ -173,26 +173,26 @@
173173
</craft> -->
174174
<craft id="octo_square_plus" config="custom" motors="8" configId="37">
175175
<geometry>
176-
<motor rotation="+1" port="1">0,1</motor>
176+
<motor rotation="+1" port="1">1,0</motor>
177177
<motor rotation="-1" port="2">0.7,0.7</motor>
178-
<motor rotation="+1" port="3">1,0</motor>
179-
<motor rotation="-1" port="4">0.7,-0.7</motor>
180-
<motor rotation="+1" port="5">0,-1</motor>
178+
<motor rotation="+1" port="3">0,1</motor>
179+
<motor rotation="-1" port="4">-0.7,0.7</motor>
180+
<motor rotation="+1" port="5">-1,0</motor>
181181
<motor rotation="-1" port="6">-0.7,-0.7</motor>
182-
<motor rotation="+1" port="7">-1,0</motor>
183-
<motor rotation="-1" port="8">-0.7,0.7</motor>
182+
<motor rotation="+1" port="7">0,-1</motor>
183+
<motor rotation="-1" port="8">0.7,-0.7</motor>
184184
</geometry>
185185
</craft>
186186
<craft id="octo_square_x" config="custom" motors="8" configId="38">
187187
<geometry>
188-
<motor rotation="+1" port="1">-1,1</motor>
189-
<motor rotation="-1" port="2">0,1</motor>
188+
<motor rotation="+1" port="1">1,-1</motor>
189+
<motor rotation="-1" port="2">1,0</motor>
190190
<motor rotation="+1" port="3">1,1</motor>
191-
<motor rotation="-1" port="4">1,0</motor>
192-
<motor rotation="+1" port="5">1,-1</motor>
193-
<motor rotation="-1" port="6">0,-1</motor>
191+
<motor rotation="-1" port="4">0,1</motor>
192+
<motor rotation="+1" port="5">-1,1</motor>
193+
<motor rotation="-1" port="6">-1,0</motor>
194194
<motor rotation="+1" port="7">-1,-1</motor>
195-
<motor rotation="-1" port="8">-1,0</motor>
195+
<motor rotation="-1" port="8">0,-1</motor>
196196
</geometry>
197197
</craft>
198198

‎quatosTool.cc

+24-20
Original file line numberDiff line numberDiff line change
@@ -15,7 +15,7 @@
1515

1616
using namespace Eigen;
1717

18-
#define QUATOSTOOL_VERSION "150118.0" // yymmdd.build
18+
#define QUATOSTOOL_VERSION "150224.0" // yymmdd.build
1919

2020
#define MAX_DEPTH 16
2121
#define DEG_TO_RAD (M_PI / 180.0f)
@@ -729,38 +729,42 @@ void quatosToolCalc(void) {
729729
// calculate x/y coordinates for each motor
730730
switch (quatosData.craftType) {
731731
case CONFIG_QUAD_PLUS:
732-
quatosData.frameX << 0.0, 1.0, 0.0, -1.0;
733-
quatosData.frameY << 1.0, 0.0, -1.0, 0.0;
732+
quatosData.frameX << 1.0, 0.0, -1.0, 0.0;
733+
quatosData.frameY << 0.0, 1.0, 0.0, -1.0;
734734
break;
735735
case CONFIG_QUAD_X:
736-
quatosData.frameX << -sqrt(2.0)/2.0, sqrt(2.0)/2.0, sqrt(2.0)/2.0, -sqrt(2.0)/2.0;
737-
quatosData.frameY << sqrt(2.0)/2.0, sqrt(2.0)/2.0, -sqrt(2.0)/2.0, -sqrt(2.0)/2.0;
736+
quatosData.frameX << sqrt(2.0)/2.0, sqrt(2.0)/2.0, -sqrt(2.0)/2.0, -sqrt(2.0)/2.0;
737+
quatosData.frameY << -sqrt(2.0)/2.0, sqrt(2.0)/2.0, sqrt(2.0)/2.0, -sqrt(2.0)/2.0;
738738
break;
739739
case CONFIG_HEX_PLUS:
740740
quatosData.frameX << 0.0, sqrt(3.0)/2.0, sqrt(3.0)/2.0, 0.0, -sqrt(3.0)/2.0, -sqrt(3.0)/2.0;
741741
quatosData.frameY << 1.0, 0.5, -0.5, -1.0, -0.5, 0.5;
742+
quatosData.frameX << 1.0, 0.5, -0.5, -1, -0.5, 0.5;
743+
quatosData.frameY << 0.0, sqrt(3)/2.0, sqrt(3.0)/2.0, 0.0, -sqrt(3.0)/2.0, -sqrt(3.0)/2.0;
742744
break;
743745
case CONFIG_HEX_X:
744-
quatosData.frameX << -0.5, 0.5, 1.0, 0.5, -0.5, -1.0;
745-
quatosData.frameY << sqrt(3.0)/2.0, sqrt(3.0)/2.0, 0.0, -sqrt(3.0)/2.0, -sqrt(3.0)/2.0, 0.0;
746+
quatosData.frameX << sqrt(3.0)/2.0, sqrt(3.0)/2.0, 0.0, -sqrt(3.0)/2.0, -sqrt(3.0)/2.0, 0.0;
747+
quatosData.frameY << -0.5, 0.5, 1.0, 0.5, -0.5, -1.0;
746748
break;
747749
case CONFIG_OCTO_PLUS:
748-
quatosData.frameX << 0, cosf(315 *DEG_TO_RAD), 1, cosf(45 *DEG_TO_RAD), 0, cosf(135 *DEG_TO_RAD), -1, cosf(225 *DEG_TO_RAD);
749-
quatosData.frameY << 1, cosf(45 *DEG_TO_RAD), 0, cosf(135 *DEG_TO_RAD), -1, cosf(225 *DEG_TO_RAD), 0, cosf(315 *DEG_TO_RAD);
750+
quatosData.frameY << 0, cosf(315 *DEG_TO_RAD), 1, cosf(45 *DEG_TO_RAD), 0, cosf(135 *DEG_TO_RAD), -1, cosf(225 *DEG_TO_RAD);
751+
quatosData.frameX << 1, cosf(45 *DEG_TO_RAD), 0, cosf(135 *DEG_TO_RAD), -1, cosf(225 *DEG_TO_RAD), 0, cosf(315 *DEG_TO_RAD);
750752
break;
751753
case CONFIG_OCTO_X:
752-
quatosData.frameX << cosf(247.5 *DEG_TO_RAD), cosf(292.5 *DEG_TO_RAD), cosf(337.5 *DEG_TO_RAD), cosf(22.5 *DEG_TO_RAD),
754+
quatosData.frameY << cosf(247.5 *DEG_TO_RAD), cosf(292.5 *DEG_TO_RAD), cosf(337.5 *DEG_TO_RAD), cosf(22.5 *DEG_TO_RAD),
753755
cosf(67.5 *DEG_TO_RAD), cosf(112.5 *DEG_TO_RAD), cosf(157.5 *DEG_TO_RAD), cosf(202.5 *DEG_TO_RAD);
754-
quatosData.frameY << cosf(337.5 *DEG_TO_RAD), cosf(22.5 *DEG_TO_RAD), cosf(67.5 *DEG_TO_RAD), cosf(112.5 *DEG_TO_RAD),
756+
quatosData.frameX << cosf(337.5 *DEG_TO_RAD), cosf(22.5 *DEG_TO_RAD), cosf(67.5 *DEG_TO_RAD), cosf(112.5 *DEG_TO_RAD),
755757
cosf(157.5 *DEG_TO_RAD), cosf(202.5 *DEG_TO_RAD), cosf(247.5 *DEG_TO_RAD), cosf(292.5 *DEG_TO_RAD);
756758
break;
757759
}
758760

759761
// calc GG offset & J matrix
760-
quatosToolObjCalc();
762+
quatosToolObjCalc();//Hope this is right!
761763

762-
quatosData.motorX = quatosData.frameX.transpose() * -1.0;
763-
quatosData.motorY = quatosData.frameY.transpose() * +1.0;
764+
//quatosData.motorX = quatosData.frameX.transpose() * -1.0;//-1 will be accounted for in MM creation!
765+
//quatosData.motorY = quatosData.frameY.transpose() * +1.0;
766+
quatosData.motorX = quatosData.frameX.transpose();
767+
quatosData.motorY = quatosData.frameY.transpose();
764768

765769
if (quatosData.craftType != CONFIG_CUSTOM) {
766770
quatosData.motorX *= quatosData.distMot;
@@ -781,9 +785,9 @@ std::cout << "quatosData.motorY: " << quatosData.motorY << std::endl;
781785
B.resize(3, 1);
782786

783787
// Roll
784-
A << quatosData.motorY,
788+
A << quatosData.motorX,//Changed from Y!!
785789
MatrixXd::Ones(1, quatosData.n),
786-
quatosData.motorX;
790+
-quatosData.motorY;
787791
B << 0,
788792
0,
789793
1;
@@ -792,9 +796,9 @@ std::cout << "quatosData.motorY: " << quatosData.motorY << std::endl;
792796
quatosData.ROLL *= B;
793797

794798
// Pitch
795-
A << quatosData.motorX,
799+
A << -quatosData.motorY,
796800
MatrixXd::Ones(1, quatosData.n),
797-
quatosData.motorY;
801+
quatosData.motorX;
798802
B << 0,
799803
0,
800804
1;
@@ -838,8 +842,8 @@ std::cout << "quatosData.motorY: " << quatosData.motorY << std::endl;
838842

839843
// M
840844
quatosData.M.resize(3, quatosData.n);
841-
quatosData.M << quatosData.motorX,
842-
quatosData.motorY,
845+
quatosData.M << -quatosData.motorY,
846+
quatosData.motorX,
843847
quatosData.propDir;
844848

845849
// Mt

‎quatos_example.xml

+8-8
Original file line numberDiff line numberDiff line change
@@ -88,10 +88,10 @@
8888
</craft>
8989
<craft id="quad_plus" config="custom" motors="4">
9090
<geometry>
91-
<motor rotation="+1" port="1">0,1</motor>
92-
<motor rotation="-1" port="2">1,0</motor>
93-
<motor rotation="+1" port="3">0,-1</motor>
94-
<motor rotation="-1" port="4">-1,0</motor>
91+
<motor rotation="+1" port="1">1,0</motor> //x axis runs from back to front, y axis runs from left to right.So front motor is 1,0
92+
<motor rotation="-1" port="2">0,1</motor> //
93+
<motor rotation="+1" port="3">-1,0</motor>
94+
<motor rotation="-1" port="4">0,-1</motor>
9595
</geometry>
9696
</craft>
9797
<craft id="octo_x" config="octo_x">
@@ -141,10 +141,10 @@
141141
<!-- example of a quad_plus entered with custom geometry -->
142142
<craft id="custom_craft" config="custom" motors="4">
143143
<geometry>
144-
<motor port="1" rotation="+1"> 0.00, +0.235</motor>
145-
<motor port="2" rotation="-1">+0.235, 0.00</motor>
146-
<motor port="8" rotation="+1"> 0.00, -0.235</motor>
147-
<motor port="9" rotation="-1">-0.235, 0.00</motor>
144+
<motor port="1" rotation="+1"> +0.235, 0.0</motor>
145+
<motor port="2" rotation="-1">0.0,.235</motor>
146+
<motor port="8" rotation="+1"> -.235,0.0</motor>
147+
<motor port="9" rotation="-1">0.0,-.235</motor>
148148
</geometry>
149149
<distance>
150150
<esc>0.117</esc>

0 commit comments

Comments
 (0)