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

Initial rewrite of command logic.

Browse Source 
Changes mission structure so that conditional and immediate commands are located between associated waypoints instead of after the second waypoint.
This commit is contained in:
Doug Weibel 2011-10-25 19:27:23 -06:00
parent 04b5776601
commit 645b9c1d48
10 changed files with 224 additions and 224 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
ArduPlane/ArduPlane.pde
View File

@ -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();

24
ArduPlane/GCS_Mavlink.pde
View File

@ -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,

8
ArduPlane/Log.pde
View File

@ -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);
}
}

12
ArduPlane/Parameters.h
View File

@ -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")),

62
ArduPlane/commands.pde
View File

@ -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
// -------------

135
ArduPlane/commands_logic.pde
View File

@ -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();
}

168
ArduPlane/commands_process.pde
View File

@ -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;
if (nav_command_ID == NO_COMMAND){ // no current navigation command loaded
//SendDebug_P("MSG <process_next_command> 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();
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 <process_next_command> 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 <process_next_command> 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: <process_must>"));
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("<process_may>"));
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, "<process_now>");
}

3
ArduPlane/defines.h
View File

@ -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

2
ArduPlane/navigation.pde
View File

@ -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;
}

6
ArduPlane/test.pde
View File

@ -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);
}