diff --git a/ArduCopter/config.h b/ArduCopter/config.h index d7066f389de70..3cfdf0ceb4c35 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -304,7 +304,7 @@ ////////////////////////////////////////////////////////////////////////////// // Ship Operations Supoort - support for landing relative to a beacon #ifndef MODE_SHIP_OPS_ENABLED -# define MODE_SHIP_OPS_ENABLED !HAL_MINIMIZE_FEATURES && MODE_FOLLOW_ENABLED +# define MODE_SHIP_OPS_ENABLED MODE_FOLLOW_ENABLED #endif ////////////////////////////////////////////////////////////////////////////// diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 3f1cac7abeb18..ab60489e36bc7 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -1527,7 +1527,9 @@ class ModeShipOperation : public Mode { Vector3f offset; bool ship_takeoff; bool pilot_correction_active; - struct { + class Ship { + public: + void reset(const Vector3f &pos_with_ofs_ned, const Vector3f &vel_ned_ms, float target_heading_deg); Vector3p pos_ned; Vector3f vel_ned; Vector3f accel_ned; diff --git a/ArduCopter/mode_ship_ops.cpp b/ArduCopter/mode_ship_ops.cpp index aed4327e22c94..043ad58861937 100644 --- a/ArduCopter/mode_ship_ops.cpp +++ b/ArduCopter/mode_ship_ops.cpp @@ -155,13 +155,7 @@ bool ModeShipOperation::init(const bool ignore_checks) float target_heading_deg = 0.0f; g2.follow.get_target_heading_deg(target_heading_deg); - ship.pos_ned = pos_with_ofs_ned.topostype(); - ship.vel_ned = vel_ned_ms; - ship.accel_ned.zero(); - ship.heading = radians(target_heading_deg); - ship.heading_rate = 0.0; - ship.heading_accel = 0.0; - ship.available = true; + ship.reset(pos_with_ofs_ned, vel_ned_ms, target_heading_deg); offset.zero(); offset.xy() = curr_pos_neu_cm.xy() - ship.pos_ned.xy().tofloat(); @@ -254,6 +248,17 @@ void ModeShipOperation::set_approach_mode(ApproachMode new_approach_mode) } +void ModeShipOperation::Ship::reset(const Vector3f &pos_with_ofs_ned, const Vector3f &vel_ned_ms, float target_heading_deg) +{ + pos_ned = pos_with_ofs_ned.topostype(); + vel_ned = vel_ned_ms; + accel_ned.zero(); + heading = radians(target_heading_deg); + heading_rate = 0.0; + heading_accel = 0.0; + available = true; +} + void ModeShipOperation::run() { float yaw_cd = attitude_control->get_att_target_euler_cd().z; @@ -304,13 +309,7 @@ void ModeShipOperation::run() if (!ship.available) { // reset ship pos, vel, accel to current value when detected. - ship.pos_ned = pos_with_ofs_ned.topostype(); - ship.vel_ned = vel_ned_ms; - ship.accel_ned.zero(); - ship.heading = radians(target_heading_deg); - ship.heading_rate = 0.0; - ship.heading_accel = 0.0; - ship.available = true; + ship.reset(pos_with_ofs_ned, vel_ned_ms, target_heading_deg); } shape_pos_vel_accel_xy(pos_with_ofs_ned.xy().topostype(), vel_ned_ms.xy(), accel_ned.xy(), diff --git a/libraries/AP_HAL_ChibiOS/hwdef/include/minimize_features.inc b/libraries/AP_HAL_ChibiOS/hwdef/include/minimize_features.inc index 0bcda34f3957c..e4261f78c78f8 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/include/minimize_features.inc +++ b/libraries/AP_HAL_ChibiOS/hwdef/include/minimize_features.inc @@ -61,3 +61,6 @@ define AP_CAMERA_SERVO_ENABLED AP_CAMERA_ENABLED # no SLCAN on these boards (use can-over-mavlink if required) define AP_CAN_SLCAN_ENABLED 0 + +# no ship-ops on small boards: +define MODE_SHIP_OPS_ENABLED 0