diff --git a/ArduPlane/config.h b/ArduPlane/config.h index 147a3cdf89..a423d8727c 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -446,10 +446,13 @@ ////////////////////////////////////////////////////////////////////////////// // MOUNT (ANTENNA OR CAMERA) // +// uses 4174 bytes of memory on 1280 chips (MNT_JSTICK_SPD_OPTION, MNT_RETRACT_OPTION, MNT_STABILIZE_OPTION and MNT_MOUNT2_OPTION disabled) +// uses 7726 bytes of memory on 2560 chips (all options are enabled) #ifndef MOUNT # define MOUNT ENABLED #endif +// second mount, can for example be used to keep an antenna pointed at the home position #ifndef MOUNT2 # define MOUNT2 DISABLED #endif @@ -470,6 +473,8 @@ # define GPS_PROTOCOL GPS_PROTOCOL_MTK # endif + # undef CAMERA + # define CAMERA DISABLED #endif diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 777aadeffd..98a7f8d839 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -5,6 +5,24 @@ #include #include +// Just so that it's completely clear... +#define ENABLED 1 +#define DISABLED 0 + +#if defined( __AVR_ATmega1280__ ) + # define MNT_JSTICK_SPD_OPTION DISABLED // Allow RC joystick to control the speed of the mount movements instead of the position of the mount + # define MNT_RETRACT_OPTION DISABLED // Use a servo to retract the mount inside the fuselage (i.e. for landings) + # define MNT_GPSPOINT_OPTION ENABLED // Point the mount to a GPS point defined via a mouse click in the Mission Planner GUI + # define MNT_STABILIZE_OPTION DISABLED // stabilize camera using frame attitude information + # define MNT_MOUNT2_OPTION DISABLED // second mount, can for example be used to keep an antenna pointed at the home position +#else + # define MNT_JSTICK_SPD_OPTION ENABLED // uses 844 bytes of memory + # define MNT_RETRACT_OPTION ENABLED // uses 244 bytes of memory + # define MNT_GPSPOINT_OPTION ENABLED // uses 580 bytes of memory + # define MNT_STABILIZE_OPTION ENABLED // uses 2424 bytes of memory + # define MNT_MOUNT2_OPTION ENABLED // uses 58 bytes of memory (must also be enabled in APM_Config.h) +#endif + const AP_Param::GroupInfo AP_Mount::var_info[] PROGMEM = { // @Param: MODE // @DisplayName: Mount operation mode @@ -13,6 +31,7 @@ const AP_Param::GroupInfo AP_Mount::var_info[] PROGMEM = { // @User: Standard AP_GROUPINFO("MODE", 0, AP_Mount, _mount_mode, MAV_MOUNT_MODE_RETRACT), // see MAV_MOUNT_MODE at ardupilotmega.h +#if MNT_RETRACT_OPTION == ENABLED // @Param: RETRACT // @DisplayName: Mount retract angles // @Description: Mount angles when in retract operation mode @@ -21,6 +40,7 @@ const AP_Param::GroupInfo AP_Mount::var_info[] PROGMEM = { // @Increment: 1 // @User: Standard AP_GROUPINFO("RETRACT", 1, AP_Mount, _retract_angles, 0), +#endif // @Param: NEUTRAL // @DisplayName: Mount neutral angles @@ -40,6 +60,7 @@ const AP_Param::GroupInfo AP_Mount::var_info[] PROGMEM = { // @User: Standard AP_GROUPINFO("CONTROL", 3, AP_Mount, _control_angles, 0), +#if MNT_STABILIZE_OPTION == ENABLED // @Param: STAB_ROLL // @DisplayName: Stabilize mount roll // @Description:enable roll stabilisation relative to Earth @@ -60,6 +81,7 @@ const AP_Param::GroupInfo AP_Mount::var_info[] PROGMEM = { // @Values: 0:Disabled,1:Enabled // @User: Standard AP_GROUPINFO("STAB_PAN", 6, AP_Mount, _stab_pan, 0), +#endif // @Param: RC_IN_ROLL // @DisplayName: roll RC input channel @@ -136,6 +158,7 @@ const AP_Param::GroupInfo AP_Mount::var_info[] PROGMEM = { // @User: Standard AP_GROUPINFO("ANGMAX_PAN", 15, AP_Mount, _pan_angle_max, 4500), +#if MNT_JSTICK_SPD_OPTION == ENABLED // @Param: JSTICK_SPD // @DisplayName: mount joystick speed // @Description: 0 for position control, small for low speeds, 10 for max speed @@ -143,6 +166,7 @@ const AP_Param::GroupInfo AP_Mount::var_info[] PROGMEM = { // @Increment: 1 // @User: Standard AP_GROUPINFO("JSTICK_SPD", 16, AP_Mount, _joystick_speed, 0), +#endif AP_GROUPEND }; @@ -164,17 +188,21 @@ AP_Mount::AP_Mount(const struct Location *current_loc, GPS *&gps, AP_AHRS *ahrs, // default unknown mount type _mount_type = k_unknown; +#if MNT_MOUNT2_OPTION == ENABLED if (id == 0) { +#endif _roll_idx = RC_Channel_aux::k_mount_roll; _tilt_idx = RC_Channel_aux::k_mount_tilt; _pan_idx = RC_Channel_aux::k_mount_pan; _open_idx = RC_Channel_aux::k_mount_open; +#if MNT_MOUNT2_OPTION == ENABLED } else { _roll_idx = RC_Channel_aux::k_mount2_roll; _tilt_idx = RC_Channel_aux::k_mount2_tilt; _pan_idx = RC_Channel_aux::k_mount2_pan; _open_idx = RC_Channel_aux::k_mount2_open; } +#endif } /// Auto-detect the mount gimbal type depending on the functions assigned to the servos @@ -226,6 +254,7 @@ void AP_Mount::update_mount_position() switch((enum MAV_MOUNT_MODE)_mount_mode.get()) { +#if MNT_RETRACT_OPTION == ENABLED // move mount to a "retracted position" or to a position where a fourth servo can retract the entire mount into the fuselage case MAV_MOUNT_MODE_RETRACT: { @@ -235,6 +264,7 @@ void AP_Mount::update_mount_position() _pan_angle = vec.z; break; } +#endif // move mount to a neutral position, typically pointing forward case MAV_MOUNT_MODE_NEUTRAL: @@ -260,6 +290,7 @@ void AP_Mount::update_mount_position() // RC radio manual angle control, but with stabilization from the AHRS case MAV_MOUNT_MODE_RC_TARGETING: { +#if MNT_JSTICK_SPD_OPTION == ENABLED if (_joystick_speed) { // for spring loaded joysticks // allow pilot speed position input to come directly from an RC_Channel if (_roll_rc_in && (rc_ch[_roll_rc_in-1])) { @@ -281,6 +312,7 @@ void AP_Mount::update_mount_position() if (_pan_control_angle > radians(_pan_angle_max*0.01)) _pan_control_angle = radians(_pan_angle_max*0.01); } } else { +#endif // allow pilot position input to come directly from an RC_Channel if (_roll_rc_in && (rc_ch[_roll_rc_in-1])) { _roll_control_angle = angle_input_rad(rc_ch[_roll_rc_in-1], _roll_angle_min, _roll_angle_max); @@ -291,11 +323,14 @@ void AP_Mount::update_mount_position() if (_pan_rc_in && (rc_ch[_pan_rc_in-1])) { _pan_control_angle = angle_input_rad(rc_ch[_pan_rc_in-1], _pan_angle_min, _pan_angle_max); } +#if MNT_JSTICK_SPD_OPTION == ENABLED } +#endif stabilize(); break; } +#if MNT_GPSPOINT_OPTION == ENABLED // point mount to a GPS point given by the mission planner case MAV_MOUNT_MODE_GPS_POINT: { @@ -305,11 +340,14 @@ void AP_Mount::update_mount_position() } break; } +#endif + default: //do nothing break; } +#if MNT_RETRACT_OPTION == ENABLED // move mount to a "retracted position" into the fuselage with a fourth servo if (g_rc_function[_open_idx]) { bool mount_open_new = (enum MAV_MOUNT_MODE)_mount_mode.get()==MAV_MOUNT_MODE_RETRACT ? 0 : 1; @@ -318,6 +356,7 @@ void AP_Mount::update_mount_position() move_servo(g_rc_function[_open_idx], mount_open_new, 0, 1); } } +#endif // write the results to the servos move_servo(g_rc_function[_roll_idx], _roll_angle*10, _roll_angle_min*0.1, _roll_angle_max*0.1); @@ -359,6 +398,7 @@ void AP_Mount::control_msg(mavlink_message_t *msg) switch ((enum MAV_MOUNT_MODE)_mount_mode.get()) { +#if MNT_RETRACT_OPTION == ENABLED case MAV_MOUNT_MODE_RETRACT: // Load and keep safe position (Roll,Pitch,Yaw) from EEPROM and stop stabilization set_retract_angles(packet.input_b*0.01, packet.input_a*0.01, packet.input_c*0.01); if (packet.save_position) @@ -366,6 +406,7 @@ void AP_Mount::control_msg(mavlink_message_t *msg) _retract_angles.save(); } break; +#endif case MAV_MOUNT_MODE_NEUTRAL: // Load and keep neutral position (Roll,Pitch,Yaw) from EEPROM set_neutral_angles(packet.input_b*0.01, packet.input_a*0.01, packet.input_c*0.01); @@ -388,6 +429,7 @@ void AP_Mount::control_msg(mavlink_message_t *msg) } break; +#if MNT_GPSPOINT_OPTION == ENABLED case MAV_MOUNT_MODE_GPS_POINT: // Load neutral position and start to point to Lat,Lon,Alt Location targetGPSLocation; targetGPSLocation.lat = packet.input_a; @@ -395,6 +437,7 @@ void AP_Mount::control_msg(mavlink_message_t *msg) targetGPSLocation.alt = packet.input_c; set_GPS_target_location(targetGPSLocation); break; +#endif case MAV_MOUNT_MODE_ENUM_END: break; @@ -426,11 +469,13 @@ void AP_Mount::status_msg(mavlink_message_t *msg) packet.pointing_a = _tilt_angle*100; // degrees*100 packet.pointing_c = _pan_angle*100; // degrees*100 break; +#if MNT_GPSPOINT_OPTION == ENABLED case MAV_MOUNT_MODE_GPS_POINT: // neutral position and start to point to Lat,Lon,Alt packet.pointing_a = _target_GPS_location.lat; // latitude packet.pointing_b = _target_GPS_location.lng; // longitude packet.pointing_c = _target_GPS_location.alt; // altitude break; +#endif case MAV_MOUNT_MODE_ENUM_END: break; } @@ -444,11 +489,13 @@ void AP_Mount::status_msg(mavlink_message_t *msg) /// Set mount point/region of interest, triggered by mission script commands void AP_Mount::set_roi_cmd(struct Location *target_loc) { +#if MNT_GPSPOINT_OPTION == ENABLED // set the target gps location _target_GPS_location = *target_loc; // set the mode to GPS tracking mode set_mode(MAV_MOUNT_MODE_GPS_POINT); +#endif } /// Set mount configuration, triggered by mission script commands @@ -501,6 +548,7 @@ AP_Mount::calc_GPS_target_angle(struct Location *target) void AP_Mount::stabilize() { +#if MNT_STABILIZE_OPTION == ENABLED if (_ahrs) { // only do the full 3D frame transform if we are doing pan control if (_stab_pan) { @@ -529,10 +577,13 @@ AP_Mount::stabilize() } } } else { +#endif _roll_angle = degrees(_roll_control_angle); _tilt_angle = degrees(_tilt_control_angle); _pan_angle = degrees(_pan_control_angle); +#if MNT_STABILIZE_OPTION == ENABLED } +#endif } /* * /// For testing and development. Called in the medium loop.