private static FlightCapability MavTypeToFlightCapability(MAVLink.MAV_TYPE mavType) { switch (mavType) { case MAVLink.MAV_TYPE.FIXED_WING: return(FlightCapability.FixedWing); // There are a lot more types but these will do for now case MAVLink.MAV_TYPE.QUADROTOR: case MAVLink.MAV_TYPE.COAXIAL: case MAVLink.MAV_TYPE.HEXAROTOR: case MAVLink.MAV_TYPE.OCTOROTOR: case MAVLink.MAV_TYPE.TRICOPTER: case MAVLink.MAV_TYPE.DODECAROTOR: return(FlightCapability.Rotary); default: return(FlightCapability.Unspecified); } }
public static Motor[] build_motors(MAVLink.MAV_TYPE frame, int frame_orientation) { motors = new Motor[0]; if (frame == MAVLink.MAV_TYPE.HEXAROTOR) // y6 { // hard coded config for supported frames if (frame_orientation == AP_MOTORS_PLUS_FRAME) { // plus frame set-up add_motor(AP_MOTORS_MOT_1, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1); add_motor(AP_MOTORS_MOT_2, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4); add_motor(AP_MOTORS_MOT_3, -120, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5); add_motor(AP_MOTORS_MOT_4, 60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2); add_motor(AP_MOTORS_MOT_5, -60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6); add_motor(AP_MOTORS_MOT_6, 120, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3); } else { // X frame set-up add_motor(AP_MOTORS_MOT_1, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2); add_motor(AP_MOTORS_MOT_2, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5); add_motor(AP_MOTORS_MOT_3, -30, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6); add_motor(AP_MOTORS_MOT_4, 150, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3); add_motor(AP_MOTORS_MOT_5, 30, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1); add_motor(AP_MOTORS_MOT_6, -150, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4); } } else if (frame == MAVLink.MAV_TYPE.HEXAROTOR && false) // y6 { if (frame_orientation >= AP_MOTORS_NEW_PLUS_FRAME) { // Y6 motor definition with all top motors spinning clockwise, all bottom motors counter clockwise add_motor_raw(AP_MOTORS_MOT_1, -1.0, 0.500, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1); add_motor_raw(AP_MOTORS_MOT_2, -1.0, 0.500, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2); add_motor_raw(AP_MOTORS_MOT_3, 0.0, -1.000, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3); add_motor_raw(AP_MOTORS_MOT_4, 0.0, -1.000, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4); add_motor_raw(AP_MOTORS_MOT_5, 1.0, 0.500, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5); add_motor_raw(AP_MOTORS_MOT_6, 1.0, 0.500, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6); } else { // original Y6 motor definition add_motor_raw(AP_MOTORS_MOT_1, -1.0, 0.666, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2); add_motor_raw(AP_MOTORS_MOT_2, 1.0, 0.666, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5); add_motor_raw(AP_MOTORS_MOT_3, 1.0, 0.666, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6); add_motor_raw(AP_MOTORS_MOT_4, 0.0, -1.333, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4); add_motor_raw(AP_MOTORS_MOT_5, -1.0, 0.666, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1); add_motor_raw(AP_MOTORS_MOT_6, 0.0, -1.333, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3); } } else if (frame == MAVLink.MAV_TYPE.OCTOROTOR) { // hard coded config for supported frames if (frame_orientation == AP_MOTORS_PLUS_FRAME) { // plus frame set-up add_motor(AP_MOTORS_MOT_1, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1); add_motor(AP_MOTORS_MOT_2, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5); add_motor(AP_MOTORS_MOT_3, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2); add_motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4); add_motor(AP_MOTORS_MOT_5, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8); add_motor(AP_MOTORS_MOT_6, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6); add_motor(AP_MOTORS_MOT_7, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7); add_motor(AP_MOTORS_MOT_8, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3); } else if (frame_orientation == AP_MOTORS_V_FRAME) { // V frame set-up add_motor_raw(AP_MOTORS_MOT_1, 1.0, 0.34, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7); add_motor_raw(AP_MOTORS_MOT_2, -1.0, -0.32, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3); add_motor_raw(AP_MOTORS_MOT_3, 1.0, -0.32, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6); add_motor_raw(AP_MOTORS_MOT_4, -0.5, -1.0, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4); add_motor_raw(AP_MOTORS_MOT_5, 1.0, 1.0, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8); add_motor_raw(AP_MOTORS_MOT_6, -1.0, 0.34, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2); add_motor_raw(AP_MOTORS_MOT_7, -1.0, 1.0, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1); add_motor_raw(AP_MOTORS_MOT_8, 0.5, -1.0, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5); } else { // X frame set-up add_motor(AP_MOTORS_MOT_1, 22.5, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1); add_motor(AP_MOTORS_MOT_2, -157.5, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5); add_motor(AP_MOTORS_MOT_3, 67.5, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2); add_motor(AP_MOTORS_MOT_4, 157.5, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4); add_motor(AP_MOTORS_MOT_5, -22.5, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8); add_motor(AP_MOTORS_MOT_6, -112.5, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6); add_motor(AP_MOTORS_MOT_7, -67.5, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7); add_motor(AP_MOTORS_MOT_8, 112.5, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3); } } else if (frame == MAVLink.MAV_TYPE.OCTOROTOR && false) // octaquad { // hard coded config for supported frames if (frame_orientation == AP_MOTORS_PLUS_FRAME) { // plus frame set-up add_motor(AP_MOTORS_MOT_1, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1); add_motor(AP_MOTORS_MOT_2, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7); add_motor(AP_MOTORS_MOT_3, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5); add_motor(AP_MOTORS_MOT_4, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3); add_motor(AP_MOTORS_MOT_5, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8); add_motor(AP_MOTORS_MOT_6, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2); add_motor(AP_MOTORS_MOT_7, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4); add_motor(AP_MOTORS_MOT_8, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6); } else if (frame_orientation == AP_MOTORS_V_FRAME) { // V frame set-up add_motor(AP_MOTORS_MOT_1, 45, 0.7981, 1); add_motor(AP_MOTORS_MOT_2, -45, -0.7981, 7); add_motor(AP_MOTORS_MOT_3, -135, 1.0000, 5); add_motor(AP_MOTORS_MOT_4, 135, -1.0000, 3); add_motor(AP_MOTORS_MOT_5, -45, 0.7981, 8); add_motor(AP_MOTORS_MOT_6, 45, -0.7981, 2); add_motor(AP_MOTORS_MOT_7, 135, 1.0000, 4); add_motor(AP_MOTORS_MOT_8, -135, -1.0000, 6); } else if (frame_orientation == AP_MOTORS_H_FRAME) { // H frame set-up - same as X but motors spin in opposite directions add_motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1); add_motor(AP_MOTORS_MOT_2, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 7); add_motor(AP_MOTORS_MOT_3, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5); add_motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3); add_motor(AP_MOTORS_MOT_5, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 8); add_motor(AP_MOTORS_MOT_6, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2); add_motor(AP_MOTORS_MOT_7, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4); add_motor(AP_MOTORS_MOT_8, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6); } else { // X frame set-up add_motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1); add_motor(AP_MOTORS_MOT_2, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7); add_motor(AP_MOTORS_MOT_3, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5); add_motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3); add_motor(AP_MOTORS_MOT_5, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8); add_motor(AP_MOTORS_MOT_6, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2); add_motor(AP_MOTORS_MOT_7, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4); add_motor(AP_MOTORS_MOT_8, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6); } } else if (frame == MAVLink.MAV_TYPE.QUADROTOR) { // hard coded config for supported frames if (frame_orientation == AP_MOTORS_PLUS_FRAME) { // plus frame set-up add_motor(AP_MOTORS_MOT_1, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2); add_motor(AP_MOTORS_MOT_2, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4); add_motor(AP_MOTORS_MOT_3, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1); add_motor(AP_MOTORS_MOT_4, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3); } else if (frame_orientation == AP_MOTORS_V_FRAME) { // V frame set-up add_motor(AP_MOTORS_MOT_1, 45, 0.7981, 1); add_motor(AP_MOTORS_MOT_2, -135, 1.0000, 3); add_motor(AP_MOTORS_MOT_3, -45, -0.7981, 4); add_motor(AP_MOTORS_MOT_4, 135, -1.0000, 2); } else if (frame_orientation == AP_MOTORS_H_FRAME) { // H frame set-up - same as X but motors spin in opposite directiSons add_motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1); add_motor(AP_MOTORS_MOT_2, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3); add_motor(AP_MOTORS_MOT_3, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4); add_motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2); } else if (frame_orientation == AP_MOTORS_VTAIL_FRAME) { /* Lynxmotion Hunter Vtail 400/500 * * Roll control comes only from the front motors, Yaw control only from the rear motors * roll factor is measured by the angle perpendicular to that of the prop arm to the roll axis (x) * pitch factor is measured by the angle perpendicular to the prop arm to the pitch axis (y) * * assumptions: * 20 20 \ / 3_____________1 \ / | \ / | \ 40 \/ 40 20 | 20 \ Tail / \ \ 2 4 \ \ All angles measured from their closest axis \ \ Note: if we want the front motors to help with yaw, \ motors 1's yaw factor should be changed to sin(radians(40)). Where "40" is the vtail angle \ motors 3's yaw factor should be changed to -sin(radians(40)) */ // front right: 70 degrees right of roll axis, 20 degrees up of pitch axis, no yaw add_motor_raw(AP_MOTORS_MOT_1, cosf(radians(160)), cosf(radians(-70)), 0, 1); // back left: no roll, 70 degrees down of pitch axis, full yaw add_motor_raw(AP_MOTORS_MOT_2, 0, cosf(radians(160)), AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3); // front left: 70 degrees left of roll axis, 20 degrees up of pitch axis, no yaw add_motor_raw(AP_MOTORS_MOT_3, cosf(radians(20)), cosf(radians(70)), 0, 4); // back right: no roll, 70 degrees down of pitch axis, full yaw add_motor_raw(AP_MOTORS_MOT_4, 0, cosf(radians(-160)), AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2); } else { // X frame set-up add_motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1); add_motor(AP_MOTORS_MOT_2, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3); add_motor(AP_MOTORS_MOT_3, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4); add_motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2); } } else if (frame == MAVLink.MAV_TYPE.TRICOPTER) { if (frame_orientation >= AP_MOTORS_NEW_PLUS_FRAME) { // Y6 motor definition with all top motors spinning clockwise, all bottom motors counter clockwise add_motor_raw(AP_MOTORS_MOT_1, -1.0, 0.500, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1); add_motor_raw(AP_MOTORS_MOT_2, -1.0, 0.500, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2); add_motor_raw(AP_MOTORS_MOT_3, 0.0, -1.000, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3); add_motor_raw(AP_MOTORS_MOT_4, 0.0, -1.000, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4); add_motor_raw(AP_MOTORS_MOT_5, 1.0, 0.500, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5); add_motor_raw(AP_MOTORS_MOT_6, 1.0, 0.500, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6); } else { // original Y6 motor definition add_motor_raw(AP_MOTORS_MOT_1, -1.0, 0.666, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2); add_motor_raw(AP_MOTORS_MOT_2, 1.0, 0.666, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5); add_motor_raw(AP_MOTORS_MOT_3, 1.0, 0.666, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6); add_motor_raw(AP_MOTORS_MOT_4, 0.0, -1.333, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4); add_motor_raw(AP_MOTORS_MOT_5, -1.0, 0.666, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1); add_motor_raw(AP_MOTORS_MOT_6, 0.0, -1.333, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3); } } return(motors); }
private int get_motormax() { var motormax = 8; if (MainV2.comPort.MAV.aptype == MAVLink.MAV_TYPE.GROUND_ROVER) { return(4); } var enable = MainV2.comPort.MAV.param.ContainsKey("FRAME") || MainV2.comPort.MAV.param.ContainsKey("Q_FRAME_TYPE") || MainV2.comPort.MAV.param.ContainsKey("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: case 1: type = MAVLink.MAV_TYPE.QUADROTOR; break; case 2: case 5: type = MAVLink.MAV_TYPE.HEXAROTOR; break; case 3: case 4: type = MAVLink.MAV_TYPE.OCTOROTOR; break; case 6: type = MAVLink.MAV_TYPE.HELICOPTER; break; case 7: type = MAVLink.MAV_TYPE.TRICOPTER; break; } frame_type = (int)MainV2.comPort.MAV.param["Q_FRAME_TYPE"].Value; } else if (MainV2.comPort.MAV.param.ContainsKey("FRAME")) { type = MainV2.comPort.MAV.aptype; frame_type = (int)MainV2.comPort.MAV.param["FRAME"].Value; } else if (MainV2.comPort.MAV.param.ContainsKey("FRAME_TYPE")) { type = MainV2.comPort.MAV.aptype; frame_type = (int)MainV2.comPort.MAV.param["FRAME_TYPE"].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); }
private int get_motormax() { var motormax = 8; var enable = MainV2.comPort.MAV.param.ContainsKey("FRAME") || MainV2.comPort.MAV.param.ContainsKey("Q_FRAME_TYPE") || MainV2.comPort.MAV.param.ContainsKey("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 if (MainV2.comPort.MAV.param.ContainsKey("FRAME")) { type = MainV2.comPort.MAV.aptype; frame_type = (int)MainV2.comPort.MAV.param["FRAME"].Value; } else if (MainV2.comPort.MAV.param.ContainsKey("FRAME_TYPE")) { type = MainV2.comPort.MAV.aptype; frame_type = (int)MainV2.comPort.MAV.param["FRAME_TYPE"].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); if (frame_type == 0)//+字型机架 { pictureBox1.BackgroundImage = Properties.Resources._4_; } else if (frame_type == 1) { pictureBox1.BackgroundImage = Properties.Resources._4x; } else { pictureBox1.BackgroundImage = null; } } else if (type == MAVLink.MAV_TYPE.HEXAROTOR) { motormax = 6; motors = Motor.build_motors(MAVLink.MAV_TYPE.HEXAROTOR, frame_type); if (frame_type == 0)//+字型机架 { pictureBox1.BackgroundImage = Properties.Resources._6_; } else if (frame_type == 1) { pictureBox1.BackgroundImage = Properties.Resources._6x; } else { pictureBox1.BackgroundImage = null; } } else if (type == MAVLink.MAV_TYPE.OCTOROTOR) { motormax = 8; motors = Motor.build_motors(MAVLink.MAV_TYPE.OCTOROTOR, frame_type); if (frame_type == 0)//+字型机架 { pictureBox1.BackgroundImage = Properties.Resources._8_; } else if (frame_type == 1) { pictureBox1.BackgroundImage = Properties.Resources._8x; } else { pictureBox1.BackgroundImage = null; } } else if (type == MAVLink.MAV_TYPE.HELICOPTER) { motormax = 0; } return(motormax); }