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

Rover: add reason to set_mode

This commit is contained in:
khancyr 2017-07-24 19:05:59 +02:00 committed by Andrew Tridgell
parent c4fa8cb7a4
commit da425c008c
11 changed files with 39 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
APMrover2/GCS_Mavlink.cpp
View File

@ -796,7 +796,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
switch (packet.command) {
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
rover.set_mode(rover.mode_rtl);
rover.set_mode(rover.mode_rtl, MODE_REASON_GCS_COMMAND);
result = MAV_RESULT_ACCEPTED;
break;
@ -861,7 +861,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
break;
case MAV_CMD_MISSION_START:
rover.set_mode(rover.mode_auto);
rover.set_mode(rover.mode_auto, MODE_REASON_GCS_COMMAND);
result = MAV_RESULT_ACCEPTED;
break;
@ -918,19 +918,19 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
switch (static_cast<uint16_t>(packet.param1)) {
case MAV_MODE_MANUAL_ARMED:
case MAV_MODE_MANUAL_DISARMED:
rover.set_mode(rover.mode_manual);
rover.set_mode(rover.mode_manual, MODE_REASON_GCS_COMMAND);
result = MAV_RESULT_ACCEPTED;
break;
case MAV_MODE_AUTO_ARMED:
case MAV_MODE_AUTO_DISARMED:
rover.set_mode(rover.mode_auto);
rover.set_mode(rover.mode_auto, MODE_REASON_GCS_COMMAND);
result = MAV_RESULT_ACCEPTED;
break;
case MAV_MODE_STABILIZE_DISARMED:
case MAV_MODE_STABILIZE_ARMED:
rover.set_mode(rover.mode_learning);
rover.set_mode(rover.mode_learning, MODE_REASON_GCS_COMMAND);
result = MAV_RESULT_ACCEPTED;
break;

2
APMrover2/Log.cpp
View File

@ -526,7 +526,7 @@ void Rover::Log_Write_Vehicle_Startup_Messages()
{
// only 200(?) bytes are guaranteed by DataFlash
Log_Write_Startup(TYPE_GROUNDSTART_MSG);
DataFlash.Log_Write_Mode(control_mode->mode_number());
DataFlash.Log_Write_Mode(control_mode->mode_number(), control_mode_reason);
Log_Write_Home_And_Origin();
gps.Write_DataFlash_Log_Startup_messages();
}

3
APMrover2/Rover.h
View File

@ -233,6 +233,7 @@ private:
// This is the state of the flight control system
// There are multiple states defined such as MANUAL, AUTO, ...
Mode *control_mode;
mode_reason_t control_mode_reason = MODE_REASON_INITIALISED;
// Used to maintain the state of the previous control switch position
// This is set to -1 when we need to re-read the switch
@ -467,7 +468,7 @@ private:
void gcs_retry_deferred(void);
Mode *control_mode_from_num(enum mode num);
bool set_mode(Mode &mode);
bool set_mode(Mode &mode, mode_reason_t reason);
bool mavlink_set_mode(uint8_t mode);
void do_erase_logs(void);

2
APMrover2/afs_rover.cpp
View File

@ -21,7 +21,7 @@ void AP_AdvancedFailsafe_Rover::terminate_vehicle(void)
rover.disarm_motors();
// Set to HOLD mode
rover.set_mode(rover.mode_hold);
rover.set_mode(rover.mode_hold, MODE_REASON_CRASH_FAILSAFE);
}
/*

4
APMrover2/commands_logic.cpp
View File

@ -126,7 +126,7 @@ bool Rover::start_command(const AP_Mission::Mission_Command& cmd)
void Rover::exit_mission()
{
gcs().send_text(MAV_SEVERITY_NOTICE, "No commands. Can't set AUTO. Setting HOLD");
set_mode(mode_hold);
set_mode(mode_hold, MODE_REASON_MISSION_END);
}
// verify_command_callback - callback function called from ap-mission at 10hz or higher when a command is being run
@ -205,7 +205,7 @@ bool Rover::verify_command(const AP_Mission::Mission_Command& cmd)
void Rover::do_RTL(void)
{
set_mode(mode_rtl);
set_mode(mode_rtl, MODE_REASON_MISSION_COMMAND);
}
void Rover::do_nav_wp(const AP_Mission::Mission_Command& cmd)

4
APMrover2/control_modes.cpp
View File

@ -72,7 +72,7 @@ void Rover::read_control_switch()
Mode *new_mode = control_mode_from_num((enum mode)modes[switchPosition].get());
if (new_mode != nullptr) {
set_mode(*new_mode);
set_mode(*new_mode, MODE_REASON_TX_COMMAND);
}
oldSwitchPosition = switchPosition;
@ -155,7 +155,7 @@ void Rover::read_trim_switch()
}
} else if (control_mode == &mode_auto) {
// if SW7 is ON in AUTO = set to RTL
set_mode(mode_rtl);
set_mode(mode_rtl, MODE_REASON_TX_COMMAND);
break;
}
}

2
APMrover2/crash_check.cpp
View File

@ -38,7 +38,7 @@ void Rover::crash_check()
// send message to gcs
gcs().send_text(MAV_SEVERITY_EMERGENCY, "Crash: Going to HOLD");
// change mode to hold and disarm
set_mode(mode_hold);
set_mode(mode_hold, MODE_REASON_CRASH_FAILSAFE);
if (g.fs_crash_check == FS_CRASH_HOLD_AND_DISARM) {
disarm_motors();
}

14
APMrover2/defines.h
View File

@ -95,7 +95,9 @@ enum mode {
#define MAVLINK_SET_ATT_TYPE_MASK_ATTITUDE_IGNORE (1<<7)
// Error message sub systems and error codes
#define ERROR_SUBSYSTEM_CRASH_CHECK 12
#define ERROR_SUBSYSTEM_FAILSAFE_FENCE 9
#define ERROR_SUBSYSTEM_FLIGHT_MODE 10
#define ERROR_SUBSYSTEM_CRASH_CHECK 12
// subsystem specific error codes -- crash checker
#define ERROR_CODE_CRASH_CHECK_CRASH 1
@ -106,3 +108,13 @@ enum fs_crash_action {
};
#define DISTANCE_HOME_MAX 0.5f // Distance max to home location before changing it when disarm
enum mode_reason_t {
MODE_REASON_INITIALISED = 0,
MODE_REASON_TX_COMMAND,
MODE_REASON_GCS_COMMAND,
MODE_REASON_FAILSAFE,
MODE_REASON_MISSION_END,
MODE_REASON_CRASH_FAILSAFE,
MODE_REASON_MISSION_COMMAND
};

4
APMrover2/failsafe.cpp
View File

@ -82,10 +82,10 @@ void Rover::failsafe_trigger(uint8_t failsafe_type, bool on)
case 0:
break;
case 1:
set_mode(mode_rtl);
set_mode(mode_rtl, MODE_REASON_FAILSAFE);
break;
case 2:
set_mode(mode_hold);
set_mode(mode_hold, MODE_REASON_FAILSAFE);
break;
}
}

2
APMrover2/mode_rtl.cpp
View File

@ -23,6 +23,6 @@ void ModeRTL::update_navigation()
{
// no loitering around the wp with the rover, goes direct to the wp position
if (rover.verify_RTL()) {
rover.set_mode(rover.mode_hold);
rover.set_mode(rover.mode_hold, MODE_REASON_MISSION_END);
}
}

14
APMrover2/system.cpp
View File

@ -213,7 +213,7 @@ void Rover::init_ardupilot()
if (initial_mode == nullptr) {
initial_mode = &mode_initializing;
}
set_mode(*initial_mode);
set_mode(*initial_mode, MODE_REASON_INITIALISED);
// set the correct flight mode
@ -232,7 +232,7 @@ void Rover::init_ardupilot()
//*********************************************************************************
void Rover::startup_ground(void)
{
set_mode(mode_initializing);
set_mode(mode_initializing, MODE_REASON_INITIALISED);
gcs().send_text(MAV_SEVERITY_INFO, "<startup_ground> Ground start");
@ -283,7 +283,7 @@ void Rover::set_reverse(bool reverse)
in_reverse = reverse;
}
bool Rover::set_mode(Mode &new_mode)
bool Rover::set_mode(Mode &new_mode, mode_reason_t reason)
{
if (control_mode == &new_mode) {
// don't switch modes if we are already in the correct mode.
@ -292,6 +292,9 @@ bool Rover::set_mode(Mode &new_mode)
Mode &old_mode = *control_mode;
if (!new_mode.enter()) {
// Log error that we failed to enter desired flight mode
Log_Write_Error(ERROR_SUBSYSTEM_FLIGHT_MODE, new_mode.mode_number());
gcs().send_text(MAV_SEVERITY_WARNING, "Flight mode change failed");
return false;
}
@ -304,7 +307,8 @@ bool Rover::set_mode(Mode &new_mode)
old_mode.exit();
if (should_log(MASK_LOG_MODE)) {
DataFlash.Log_Write_Mode(control_mode->mode_number());
control_mode_reason = reason;
DataFlash.Log_Write_Mode(control_mode->mode_number(), reason);
}
notify_mode((enum mode)control_mode->mode_number());
@ -320,7 +324,7 @@ bool Rover::mavlink_set_mode(uint8_t mode)
if (new_mode == nullptr) {
return false;
}
return set_mode(*new_mode);
return set_mode(*new_mode, MODE_REASON_GCS_COMMAND);
}
void Rover::startup_INS_ground(void)