aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Plane: Sends MAV_SYS_STATUS_GEOFENCE and also allows GCS to enable/disable fence.

Browse Source
This commit is contained in:
Michael Day 2014-02-13 14:49:42 -08:00 committed by Andrew Tridgell
parent e1d193f539
commit 25f23cf16c
8 changed files with 187 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
ArduPlane/ArduPlane.pde
View File

@ -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);
}
}

36
ArduPlane/GCS_Mavlink.pde
View File

@ -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;
}

2
ArduPlane/Parameters.h
View File

@ -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;

7
ArduPlane/Parameters.pde
View File

@ -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

11
ArduPlane/commands_logic.pde
View File

@ -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;

9
ArduPlane/defines.h
View File

@ -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

109
ArduPlane/geofence.pde
View File

@ -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

5
ArduPlane/system.pde
View File

@ -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)