Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 00:28:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: added error checking to rally load save

Browse Source 
sanity check the rally points
This commit is contained in:
Andrew Tridgell 2013-10-03 12:20:54 +10:00
parent 7a9ed0a5a1
commit 011915eb1c
4 changed files with 73 additions and 89 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
ArduPlane/GCS_Mavlink.pde
View File

@ -1816,18 +1816,25 @@ mission_failed:
if (mavlink_check_target(packet.target_system, packet.target_component))
break;
if (packet.count != g.rally_total) {
send_text_P(SEVERITY_LOW,PSTR("bad rally point message"));
} else {
RallyLocation rally_point;
rally_point.lat = packet.lat;
rally_point.lng = packet.lng;
rally_point.alt = packet.alt;
rally_point.break_alt = packet.break_alt;
rally_point.land_dir = packet.land_dir;
rally_point.flags = packet.flags;
set_rally_point_with_index(rally_point, packet.idx);
if (packet.idx >= g.rally_total ||
packet.idx >= MAX_RALLYPOINTS) {
send_text_P(SEVERITY_LOW,PSTR("bad rally point message ID"));
break;
}
if (packet.count != g.rally_total) {
send_text_P(SEVERITY_LOW,PSTR("bad rally point message count"));
break;
}
RallyLocation rally_point;
rally_point.lat = packet.lat;
rally_point.lng = packet.lng;
rally_point.alt = packet.alt;
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);
break;
}
@ -1838,12 +1845,19 @@ mission_failed:
if (mavlink_check_target(packet.target_system, packet.target_component))
break;
if (packet.idx > g.rally_total) {
send_text_P(SEVERITY_LOW, PSTR("bad rally point"));
} else {
RallyLocation rally_point = get_rally_point_with_index(packet.idx);
mavlink_msg_rally_point_send(chan, msg->sysid, msg->compid, packet.idx, g.rally_total, rally_point.lat, rally_point.lng, rally_point.alt, rally_point.break_alt, rally_point.land_dir, rally_point.flags);
send_text_P(SEVERITY_LOW, PSTR("bad rally point index"));
break;
}
RallyLocation rally_point;
if (!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(chan, msg->sysid, msg->compid, packet.idx,
g.rally_total, rally_point.lat, rally_point.lng,
rally_point.alt, rally_point.break_alt, rally_point.land_dir,
rally_point.flags);
break;
}

4
ArduPlane/Parameters.pde
View File

@ -231,7 +231,7 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Param: FENCE_TOTAL
// @DisplayName: Fence Total
// @Description: Number of geofence points currently loaded
// @User: Standard
// @User: Advanced
GSCALAR(fence_total, "FENCE_TOTAL", 0),
// @Param: FENCE_CHANNEL
@ -262,7 +262,7 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Param: RALLY_TOTAL
// @DisplayName: Rally Total
// @Description: Number of rally points currently loaded
// @User: Standard
// @User: Advanced
GSCALAR(rally_total, "RALLY_TOTAL", 0),
// @Param: ARSPD_FBW_MIN

22
ArduPlane/commands_logic.pde
View File

@ -234,18 +234,16 @@ static void do_RTL(void)
control_mode = RTL;
prev_WP = current_loc;
if ((unsigned) g.rally_total.get() < 1) {
next_WP = home;
// Altitude to hold over home
// Set by configuration tool
// -------------------------
next_WP.alt = read_alt_to_hold();
} else { //we have setup Rally points: use them instead of Home for RTL
RallyLocation ral_loc = find_best_rally_point();
next_WP = rally_location_to_location(ral_loc);
RallyLocation ral_loc;
if (find_best_rally_point(current_loc, home, ral_loc)) {
//we have setup Rally points: use them instead of Home for RTL
next_WP = rally_location_to_location(ral_loc, home);
} else {
next_WP = home;
// Altitude to hold over home
// Set by configuration tool
// -------------------------
next_WP.alt = read_alt_to_hold();
}
if (g.loiter_radius < 0) {

92
ArduPlane/rally.pde
View File

@ -5,88 +5,60 @@
*/
//get a rally point
static RallyLocation get_rally_point_with_index(unsigned i)
static bool get_rally_point_with_index(unsigned i, RallyLocation &ret)
{
uint16_t mem;
RallyLocation ret;
if (i > (unsigned) g.rally_total.get()) {
ret.lat = 0; ret.lng = 0; ret.alt = 0;
ret.land_dir = 0; ret.flags = 0;
return ret;
if (i >= (unsigned)g.rally_total) {
return false;
}
//read rally point
mem = RALLY_START_BYTE + (i * RALLY_WP_SIZE);
ret.lat = hal.storage->read_dword(mem);
mem += sizeof(uint32_t);
ret.lng = hal.storage->read_dword(mem);
mem += sizeof(uint32_t);
ret.alt = hal.storage->read_word(mem);
mem += sizeof(int16_t);
ret.break_alt = hal.storage->read_word(mem);
mem += sizeof(int16_t);
ret.land_dir = hal.storage->read_word(mem);
mem += sizeof(uint16_t);
ret.flags = hal.storage->read_byte(mem);
return ret;
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 void set_rally_point_with_index(RallyLocation &rallyLoc, unsigned i)
static bool set_rally_point_with_index(unsigned i, const RallyLocation &rallyLoc)
{
uint16_t mem;
if (i >= (unsigned) g.rally_total.get()) {
if (i >= (unsigned) g.rally_total) {
//not allowed
return;
return false;
}
if (i >= MAX_RALLYPOINTS) {
//also not allowed
return;
return false;
}
mem = RALLY_START_BYTE + (i * RALLY_WP_SIZE);
hal.storage->write_dword(mem, rallyLoc.lat);
mem += sizeof(uint32_t);
hal.storage->write_dword(mem, rallyLoc.lng);
mem += sizeof(uint32_t);
hal.storage->write_word(mem, rallyLoc.alt);
mem += sizeof(int16_t);
hal.storage->write_word(mem, rallyLoc.break_alt);
mem += sizeof(int16_t);
hal.storage->write_word(mem, rallyLoc.land_dir);
mem += sizeof(uint32_t);
hal.storage->write_byte(mem, rallyLoc.flags);
hal.storage->write_block(RALLY_START_BYTE + (i * sizeof(RallyLocation)), &rallyLoc, sizeof(RallyLocation));
return true;
}
//'best' means 'closest to my current location' for now.
RallyLocation find_best_rally_point() {
RallyLocation ret;
RallyLocation next_rally;
Location rally_loc;
float dis;
float min_dis = 999999999.9f;
// '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.get(); i++) {
next_rally = get_rally_point_with_index(i);
rally_loc = rally_location_to_location(next_rally);
dis = get_distance(current_loc, rally_loc);
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) {
if (dis < min_dis || min_dis < 0) {
min_dis = dis;
ret = next_rally;
}
}
return ret;
return min_dis >= 0;
}
//translate a RallyLocation to a Location
Location rally_location_to_location (const RallyLocation &r_loc) {
// translate a RallyLocation to a Location
static Location rally_location_to_location(const RallyLocation &r_loc, const Location &homeloc)
{
Location ret;
ret.id = MAV_CMD_NAV_LOITER_UNLIM;
@ -95,7 +67,7 @@ Location rally_location_to_location (const RallyLocation &r_loc) {
//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 + home.alt;
ret.alt = r_loc.alt + homeloc.alt;
ret.lat = r_loc.lat;
ret.lng = r_loc.lng;