From 6b3610ea70bbf6d2c20b530bb6d384d06116203f Mon Sep 17 00:00:00 2001 From: Jacob Walser Date: Tue, 13 Dec 2016 20:15:37 -0500 Subject: [PATCH] Sub: Set frame configuration with parameter instead of make target Now only one firmware is required for ArduSub --- ArduSub/APM_Config.h | 9 --------- ArduSub/GCS_Mavlink.cpp | 3 --- ArduSub/Log.cpp | 4 +--- ArduSub/Parameters.cpp | 15 ++++++++------- ArduSub/Parameters.h | 2 ++ ArduSub/Sub.h | 18 +----------------- ArduSub/config.h | 21 --------------------- ArduSub/defines.h | 17 ----------------- 8 files changed, 12 insertions(+), 77 deletions(-) diff --git a/ArduSub/APM_Config.h b/ArduSub/APM_Config.h index 61acb3bda0..c4fb1cdfcf 100644 --- a/ArduSub/APM_Config.h +++ b/ArduSub/APM_Config.h @@ -5,15 +5,6 @@ // If you used to define your CONFIG_APM_HARDWARE setting here, it is no longer // valid! You should switch to using a HAL_BOARD flag in your local config.mk. -//#define FRAME_CONFIG VECTORED_FRAME -/* options: - * BLUEROV_FRAME - * VECTORED_FRAME - * VECTORED6DOF_FRAME - * SIMPLEROV_FRAME - * VECTORED90_FRAME - */ - // uncomment the lines below to disable features (flash sizes listed are for APM2 boards and will underestimate savings on Pixhawk and other boards) //#define LOGGING_ENABLED DISABLED // disable dataflash logging to save 11K of flash space //#define MOUNT DISABLED // disable the camera gimbal to save 8K of flash space diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index 4bcbc14438..0b55c70608 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -1075,7 +1075,6 @@ void GCS_MAVLINK::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); handle_param_request_list(msg); break; } @@ -1602,8 +1601,6 @@ void GCS_MAVLINK::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); - // send system ID if we can char sysid[40]; if (hal.util->get_system_id(sysid)) { diff --git a/ArduSub/Log.cpp b/ArduSub/Log.cpp index 280d821f83..828d51d6fa 100644 --- a/ArduSub/Log.cpp +++ b/ArduSub/Log.cpp @@ -699,8 +699,7 @@ const struct LogStructure Sub::log_structure[] = { void Sub::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", + "\nFree RAM: %u\n", (unsigned) hal.util->available_memory()); cliSerial->println(HAL_BOARD_NAME); @@ -714,7 +713,6 @@ void Sub::Log_Read(uint16_t list_entry, uint16_t start_page, uint16_t end_page) void Sub::Log_Write_Vehicle_Startup_Messages() { // only 200(?) bytes are guaranteed by DataFlash - DataFlash.Log_Write_Message("Frame: " FRAME_CONFIG_STRING); DataFlash.Log_Write_Mode(control_mode, control_mode_reason); } diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp index 491924d65a..c7f4300a00 100644 --- a/ArduSub/Parameters.cpp +++ b/ArduSub/Parameters.cpp @@ -515,6 +515,14 @@ const AP_Param::Info Sub::var_info[] = { // @Range: 1000 2000 GSCALAR(cam_tilt_center, "CAM_CENTER", 1500), + // @Param: FRAME_CONFIG + // @DisplayName: Frame configuration + // @Description: Set this parameter according to your vehicle/motor configuration + // @User: Standard + // @RebootRequired: True + // @Values: 0:BlueROV1, 1:SimpleROV, 2:BlueROV2/Vectored, 3:Vectored6DOF, 4:Vectored90 + GSCALAR(frame_configuration, "FRAME_CONFIG", AS_MOTORS_VECTORED_FRAME), + // @Group: BTN0_ // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp GGROUP(jbtn_0, "BTN0_", JSButton), @@ -840,17 +848,10 @@ const AP_Param::Info Sub::var_info[] = { GOBJECT(rally, "RALLY_", AP_Rally), #endif -//#if (FRAME_CONFIG == VECTORED_FRAME || FRAME_CONFIG == BLUEROV_FRAME || FRAME_CONFIG == VECTORED6DOF_FRAME || FRAME_CONFIG == SIMPLEROV || FRAME_CONFIG == VECTORED90_FRAME) // @Group: MOT_ // @Path: ../libraries/AP_Motors/AP_Motors6DOF.cpp,../libraries/AP_Motors/AP_MotorsMulticopter.cpp GOBJECT(motors, "MOT_", AP_Motors6DOF), -//#else -// // @Group: MOT_ -// // @Path: ../libraries/AP_Motors/AP_MotorsMulticopter.cpp -// GOBJECT(motors, "MOT_", AP_MotorsMulticopter), -//#endif - #if RCMAP_ENABLED == ENABLED // @Group: RCMAP_ // @Path: ../libraries/AP_RCMapper/AP_RCMapper.cpp diff --git a/ArduSub/Parameters.h b/ArduSub/Parameters.h index e62f172e2f..451fe1393c 100644 --- a/ArduSub/Parameters.h +++ b/ArduSub/Parameters.h @@ -206,6 +206,7 @@ public: k_param_rc_feel_rp, k_param_throttle_gain, k_param_cam_tilt_center, + k_param_frame_configuration, // Acro Mode parameters k_param_acro_yaw_p = 220, // Used in all modes for get_pilot_desired_yaw_rate @@ -399,6 +400,7 @@ public: #endif AP_Float surface_depth; + AP_Int8 frame_configuration; // Note: keep initializers here in the same order as they are declared // above. diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index b7abe5a9ac..fba28f0f4d 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -299,23 +299,7 @@ private: uint8_t compass : 1; // true if compass is healthy } sensor_health; - // Motor Output -#if FRAME_CONFIG == BLUEROV_FRAME - #define MOTOR_CLASS AP_MotorsBlueROV6DOF -#elif FRAME_CONFIG == VECTORED_FRAME - #define MOTOR_CLASS AP_MotorsVectoredROV -#elif FRAME_CONFIG == VECTORED6DOF_FRAME - #define MOTOR_CLASS AP_MotorsVectored6DOF -#elif FRAME_CONFIG == SIMPLEROV_FRAME - #define MOTOR_CLASS AP_MotorsSimpleROV -#elif FRAME_CONFIG == VECTORED90_FRAME - #define MOTOR_CLASS AP_MotorsVectored90 - -#else - #error Unrecognised frame type -#endif - - MOTOR_CLASS motors; + AP_Motors6DOF motors; // GPS variables // Sometimes we need to remove the scaling for distance calcs diff --git a/ArduSub/config.h b/ArduSub/config.h index cc3c2c5a7a..bb1e11497a 100644 --- a/ArduSub/config.h +++ b/ArduSub/config.h @@ -54,27 +54,6 @@ # define MAIN_LOOP_SECONDS 0.0025f # define MAIN_LOOP_MICROS 2500 -////////////////////////////////////////////////////////////////////////////// -// FRAME_CONFIG -// -#ifndef FRAME_CONFIG - # define FRAME_CONFIG VECTORED6DOF_FRAME -#endif - -#if FRAME_CONFIG == BLUEROV_FRAME - # define FRAME_CONFIG_STRING "ROV_BLUEROV_FRAME" -#elif FRAME_CONFIG == VECTORED_FRAME - # define FRAME_CONFIG_STRING "ROV_VECTORED_FRAME" -#elif FRAME_CONFIG == VECTORED6DOF_FRAME - # define FRAME_CONFIG_STRING "ROV_VECTORED6DOF_FRAME" -#elif FRAME_CONFIG == SIMPLEROV_FRAME - # define FRAME_CONFIG_STRING "ROV_SIMPLEROV_FRAME" -#elif FRAME_CONFIG == VECTORED90_FRAME -# define FRAME_CONFIG_STRING "ROV_VECTORED90_FRAME" -#else - # define FRAME_CONFIG_STRING "UNKNOWN" -#endif - #ifndef SURFACE_DEPTH_DEFAULT # define SURFACE_DEPTH_DEFAULT -10.0f // pressure sensor reading 10cm depth means craft is considered surfaced #endif diff --git a/ArduSub/defines.h b/ArduSub/defines.h index 3f5911e8a2..fe545c79d8 100644 --- a/ArduSub/defines.h +++ b/ArduSub/defines.h @@ -86,23 +86,6 @@ enum aux_sw_func { AUXSW_RELAY4 = 36, // Relay4 pin on/off (in Mission planner set CH10_OPT = 36) }; -// Frame types -#define UNDEFINED_FRAME 0 -#define QUAD_FRAME 1 -#define TRI_FRAME 2 -#define HEXA_FRAME 3 -#define Y6_FRAME 4 -#define OCTA_FRAME 5 -#define HELI_FRAME 6 -#define OCTA_QUAD_FRAME 7 -#define SINGLE_FRAME 8 -#define COAX_FRAME 9 -#define BLUEROV_FRAME 10 -#define VECTORED_FRAME 11 -#define VECTORED6DOF_FRAME 12 -#define SIMPLEROV_FRAME 13 -#define VECTORED90_FRAME 14 - // HIL enumerations #define HIL_MODE_DISABLED 0 #define HIL_MODE_SENSORS 1