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

Copter: move mission into mode_auto

Browse Source
This commit is contained in:
Peter Barker 2018-04-25 09:31:01 +10:00 committed by Randy Mackay
parent 4a449cd307
commit 73940df48e
6 changed files with 73 additions and 74 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
ArduCopter/Copter.h
View File

@ -268,24 +268,6 @@ private:
SITL::SITL sitl;
#endif
// Mission library
#if MODE_AUTO_ENABLED == ENABLED
AP_Mission mission{
FUNCTOR_BIND_MEMBER(&Copter::start_command, bool, const AP_Mission::Mission_Command &),
FUNCTOR_BIND_MEMBER(&Copter::verify_command_callback, bool, const AP_Mission::Mission_Command &),
FUNCTOR_BIND_MEMBER(&Copter::exit_mission, void)};
bool start_command(const AP_Mission::Mission_Command& cmd) {
return mode_auto.start_command(cmd);
}
bool verify_command_callback(const AP_Mission::Mission_Command& cmd) {
return mode_auto.verify_command_callback(cmd);
}
void exit_mission() {
mode_auto.exit_mission();
}
#endif
// Arming/Disarming mangement class
AP_Arming_Copter arming;
@ -542,8 +524,8 @@ private:
AP_LandingGear landinggear;
// terrain handling
#if AP_TERRAIN_AVAILABLE && AC_TERRAIN
AP_Terrain terrain{mission, rally};
#if AP_TERRAIN_AVAILABLE && AC_TERRAIN && MODE_AUTO_ENABLED == ENABLED
AP_Terrain terrain{mode_auto.mission, rally};
#endif
// Precision Landing
@ -932,6 +914,7 @@ private:
ModeAltHold mode_althold;
#if MODE_AUTO_ENABLED == ENABLED
ModeAuto mode_auto;
AP_Mission &mission = mode_auto.mission; // so parameters work only!
#endif
#if AUTOTUNE_ENABLED == ENABLED
ModeAutoTune mode_autotune;

6
ArduCopter/GCS_Mavlink.cpp
View File

@ -782,8 +782,8 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_
case MAV_CMD_MISSION_START:
if (copter.motors->armed() && copter.set_mode(AUTO, MODE_REASON_GCS_COMMAND)) {
copter.set_auto_armed(true);
if (copter.mission.state() != AP_Mission::MISSION_RUNNING) {
copter.mission.start_or_resume();
if (copter.mode_auto.mission.state() != AP_Mission::MISSION_RUNNING) {
copter.mode_auto.mission.start_or_resume();
}
return MAV_RESULT_ACCEPTED;
}
@ -1490,7 +1490,7 @@ bool GCS_MAVLINK_Copter::accept_packet(const mavlink_status_t &status, mavlink_m
AP_Mission *GCS_MAVLINK_Copter::get_mission()
{
#if MODE_AUTO_ENABLED == ENABLED
return &copter.mission;
return &copter.mode_auto.mission;
#else
return nullptr;
#endif

13
ArduCopter/mode.h
View File

@ -307,14 +307,14 @@ public:
void payload_place_start();
// only out here temporarily
bool start_command(const AP_Mission::Mission_Command& cmd);
bool verify_command_callback(const AP_Mission::Mission_Command& cmd);
void exit_mission();
// for GCS_MAVLink to call:
bool do_guided(const AP_Mission::Mission_Command& cmd);
AP_Mission mission{
FUNCTOR_BIND_MEMBER(&Copter::ModeAuto::start_command, bool, const AP_Mission::Mission_Command &),
FUNCTOR_BIND_MEMBER(&Copter::ModeAuto::verify_command, bool, const AP_Mission::Mission_Command &),
FUNCTOR_BIND_MEMBER(&Copter::ModeAuto::exit_mission, void)};
protected:
const char *name() const override { return "AUTO"; }
@ -328,7 +328,9 @@ protected:
private:
bool start_command(const AP_Mission::Mission_Command& cmd);
bool verify_command(const AP_Mission::Mission_Command& cmd);
void exit_mission();
void takeoff_run();
void wp_run();
@ -434,7 +436,6 @@ private:
float descend_start_altitude;
float descend_max; // centimetres
} nav_payload_place;
};
#if AUTOTUNE_ENABLED == ENABLED

99
ArduCopter/mode_auto.cpp
View File

@ -22,11 +22,11 @@
// auto_init - initialise auto controller
bool Copter::ModeAuto::init(bool ignore_checks)
{
if ((copter.position_ok() && copter.mission.num_commands() > 1) || ignore_checks) {
if ((copter.position_ok() && mission.num_commands() > 1) || ignore_checks) {
_mode = Auto_Loiter;
// reject switching to auto mode if landed with motors armed but first command is not a takeoff (reduce chance of flips)
if (motors->armed() && ap.land_complete && !copter.mission.starts_with_takeoff_cmd()) {
if (motors->armed() && ap.land_complete && !mission.starts_with_takeoff_cmd()) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Auto: Missing Takeoff Cmd");
return false;
}
@ -44,7 +44,7 @@ bool Copter::ModeAuto::init(bool ignore_checks)
copter.mode_guided.limit_clear();
// start/resume the mission (based on MIS_RESTART parameter)
copter.mission.start_or_resume();
mission.start_or_resume();
return true;
} else {
return false;
@ -380,7 +380,7 @@ bool Copter::ModeAuto::start_command(const AP_Mission::Mission_Command& cmd)
{
// To-Do: logging when new commands start/end
if (copter.should_log(MASK_LOG_CMD)) {
copter.DataFlash.Log_Write_Mission_Cmd(copter.mission, cmd);
copter.DataFlash.Log_Write_Mission_Cmd(mission, cmd);
}
switch(cmd.id) {
@ -513,23 +513,6 @@ bool Copter::ModeAuto::start_command(const AP_Mission::Mission_Command& cmd)
return true;
}
// verify_command_callback - callback function called from ap-mission at 10hz or higher when a command is being run
// we double check that the flight mode is AUTO to avoid the possibility of ap-mission triggering actions while we're not in AUTO mode
bool Copter::ModeAuto::verify_command_callback(const AP_Mission::Mission_Command& cmd)
{
if (copter.flightmode == &copter.mode_auto) {
bool cmd_complete = verify_command(cmd);
// send message to GCS
if (cmd_complete) {
gcs().send_mission_item_reached_message(cmd.index);
}
return cmd_complete;
}
return false;
}
// exit_mission - function that is called once the mission completes
void Copter::ModeAuto::exit_mission()
{
@ -602,7 +585,7 @@ bool Copter::ModeAuto::get_wp(Location_Class& destination)
// update mission
void Copter::ModeAuto::run_autopilot()
{
copter.mission.update();
mission.update();
}
/*******************************************************************************
@ -614,61 +597,83 @@ should move onto the next mission element.
Return true if we do not recognize the command so that we move on to the next command
*******************************************************************************/
// verify_command - callback function called from ap-mission at 10hz or higher when a command is being run
// we double check that the flight mode is AUTO to avoid the possibility of ap-mission triggering actions while we're not in AUTO mode
bool Copter::ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd)
{
if (copter.flightmode != &copter.mode_auto) {
return false;
}
bool cmd_complete = false;
switch (cmd.id) {
//
// navigation commands
//
case MAV_CMD_NAV_TAKEOFF:
return verify_takeoff();
cmd_complete = verify_takeoff();
break;
case MAV_CMD_NAV_WAYPOINT:
return verify_nav_wp(cmd);
cmd_complete = verify_nav_wp(cmd);
break;
case MAV_CMD_NAV_LAND:
return verify_land();
cmd_complete = verify_land();
break;
case MAV_CMD_NAV_PAYLOAD_PLACE:
return verify_payload_place();
cmd_complete = verify_payload_place();
break;
case MAV_CMD_NAV_LOITER_UNLIM:
return verify_loiter_unlimited();
cmd_complete = verify_loiter_unlimited();
break;
case MAV_CMD_NAV_LOITER_TURNS:
return verify_circle(cmd);
cmd_complete = verify_circle(cmd);
break;
case MAV_CMD_NAV_LOITER_TIME:
return verify_loiter_time();
cmd_complete = verify_loiter_time();
break;
case MAV_CMD_NAV_LOITER_TO_ALT:
return verify_loiter_to_alt();
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
return verify_RTL();
cmd_complete = verify_RTL();
break;
case MAV_CMD_NAV_SPLINE_WAYPOINT:
return verify_spline_wp(cmd);
cmd_complete = verify_spline_wp(cmd);
break;
#if NAV_GUIDED == ENABLED
case MAV_CMD_NAV_GUIDED_ENABLE:
return verify_nav_guided_enable(cmd);
cmd_complete = verify_nav_guided_enable(cmd);
break;
#endif
case MAV_CMD_NAV_DELAY:
return verify_nav_delay(cmd);
cmd_complete = verify_nav_delay(cmd);
break;
///
/// conditional commands
///
case MAV_CMD_CONDITION_DELAY:
return verify_wait_delay();
cmd_complete = verify_wait_delay();
break;
case MAV_CMD_CONDITION_DISTANCE:
return verify_within_distance();
cmd_complete = verify_within_distance();
break;
case MAV_CMD_CONDITION_YAW:
return verify_yaw();
cmd_complete = verify_yaw();
break;
// do commands (always return true)
case MAV_CMD_DO_CHANGE_SPEED:
@ -679,14 +684,24 @@ bool Copter::ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd)
case MAV_CMD_DO_GUIDED_LIMITS:
case MAV_CMD_DO_FENCE_ENABLE:
case MAV_CMD_DO_WINCH:
return true;
cmd_complete = true;
break;
default:
// error message
gcs().send_text(MAV_SEVERITY_WARNING,"Skipping invalid cmd #%i",cmd.id);
// return true if we do not recognize the command so that we move on to the next command
return true;
cmd_complete = true;
break;
}
// send message to GCS
if (cmd_complete) {
gcs().send_mission_item_reached_message(cmd.index);
}
return cmd_complete;
}
// auto_takeoff_run - takeoff in auto mode
@ -1089,7 +1104,7 @@ void Copter::ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd)
// if no delay as well as not final waypoint set the waypoint as "fast"
AP_Mission::Mission_Command temp_cmd;
if (loiter_time_max == 0 && copter.mission.get_next_nav_cmd(cmd.index+1, temp_cmd)) {
if (loiter_time_max == 0 && mission.get_next_nav_cmd(cmd.index+1, temp_cmd)) {
copter.wp_nav->set_fast_waypoint(true);
}
}
@ -1229,9 +1244,9 @@ void Copter::ModeAuto::do_spline_wp(const AP_Mission::Mission_Command& cmd)
// if previous command was a wp_nav command with no delay set stopped_at_start to false
// To-Do: move processing of delay into wp-nav controller to allow it to determine the stopped_at_start value itself?
uint16_t prev_cmd_idx = copter.mission.get_prev_nav_cmd_index();
uint16_t prev_cmd_idx = mission.get_prev_nav_cmd_index();
if (prev_cmd_idx != AP_MISSION_CMD_INDEX_NONE) {
if (copter.mission.read_cmd_from_storage(prev_cmd_idx, temp_cmd)) {
if (mission.read_cmd_from_storage(prev_cmd_idx, temp_cmd)) {
if ((temp_cmd.id == MAV_CMD_NAV_WAYPOINT || temp_cmd.id == MAV_CMD_NAV_SPLINE_WAYPOINT) && temp_cmd.p1 == 0) {
stopped_at_start = false;
}
@ -1240,7 +1255,7 @@ void Copter::ModeAuto::do_spline_wp(const AP_Mission::Mission_Command& cmd)
// if there is no delay at the end of this segment get next nav command
Location_Class next_loc;
if (cmd.p1 == 0 && copter.mission.get_next_nav_cmd(cmd.index+1, temp_cmd)) {
if (cmd.p1 == 0 && mission.get_next_nav_cmd(cmd.index+1, temp_cmd)) {
next_loc = temp_cmd.content.location;
// default lat, lon to first waypoint's lat, lon
if (next_loc.lat == 0 && next_loc.lng == 0) {

2
ArduCopter/motors.cpp
View File

@ -282,7 +282,7 @@ void Copter::init_disarm_motors()
#if MODE_AUTO_ENABLED == ENABLED
// reset the mission
mission.reset();
mode_auto.mission.reset();
#endif
DataFlash_Class::instance()->set_vehicle_armed(false);

4
ArduCopter/system.cpp
View File

@ -233,7 +233,7 @@ void Copter::init_ardupilot()
#if MODE_AUTO_ENABLED == ENABLED
// initialise mission library
mission.init();
mode_auto.mission.init();
#endif
#if MODE_SMARTRTL_ENABLED == ENABLED
@ -243,7 +243,7 @@ void Copter::init_ardupilot()
// initialise DataFlash library
#if MODE_AUTO_ENABLED == ENABLED
DataFlash.set_mission(&mission);
DataFlash.set_mission(&mode_auto.mission);
#endif
DataFlash.setVehicle_Startup_Log_Writer(FUNCTOR_BIND(&copter, &Copter::Log_Write_Vehicle_Startup_Messages, void));