aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Rover: remove do_takeoff, do_change_alt support

Browse Source
This commit is contained in:
Randy Mackay 2014-03-17 16:50:27 +09:00
parent 2184ff1e58
commit de119e07bb
3 changed files with 0 additions and 55 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
APMrover2/APMrover2.pde
View File

@ -502,8 +502,6 @@ static int32_t condition_value;
// A starting value used to check the status of a conditional command.
// For example in a delay command the condition_start records that start time for the delay
static int32_t condition_start;
// A value used in condition commands. For example the rate at which to change altitude.
static int16_t condition_rate;
////////////////////////////////////////////////////////////////////////////////
// 3D Location vectors

10
APMrover2/GCS_Mavlink.pde
View File

@ -1293,11 +1293,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// set auto continue to 1
ret_packet.autocontinue = 1; // 1 (true), 0 (false)
// rover specific overrides to packet values
if (cmd.id == MAV_CMD_CONDITION_CHANGE_ALT) {
ret_packet.param1 = cmd.content.location.lat; // Copter and Plane set param1 = cmd.p1/100
}
mavlink_msg_mission_item_send(chan,msg->sysid,
msg->compid,
packet.seq,
@ -1505,11 +1500,6 @@ mission_item_send_failed:
goto mission_failed;
}
// rover specific overrides to mavlink to mission command conversion
if (cmd.id == MAV_CMD_CONDITION_CHANGE_ALT) {
cmd.content.location.lat = packet.param1;
}
if(packet.current == 2){ //current = 2 is a flag to tell us this is a "guided mode" waypoint and not for the mission
guided_WP = cmd.content.location;

43
APMrover2/commands_logic.pde
View File

@ -1,11 +1,9 @@
/// -*- 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);
@ -24,10 +22,6 @@ start_command(const AP_Mission::Mission_Command& cmd)
gcs_send_text_fmt(PSTR("Executing command ID #%i"),cmd.id);
switch(cmd.id){
case MAV_CMD_NAV_TAKEOFF:
do_takeoff(cmd);
break;
case MAV_CMD_NAV_WAYPOINT: // Navigate to Waypoint
do_nav_wp(cmd);
break;
@ -45,10 +39,6 @@ start_command(const AP_Mission::Mission_Command& cmd)
do_within_distance(cmd);
break;
case MAV_CMD_CONDITION_CHANGE_ALT:
do_change_alt(cmd);
break;
// Do commands
case MAV_CMD_DO_CHANGE_SPEED:
do_change_speed(cmd);
@ -148,9 +138,6 @@ static bool verify_command(const AP_Mission::Mission_Command& cmd)
switch(cmd.id) {
case MAV_CMD_NAV_TAKEOFF:
return verify_takeoff();
case MAV_CMD_NAV_WAYPOINT:
return verify_nav_wp(cmd);
@ -165,10 +152,6 @@ static bool verify_command(const AP_Mission::Mission_Command& cmd)
return verify_within_distance();
break;
case MAV_CMD_CONDITION_CHANGE_ALT:
return verify_change_alt();
break;
default:
gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_conditon: Unsupported command"));
return true;
@ -188,11 +171,6 @@ static void do_RTL(void)
next_WP = home;
}
static void do_takeoff(const AP_Mission::Mission_Command& cmd)
{
set_next_WP(cmd.content.location);
}
static void do_nav_wp(const AP_Mission::Mission_Command& cmd)
{
set_next_WP(cmd.content.location);
@ -201,10 +179,6 @@ static void do_nav_wp(const AP_Mission::Mission_Command& cmd)
/********************************************************************************/
// Verify Nav (Must) commands
/********************************************************************************/
static bool verify_takeoff()
{ return true;
}
static bool verify_nav_wp(const AP_Mission::Mission_Command& cmd)
{
if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) {
@ -253,14 +227,6 @@ static void do_wait_delay(const AP_Mission::Mission_Command& cmd)
condition_value = cmd.content.delay.seconds * 1000; // convert seconds to milliseconds
}
static void do_change_alt(const AP_Mission::Mission_Command& cmd)
{
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
}
static void do_within_distance(const AP_Mission::Mission_Command& cmd)
{
condition_value = cmd.content.distance.meters;
@ -279,15 +245,6 @@ static bool verify_wait_delay()
return false;
}
static bool verify_change_alt()
{
if( (condition_rate>=0 && current_loc.alt >= condition_value) || (condition_rate<=0 && current_loc.alt <= condition_value)) {
condition_value = 0;
return true;
}
return false;
}
static bool verify_within_distance()
{
if (wp_distance < condition_value){