diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 4754b7354d..edfeec032f 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -380,6 +380,7 @@ static bool usb_connected; // This is the state of the flight control system // There are multiple states defined such as MANUAL, FBW-A, AUTO static enum FlightMode control_mode = INITIALISING; +static enum FlightMode previous_mode = INITIALISING; // Used to maintain the state of the previous control switch position // This is set to 254 when we need to re-read the switch @@ -557,6 +558,8 @@ static bool auto_throttle_mode; // this controls throttle suppression in auto modes static bool throttle_suppressed; +AP_SpdHgtControl::FlightStage flight_stage = AP_SpdHgtControl::FLIGHT_NORMAL; + //////////////////////////////////////////////////////////////////////////////// // Loiter management //////////////////////////////////////////////////////////////////////////////// @@ -1381,6 +1384,25 @@ static void update_navigation() } } +static void update_flight_stage(AP_SpdHgtControl::FlightStage fs) { + //if just now entering land flight stage + if (fs == AP_SpdHgtControl::FLIGHT_LAND_APPROACH && + flight_stage != AP_SpdHgtControl::FLIGHT_LAND_APPROACH) { + +#if GEOFENCE_ENABLED == ENABLED + if (g.fence_autoenable == 1) { + if (! geofence_set_enabled(false, AUTO_TOGGLED)) { + gcs_send_text_P(SEVERITY_HIGH, PSTR("Disable fence failed (autodisable)")); + } else { + gcs_send_text_P(SEVERITY_HIGH, PSTR("Fence disabled (autodisable)")); + } + } +#endif + + } + + flight_stage = fs; +} static void update_alt() { @@ -1392,16 +1414,16 @@ static void update_alt() geofence_check(true); // Update the speed & height controller states - if (auto_throttle_mode && !throttle_suppressed) { - AP_SpdHgtControl::FlightStage flight_stage = AP_SpdHgtControl::FLIGHT_NORMAL; - + if (auto_throttle_mode && !throttle_suppressed) { if (control_mode==AUTO) { if (takeoff_complete == false) { - flight_stage = AP_SpdHgtControl::FLIGHT_TAKEOFF; + update_flight_stage(AP_SpdHgtControl::FLIGHT_TAKEOFF); } else if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND && land_complete == true) { - flight_stage = AP_SpdHgtControl::FLIGHT_LAND_FINAL; + update_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_FINAL); } else if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND) { - flight_stage = AP_SpdHgtControl::FLIGHT_LAND_APPROACH; + update_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_APPROACH); + } else { + update_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL); } } diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 27cf7929f7..3c70b4dd3e 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -149,14 +149,21 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan) if (g_gps != NULL && g_gps->status() > GPS::NO_GPS) { control_sensors_present |= MAV_SYS_STATUS_SENSOR_GPS; } + if (geofence_present()) { + control_sensors_present |= MAV_SYS_STATUS_GEOFENCE; + } - // all present sensors enabled by default except rate control, attitude stabilization, yaw, altitude, position control and motor output which we will set individually - control_sensors_enabled = control_sensors_present & (~MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL & ~MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION & ~MAV_SYS_STATUS_SENSOR_YAW_POSITION & ~MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL & ~MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL & ~MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS); + // all present sensors enabled by default except rate control, attitude stabilization, yaw, altitude, position control, geofence and motor output which we will set individually + control_sensors_enabled = control_sensors_present & (~MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL & ~MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION & ~MAV_SYS_STATUS_SENSOR_YAW_POSITION & ~MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL & ~MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL & ~MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS & ~MAV_SYS_STATUS_GEOFENCE); if (airspeed.enabled() && airspeed.use()) { control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE; } + if (geofence_enabled()) { + control_sensors_enabled |= MAV_SYS_STATUS_GEOFENCE; + } + switch (control_mode) { case MANUAL: break; @@ -202,10 +209,12 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan) break; } - // default to all healthy + // default: all present sensors healthy except 3D_MAG, GPS, DIFFERNTIAL_PRESSURE. GEOFENCE always defaults to healthy. control_sensors_health = control_sensors_present & ~(MAV_SYS_STATUS_SENSOR_3D_MAG | MAV_SYS_STATUS_SENSOR_GPS | MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE); + control_sensors_health |= MAV_SYS_STATUS_GEOFENCE; + if (g.compass_enabled && compass.healthy(0) && ahrs.use_compass()) { control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG; } @@ -218,6 +227,9 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan) if (airspeed.healthy()) { control_sensors_health |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE; } + if (geofence_breached()) { + control_sensors_health &= ~MAV_SYS_STATUS_GEOFENCE; + } int16_t battery_current = -1; int8_t battery_remaining = -1; @@ -1271,6 +1283,24 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) } break; + case MAV_CMD_DO_FENCE_ENABLE: + result = MAV_RESULT_ACCEPTED; + + if (!geofence_present()) { + result = MAV_RESULT_FAILED; + } else { + if (packet.param1 == 0) { + if (! geofence_set_enabled(false, GCS_TOGGLED)) { + result = MAV_RESULT_FAILED; + } + } else { + if (! geofence_set_enabled(true, GCS_TOGGLED)) { + result = MAV_RESULT_FAILED; + } + } + } + break; + default: break; } diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 58ba52af80..2c8da35aeb 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -110,6 +110,7 @@ public: // 105: Extra parameters k_param_fence_retalt = 105, + k_param_fence_autoenable, // 110: Telemetry control // @@ -339,6 +340,7 @@ public: AP_Int16 fence_minalt; // meters AP_Int16 fence_maxalt; // meters AP_Int16 fence_retalt; // meters + AP_Int8 fence_autoenable; #endif AP_Int8 rally_total; diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde index e6074db28d..ce83b648a8 100644 --- a/ArduPlane/Parameters.pde +++ b/ArduPlane/Parameters.pde @@ -271,6 +271,13 @@ const AP_Param::Info var_info[] PROGMEM = { // @Increment: 1 // @User: Standard GSCALAR(fence_retalt, "FENCE_RETALT", 0), + + // @Param: FENCE_AUTOENABLE + // @DisplayName: Fence automatically enabled after auto takeoff and automatically disabled when starting an auto landing. Note that this does NOT remove the need to first create a geofence. + // @Description: When set to 1, gefence automatically enables after an auto takeoff and automatically disables at the beginning of an auto landing. When on the ground before takeoff the fence is disabled. + // @Range: 0 1 + // @User: Standard + GSCALAR(fence_autoenable, "FENCE_AUTOENABLE", 0), #endif // @Param: RALLY_TOTAL diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde index 54f7bbf30f..70fabe61f7 100644 --- a/ArduPlane/commands_logic.pde +++ b/ArduPlane/commands_logic.pde @@ -333,6 +333,17 @@ static bool verify_takeoff() steer_state.hold_course_cd = -1; takeoff_complete = true; next_WP_loc = prev_WP_loc = current_loc; + +#if GEOFENCE_ENABLED == ENABLED + if (g.fence_autoenable == 1) { + if (! geofence_set_enabled(true, AUTO_TOGGLED)) { + gcs_send_text_P(SEVERITY_HIGH, PSTR("Enable fence failed (cannot autoenable")); + } else { + gcs_send_text_P(SEVERITY_HIGH, PSTR("Fence enabled. (autoenabled)")); + } + } +#endif + return true; } else { return false; diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index 78841dba43..33a1c1906f 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -95,6 +95,15 @@ enum ChannelMixing { MIXING_DNDN = 4 }; +/* + * The cause for the most recent fence enable + */ +typedef enum GeofenceEnableReason { + NOT_ENABLED = 0, //The fence is not enabled + PWM_TOGGLED, //Fence enabled/disabled by PWM signal + AUTO_TOGGLED, //Fence auto enabled/disabled at takeoff. + GCS_TOGGLED //Fence enabled/disabled by the GCS via Mavlink +} GeofenceEnableReason; //repeating events #define NO_REPEAT 0 diff --git a/ArduPlane/geofence.pde b/ArduPlane/geofence.pde index 9a376ae322..8b5d0e6d89 100644 --- a/ArduPlane/geofence.pde +++ b/ArduPlane/geofence.pde @@ -5,7 +5,6 @@ */ #if GEOFENCE_ENABLED == ENABLED - /* * The state of geo-fencing. This structure is dynamically allocated * the first time it is used. This means we only pay for the pointer @@ -19,6 +18,11 @@ static struct GeofenceState { uint8_t num_points; bool boundary_uptodate; bool fence_triggered; + bool is_pwm_enabled; //true if above FENCE_ENABLE_PWM threshold + bool previous_is_pwm_enabled; //true if above FENCE_ENALBE_PWM threshold + // last time we checked + bool is_enabled; + GeofenceEnableReason enable_reason; uint16_t breach_count; uint8_t breach_type; uint32_t breach_time; @@ -121,16 +125,66 @@ failed: gcs_send_text_P(SEVERITY_HIGH,PSTR("geo-fence setup error")); } +/* + * return true if a geo-fence has been uploaded (not necessarily enabled) + */ +static bool geofence_present(void) +{ + //require at least a return point and a triangle + //to define a geofence area: + if (g.fence_total < 4) { + return false; + } + return true; +} + +static void geofence_update_pwm_enabled_state() { + if (geofence_state == NULL) { + return; + } + + geofence_state->previous_is_pwm_enabled = geofence_state->is_pwm_enabled; + + if (g.fence_channel == 0) { + geofence_state->is_pwm_enabled = false; + } else { + geofence_state->is_pwm_enabled = + (hal.rcin->read(g.fence_channel-1) > FENCE_ENABLE_PWM); + } + + if (geofence_state->is_pwm_enabled == true && + geofence_state->previous_is_pwm_enabled == false) { + geofence_set_enabled(true, PWM_TOGGLED); + + } else if (geofence_state->is_pwm_enabled == false && + geofence_state->previous_is_pwm_enabled == true) { + geofence_set_enabled(false, PWM_TOGGLED); + } +} + +//return true on success, false on failure +static bool geofence_set_enabled(bool enable, GeofenceEnableReason r) { + if (geofence_state == NULL) { + return false; + } + + geofence_state->is_enabled = enable; + geofence_state->enable_reason = r; + + return true; +} + /* * return true if geo-fencing is enabled */ static bool geofence_enabled(void) { + geofence_update_pwm_enabled_state(); + if (g.fence_action == FENCE_ACTION_NONE || - g.fence_total < 5 || + !geofence_present() || (g.fence_action != FENCE_ACTION_REPORT && - (g.fence_channel == 0 || - hal.rcin->read(g.fence_channel-1) < FENCE_ENABLE_PWM))) { + (geofence_state != NULL && !geofence_state->is_enabled))) { // geo-fencing is disabled if (geofence_state != NULL) { // re-arm for when the channel trigger is switched on @@ -182,15 +236,14 @@ static void geofence_check(bool altitude_check_only) // GUIDED to the return point if (geofence_state != NULL && (g.fence_action == FENCE_ACTION_GUIDED || g.fence_action == FENCE_ACTION_GUIDED_THR_PASS) && - g.fence_channel != 0 && control_mode == GUIDED && - g.fence_total >= 5 && + geofence_present() && geofence_state->boundary_uptodate && geofence_state->old_switch_position == oldSwitchPosition && guided_WP_loc.lat == geofence_state->boundary[0].x && guided_WP_loc.lng == geofence_state->boundary[0].y) { geofence_state->old_switch_position = 254; - reset_control_switch(); + set_mode(get_previous_mode()); } return; } @@ -266,16 +319,26 @@ static void geofence_check(bool altitude_check_only) case FENCE_ACTION_GUIDED: case FENCE_ACTION_GUIDED_THR_PASS: - if (g.fence_retalt > 0) { - //fly to the return point using fence_retalt - guided_WP_loc.alt = home.alt + 100.0f*g.fence_retalt; - } else if (g.fence_minalt >= g.fence_maxalt) { - // invalid min/max, use RTL_altitude - guided_WP_loc.alt = home.alt + g.RTL_altitude_cm; - } else { - // fly to the return point, with an altitude half way between - // min and max - guided_WP_loc.alt = home.alt + 100.0*(g.fence_minalt + g.fence_maxalt)/2; + if (g.fence_ret_rally != 0) { //return to a rally point + guided_WP_loc = rally_find_best_location(current_loc, home); + + } else { //return to fence return point, not a rally point + if (g.fence_retalt > 0) { + //fly to the return point using fence_retalt + guided_WP_loc.alt = home.alt + 100.0*g.fence_retalt; + } else if (g.fence_minalt >= g.fence_maxalt) { + // invalid min/max, use RTL_altitude + guided_WP_loc.alt = home.alt + g.RTL_altitude_cm; + } else { + // fly to the return point, with an altitude half way between + // min and max + guided_WP_loc.alt = home.alt + 100.0*(g.fence_minalt + g.fence_maxalt)/2; + } + //guided_WP_loc.id = 0; + //guided_WP_loc.p1 = 0; + guided_WP_loc.options = 0; + guided_WP_loc.lat = geofence_state->boundary[0].x; + guided_WP_loc.lng = geofence_state->boundary[0].y; } guided_WP_loc.options = 0; guided_WP_loc.lat = geofence_state->boundary[0].x; @@ -349,4 +412,16 @@ static bool geofence_enabled(void) { return false; } +static bool geofence_present(void) { + return false; +} + +static bool geofence_set_enabled(bool enable, GeofenceEnableReason r) { + return false; +} + +bool geofence_breached(void) { + return false; +} + #endif // GEOFENCE_ENABLED diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index 6802308c9a..b4db6a3699 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -299,6 +299,10 @@ static void startup_ground(void) gcs_send_text_P(SEVERITY_LOW,PSTR("\n\n Ready to FLY.")); } +static enum FlightMode get_previous_mode() { + return previous_mode; +} + static void set_mode(enum FlightMode mode) { if(control_mode == mode) { @@ -312,6 +316,7 @@ static void set_mode(enum FlightMode mode) exit_mode(control_mode); // set mode + previous_mode = control_mode; control_mode = mode; switch(control_mode)