Copter: add FRAME_CLASS parameter

ArduCopter/ArduCopter.cpp

@@ -489,10 +489,10 @@ void Copter::one_hz_loop()
 
         update_using_interlock();
 
-#if FRAME_CONFIG != HELI_FRAME
-        // check the user hasn't updated the frame orientation
-        motors.set_frame_orientation(g.frame_orientation);
+        // check the user hasn't updated the frame class or type
+        motors.set_frame_class_and_type((AP_Motors::motor_frame_class)g2.frame_class.get(), (AP_Motors::motor_frame_type)g.frame_orientation.get());
 
+#if FRAME_CONFIG != HELI_FRAME
         // set all throttle channel settings
         motors.set_throttle_range(channel_throttle->get_radio_min(), channel_throttle->get_radio_max());
 #endif

ArduCopter/Copter.h

@@ -325,36 +325,11 @@ private:
         uint8_t compass     : 1;    // true if compass is healthy
     } sensor_health;
 
-    // setup FRAME_MAV_TYPE
-#if (FRAME_CONFIG == QUAD_FRAME)
- #define FRAME_MAV_TYPE MAV_TYPE_QUADROTOR
-#elif (FRAME_CONFIG == TRI_FRAME)
- #define FRAME_MAV_TYPE MAV_TYPE_TRICOPTER
-#elif (FRAME_CONFIG == HEXA_FRAME || FRAME_CONFIG == Y6_FRAME)
- #define FRAME_MAV_TYPE MAV_TYPE_HEXAROTOR
-#elif (FRAME_CONFIG == OCTA_FRAME || FRAME_CONFIG == OCTA_QUAD_FRAME)
- #define FRAME_MAV_TYPE MAV_TYPE_OCTOROTOR
-#elif (FRAME_CONFIG == HELI_FRAME)
- #define FRAME_MAV_TYPE MAV_TYPE_HELICOPTER
-#elif (FRAME_CONFIG == SINGLE_FRAME || FRAME_CONFIG == COAX_FRAME)  //because mavlink did not define a singlecopter, we use a quad
- #define FRAME_MAV_TYPE MAV_TYPE_QUADROTOR
-#else
- #error Unrecognised frame type
-#endif
-
     // Motor Output
-#if FRAME_CONFIG == QUAD_FRAME
- #define MOTOR_CLASS AP_MotorsQuad
+#if FRAME_CONFIG == QUAD_FRAME || FRAME_CONFIG == HEXA_FRAME || FRAME_CONFIG == Y6_FRAME || FRAME_CONFIG == OCTA_FRAME || FRAME_CONFIG == OCTA_QUAD_FRAME
+ #define MOTOR_CLASS AP_MotorsMatrix
 #elif FRAME_CONFIG == TRI_FRAME
  #define MOTOR_CLASS AP_MotorsTri
-#elif FRAME_CONFIG == HEXA_FRAME
- #define MOTOR_CLASS AP_MotorsHexa
-#elif FRAME_CONFIG == Y6_FRAME
- #define MOTOR_CLASS AP_MotorsY6
-#elif FRAME_CONFIG == OCTA_FRAME
- #define MOTOR_CLASS AP_MotorsOcta
-#elif FRAME_CONFIG == OCTA_QUAD_FRAME
- #define MOTOR_CLASS AP_MotorsOctaQuad
 #elif FRAME_CONFIG == HELI_FRAME
  #define MOTOR_CLASS AP_MotorsHeli_Single
 #elif FRAME_CONFIG == SINGLE_FRAME
@@ -1108,6 +1083,8 @@ private:
     void update_auto_armed();
     void check_usb_mux(void);
     bool should_log(uint32_t mask);
+    uint8_t get_frame_mav_type();
+    const char* get_frame_string();
     bool current_mode_has_user_takeoff(bool must_navigate);
     bool do_user_takeoff(float takeoff_alt_cm, bool must_navigate);
     void takeoff_timer_start(float alt_cm);

ArduCopter/GCS_Mavlink.cpp

@@ -78,7 +78,7 @@ NOINLINE void Copter::send_heartbeat(mavlink_channel_t chan)
     // indicate we have set a custom mode
     base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
 
-    gcs[chan-MAVLINK_COMM_0].send_heartbeat(FRAME_MAV_TYPE,
+    gcs[chan-MAVLINK_COMM_0].send_heartbeat(get_frame_mav_type(),
                                             base_mode,
                                             custom_mode,
                                             system_status);
@@ -892,7 +892,7 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
 #if defined(PX4_GIT_VERSION) && defined(NUTTX_GIT_VERSION)
         send_text(MAV_SEVERITY_INFO, "PX4: " PX4_GIT_VERSION " NuttX: " NUTTX_GIT_VERSION);
 #endif
-        send_text(MAV_SEVERITY_INFO, "Frame: " FRAME_CONFIG_STRING);
+        GCS_MAVLINK::send_statustext_chan(MAV_SEVERITY_INFO, chan, "Frame: %s", copter.get_frame_string());
         handle_param_request_list(msg);
         break;
     }
@@ -1457,7 +1457,7 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
             send_text(MAV_SEVERITY_INFO, "PX4: " PX4_GIT_VERSION " NuttX: " NUTTX_GIT_VERSION);
             #endif
 
-            send_text(MAV_SEVERITY_INFO, "Frame: " FRAME_CONFIG_STRING);
+            GCS_MAVLINK::send_statustext_chan(MAV_SEVERITY_INFO, chan, "Frame: %s", copter.get_frame_string());
 
             // send system ID if we can
             char sysid[40];

ArduCopter/Log.cpp

@@ -903,9 +903,10 @@ const struct LogStructure Copter::log_structure[] = {
 void Copter::Log_Read(uint16_t list_entry, uint16_t start_page, uint16_t end_page)
 {
     cliSerial->printf("\n" FIRMWARE_STRING
-                             "\nFree RAM: %u\n"
-                             "\nFrame: " FRAME_CONFIG_STRING "\n",
-                        (unsigned) hal.util->available_memory());
+                        "\nFree RAM: %u\n"
+                        "\nFrame: %s\n",
+                        (unsigned) hal.util->available_memory(),
+                        get_frame_string());
 
     cliSerial->println(HAL_BOARD_NAME);
 
@@ -918,7 +919,9 @@ void Copter::Log_Read(uint16_t list_entry, uint16_t start_page, uint16_t end_pag
 void Copter::Log_Write_Vehicle_Startup_Messages()
 {
     // only 200(?) bytes are guaranteed by DataFlash
-    DataFlash.Log_Write_Message("Frame: " FRAME_CONFIG_STRING);
+    char frame_buf[20];
+    sprintf(frame_buf, "Frame: %s", get_frame_string());
+    DataFlash.Log_Write_Message(frame_buf);
     DataFlash.Log_Write_Mode(control_mode, control_mode_reason);
     DataFlash.Log_Write_Rally(rally);
 }

ArduCopter/Parameters.cpp

@@ -390,7 +390,7 @@ const AP_Param::Info Copter::var_info[] = {
     // @Description: Controls motor mixing for multicopters.  Not used for Tri or Traditional Helicopters.
     // @Values: 0:Plus, 1:X, 2:V, 3:H, 4:V-Tail, 5:A-Tail, 10:Y6B (New)
     // @User: Standard
-    GSCALAR(frame_orientation, "FRAME",             AP_MOTORS_X_FRAME),
+    GSCALAR(frame_orientation, "FRAME",             AP_Motors::MOTOR_FRAME_TYPE_X),
 
     // @Param: CH7_OPT
     // @DisplayName: Channel 7 option
@@ -1043,6 +1043,13 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @Path: ../libraries/AP_Beacon/AP_Beacon.cpp
     AP_SUBGROUPINFO(beacon, "BCN", 14, ParametersG2, AP_Beacon),
 
+    // @Param: FRAME_CLASS
+    // @DisplayName: Frame Class
+    // @Description: Controls major frame class for multicopter component
+    // @Values: 0:Undefined, 1:Quad, 2:Hexa, 3:Octa, 4:OctaQuad, 5:Y6, 6:Heli, 7:Tri, 8:SingleCopter, 9:CoaxCopter
+    // @User: Standard
+    AP_GROUPINFO("FRAME_CLASS", 15, ParametersG2, frame_class, 0),
+
     AP_GROUPEND
 };
 
@@ -1168,7 +1175,7 @@ void Copter::convert_pid_parameters(void)
 
 #if FRAME_CONFIG == QUAD_FRAME || FRAME_CONFIG == HEXA_FRAME || FRAME_CONFIG == Y6_FRAME  || FRAME_CONFIG == OCTA_FRAME || FRAME_CONFIG == OCTA_QUAD_FRAME
     // Multicopter x-frame gains are 40% lower because -1 or +1 input to motors now results in maximum rotation
-    if (g.frame_orientation == AP_MOTORS_X_FRAME || g.frame_orientation == AP_MOTORS_V_FRAME || g.frame_orientation == AP_MOTORS_H_FRAME) {
+    if (g.frame_orientation == AP_Motors::MOTOR_FRAME_TYPE_X || g.frame_orientation == AP_Motors::MOTOR_FRAME_TYPE_V || g.frame_orientation == AP_Motors::MOTOR_FRAME_TYPE_H) {
         pid_scaler = 0.9f;
     }
 #endif

ArduCopter/Parameters.h

@@ -587,6 +587,9 @@ public:
     // acro exponent parameters
     AP_Float acro_y_expo;
     AP_Float acro_thr_mid;
+
+    // frame class
+    AP_Int8 frame_class;
 };
 
 extern const AP_Param::Info        var_info[];

ArduCopter/config.h

@@ -64,28 +64,6 @@
  # define FRAME_CONFIG   QUAD_FRAME
 #endif
 
-#if FRAME_CONFIG == QUAD_FRAME
- # define FRAME_CONFIG_STRING "QUAD"
-#elif FRAME_CONFIG == TRI_FRAME
- # define FRAME_CONFIG_STRING "TRI"
-#elif FRAME_CONFIG == HEXA_FRAME
- # define FRAME_CONFIG_STRING "HEXA"
-#elif FRAME_CONFIG == Y6_FRAME
- # define FRAME_CONFIG_STRING "Y6"
-#elif FRAME_CONFIG == OCTA_FRAME
- # define FRAME_CONFIG_STRING "OCTA"
-#elif FRAME_CONFIG == OCTA_QUAD_FRAME
- # define FRAME_CONFIG_STRING "OCTA_QUAD"
-#elif FRAME_CONFIG == HELI_FRAME
- # define FRAME_CONFIG_STRING "HELI"
-#elif FRAME_CONFIG == SINGLE_FRAME
- # define FRAME_CONFIG_STRING "SINGLE"
-#elif FRAME_CONFIG == COAX_FRAME
- # define FRAME_CONFIG_STRING "COAX"
-#else
- # define FRAME_CONFIG_STRING "UNKNOWN"
-#endif
-
 /////////////////////////////////////////////////////////////////////////////////
 // TradHeli defaults
 #if FRAME_CONFIG == HELI_FRAME

ArduCopter/radio.cpp

@@ -53,9 +53,8 @@ void Copter::init_rc_in()
 void Copter::init_rc_out()
 {
     motors.set_update_rate(g.rc_speed);
-    motors.set_frame_orientation(g.frame_orientation);
     motors.set_loop_rate(scheduler.get_loop_rate_hz());
-    motors.Init();                                              // motor initialisation
+    motors.init((AP_Motors::motor_frame_class)g2.frame_class.get(), (AP_Motors::motor_frame_type)g.frame_orientation.get());
 #if FRAME_CONFIG != HELI_FRAME
     motors.set_throttle_range(channel_throttle->get_radio_min(), channel_throttle->get_radio_max());
 #endif

ArduCopter/setup.cpp

@@ -316,20 +316,7 @@ void Copter::report_frame()
 {
     cliSerial->println("Frame");
     print_divider();
-
- #if FRAME_CONFIG == QUAD_FRAME
-    cliSerial->println("Quad frame");
- #elif FRAME_CONFIG == TRI_FRAME
-    cliSerial->println("TRI frame");
- #elif FRAME_CONFIG == HEXA_FRAME
-    cliSerial->println("Hexa frame");
- #elif FRAME_CONFIG == Y6_FRAME
-    cliSerial->println("Y6 frame");
- #elif FRAME_CONFIG == OCTA_FRAME
-    cliSerial->println("Octa frame");
- #elif FRAME_CONFIG == HELI_FRAME
-    cliSerial->println("Heli frame");
- #endif
+    cliSerial->println(get_frame_string());
 
     print_blanks(2);
 }

ArduCopter/system.cpp

@@ -157,8 +157,10 @@ void Copter::init_ardupilot()
 
 #if FRSKY_TELEM_ENABLED == ENABLED
     // setup frsky, and pass a number of parameters to the library
-    frsky_telemetry.init(serial_manager, FIRMWARE_STRING " " FRAME_CONFIG_STRING,
-                         FRAME_MAV_TYPE,
+    char firmware_buf[40];
+    sprintf(firmware_buf, FIRMWARE_STRING " %s", get_frame_string());
+    frsky_telemetry.init(serial_manager, firmware_buf,
+                         get_frame_mav_type(),
                          &g.fs_batt_voltage, &g.fs_batt_mah, &ap.value);
 #endif
 
@@ -457,3 +459,59 @@ bool Copter::should_log(uint32_t mask)
     return false;
 #endif
 }
+
+// return MAV_TYPE corresponding to frame class
+uint8_t Copter::get_frame_mav_type()
+{
+    switch ((AP_Motors::motor_frame_class)g2.frame_class.get()) {
+        case AP_Motors::MOTOR_FRAME_QUAD:
+            return MAV_TYPE_QUADROTOR;
+        case AP_Motors::MOTOR_FRAME_HEXA:
+        case AP_Motors::MOTOR_FRAME_Y6:
+            return MAV_TYPE_HEXAROTOR;
+        case AP_Motors::MOTOR_FRAME_OCTA:
+        case AP_Motors::MOTOR_FRAME_OCTAQUAD:
+            return MAV_TYPE_OCTOROTOR;
+        case AP_Motors::MOTOR_FRAME_HELI:
+            return MAV_TYPE_HELICOPTER;
+        case AP_Motors::MOTOR_FRAME_TRI:
+            return MAV_TYPE_TRICOPTER;
+        case AP_Motors::MOTOR_FRAME_SINGLE:
+        case AP_Motors::MOTOR_FRAME_COAX:
+            return MAV_TYPE_COAXIAL;
+        case AP_Motors::MOTOR_FRAME_UNDEFINED:
+        default:
+            return MAV_TYPE_GENERIC;
+    }
+    // we should never get this far
+    return MAV_TYPE_GENERIC;
+}
+
+// return string corresponding to frame_class
+const char* Copter::get_frame_string()
+{
+    switch ((AP_Motors::motor_frame_class)g2.frame_class.get()) {
+        case AP_Motors::MOTOR_FRAME_QUAD:
+            return "QUAD";
+        case AP_Motors::MOTOR_FRAME_HEXA:
+            return "HEXA";
+        case AP_Motors::MOTOR_FRAME_Y6:
+            return "Y6";
+        case AP_Motors::MOTOR_FRAME_OCTA:
+            return "OCTA";
+        case AP_Motors::MOTOR_FRAME_OCTAQUAD:
+            return "OCTA_QUAD";
+        case AP_Motors::MOTOR_FRAME_HELI:
+            return "HELI";
+        case AP_Motors::MOTOR_FRAME_TRI:
+            return "TRI";
+        case AP_Motors::MOTOR_FRAME_SINGLE:
+            return "SINGLE";
+        case AP_Motors::MOTOR_FRAME_COAX:
+            return "COAX";
+        case AP_Motors::MOTOR_FRAME_UNDEFINED:
+        default:
+            return "UNKNOWN";
+    }
+}
+