zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Plane: added error checking to rally load save 

sanity check the rally points
master
Andrew Tridgell 12 years ago
parent
7a9ed0a5a1
commit
011915eb1c
4 changed files with 72 additions and 88 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 42
      ArduPlane/GCS_Mavlink.pde
  2. 4
      ArduPlane/Parameters.pde
  3. 22
      ArduPlane/commands_logic.pde
  4. 92
      ArduPlane/rally.pde

42
ArduPlane/GCS_Mavlink.pde
Unescape View File

@ -1816,18 +1816,25 @@ mission_failed:
if (mavlink_check_target(packet.target_system, packet.target_component)) if (mavlink_check_target(packet.target_system, packet.target_component))
break; break;
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) { if (packet.count != g.rally_total) {
send_text_P(SEVERITY_LOW,PSTR("bad rally point message")); send_text_P(SEVERITY_LOW,PSTR("bad rally point message count"));
} else { 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(rally_point, packet.idx);
} }
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; break;
} }
@ -1838,12 +1845,19 @@ mission_failed:
if (mavlink_check_target(packet.target_system, packet.target_component)) if (mavlink_check_target(packet.target_system, packet.target_component))
break; break;
if (packet.idx > g.rally_total) { if (packet.idx > g.rally_total) {
send_text_P(SEVERITY_LOW, PSTR("bad rally point")); send_text_P(SEVERITY_LOW, PSTR("bad rally point index"));
} else { break;
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); 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; break;
} }

4
ArduPlane/Parameters.pde
Unescape View File

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

22
ArduPlane/commands_logic.pde
Unescape View File

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

92
ArduPlane/rally.pde
Unescape View File

@ -5,88 +5,60 @@
*/ */
//get a rally point //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; if (i >= (unsigned)g.rally_total) {
RallyLocation ret; return false;
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;
} }
hal.storage->read_block(&ret, RALLY_START_BYTE + (i * sizeof(RallyLocation)), sizeof(RallyLocation));
//read rally point if (ret.lat == 0 && ret.lng == 0) {
mem = RALLY_START_BYTE + (i * RALLY_WP_SIZE); // sanity check ...
ret.lat = hal.storage->read_dword(mem); return false;
mem += sizeof(uint32_t); }
ret.lng = hal.storage->read_dword(mem); return true;
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;
} }
//save a rally point //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) {
if (i >= (unsigned) g.rally_total.get()) {
//not allowed //not allowed
return; return false;
} }
if (i >= MAX_RALLYPOINTS) { if (i >= MAX_RALLYPOINTS) {
//also not allowed //also not allowed
return; return false;
} }
hal.storage->write_block(RALLY_START_BYTE + (i * sizeof(RallyLocation)), &rallyLoc, sizeof(RallyLocation));
mem = RALLY_START_BYTE + (i * RALLY_WP_SIZE); return true;
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);
} }
//'best' means 'closest to my current location' for now. // 'best' means 'closest to Location loc' for now.
RallyLocation find_best_rally_point() { static bool find_best_rally_point(const Location &myloc, const Location &homeloc, RallyLocation &ret)
RallyLocation ret; {
RallyLocation next_rally; float min_dis = -1;
Location rally_loc;
float dis;
float min_dis = 999999999.9f;
for (unsigned i = 0; i < (unsigned) g.rally_total.get(); i++) { for (unsigned i = 0; i < (unsigned) g.rally_total; i++) {
next_rally = get_rally_point_with_index(i); RallyLocation next_rally;
rally_loc = rally_location_to_location(next_rally); if (!get_rally_point_with_index(i, next_rally)) {
dis = get_distance(current_loc, rally_loc); 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; min_dis = dis;
ret = next_rally; ret = next_rally;
} }
} }
return ret; return min_dis >= 0;
} }
//translate a RallyLocation to a Location // translate a RallyLocation to a Location
Location rally_location_to_location (const RallyLocation &r_loc) { static Location rally_location_to_location(const RallyLocation &r_loc, const Location &homeloc)
{
Location ret; Location ret;
ret.id = MAV_CMD_NAV_LOITER_UNLIM; 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 //Currently can't do true AGL on the APM. Relative altitudes are
//relative to HOME point's altitude. Terrain on the board is inbound //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: //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.lat = r_loc.lat;
ret.lng = r_loc.lng; ret.lng = r_loc.lng;

Write Preview
Loading…
Cancel
Save