diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index fbee3da2de..2ceb2ff77a 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -266,19 +266,20 @@ static long crosstrack_bearing; // deg * 100 : 0 to 360 desired angle of pla static float nav_gain_scaler = 1; // Gain scaling for headwind/tailwind TODO: why does this variable need to be initialized to 1? static long hold_course = -1; // deg * 100 dir of plane -static byte command_must_index; // current command memory location -static byte command_may_index; // current command memory location -static byte command_must_ID; // current command ID -static byte command_may_ID; // current command ID +static byte command_index; // current command memory location +static byte nav_command_index; // active nav command memory location +static byte non_nav_command_index; // active non-nav command memory location +static byte nav_command_ID = NO_COMMAND; // active nav command ID +static byte non_nav_command_ID = NO_COMMAND; // active non-nav command ID // Airspeed // -------- static int airspeed; // m/s * 100 -static int airspeed_nudge; // m/s * 100 : additional airspeed based on throttle stick position in top 1/2 of range -static float airspeed_error; // m/s * 100 -static float airspeed_fbwB; // m/s * 100 -static long energy_error; // energy state error (kinetic + potential) for altitude hold -static long airspeed_energy_error; // kinetic portion of energy error +static int airspeed_nudge; // m/s * 100 : additional airspeed based on throttle stick position in top 1/2 of range +static float airspeed_error; // m/s * 100 +static float airspeed_fbwB; // m/s * 100 +static long energy_error; // energy state error (kinetic + potential) for altitude hold +static long airspeed_energy_error; // kinetic portion of energy error // Location Errors // --------------- @@ -360,8 +361,9 @@ static struct Location home; // home location static struct Location prev_WP; // last waypoint static struct Location current_loc; // current location static struct Location next_WP; // next waypoint -static struct Location next_command; // command preloaded -static struct Location guided_WP; // guided mode waypoint +static struct Location guided_WP; // guided mode waypoint +static struct Location next_nav_command; // command preloaded +static struct Location next_nonnav_command; // command preloaded static long target_altitude; // used for altitude management between waypoints static long offset_altitude; // used for altitude management between waypoints static bool home_is_set; // Flag for if we have g_gps lock and have set the home location @@ -739,7 +741,7 @@ static void update_current_flight_mode(void) if(control_mode == AUTO){ crash_checker(); - switch(command_must_ID){ + switch(nav_command_ID){ case MAV_CMD_NAV_TAKEOFF: if (hold_course > -1) { calc_nav_roll(); diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 23a4d4b298..22bbd0dfe3 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -532,7 +532,7 @@ static void NOINLINE send_current_waypoint(mavlink_channel_t chan) { mavlink_msg_waypoint_current_send( chan, - g.waypoint_index); + g.command_index); } static void NOINLINE send_statustext(mavlink_channel_t chan) @@ -838,7 +838,7 @@ GCS_MAVLINK::update(void) } if (waypoint_receiving && - waypoint_request_i <= (unsigned)g.waypoint_total) { + waypoint_request_i <= (unsigned)g.command_total) { send_message(MSG_NEXT_WAYPOINT); } @@ -1278,7 +1278,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) mavlink_msg_waypoint_count_send( chan,msg->sysid, msg->compid, - g.waypoint_total + 1); // + home + g.command_total + 1); // + home waypoint_timelast_send = millis(); waypoint_sending = true; @@ -1304,7 +1304,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; // send waypoint - tell_command = get_wp_with_index(packet.seq); + tell_command = get_cmd_with_index(packet.seq); // set frame of waypoint uint8_t frame; @@ -1320,7 +1320,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // time that the mav should loiter in milliseconds uint8_t current = 0; // 1 (true), 0 (false) - if (packet.seq == (uint16_t)g.waypoint_index) + if (packet.seq == (uint16_t)g.command_index) current = 1; uint8_t autocontinue = 1; // 1 (true), 0 (false) @@ -1435,8 +1435,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) mavlink_msg_waypoint_clear_all_decode(msg, &packet); if (mavlink_check_target(packet.target_system, packet.target_component)) break; - // clear all waypoints - g.waypoint_total.set_and_save(0); + // clear all commands + g.command_total.set_and_save(0); // note that we don't send multiple acks, as otherwise a // GCS that is doing a clear followed by a set may see @@ -1455,7 +1455,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // set current command change_command(packet.seq); - mavlink_msg_waypoint_current_send(chan, g.waypoint_index); + mavlink_msg_waypoint_current_send(chan, g.command_index); break; } @@ -1470,7 +1470,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) if (packet.count > MAX_WAYPOINTS) { packet.count = MAX_WAYPOINTS; } - g.waypoint_total.set_and_save(packet.count - 1); + g.command_total.set_and_save(packet.count - 1); waypoint_timelast_receive = millis(); waypoint_receiving = true; @@ -1645,13 +1645,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) goto mission_failed; } - set_wp_with_index(tell_command, packet.seq); + set_cmd_with_index(tell_command, packet.seq); // update waypoint receiving state machine waypoint_timelast_receive = millis(); waypoint_request_i++; - if (waypoint_request_i > (uint16_t)g.waypoint_total){ + if (waypoint_request_i > (uint16_t)g.command_total){ mavlink_msg_waypoint_ack_send( chan, msg->sysid, @@ -1988,7 +1988,7 @@ void GCS_MAVLINK::queued_waypoint_send() { if (waypoint_receiving && - waypoint_request_i <= (unsigned)g.waypoint_total) { + waypoint_request_i <= (unsigned)g.command_total) { mavlink_msg_waypoint_request_send( chan, waypoint_dest_sysid, diff --git a/ArduPlane/Log.pde b/ArduPlane/Log.pde index 09bfdd7cf7..310c837cef 100644 --- a/ArduPlane/Log.pde +++ b/ArduPlane/Log.pde @@ -380,15 +380,15 @@ static void Log_Write_Startup(byte type) DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(LOG_STARTUP_MSG); DataFlash.WriteByte(type); - DataFlash.WriteByte(g.waypoint_total); + DataFlash.WriteByte(g.command_total); DataFlash.WriteByte(END_BYTE); // create a location struct to hold the temp Waypoints for printing - struct Location cmd = get_wp_with_index(0); + struct Location cmd = get_cmd_with_index(0); Log_Write_Cmd(0, &cmd); - for (int i = 1; i <= g.waypoint_total; i++){ - cmd = get_wp_with_index(i); + for (int i = 1; i <= g.command_total; i++){ + cmd = get_cmd_with_index(i); Log_Write_Cmd(i, &cmd); } } diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index e831686f48..330f19bd5b 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -162,8 +162,8 @@ public: // 220: Waypoint data // k_param_waypoint_mode = 220, - k_param_waypoint_total, - k_param_waypoint_index, + k_param_command_total, + k_param_command_index, k_param_waypoint_radius, k_param_loiter_radius, @@ -254,8 +254,8 @@ public: // Waypoints // AP_Int8 waypoint_mode; - AP_Int8 waypoint_total; - AP_Int8 waypoint_index; + AP_Int8 command_total; + AP_Int8 command_index; AP_Int8 waypoint_radius; AP_Int8 loiter_radius; @@ -371,8 +371,8 @@ public: airspeed_offset (0, k_param_airspeed_offset, PSTR("ARSPD_OFFSET")), /* XXX waypoint_mode missing here */ - waypoint_total (0, k_param_waypoint_total, PSTR("WP_TOTAL")), - waypoint_index (0, k_param_waypoint_index, PSTR("WP_INDEX")), + command_total (0, k_param_command_total, PSTR("CMD_TOTAL")), + command_index (0, k_param_command_index, PSTR("CMD_INDEX")), waypoint_radius (WP_RADIUS_DEFAULT, k_param_waypoint_radius, PSTR("WP_RADIUS")), loiter_radius (LOITER_RADIUS_DEFAULT, k_param_loiter_radius, PSTR("WP_LOITER_RAD")), diff --git a/ArduPlane/commands.pde b/ArduPlane/commands.pde index e9d5ebd0aa..7e032991c1 100644 --- a/ArduPlane/commands.pde +++ b/ArduPlane/commands.pde @@ -1,22 +1,42 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* Functions in this file: + void init_commands() + void update_auto() + void reload_commands_airstart() + struct Location get_cmd_with_index(int i) + void set_cmd_with_index(struct Location temp, int i) + void increment_cmd_index() + void decrement_cmd_index() + long read_alt_to_hold() + void set_next_WP(struct Location *wp) + void set_guided_WP(void) + void init_home() +************************************************************ +*/ + static void init_commands() { - //read_EEPROM_waypoint_info(); - g.waypoint_index.set_and_save(0); - command_must_index = 0; - command_may_index = 0; - next_command.id = CMD_BLANK; + g.command_index.set_and_save(0); + nav_command_ID = NO_COMMAND; + non_nav_command_ID = NO_COMMAND; + next_nav_command.id = CMD_BLANK; } static void update_auto() { - if (g.waypoint_index >= g.waypoint_total) { + if (g.command_index >= g.command_total) { handle_no_commands(); - if(g.waypoint_total == 0) { + if(g.command_total == 0) { next_WP.lat = home.lat + 1000; // so we don't have bad calcs next_WP.lng = home.lng + 1000; // so we don't have bad calcs } + } else { + g.command_index--; + nav_command_ID = NO_COMMAND; + non_nav_command_ID = NO_COMMAND; + next_nav_command.id = CMD_BLANK; + process_next_command(); } } @@ -24,20 +44,20 @@ static void update_auto() static void reload_commands_airstart() { init_commands(); - g.waypoint_index.load(); // XXX can we assume it's been loaded already by ::load_all? - decrement_WP_index(); + g.command_index.load(); // XXX can we assume it's been loaded already by ::load_all? + decrement_cmd_index(); } // Getters // ------- -static struct Location get_wp_with_index(int i) +static struct Location get_cmd_with_index(int i) { struct Location temp; long mem; // Find out proper location in memory by using the start_byte position + the index // -------------------------------------------------------------------------------- - if (i > g.waypoint_total) { + if (i > g.command_total) { temp.id = CMD_BLANK; }else{ // read WP position @@ -70,9 +90,9 @@ static struct Location get_wp_with_index(int i) // Setters // ------- -static void set_wp_with_index(struct Location temp, int i) +static void set_cmd_with_index(struct Location temp, int i) { - i = constrain(i, 0, g.waypoint_total.get()); + i = constrain(i, 0, g.command_total.get()); uint32_t mem = WP_START_BYTE + (i * WP_SIZE); // Set altitude options bitmask @@ -101,17 +121,17 @@ static void set_wp_with_index(struct Location temp, int i) eeprom_write_dword((uint32_t *) mem, temp.lng); } -static void increment_WP_index() +static void increment_cmd_index() { - if (g.waypoint_index <= g.waypoint_total) { - g.waypoint_index.set_and_save(g.waypoint_index + 1); + if (g.command_index <= g.command_total) { + g.command_index.set_and_save(g.command_index + 1); } } -static void decrement_WP_index() +static void decrement_cmd_index() { - if (g.waypoint_index > 0) { - g.waypoint_index.set_and_save(g.waypoint_index - 1); + if (g.command_index > 0) { + g.command_index.set_and_save(g.command_index - 1); } } @@ -225,9 +245,9 @@ void init_home() gcs_send_text_fmt(PSTR("gps alt: %lu"), (unsigned long)home.alt); - // Save Home to EEPROM + // Save Home to EEPROM - Command 0 // ------------------- - set_wp_with_index(home, 0); + set_cmd_with_index(home, 0); // Save prev loc // ------------- diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde index 8f73520e31..996fa11f44 100644 --- a/ArduPlane/commands_logic.pde +++ b/ArduPlane/commands_logic.pde @@ -4,13 +4,13 @@ // Command Event Handlers /********************************************************************************/ static void -handle_process_must() +handle_process_nav_cmd() { // reset navigation integrators // ------------------------- reset_I(); - switch(next_command.id){ + switch(next_nav_command.id){ case MAV_CMD_NAV_TAKEOFF: do_takeoff(); @@ -46,9 +46,9 @@ handle_process_must() } static void -handle_process_may() +handle_process_condition_command() { - switch(next_command.id){ + switch(next_nonnav_command.id){ case MAV_CMD_CONDITION_DELAY: do_wait_delay(); @@ -62,15 +62,14 @@ handle_process_may() do_change_alt(); break; - /* case MAV_CMD_NAV_LAND_OPTIONS: // TODO - Add the command or equiv to MAVLink (repair in verify_may() also) + /* case MAV_CMD_NAV_LAND_OPTIONS: // TODO - Add the command or equiv to MAVLink (repair in verify_condition() also) gcs_send_text_P(SEVERITY_LOW,PSTR("Landing options set")); // pitch in deg, airspeed m/s, throttle %, track WP 1 or 0 - landing_pitch = next_command.lng * 100; - g.airspeed_cruise = next_command.alt * 100; - g.throttle_cruise = next_command.lat; - landing_distance = next_command.p1; - //landing_roll = command.lng; + landing_pitch = next_nav_command.lng * 100; + g.airspeed_cruise = next_nav_command.alt * 100; + g.throttle_cruise = next_nav_command.lat; + landing_distance = next_nav_command.p1; SendDebug_P("MSG: throttle_cruise = "); SendDebugln(g.throttle_cruise,DEC); @@ -82,9 +81,9 @@ handle_process_may() } } -static void handle_process_now() +static void handle_process_do_command() { - switch(next_command.id){ + switch(next_nonnav_command.id){ case MAV_CMD_DO_JUMP: do_jump(); @@ -118,18 +117,18 @@ static void handle_process_now() static void handle_no_commands() { - next_command = home; - next_command.alt = read_alt_to_hold(); - next_command.id = MAV_CMD_NAV_LOITER_UNLIM; + next_nav_command = home; + next_nav_command.alt = read_alt_to_hold(); + next_nav_command.id = MAV_CMD_NAV_LOITER_UNLIM; } /********************************************************************************/ // Verify command Handlers /********************************************************************************/ -static bool verify_must() +static bool verify_nav_command() // Returns true if command complete { - switch(command_must_ID) { + switch(nav_command_ID) { case MAV_CMD_NAV_TAKEOFF: return verify_takeoff(); @@ -160,15 +159,15 @@ static bool verify_must() break; default: - gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_must: Invalid or no current Nav cmd")); + gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_nav: Invalid or no current Nav cmd")); return false; break; } } -static bool verify_may() +static bool verify_condition_command() // Returns true if command complete { - switch(command_may_ID) { + switch(non_nav_command_ID) { case NO_COMMAND: break; @@ -185,7 +184,7 @@ static bool verify_may() break; default: - gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_may: Invalid or no current Condition cmd")); + gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_conditon: Invalid or no current Condition cmd")); break; } return false; @@ -212,12 +211,12 @@ static void do_RTL(void) static void do_takeoff() { - set_next_WP(&next_command); + set_next_WP(&next_nav_command); // pitch in deg, airspeed m/s, throttle %, track WP 1 or 0 - takeoff_pitch = (int)next_command.p1 * 100; + takeoff_pitch = (int)next_nav_command.p1 * 100; //Serial.printf_P(PSTR("TO pitch:")); Serial.println(takeoff_pitch); //Serial.printf_P(PSTR("home.alt:")); Serial.println(home.alt); - takeoff_altitude = next_command.alt; + takeoff_altitude = next_nav_command.alt; //Serial.printf_P(PSTR("takeoff_altitude:")); Serial.println(takeoff_altitude); next_WP.lat = home.lat + 1000; // so we don't have bad calcs next_WP.lng = home.lng + 1000; // so we don't have bad calcs @@ -227,30 +226,30 @@ static void do_takeoff() static void do_nav_wp() { - set_next_WP(&next_command); + set_next_WP(&next_nav_command); } static void do_land() { - set_next_WP(&next_command); + set_next_WP(&next_nav_command); } static void do_loiter_unlimited() { - set_next_WP(&next_command); + set_next_WP(&next_nav_command); } static void do_loiter_turns() { - set_next_WP(&next_command); - loiter_total = next_command.p1 * 360; + set_next_WP(&next_nav_command); + loiter_total = next_nav_command.p1 * 360; } static void do_loiter_time() { - set_next_WP(&next_command); + set_next_WP(&next_nav_command); loiter_time = millis(); - loiter_time_max = next_command.p1; // units are (seconds * 10) + loiter_time_max = next_nav_command.p1; // units are (seconds * 10) } /********************************************************************************/ @@ -287,7 +286,7 @@ static bool verify_takeoff() static bool verify_land() { - // we don't verify landing - we never go to a new Must command after Land + // we don't verify landing - we never go to a new Nav command after Land if (((wp_distance > 0) && (wp_distance <= (2*g_gps->ground_speed/100))) || (current_loc.alt <= next_WP.alt + 300)){ @@ -317,7 +316,7 @@ static bool verify_nav_wp() hold_course = -1; update_crosstrack(); if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) { - gcs_send_text_fmt(PSTR("Reached Waypoint #%i"),command_must_index); + gcs_send_text_fmt(PSTR("Reached Waypoint #%i"),nav_command_index); return true; } // add in a more complex case @@ -341,7 +340,7 @@ static bool verify_loiter_time() update_loiter(); calc_bearing_error(); if ((millis() - loiter_time) > (unsigned long)loiter_time_max * 10000l) { // scale loiter_time_max from (sec*10) to milliseconds - gcs_send_text_P(SEVERITY_LOW,PSTR("verify_must: LOITER time complete")); + gcs_send_text_P(SEVERITY_LOW,PSTR("verify_nav: LOITER time complete")); return true; } return false; @@ -353,7 +352,7 @@ static bool verify_loiter_turns() calc_bearing_error(); if(loiter_sum > loiter_total) { loiter_total = 0; - gcs_send_text_P(SEVERITY_LOW,PSTR("verify_must: LOITER orbits complete")); + gcs_send_text_P(SEVERITY_LOW,PSTR("verify_nav: LOITER orbits complete")); // clear the command queue; return true; } @@ -377,13 +376,13 @@ static bool verify_RTL() static void do_wait_delay() { condition_start = millis(); - condition_value = next_command.lat * 1000; // convert to milliseconds + condition_value = next_nonnav_command.lat * 1000; // convert to milliseconds } static void do_change_alt() { - condition_rate = next_command.lat; - condition_value = next_command.alt; + condition_rate = next_nonnav_command.lat; + condition_value = next_nonnav_command.alt; target_altitude = current_loc.alt + (condition_rate / 10); // Divide by ten for 10Hz update next_WP.alt = condition_value; // For future nav calculations offset_altitude = 0; // For future nav calculations @@ -391,7 +390,7 @@ static void do_change_alt() static void do_within_distance() { - condition_value = next_command.lat; + condition_value = next_nonnav_command.lat; } /********************************************************************************/ @@ -438,53 +437,55 @@ static void do_loiter_at_location() static void do_jump() { struct Location temp; - if(next_command.lat > 0) { + if(next_nonnav_command.lat > 0) { - command_must_index = 0; - command_may_index = 0; - temp = get_wp_with_index(g.waypoint_index); - temp.lat = next_command.lat - 1; // Decrement repeat counter + nav_command_ID = NO_COMMAND; + non_nav_command_ID = NO_COMMAND; + temp = get_cmd_with_index(g.command_index); + temp.lat = next_nonnav_command.lat - 1; // Decrement repeat counter - set_wp_with_index(temp, g.waypoint_index); - g.waypoint_index.set_and_save(next_command.p1 - 1); - } else if (next_command.lat == -1) { - g.waypoint_index.set_and_save(next_command.p1 - 1); + set_cmd_with_index(temp, g.command_index); + g.command_index.set_and_save(next_nonnav_command.p1 - 1); + } else if (next_nonnav_command.lat == -1) { // A repeat count of -1 = repeat forever + nav_command_ID = NO_COMMAND; + non_nav_command_ID = NO_COMMAND; + g.command_index.set_and_save(next_nonnav_command.p1 - 1); } } static void do_change_speed() { // Note: we have no implementation for commanded ground speed, only air speed and throttle - if(next_command.alt > 0) - g.airspeed_cruise.set_and_save(next_command.alt * 100); + if(next_nonnav_command.alt > 0) + g.airspeed_cruise.set_and_save(next_nonnav_command.alt * 100); - if(next_command.lat > 0) - g.throttle_cruise.set_and_save(next_command.lat); + if(next_nonnav_command.lat > 0) + g.throttle_cruise.set_and_save(next_nonnav_command.lat); } static void do_set_home() { - if(next_command.p1 == 1 && GPS_enabled) { + if(next_nonnav_command.p1 == 1 && GPS_enabled) { init_home(); } else { home.id = MAV_CMD_NAV_WAYPOINT; - home.lng = next_command.lng; // Lon * 10**7 - home.lat = next_command.lat; // Lat * 10**7 - home.alt = max(next_command.alt, 0); + home.lng = next_nonnav_command.lng; // Lon * 10**7 + home.lat = next_nonnav_command.lat; // Lat * 10**7 + home.alt = max(next_nonnav_command.alt, 0); home_is_set = true; } } static void do_set_servo() { - APM_RC.OutputCh(next_command.p1 - 1, next_command.alt); + APM_RC.OutputCh(next_nonnav_command.p1 - 1, next_nonnav_command.alt); } static void do_set_relay() { - if (next_command.p1 == 1) { + if (next_nonnav_command.p1 == 1) { relay.on(); - } else if (next_command.p1 == 0) { + } else if (next_nonnav_command.p1 == 0) { relay.off(); }else{ relay.toggle(); @@ -493,16 +494,16 @@ static void do_set_relay() static void do_repeat_servo() { - event_id = next_command.p1 - 1; + event_id = next_nonnav_command.p1 - 1; - if(next_command.p1 >= CH_5 + 1 && next_command.p1 <= CH_8 + 1) { + if(next_nonnav_command.p1 >= CH_5 + 1 && next_nonnav_command.p1 <= CH_8 + 1) { event_timer = 0; - event_delay = next_command.lng * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds) - event_repeat = next_command.lat * 2; - event_value = next_command.alt; + event_delay = next_nonnav_command.lng * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds) + event_repeat = next_nonnav_command.lat * 2; + event_value = next_nonnav_command.alt; - switch(next_command.p1) { + switch(next_nonnav_command.p1) { case CH_5: event_undo_value = g.rc_5.radio_trim; break; @@ -524,7 +525,7 @@ static void do_repeat_relay() { event_id = RELAY_TOGGLE; event_timer = 0; - event_delay = next_command.lat * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds) - event_repeat = next_command.alt * 2; + event_delay = next_nonnav_command.lat * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds) + event_repeat = next_nonnav_command.alt * 2; update_events(); } diff --git a/ArduPlane/commands_process.pde b/ArduPlane/commands_process.pde index 35d3a02e64..b1b010650f 100644 --- a/ArduPlane/commands_process.pde +++ b/ArduPlane/commands_process.pde @@ -4,14 +4,13 @@ //---------------------------------------- static void change_command(uint8_t cmd_index) { - struct Location temp = get_wp_with_index(cmd_index); + struct Location temp = get_cmd_with_index(cmd_index); if (temp.id > MAV_CMD_NAV_LAST ){ gcs_send_text_P(SEVERITY_LOW,PSTR("Bad Request - cannot change to non-Nav cmd")); } else { - command_must_index = NO_COMMAND; - next_command.id = NO_COMMAND; - g.waypoint_index.set_and_save(cmd_index - 1); - load_next_command_from_EEPROM(); + nav_command_ID = NO_COMMAND; + next_nav_command.id = NO_COMMAND; + g.command_index.set_and_save(cmd_index - 1); process_next_command(); } } @@ -21,136 +20,113 @@ static void change_command(uint8_t cmd_index) // -------------------- static void update_commands(void) { - // This function loads commands into three buffers - // when a new command is loaded, it is processed with process_XXX() - // ---------------------------------------------------------------- if(home_is_set == false){ return; // don't do commands } if(control_mode == AUTO){ - load_next_command_from_EEPROM(); process_next_command(); } // Other (eg GCS_Auto) modes may be implemented here } static void verify_commands(void) { - if(verify_must()){ - command_must_index = NO_COMMAND; + if(verify_nav_command()){ + nav_command_ID = NO_COMMAND; } - if(verify_may()){ - command_may_index = NO_COMMAND; - command_may_ID = NO_COMMAND; + if(verify_condition_command()){ + non_nav_command_ID = NO_COMMAND; } } -static void load_next_command_from_EEPROM() -{ - // fetch next command if the next command queue is empty - // ----------------------------------------------------- - if(next_command.id == NO_COMMAND){ - next_command = get_wp_with_index(g.waypoint_index + 1); - } - - // If the preload failed, return or just Loiter - // generate a dynamic command for RTL - // -------------------------------------------- - if(next_command.id == NO_COMMAND){ - // we are out of commands! - gcs_send_text_P(SEVERITY_LOW,PSTR("out of commands!")); - handle_no_commands(); - } -} static void process_next_command() { + // This function makes sure that we always have a current navigation command + // and loads conditional or immediate commands if applicable + + struct Location temp; + // these are Navigation/Must commands // --------------------------------- - if (command_must_index == 0){ // no current command loaded - if (next_command.id < MAV_CMD_NAV_LAST ){ - increment_WP_index(); - //save_command_index(); // TO DO - fix - to Recover from in air Restart - command_must_index = g.waypoint_index; - - //SendDebug_P("MSG new command_must_id "); - //SendDebug(next_command.id,DEC); - //SendDebug_P(" index:"); - //SendDebugln(command_must_index,DEC); - if (g.log_bitmask & MASK_LOG_CMD) - Log_Write_Cmd(g.waypoint_index, &next_command); - process_must(); + if (nav_command_ID == NO_COMMAND){ // no current navigation command loaded + + temp.id = MAV_CMD_NAV_LAST; + while(temp.id >= MAV_CMD_NAV_LAST && nav_command_index <= g.command_total) { + nav_command_index++; + temp = get_cmd_with_index(nav_command_index); + } + if(nav_command_index > g.command_total){ + // we are out of commands! + gcs_send_text_P(SEVERITY_LOW,PSTR("out of commands!")); + handle_no_commands(); + } else { + next_nav_command = temp; + nav_command_ID = next_nav_command.id; + non_nav_command_index = NO_COMMAND; // This will cause the next intervening non-nav command (if any) to be loaded + non_nav_command_ID = NO_COMMAND; + + if (g.log_bitmask & MASK_LOG_CMD) { + Log_Write_Cmd(g.command_index, &next_nav_command); + } + process_nav_cmd(); } } - // these are Condition/May commands - // ---------------------- - if (command_may_index == 0){ - if (next_command.id > MAV_CMD_NAV_LAST && next_command.id < MAV_CMD_CONDITION_LAST ){ - increment_WP_index(); // this command is from the command list in EEPROM - command_may_index = g.waypoint_index; - //SendDebug_P("MSG new command_may_id "); - //SendDebug(next_command.id,DEC); - //Serial.printf_P(PSTR("new command_may_index ")); - //Serial.println(command_may_index,DEC); - if (g.log_bitmask & MASK_LOG_CMD) - Log_Write_Cmd(g.waypoint_index, &next_command); - process_may(); + // these are Condition/May and Do/Now commands + // ------------------------------------------- + if (non_nav_command_index == NO_COMMAND) { // If the index is NO_COMMAND then we have just loaded a nav command + non_nav_command_index = nav_command_index + 1; + } else if (non_nav_command_ID == NO_COMMAND) { // If the ID is NO_COMMAND then we have just completed a non-nav command + non_nav_command_index++; + } + + if(nav_command_index < g.command_total && non_nav_command_ID == NO_COMMAND) { + temp = get_cmd_with_index(non_nav_command_index); + if(temp.id <= MAV_CMD_NAV_LAST) { // The next command is a nav command. No non-nav commands to do + g.command_index.set_and_save(nav_command_index); + non_nav_command_index = nav_command_index; + non_nav_command_ID = WAIT_COMMAND; + } else { // The next command is a non-nav command. Prepare to execute it. + g.command_index.set_and_save(non_nav_command_index); + next_nonnav_command = temp; + non_nav_command_ID = next_nonnav_command.id; + if (g.log_bitmask & MASK_LOG_CMD) { + Log_Write_Cmd(g.command_index, &next_nonnav_command); + } + + process_non_nav_command(); } - // these are Do/Now commands - // --------------------------- - if (next_command.id > MAV_CMD_CONDITION_LAST){ - increment_WP_index(); // this command is from the command list in EEPROM - //SendDebug_P("MSG new command_now_id "); - //SendDebug(next_command.id,DEC); - if (g.log_bitmask & MASK_LOG_CMD) - Log_Write_Cmd(g.waypoint_index, &next_command); - process_now(); - } } } /**************************************************/ // These functions implement the commands. /**************************************************/ -static void process_must() +static void process_nav_cmd() { - gcs_send_text_P(SEVERITY_LOW,PSTR("New cmd: ")); + gcs_send_text_P(SEVERITY_LOW,PSTR("New nav command loaded")); - // clear May indexes - command_may_index = NO_COMMAND; - command_may_ID = NO_COMMAND; + // clear non-nav command ID and index + non_nav_command_index = NO_COMMAND; // Redundant - remove? + non_nav_command_ID = NO_COMMAND; // Redundant - remove? - command_must_ID = next_command.id; - handle_process_must(); + handle_process_nav_cmd(); - // invalidate command so a new one is loaded - // ----------------------------------------- - next_command.id = NO_COMMAND; } -static void process_may() +static void process_non_nav_command() { - gcs_send_text_P(SEVERITY_LOW,PSTR("")); + gcs_send_text_P(SEVERITY_LOW,PSTR("new non-nav command loaded")); - command_may_ID = next_command.id; - handle_process_may(); - - // invalidate command so a new one is loaded - // ----------------------------------------- - next_command.id = NO_COMMAND; + if(non_nav_command_ID < MAV_CMD_CONDITION_LAST) { + handle_process_condition_command(); + } else { + handle_process_do_command(); + // flag command ID so a new one is loaded + // ----------------------------------------- + non_nav_command_ID = NO_COMMAND; + } } - -static void process_now() -{ - handle_process_now(); - - // invalidate command so a new one is loaded - // ----------------------------------------- - next_command.id = NO_COMMAND; - - gcs_send_text(SEVERITY_LOW, ""); -} - diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index b049d477bd..5117f44156 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -77,6 +77,7 @@ // Commands - Note that APM now uses a subset of the MAVLink protocol commands. See enum MAV_CMD in the GCS_Mavlink library #define CMD_BLANK 0 // there is no command stored in the mem location requested #define NO_COMMAND 0 +#define WAIT_COMMAND 999 // Command/Waypoint/Location Options Bitmask //-------------------- @@ -169,7 +170,7 @@ enum gcs_severity { // Events // ------ #define EVENT_WILL_REACH_WAYPOINT 1 -#define EVENT_SET_NEW_WAYPOINT_INDEX 2 +#define EVENT_SET_NEW_COMMAND_INDEX 2 #define EVENT_LOADED_WAYPOINT 3 #define EVENT_LOOP 4 diff --git a/ArduPlane/navigation.pde b/ArduPlane/navigation.pde index 3c67117767..2f1e803c7a 100644 --- a/ArduPlane/navigation.pde +++ b/ArduPlane/navigation.pde @@ -100,7 +100,7 @@ static void calc_altitude_error() }else{ target_altitude = constrain(target_altitude, prev_WP.alt, next_WP.alt); } - } else if (command_may_ID != MAV_CMD_CONDITION_CHANGE_ALT) { + } else if (non_nav_command_ID != MAV_CMD_CONDITION_CHANGE_ALT) { target_altitude = next_WP.alt; } diff --git a/ArduPlane/test.pde b/ArduPlane/test.pde index 9c0adbd26d..b60876b509 100644 --- a/ArduPlane/test.pde +++ b/ArduPlane/test.pde @@ -306,12 +306,12 @@ test_wp(uint8_t argc, const Menu::arg *argv) Serial.printf_P(PSTR("Hold altitude of %dm\n"), (int)g.RTL_altitude/100); } - Serial.printf_P(PSTR("%d waypoints\n"), (int)g.waypoint_total); + Serial.printf_P(PSTR("%d waypoints\n"), (int)g.command_total); Serial.printf_P(PSTR("Hit radius: %d\n"), (int)g.waypoint_radius); Serial.printf_P(PSTR("Loiter radius: %d\n\n"), (int)g.loiter_radius); - for(byte i = 0; i <= g.waypoint_total; i++){ - struct Location temp = get_wp_with_index(i); + for(byte i = 0; i <= g.command_total; i++){ + struct Location temp = get_cmd_with_index(i); test_wp_print(&temp, i); }