Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Rover: Mission integration into command execution

This commit is contained in:
Randy Mackay 2014-03-10 17:45:26 +09:00
parent df1b2e1192
commit 4ca3a92655
6 changed files with 120 additions and 393 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
APMrover2/Steering.pde
View File

@ -138,13 +138,13 @@ static void calc_lateral_acceleration()
{
switch (control_mode) {
case AUTO:
nav_controller->update_waypoint(prev_WP, next_WP);
nav_controller->update_waypoint(prev_WP.content.location, next_WP.content.location);
break;
case RTL:
case GUIDED:
case STEERING:
nav_controller->update_waypoint(current_loc, next_WP);
nav_controller->update_waypoint(current_loc, next_WP.content.location);
break;
default:
return;

118
APMrover2/commands.pde
View File

@ -1,106 +1,19 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/* Functions in this file:
void init_commands()
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_next_WP(const AP_Mission::Mission_Command& cmd)
void set_guided_WP(void)
void init_home()
void restart_nav()
************************************************************
*/
static void init_commands()
{
g.command_index.set_and_save(0);
nav_command_ID = NO_COMMAND;
non_nav_command_ID = NO_COMMAND;
next_nav_command.id = CMD_BLANK;
}
// Getters
// -------
static struct Location get_cmd_with_index(int i)
{
struct Location temp;
uint16_t mem;
// Find out proper location in memory by using the start_byte position + the index
// --------------------------------------------------------------------------------
if (i > g.command_total) {
memset(&temp, 0, sizeof(temp));
temp.id = CMD_BLANK;
}else{
// read WP position
mem = (WP_START_BYTE) + (i * WP_SIZE);
temp.id = hal.storage->read_byte(mem);
mem++;
temp.options = hal.storage->read_byte(mem);
mem++;
temp.p1 = hal.storage->read_byte(mem);
mem++;
temp.alt = (long)hal.storage->read_dword(mem);
mem += 4;
temp.lat = (long)hal.storage->read_dword(mem);
mem += 4;
temp.lng = (long)hal.storage->read_dword(mem);
}
// Add on home altitude if we are a nav command (or other command with altitude) and stored alt is relative
if((temp.id < MAV_CMD_NAV_LAST || temp.id == MAV_CMD_CONDITION_CHANGE_ALT) && temp.options & LOCATION_MASK_OPTIONS_RELATIVE_ALT){
temp.alt += home.alt;
}
return temp;
}
// Setters
// -------
static void set_cmd_with_index(struct Location temp, int i)
{
i = constrain_int16(i, 0, g.command_total.get());
uint16_t mem = WP_START_BYTE + (i * WP_SIZE);
// Set altitude options bitmask
// XXX What is this trying to do?
if ((temp.options & LOCATION_MASK_OPTIONS_RELATIVE_ALT) && i != 0){
temp.options = LOCATION_MASK_OPTIONS_RELATIVE_ALT;
} else {
temp.options = 0;
}
hal.storage->write_byte(mem, temp.id);
mem++;
hal.storage->write_byte(mem, temp.options);
mem++;
hal.storage->write_byte(mem, temp.p1);
mem++;
hal.storage->write_dword(mem, temp.alt);
mem += 4;
hal.storage->write_dword(mem, temp.lat);
mem += 4;
hal.storage->write_dword(mem, temp.lng);
}
/*
This function stores waypoint commands
It looks to see what the next command type is and finds the last command.
*/
static void set_next_WP(const struct Location *wp)
static void set_next_WP(const AP_Mission::Mission_Command& cmd)
{
// copy the current WP into the OldWP slot
// ---------------------------------------
@ -108,20 +21,20 @@ static void set_next_WP(const struct Location *wp)
// Load the next_WP slot
// ---------------------
next_WP = *wp;
next_WP = cmd;
// are we already past the waypoint? This happens when we jump
// waypoints, and it can cause us to skip a waypoint. If we are
// past the waypoint when we start on a leg, then use the current
// location as the previous waypoint, to prevent immediately
// considering the waypoint complete
if (location_passed_point(current_loc, prev_WP, next_WP)) {
if (location_passed_point(current_loc, prev_WP.content.location, next_WP.content.location)) {
gcs_send_text_P(SEVERITY_LOW, PSTR("Resetting prev_WP"));
prev_WP = current_loc;
prev_WP.content.location = current_loc;
}
// this is handy for the groundstation
wp_totalDistance = get_distance(current_loc, next_WP);
wp_totalDistance = get_distance(current_loc, next_WP.content.location);
wp_distance = wp_totalDistance;
}
@ -129,14 +42,14 @@ static void set_guided_WP(void)
{
// copy the current location into the OldWP slot
// ---------------------------------------
prev_WP = current_loc;
prev_WP.content.location = current_loc;
// Load the next_WP slot
// ---------------------
next_WP = guided_WP;
next_WP.content.location = guided_WP;
// this is handy for the groundstation
wp_totalDistance = get_distance(current_loc, next_WP);
wp_totalDistance = get_distance(current_loc, next_WP.content.location);
wp_distance = wp_totalDistance;
}
@ -154,13 +67,12 @@ void init_home()
ahrs.set_home(g_gps->latitude, g_gps->longitude, g_gps->altitude_cm);
home_is_set = true;
// Save Home to EEPROM - Command 0
// -------------------
set_cmd_with_index(home, 0);
// Save Home to EEPROM
mission.write_home_to_storage();
// Save prev loc
// -------------
next_WP = prev_WP = home;
next_WP.content.location = prev_WP.content.location = home;
// Load home for a default guided_WP
// -------------
@ -170,8 +82,6 @@ void init_home()
static void restart_nav()
{
g.pidSpeedThrottle.reset_I();
prev_WP = current_loc;
nav_command_ID = NO_COMMAND;
nav_command_index = 0;
process_next_command();
prev_WP.content.location = current_loc;
mission.resume();
}

243
APMrover2/commands_logic.pde
View File

@ -1,103 +1,90 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// forward declarations to make compiler happy
static void do_takeoff(const AP_Mission::Mission_Command& cmd);
static void do_nav_wp(const AP_Mission::Mission_Command& cmd);
static void do_wait_delay(const AP_Mission::Mission_Command& cmd);
static void do_within_distance(const AP_Mission::Mission_Command& cmd);
static void do_change_alt(const AP_Mission::Mission_Command& cmd);
static void do_change_speed(const AP_Mission::Mission_Command& cmd);
static void do_set_home(const AP_Mission::Mission_Command& cmd);
static bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);
/********************************************************************************/
// Command Event Handlers
/********************************************************************************/
static void
handle_process_nav_cmd()
static bool
start_command(const AP_Mission::Mission_Command& cmd)
{
gcs_send_text_fmt(PSTR("Executing command ID #%i"),next_nav_command.id);
gcs_send_text_fmt(PSTR("Executing command ID #%i"),cmd.id);
switch(next_nav_command.id){
switch(cmd.id){
case MAV_CMD_NAV_TAKEOFF:
do_takeoff();
do_takeoff(cmd);
break;
case MAV_CMD_NAV_WAYPOINT: // Navigate to Waypoint
do_nav_wp();
do_nav_wp(cmd);
break;
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
do_RTL();
break;
default:
break;
}
}
static void
handle_process_condition_command()
{
gcs_send_text_fmt(PSTR("Executing command ID #%i"),next_nonnav_command.id);
switch(next_nonnav_command.id){
// Conditional commands
case MAV_CMD_CONDITION_DELAY:
do_wait_delay();
do_wait_delay(cmd);
break;
case MAV_CMD_CONDITION_DISTANCE:
do_within_distance();
do_within_distance(cmd);
break;
case MAV_CMD_CONDITION_CHANGE_ALT:
do_change_alt();
break;
default:
break;
}
}
static void handle_process_do_command()
{
gcs_send_text_fmt(PSTR("Executing command ID #%i"),next_nonnav_command.id);
switch(next_nonnav_command.id){
case MAV_CMD_DO_JUMP:
do_jump();
do_change_alt(cmd);
break;
// Do commands
case MAV_CMD_DO_CHANGE_SPEED:
do_change_speed();
do_change_speed(cmd);
break;
case MAV_CMD_DO_SET_HOME:
do_set_home();
do_set_home(cmd);
break;
case MAV_CMD_DO_SET_SERVO:
ServoRelayEvents.do_set_servo(next_nonnav_command.p1, next_nonnav_command.alt);
ServoRelayEvents.do_set_servo(cmd.p1, cmd.content.location.alt);
break;
case MAV_CMD_DO_SET_RELAY:
ServoRelayEvents.do_set_relay(next_nonnav_command.p1, next_nonnav_command.alt);
ServoRelayEvents.do_set_relay(cmd.p1, cmd.content.location.alt);
break;
case MAV_CMD_DO_REPEAT_SERVO:
ServoRelayEvents.do_repeat_servo(next_nonnav_command.p1, next_nonnav_command.alt,
next_nonnav_command.lat, next_nonnav_command.lng);
ServoRelayEvents.do_repeat_servo(cmd.p1, cmd.content.location.alt,
cmd.content.location.lat, cmd.content.location.lng);
break;
case MAV_CMD_DO_REPEAT_RELAY:
ServoRelayEvents.do_repeat_relay(next_nonnav_command.p1, next_nonnav_command.alt,
next_nonnav_command.lat);
ServoRelayEvents.do_repeat_relay(cmd.p1, cmd.content.location.alt,
cmd.content.location.lat);
break;
#if CAMERA == ENABLED
case MAV_CMD_DO_CONTROL_VIDEO: // Control on-board camera capturing. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty|
break;
case MAV_CMD_DO_CONTROL_VIDEO: // Control on-board camera capturing. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty|
break;
case MAV_CMD_DO_DIGICAM_CONFIGURE: // Mission command to configure an on-board camera controller system. |Modes: P, TV, AV, M, Etc| Shutter speed: Divisor number for one second| Aperture: F stop number| ISO number e.g. 80, 100, 200, Etc| Exposure type enumerator| Command Identity| Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)|
break;
case MAV_CMD_DO_DIGICAM_CONFIGURE: // Mission command to configure an on-board camera controller system. |Modes: P, TV, AV, M, Etc| Shutter speed: Divisor number for one second| Aperture: F stop number| ISO number e.g. 80, 100, 200, Etc| Exposure type enumerator| Command Identity| Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)|
break;
case MAV_CMD_DO_DIGICAM_CONTROL: // Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty|
do_take_picture();
break;
case MAV_CMD_DO_DIGICAM_CONTROL: // Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty|
do_take_picture();
break;
case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
camera.set_trigger_distance(next_nonnav_command.alt);
break;
case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
camera.set_trigger_distance(cmd.content.location.alt);
break;
#endif
#if MOUNT == ENABLED
@ -121,64 +108,56 @@ static void handle_process_do_command()
camera_mount.control_cmd();
break;
#endif
default:
// return false for unhandled commands
return false;
}
// if we got this far we must have been successful
return true;
}
static void handle_no_commands()
{
static void exit_mission()
{
gcs_send_text_fmt(PSTR("No commands - setting HOLD"));
set_mode(HOLD);
}
/********************************************************************************/
// Verify command Handlers
// Returns true if command complete
/********************************************************************************/
static bool verify_nav_command() // Returns true if command complete
static bool verify_command(const AP_Mission::Mission_Command& cmd)
{
switch(nav_command_ID) {
switch(cmd.id) {
case MAV_CMD_NAV_TAKEOFF:
return verify_takeoff();
case MAV_CMD_NAV_WAYPOINT:
return verify_nav_wp();
return verify_nav_wp(cmd);
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
return verify_RTL();
default:
gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_nav: Invalid or no current Nav cmd"));
return false;
}
}
case MAV_CMD_CONDITION_DELAY:
return verify_wait_delay();
break;
static bool verify_condition_command() // Returns true if command complete
{
switch(non_nav_command_ID) {
case NO_COMMAND:
break;
case MAV_CMD_CONDITION_DISTANCE:
return verify_within_distance();
break;
case MAV_CMD_CONDITION_DELAY:
return verify_wait_delay();
break;
case MAV_CMD_CONDITION_CHANGE_ALT:
return verify_change_alt();
break;
case MAV_CMD_CONDITION_DISTANCE:
return verify_within_distance();
break;
case MAV_CMD_CONDITION_CHANGE_ALT:
return verify_change_alt();
break;
case WAIT_COMMAND:
return 0;
break;
default:
gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_conditon: Invalid or no current Condition cmd"));
break;
default:
gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_conditon: Unsupported command"));
return true;
break;
}
return false;
}
@ -189,19 +168,19 @@ static bool verify_condition_command() // Returns true if command complete
static void do_RTL(void)
{
prev_WP = current_loc;
prev_WP.content.location = current_loc;
control_mode = RTL;
next_WP = home;
next_WP.content.location = home;
}
static void do_takeoff()
static void do_takeoff(const AP_Mission::Mission_Command& cmd)
{
set_next_WP(&next_nav_command);
set_next_WP(cmd);
}
static void do_nav_wp()
static void do_nav_wp(const AP_Mission::Mission_Command& cmd)
{
set_next_WP(&next_nav_command);
set_next_WP(cmd);
}
/********************************************************************************/
@ -211,20 +190,20 @@ static bool verify_takeoff()
{ return true;
}
static bool verify_nav_wp()
static bool verify_nav_wp(const AP_Mission::Mission_Command& cmd)
{
if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) {
gcs_send_text_fmt(PSTR("Reached Waypoint #%i dist %um"),
(unsigned)nav_command_index,
(unsigned)get_distance(current_loc, next_WP));
(unsigned)cmd.index,
(unsigned)get_distance(current_loc, next_WP.content.location));
return true;
}
// have we gone past the waypoint?
if (location_passed_point(current_loc, prev_WP, next_WP)) {
if (location_passed_point(current_loc, prev_WP.content.location, next_WP.content.location)) {
gcs_send_text_fmt(PSTR("Passed Waypoint #%i dist %um"),
(unsigned)nav_command_index,
(unsigned)get_distance(current_loc, next_WP));
(unsigned)cmd.index,
(unsigned)get_distance(current_loc, next_WP.content.location));
return true;
}
@ -240,9 +219,9 @@ static bool verify_RTL()
}
// have we gone past the waypoint?
if (location_passed_point(current_loc, prev_WP, next_WP)) {
if (location_passed_point(current_loc, prev_WP.content.location, next_WP.content.location)) {
gcs_send_text_fmt(PSTR("Reached Home dist %um"),
(unsigned)get_distance(current_loc, next_WP));
(unsigned)get_distance(current_loc, next_WP.content.location));
return true;
}
@ -253,23 +232,23 @@ static bool verify_RTL()
// Condition (May) commands
/********************************************************************************/
static void do_wait_delay()
static void do_wait_delay(const AP_Mission::Mission_Command& cmd)
{
condition_start = millis();
condition_value = next_nonnav_command.lat * 1000; // convert to milliseconds
condition_value = cmd.content.location.lat * 1000; // convert to milliseconds
}
static void do_change_alt()
static void do_change_alt(const AP_Mission::Mission_Command& cmd)
{
condition_rate = abs((int)next_nonnav_command.lat);
condition_value = next_nonnav_command.alt;
condition_rate = abs((int)cmd.content.location.lat);
condition_value = cmd.content.location.alt;
if(condition_value < current_loc.alt) condition_rate = -condition_rate;
next_WP.alt = condition_value; // For future nav calculations
next_WP.content.location.alt = condition_value; // For future nav calculations
}
static void do_within_distance()
static void do_within_distance(const AP_Mission::Mission_Command& cmd)
{
condition_value = next_nonnav_command.lat;
condition_value = cmd.content.location.lat;
}
/********************************************************************************/
@ -307,60 +286,30 @@ static bool verify_within_distance()
// Do (Now) commands
/********************************************************************************/
static void do_jump()
static void do_change_speed(const AP_Mission::Mission_Command& cmd)
{
struct Location temp;
gcs_send_text_fmt(PSTR("In jump. Jumps left: %i"),next_nonnav_command.lat);
if(next_nonnav_command.lat > 0) {
nav_command_ID = NO_COMMAND;
next_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_cmd_with_index(temp, g.command_index);
gcs_send_text_fmt(PSTR("setting command index: %i"),next_nonnav_command.p1 - 1);
g.command_index.set_and_save(next_nonnav_command.p1 - 1);
nav_command_index = next_nonnav_command.p1 - 1;
next_WP = prev_WP; // Need to back "next_WP" up as it was set to the next waypoint following the jump
process_next_command();
} 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;
gcs_send_text_fmt(PSTR("setting command index: %i"),next_nonnav_command.p1 - 1);
g.command_index.set_and_save(next_nonnav_command.p1 - 1);
nav_command_index = next_nonnav_command.p1 - 1;
next_WP = prev_WP; // Need to back "next_WP" up as it was set to the next waypoint following the jump
process_next_command();
}
}
static void do_change_speed()
{
switch (next_nonnav_command.p1)
switch (cmd.p1)
{
case 0:
if (next_nonnav_command.alt > 0) {
g.speed_cruise.set(next_nonnav_command.alt);
if (cmd.content.location.alt > 0) {
g.speed_cruise.set(cmd.content.location.alt);
gcs_send_text_fmt(PSTR("Cruise speed: %.1f"), g.speed_cruise.get());
}
break;
}
if (next_nonnav_command.lat > 0) {
g.throttle_cruise.set(next_nonnav_command.lat);
if (cmd.content.location.lat > 0) {
g.throttle_cruise.set(cmd.content.location.lat);
gcs_send_text_fmt(PSTR("Cruise throttle: %.1f"), g.throttle_cruise.get());
}
}
static void do_set_home()
static void do_set_home(const AP_Mission::Mission_Command& cmd)
{
if(next_nonnav_command.p1 == 1 && have_position) {
if(cmd.p1 == 1 && have_position) {
init_home();
} else {
ahrs.set_home(next_nonnav_command.lat, next_nonnav_command.lng, next_nonnav_command.alt);
ahrs.set_home(cmd.content.location.lat, cmd.content.location.lng, cmd.content.location.alt);
home_is_set = true;
}
}

140
APMrover2/commands_process.pde
View File

@ -1,140 +1,12 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// For changing active command mid-mission
//----------------------------------------
static void change_command(uint8_t 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 {
gcs_send_text_fmt(PSTR("Received Request - jump to command #%i"),cmd_index);
nav_command_ID = NO_COMMAND;
next_nav_command.id = NO_COMMAND;
non_nav_command_ID = NO_COMMAND;
nav_command_index = cmd_index - 1;
g.command_index.set_and_save(cmd_index);
update_commands();
}
}
// called by 10 Hz loop
// called by update navigation at 10Hz
// --------------------
static void update_commands(void)
{
if(control_mode == AUTO){
if(home_is_set == true && g.command_total > 1){
process_next_command();
}
} // Other (eg GCS_Auto) modes may be implemented here
if(control_mode == AUTO) {
if(home_is_set == true && mission.num_commands() > 1) {
mission.update();
}
}
}
static void verify_commands(void)
{
if(verify_nav_command()){
nav_command_ID = NO_COMMAND;
}
if(verify_condition_command()){
non_nav_command_ID = NO_COMMAND;
}
}
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;
uint8_t old_index = 0;
// these are Navigation/Must commands
// ---------------------------------
if (nav_command_ID == NO_COMMAND){ // no current navigation command loaded
old_index = nav_command_index;
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);
}
gcs_send_text_fmt(PSTR("Nav command index updated to #%i"),nav_command_index);
if(nav_command_index > g.command_total){
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;
process_nav_cmd();
}
}
// 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 = old_index + 1;
//gcs_send_text_fmt(PSTR("Non-Nav command index #%i"),non_nav_command_index);
} 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++;
}
//gcs_send_text_fmt(PSTR("Nav command index #%i"),nav_command_index);
//gcs_send_text_fmt(PSTR("Non-Nav command index #%i"),non_nav_command_index);
//gcs_send_text_fmt(PSTR("Non-Nav command ID #%i"),non_nav_command_ID);
if(nav_command_index <= (int)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;
gcs_send_text_fmt(PSTR("Non-Nav command ID updated to #%i"),non_nav_command_ID);
} 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;
gcs_send_text_fmt(PSTR("Non-Nav command ID updated to #%i"),non_nav_command_ID);
process_non_nav_command();
}
}
}
/**************************************************/
// These functions implement the commands.
/**************************************************/
static void process_nav_cmd()
{
//gcs_send_text_P(SEVERITY_LOW,PSTR("New nav command loaded"));
// clear non-nav command ID and index
non_nav_command_index = NO_COMMAND; // Redundant - remove?
non_nav_command_ID = NO_COMMAND; // Redundant - remove?
handle_process_nav_cmd();
}
static void process_non_nav_command()
{
//gcs_send_text_P(SEVERITY_LOW,PSTR("new non-nav command loaded"));
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;
}
}

4
APMrover2/navigation.pde
View File

@ -11,13 +11,13 @@ static void navigate()
return;
}
if ((next_WP.lat == 0)||(home_is_set==false)){
if ((next_WP.content.location.lat == 0)||(home_is_set==false)){
return;
}
// waypoint distance from rover
// ----------------------------
wp_distance = get_distance(current_loc, next_WP);
wp_distance = get_distance(current_loc, next_WP.content.location);
if (wp_distance < 0){
gcs_send_text_P(SEVERITY_HIGH,PSTR("<navigate> WP error - distance < 0"));

4
APMrover2/system.pde
View File

@ -267,10 +267,6 @@ static void startup_ground(void)
// ---------------------------
trim_radio();
// initialize commands
// -------------------
init_commands();
hal.uartA->set_blocking_writes(false);
hal.uartC->set_blocking_writes(false);