Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: Now using AP_Rally library.

This commit is contained in:
Michael Day 2014-04-18 12:19:13 -07:00 committed by Andrew Tridgell
parent a2aab2ab5e
commit 568fc9e6c9
7 changed files with 24 additions and 129 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ArduPlane/ArduPlane.pde
View File

@ -79,6 +79,8 @@
#include <AP_BoardConfig.h>
#include <AP_ServoRelayEvents.h>
#include <AP_Rally.h>
// Pre-AP_HAL compatibility
#include "compat.h"
@ -320,6 +322,9 @@ static AP_ServoRelayEvents ServoRelayEvents(relay);
static AP_Camera camera(&relay);
#endif
//Rally Ponints
AP_Rally rally(ahrs, MAX_RALLYPOINTS, RALLY_START_BYTE);
////////////////////////////////////////////////////////////////////////////////
// Global variables
////////////////////////////////////////////////////////////////////////////////

12
ArduPlane/GCS_Mavlink.pde
View File

@ -1485,13 +1485,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
if (mavlink_check_target(packet.target_system, packet.target_component))
break;
if (packet.idx >= g.rally_total ||
if (packet.idx >= rally.get_rally_total() ||
packet.idx >= MAX_RALLYPOINTS) {
send_text_P(SEVERITY_LOW,PSTR("bad rally point message ID"));
break;
}
if (packet.count != g.rally_total) {
if (packet.count != rally.get_rally_total()) {
send_text_P(SEVERITY_LOW,PSTR("bad rally point message count"));
break;
}
@ -1503,7 +1503,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
rally_point.break_alt = packet.break_alt;
rally_point.land_dir = packet.land_dir;
rally_point.flags = packet.flags;
set_rally_point_with_index(packet.idx, rally_point);
rally.set_rally_point_with_index(packet.idx, rally_point);
break;
}
@ -1513,19 +1513,19 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
mavlink_msg_rally_fetch_point_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component))
break;
if (packet.idx > g.rally_total) {
if (packet.idx > rally.get_rally_total()) {
send_text_P(SEVERITY_LOW, PSTR("bad rally point index"));
break;
}
RallyLocation rally_point;
if (!get_rally_point_with_index(packet.idx, rally_point)) {
if (!rally.get_rally_point_with_index(packet.idx, rally_point)) {
send_text_P(SEVERITY_LOW, PSTR("failed to set rally point"));
break;
}
mavlink_msg_rally_point_send_buf(msg,
chan, msg->sysid, msg->compid, packet.idx,
g.rally_total, rally_point.lat, rally_point.lng,
rally.get_rally_total(), rally_point.lat, rally_point.lng,
rally_point.alt, rally_point.break_alt, rally_point.land_dir,
rally_point.flags);
break;

8
ArduPlane/Parameters.h
View File

@ -96,7 +96,7 @@ public:
k_param_waypoint_max_radius,
k_param_ground_steer_alt,
k_param_ground_steer_dps,
k_param_rally_limit_km,
k_param_rally_limit_km_old, //unused anymore -- just holding this index
k_param_hil_err_limit,
k_param_sonar,
k_param_log_bitmask,
@ -106,6 +106,7 @@ public:
k_param_flaperon_output,
k_param_gps,
k_param_autotune_level,
k_param_rally,
// 100: Arming parameters
k_param_arming = 100,
@ -268,7 +269,7 @@ public:
k_param_L1_controller,
k_param_rcmap,
k_param_TECS_controller,
k_param_rally_total,
k_param_rally_total_old, //unused
k_param_steerController,
//
@ -347,9 +348,6 @@ public:
AP_Int8 fence_ret_rally;
#endif
AP_Int8 rally_total;
AP_Float rally_limit_km;
// Fly-by-wire
//
AP_Int8 flybywire_elev_reverse;

20
ArduPlane/Parameters.pde
View File

@ -295,20 +295,6 @@ const AP_Param::Info var_info[] PROGMEM = {
GSCALAR(fence_ret_rally, "FENCE_RET_RALLY", 0),
#endif
// @Param: RALLY_TOTAL
// @DisplayName: Rally Total
// @Description: Number of rally points currently loaded
// @User: Advanced
GSCALAR(rally_total, "RALLY_TOTAL", 0),
// @Param: RALLY_LIMIT_KM
// @DisplayName: Rally Limit
// @Description: Maximum distance to rally point. If the closest rally point is more than this number of kilometers from the current position and the home location is closer than any of the rally points from the current position then do RTL to home rather than to the closest rally point. This prevents a leftover rally point from a different airfield being used accidentally. If this is set to 0 then the closest rally point is always used.
// @User: Advanced
// @Units: kilometers
// @Increment: 0.1
GSCALAR(rally_limit_km, "RALLY_LIMIT_KM", 5),
// @Param: ARSPD_FBW_MIN
// @DisplayName: Fly By Wire Minimum Airspeed
// @Description: Airspeed corresponding to minimum throttle in auto throttle modes (FBWB, CRUISE, AUTO, GUIDED, LOITER, CIRCLE and RTL). This is a calibrated (apparent) airspeed.
@ -1004,6 +990,10 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Path: ../libraries/AP_Mission/AP_Mission.cpp
GOBJECT(mission, "MIS_", AP_Mission),
// @Group: RALLY_
// @Path: ../libraries/AP_Rally/AP_Rally.cpp
GOBJECT(rally, "RALLY_", AP_Rally),
AP_VAREND
};
@ -1039,6 +1029,8 @@ const AP_Param::ConversionInfo conversion_table[] PROGMEM = {
{ Parameters::k_param_curr_amp_offset, 0, AP_PARAM_FLOAT, "BATT_AMP_OFFSET" },
{ Parameters::k_param_pack_capacity, 0, AP_PARAM_INT32, "BATT_CAPACITY" },
{ Parameters::k_param_log_bitmask_old, 0, AP_PARAM_INT16, "LOG_BITMASK" },
{ Parameters::k_param_rally_limit_km_old, 0, AP_PARAM_FLOAT, "RALLY_LIMIT_KM" },
{ Parameters::k_param_rally_total_old, 0, AP_PARAM_INT8, "RALLY_TOTAL" },
};
static void load_parameters(void)

5
ArduPlane/commands_logic.pde
View File

@ -241,7 +241,7 @@ static void do_RTL(void)
{
control_mode = RTL;
prev_WP_loc = current_loc;
next_WP_loc = rally_find_best_location(current_loc, home);
next_WP_loc = rally.calc_best_rally_or_home_location(current_loc, read_alt_to_hold());
if (g.loiter_radius < 0) {
loiter.direction = -1;
@ -620,7 +620,8 @@ static void exit_mission_callback()
if (control_mode == AUTO) {
gcs_send_text_fmt(PSTR("Returning to Home"));
memset(&auto_rtl_command, 0, sizeof(auto_rtl_command));
auto_rtl_command.content.location = rally_find_best_location(current_loc, home);
auto_rtl_command.content.location =
rally.calc_best_rally_or_home_location(current_loc, read_alt_to_hold());
auto_rtl_command.id = MAV_CMD_NAV_LOITER_UNLIM;
setup_glide_slope();
start_command(auto_rtl_command);

2
ArduPlane/geofence.pde
View File

@ -339,7 +339,7 @@ static void geofence_check(bool altitude_check_only)
case FENCE_ACTION_GUIDED:
case FENCE_ACTION_GUIDED_THR_PASS:
if (g.fence_ret_rally != 0) { //return to a rally point
guided_WP_loc = rally_find_best_location(current_loc, home);
guided_WP_loc = rally.calc_best_rally_or_home_location(current_loc, read_alt_to_hold());
} else { //return to fence return point, not a rally point
if (g.fence_retalt > 0) {

101
ArduPlane/rally.pde
View File

@ -1,101 +0,0 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
* rally point support
* Michael Day, September 2013
*/
//get a rally point
static bool get_rally_point_with_index(unsigned i, RallyLocation &ret)
{
if (i >= (unsigned)g.rally_total) {
return false;
}
hal.storage->read_block(&ret, RALLY_START_BYTE + (i * sizeof(RallyLocation)), sizeof(RallyLocation));
if (ret.lat == 0 && ret.lng == 0) {
// sanity check ...
return false;
}
return true;
}
//save a rally point
static bool set_rally_point_with_index(unsigned i, const RallyLocation &rallyLoc)
{
if (i >= (unsigned) g.rally_total) {
//not allowed
return false;
}
if (i >= MAX_RALLYPOINTS) {
//also not allowed
return false;
}
hal.storage->write_block(RALLY_START_BYTE + (i * sizeof(RallyLocation)), &rallyLoc, sizeof(RallyLocation));
return true;
}
// 'best' means 'closest to Location loc' for now.
static bool find_best_rally_point(const Location &myloc, const Location &homeloc, RallyLocation &ret)
{
float min_dis = -1;
for (unsigned i = 0; i < (unsigned) g.rally_total; i++) {
RallyLocation next_rally;
if (!get_rally_point_with_index(i, next_rally)) {
continue;
}
Location rally_loc = rally_location_to_location(next_rally, homeloc);
float dis = get_distance(myloc, rally_loc);
if (dis < min_dis || min_dis < 0) {
min_dis = dis;
ret = next_rally;
}
}
if (g.rally_limit_km > 0 && min_dis > g.rally_limit_km*1000.0f &&
get_distance(myloc, homeloc) < min_dis) {
// return false, which makes home be used instead
return false;
}
return min_dis >= 0;
}
// translate a RallyLocation to a Location
static Location rally_location_to_location(const RallyLocation &r_loc, const Location &homeloc)
{
Location ret = {};
// we return an absolute altitude, as we add homeloc.alt below
ret.flags.relative_alt = false;
//Currently can't do true AGL on the APM. Relative altitudes are
//relative to HOME point's altitude. Terrain on the board is inbound
//for the PX4, though. This line will need to be updated when that happens:
ret.alt = (r_loc.alt*100UL) + homeloc.alt;
ret.lat = r_loc.lat;
ret.lng = r_loc.lng;
return ret;
}
// return best RTL location from current position
static Location rally_find_best_location(const Location &myloc, const Location &homeloc)
{
RallyLocation ral_loc = {};
Location ret = {};
if (find_best_rally_point(myloc, home, ral_loc)) {
//we have setup Rally points: use them instead of Home for RTL
ret = rally_location_to_location(ral_loc, home);
} else {
ret = homeloc;
// Altitude to hold over home
ret.alt = read_alt_to_hold();
// read_alt_to_hold returns an absolute altitude
ret.flags.relative_alt = false;
}
return ret;
}