public Motor(double angle, bool clockwise, double servo)
{
self = this;
self.angle = angle;
self.clockwise = clockwise;
self.servo = servo;
}
public Motor(double angle, bool clockwise, double servo, int testing_order)
{
self = this;
self.angle = (angle + 360) % 360;
self.clockwise = clockwise;
self.servo = servo;
self.testing_order = testing_order;
}
/*
#if (FRAME_CONFIG == QUAD_FRAME)
MAV_TYPE_QUADROTOR,
#elif (FRAME_CONFIG == TRI_FRAME)
MAV_TYPE_TRICOPTER,
#elif (FRAME_CONFIG == HEXA_FRAME || FRAME_CONFIG == Y6_FRAME)
MAV_TYPE_HEXAROTOR,
#elif (FRAME_CONFIG == OCTA_FRAME || FRAME_CONFIG == OCTA_QUAD_FRAME)
MAV_TYPE_OCTOROTOR,
#elif (FRAME_CONFIG == HELI_FRAME)
MAV_TYPE_HELICOPTER,
#elif (FRAME_CONFIG == SINGLE_FRAME) //because mavlink did not define a singlecopter, we use a rocket
MAV_TYPE_ROCKET,
#elif (FRAME_CONFIG == COAX_FRAME) //because mavlink did not define a singlecopter, we use a rocket
MAV_TYPE_ROCKET,
#else
#error Unrecognised frame type
#endif*/
public void Activate()
{
var x = 20;
var y = 40;
var motormax = 8;
if (!MainV2.comPort.MAV.param.ContainsKey("FRAME"))
{
Enabled = false;
return;
}
var motors = new Motor[0];
if (MainV2.comPort.MAV.aptype == MAVLink.MAV_TYPE.TRICOPTER)
{
motormax = 4;
motors = Motor.build_motors(MAVLink.MAV_TYPE.TRICOPTER, (int) (float) MainV2.comPort.MAV.param["FRAME"]);
}
else if (MainV2.comPort.MAV.aptype == MAVLink.MAV_TYPE.QUADROTOR)
{
motormax = 4;
motors = Motor.build_motors(MAVLink.MAV_TYPE.QUADROTOR, (int) (float) MainV2.comPort.MAV.param["FRAME"]);
}
else if (MainV2.comPort.MAV.aptype == MAVLink.MAV_TYPE.HEXAROTOR)
{
motormax = 6;
motors = Motor.build_motors(MAVLink.MAV_TYPE.HEXAROTOR, (int) (float) MainV2.comPort.MAV.param["FRAME"]);
}
else if (MainV2.comPort.MAV.aptype == MAVLink.MAV_TYPE.OCTOROTOR)
{
motormax = 8;
motors = Motor.build_motors(MAVLink.MAV_TYPE.OCTOROTOR, (int) (float) MainV2.comPort.MAV.param["FRAME"]);
}
else if (MainV2.comPort.MAV.aptype == MAVLink.MAV_TYPE.HELICOPTER)
{
motormax = 0;
}
for (var a = 1; a <= motormax; a++)
{
var but = new MyButton();
but.Text = "Test motor " + (char) ((a - 1) + 'A');
but.Location = new Point(x, y);
but.Click += but_Click;
but.Tag = a;
Controls.Add(but);
y += 25;
}
}
private int get_motormax()
{
var motormax = 8;
if (!MainV2.comPort.MAV.param.ContainsKey("FRAME"))
{
Enabled = false;
return motormax;
}
var motors = new Motor[0];
if (MainV2.comPort.MAV.aptype == MAVLink.MAV_TYPE.TRICOPTER)
{
motormax = 4;
motors = Motor.build_motors(MAVLink.MAV_TYPE.TRICOPTER, (int)(double)MainV2.comPort.MAV.param["FRAME"]);
}
else if (MainV2.comPort.MAV.aptype == MAVLink.MAV_TYPE.QUADROTOR)
{
motormax = 4;
motors = Motor.build_motors(MAVLink.MAV_TYPE.QUADROTOR, (int)(double)MainV2.comPort.MAV.param["FRAME"]);
}
else if (MainV2.comPort.MAV.aptype == MAVLink.MAV_TYPE.HEXAROTOR)
{
motormax = 6;
motors = Motor.build_motors(MAVLink.MAV_TYPE.HEXAROTOR, (int)(double)MainV2.comPort.MAV.param["FRAME"]);
}
else if (MainV2.comPort.MAV.aptype == MAVLink.MAV_TYPE.OCTOROTOR)
{
motormax = 8;
motors = Motor.build_motors(MAVLink.MAV_TYPE.OCTOROTOR, (int)(double)MainV2.comPort.MAV.param["FRAME"]);
}
else if (MainV2.comPort.MAV.aptype == MAVLink.MAV_TYPE.HELICOPTER)
{
motormax = 0;
}
return motormax;
}
private static void add_motor_raw(int motor_num, double roll_fac, double pitch_fac, double yaw_fac, int testing_order)
{
if (motors.Length < (motor_num + 1))
{
Array.Resize(ref motors, motor_num + 1);
}
motors[motor_num] = new Motor(Math.Atan2(-roll_fac, pitch_fac) * rad2deg, yaw_fac > 0, motor_num, testing_order);
}
private int get_motormax()
{
var motormax = 8;
var enable = MainV2.comPort.MAV.param.ContainsKey("FRAME") || MainV2.comPort.MAV.param.ContainsKey("Q_FRAME_TYPE");
if (!enable)
{
Enabled = false;
return motormax;
}
MAVLink.MAV_TYPE type = MAVLink.MAV_TYPE.QUADROTOR;
int frame_type = 0; // + frame
if (MainV2.comPort.MAV.param.ContainsKey("Q_FRAME_CLASS"))
{
var value = (int)MainV2.comPort.MAV.param["Q_FRAME_CLASS"].Value;
switch (value)
{
case 0:
type = MAVLink.MAV_TYPE.QUADROTOR;
break;
case 1:
type = MAVLink.MAV_TYPE.HEXAROTOR;
break;
case 2:
type = MAVLink.MAV_TYPE.OCTOROTOR;
break;
case 3:
type = MAVLink.MAV_TYPE.OCTOROTOR;
break;
}
frame_type = (int)MainV2.comPort.MAV.param["Q_FRAME_TYPE"].Value;
}
else
{
type = MainV2.comPort.MAV.aptype;
frame_type = (int)MainV2.comPort.MAV.param["FRAME"].Value;
}
var motors = new Motor[0];
if (type == MAVLink.MAV_TYPE.TRICOPTER)
{
motormax = 4;
motors = Motor.build_motors(MAVLink.MAV_TYPE.TRICOPTER, frame_type);
}
else if (type == MAVLink.MAV_TYPE.QUADROTOR)
{
motormax = 4;
motors = Motor.build_motors(MAVLink.MAV_TYPE.QUADROTOR, frame_type);
}
else if (type == MAVLink.MAV_TYPE.HEXAROTOR)
{
motormax = 6;
motors = Motor.build_motors(MAVLink.MAV_TYPE.HEXAROTOR, frame_type);
}
else if (type == MAVLink.MAV_TYPE.OCTOROTOR)
{
motormax = 8;
motors = Motor.build_motors(MAVLink.MAV_TYPE.OCTOROTOR, frame_type);
}
else if (type == MAVLink.MAV_TYPE.HELICOPTER)
{
motormax = 0;
}
return motormax;
}
