diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 26c3aab5c6..f5fb730531 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -1809,6 +1809,44 @@ mission_failed: } #endif // GEOFENCE_ENABLED + // receive a rally point from GCS and store in EEPROM + case MAVLINK_MSG_ID_RALLY_POINT: { + mavlink_rally_point_t packet; + mavlink_msg_rally_point_decode(msg, &packet); + 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); + } + break; + } + + //send a rally point to the GCS + case MAVLINK_MSG_ID_RALLY_FETCH_POINT: { + mavlink_rally_fetch_point_t packet; + 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) { + 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); + } + + break; + } + case MAVLINK_MSG_ID_PARAM_SET: { AP_Param *vp; diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 2ed01a6dfc..eac3b2a211 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -241,6 +241,8 @@ public: k_param_L1_controller, k_param_rcmap, k_param_TECS_controller, + + k_param_rally_total, // // 240: PID Controllers @@ -308,6 +310,8 @@ public: AP_Int16 fence_maxalt; // meters #endif + AP_Int8 rally_total; + // Fly-by-wire // AP_Int8 flybywire_elev_reverse; diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde index d251f604f9..6a51db7367 100644 --- a/ArduPlane/Parameters.pde +++ b/ArduPlane/Parameters.pde @@ -259,6 +259,12 @@ const AP_Param::Info var_info[] PROGMEM = { GSCALAR(fence_maxalt, "FENCE_MAXALT", 0), #endif + // @Param: RALLY_TOTAL + // @DisplayName: Rally Total + // @Description: Number of rally points currently loaded + // @User: Standard + GSCALAR(rally_total, "RALLY_TOTAL", 0), + // @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. diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde index bdee929e99..5b87bc17c3 100644 --- a/ArduPlane/commands_logic.pde +++ b/ArduPlane/commands_logic.pde @@ -233,7 +233,20 @@ static void do_RTL(void) { control_mode = RTL; prev_WP = current_loc; - next_WP = home; + + 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); + } if (g.loiter_radius < 0) { loiter.direction = -1; @@ -241,11 +254,6 @@ static void do_RTL(void) loiter.direction = 1; } - // Altitude to hold over home - // Set by configuration tool - // ------------------------- - next_WP.alt = read_alt_to_hold(); - setup_glide_slope(); if (g.log_bitmask & MASK_LOG_MODE) diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index a7797f9c36..fa5bd209c9 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -219,7 +219,12 @@ enum log_messages { #define FENCE_WP_SIZE sizeof(Vector2l) #define FENCE_START_BYTE (EEPROM_MAX_ADDR-(MAX_FENCEPOINTS*FENCE_WP_SIZE)) -#define MAX_WAYPOINTS ((FENCE_START_BYTE - WP_START_BYTE) / WP_SIZE) - 1 // - +// rally points shoehorned between fence points and waypoints +#define MAX_RALLYPOINTS 10 +#define RALLY_WP_SIZE 15 +#define RALLY_START_BYTE (FENCE_START_BYTE-(MAX_RALLYPOINTS*RALLY_WP_SIZE)) + +#define MAX_WAYPOINTS ((RALLY_START_BYTE - WP_START_BYTE) / WP_SIZE) - 1 // - // 1 // to // be diff --git a/ArduPlane/rally.pde b/ArduPlane/rally.pde new file mode 100644 index 0000000000..70311a5cc4 --- /dev/null +++ b/ArduPlane/rally.pde @@ -0,0 +1,104 @@ +// -*- 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 RallyLocation get_rally_point_with_index(unsigned i) +{ + 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; + } + + //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; +} + +//save a rally point +static void set_rally_point_with_index(RallyLocation &rallyLoc, unsigned i) +{ + uint16_t mem; + + if (i >= (unsigned) g.rally_total.get()) { + //not allowed + return; + } + + if (i >= MAX_RALLYPOINTS) { + //also not allowed + return; + } + + 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); +} + +//'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; + + 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); + + if (dis < min_dis) { + min_dis = dis; + ret = next_rally; + } + } + + return ret; +} + +//translate a RallyLocation to a Location +Location rally_location_to_location (const RallyLocation &r_loc) { + Location ret; + + ret.id = MAV_CMD_NAV_LOITER_UNLIM; + ret.options = MASK_OPTIONS_RELATIVE_ALT; + + //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.lat = r_loc.lat; + ret.lng = r_loc.lng; + + return ret; +}