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

Copter: add nav_script_time mission command support

This commit is contained in:
Randy Mackay 2022-02-17 21:59:16 +09:00
parent 87439eec80
commit 1251a91430
4 changed files with 130 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
ArduCopter/Copter.cpp
View File

@ -418,6 +418,32 @@ bool Copter::set_circle_rate(float rate_dps)
return true;
}
// returns true if mode supports NAV_SCRIPT_TIME mission commands
bool Copter::nav_scripting_enable(uint8_t mode)
{
return mode == (uint8_t)mode_auto.mode_number();
}
// lua scripts use this to retrieve the contents of the active command
bool Copter::nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2)
{
if (flightmode != &mode_auto) {
return false;
}
return mode_auto.nav_script_time(id, cmd, arg1, arg2);
}
// lua scripts use this to indicate when they have complete the command
void Copter::nav_script_time_done(uint16_t id)
{
if (flightmode != &mode_auto) {
return;
}
return mode_auto.nav_script_time_done(id);
}
#endif // AP_SCRIPTING_ENABLED

3
ArduCopter/Copter.h
View File

@ -653,6 +653,9 @@ private:
bool set_target_angle_and_climbrate(float roll_deg, float pitch_deg, float yaw_deg, float climb_rate_ms, bool use_yaw_rate, float yaw_rate_degs) override;
bool get_circle_radius(float &radius_m) override;
bool set_circle_rate(float rate_dps) override;
bool nav_scripting_enable(uint8_t mode) override;
bool nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2) override;
void nav_script_time_done(uint16_t id) override;
#endif // AP_SCRIPTING_ENABLED
void rc_loop();
void throttle_loop();

26
ArduCopter/mode.h
View File

@ -390,7 +390,7 @@ public:
bool has_manual_throttle() const override { return false; }
bool allows_arming(AP_Arming::Method method) const override;
bool is_autopilot() const override { return true; }
bool in_guided_mode() const override { return mode() == SubMode::NAVGUIDED; }
bool in_guided_mode() const override { return mode() == SubMode::NAVGUIDED || mode() == SubMode::NAV_SCRIPT_TIME; }
// Auto modes
enum class SubMode : uint8_t {
@ -404,6 +404,7 @@ public:
LOITER,
LOITER_TO_ALT,
NAV_PAYLOAD_PLACE,
NAV_SCRIPT_TIME,
};
// Auto
@ -438,6 +439,10 @@ public:
// Go straight to landing sequence via DO_LAND_START, if succeeds pretend to be Auto RTL mode
bool jump_to_landing_sequence_auto_RTL(ModeReason reason);
// lua accessors for nav script time support
bool nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2);
void nav_script_time_done(uint16_t id);
AP_Mission mission{
FUNCTOR_BIND_MEMBER(&ModeAuto::start_command, bool, const AP_Mission::Mission_Command &),
FUNCTOR_BIND_MEMBER(&ModeAuto::verify_command, bool, const AP_Mission::Mission_Command &),
@ -522,6 +527,9 @@ private:
#endif
void do_payload_place(const AP_Mission::Mission_Command& cmd);
void do_RTL(void);
#if AP_SCRIPTING_ENABLED
void do_nav_script_time(const AP_Mission::Mission_Command& cmd);
#endif
bool verify_takeoff();
bool verify_land();
@ -540,6 +548,9 @@ private:
bool verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd);
#endif
bool verify_nav_delay(const AP_Mission::Mission_Command& cmd);
#if AP_SCRIPTING_ENABLED
bool verify_nav_script_time();
#endif
// Loiter control
uint16_t loiter_time_max; // How long we should stay in Loiter Mode for mission scripting (time in seconds)
@ -582,6 +593,19 @@ private:
// True if we have entered AUTO to perform a DO_LAND_START landing sequence and we should report as AUTO RTL mode
bool auto_RTL;
#if AP_SCRIPTING_ENABLED
// nav_script_time command variables
struct {
bool done; // true once lua script indicates it has completed
uint16_t id; // unique id to avoid race conditions between commands and lua scripts
uint32_t start_ms; // system time nav_script_time command was received (used for timeout)
uint8_t command; // command number provided by mission command
uint8_t timeout_s; // timeout (in seconds) provided by mission command
float arg1; // 1st argument provided by mission command
float arg2; // 2nd argument provided by mission command
} nav_scripting;
#endif
};
#if AUTOTUNE_ENABLED == ENABLED

79
ArduCopter/mode_auto.cpp
View File

@ -135,7 +135,8 @@ void ModeAuto::run()
break;
case SubMode::NAVGUIDED:
#if NAV_GUIDED == ENABLED
case SubMode::NAV_SCRIPT_TIME:
#if NAV_GUIDED == ENABLED || AP_SCRIPTING_ENABLED
nav_guided_run();
#endif
break;
@ -199,6 +200,31 @@ bool ModeAuto::jump_to_landing_sequence_auto_RTL(ModeReason reason)
return false;
}
// lua scripts use this to retrieve the contents of the active command
bool ModeAuto::nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2)
{
#if AP_SCRIPTING_ENABLED
if (_mode == SubMode::NAV_SCRIPT_TIME) {
id = nav_scripting.id;
cmd = nav_scripting.command;
arg1 = nav_scripting.arg1;
arg2 = nav_scripting.arg2;
return true;
}
#endif
return false;
}
// lua scripts use this to indicate when they have complete the command
void ModeAuto::nav_script_time_done(uint16_t id)
{
#if AP_SCRIPTING_ENABLED
if ((_mode == SubMode::NAV_SCRIPT_TIME) && (id == nav_scripting.id)) {
nav_scripting.done = true;
}
#endif
}
// auto_loiter_start - initialises loitering in auto mode
// returns success/failure because this can be called by exit_mission
bool ModeAuto::loiter_start()
@ -520,6 +546,12 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd)
do_payload_place(cmd);
break;
#if AP_SCRIPTING_ENABLED
case MAV_CMD_NAV_SCRIPT_TIME:
do_nav_script_time(cmd);
break;
#endif
//
// conditional commands
//
@ -749,6 +781,12 @@ bool ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd)
cmd_complete = verify_nav_delay(cmd);
break;
#if AP_SCRIPTING_ENABLED
case MAV_CMD_NAV_SCRIPT_TIME:
cmd_complete = verify_nav_script_time();
break;
#endif
///
/// conditional commands
///
@ -904,7 +942,7 @@ void ModeAuto::circle_run()
}
}
#if NAV_GUIDED == ENABLED
#if NAV_GUIDED == ENABLED || AP_SCRIPTING_ENABLED
// auto_nav_guided_run - allows control by external navigation controller
// called by auto_run at 100hz or more
void ModeAuto::nav_guided_run()
@ -912,7 +950,7 @@ void ModeAuto::nav_guided_run()
// call regular guided flight mode run function
copter.mode_guided.run();
}
#endif // NAV_GUIDED
#endif // NAV_GUIDED || AP_SCRIPTING_ENABLED
// auto_loiter_run - loiter in AUTO flight mode
// called by auto_run at 100hz or more
@ -1442,6 +1480,28 @@ void ModeAuto::do_nav_delay(const AP_Mission::Mission_Command& cmd)
gcs().send_text(MAV_SEVERITY_INFO, "Delaying %u sec", (unsigned)(nav_delay_time_max_ms/1000));
}
#if AP_SCRIPTING_ENABLED
// start accepting position, velocity and acceleration targets from lua scripts
void ModeAuto::do_nav_script_time(const AP_Mission::Mission_Command& cmd)
{
// call regular guided flight mode initialisation
if (copter.mode_guided.init(true)) {
_mode = SubMode::NAV_SCRIPT_TIME;
nav_scripting.done = false;
nav_scripting.id++;
nav_scripting.start_ms = millis();
nav_scripting.command = cmd.content.nav_script_time.command;
nav_scripting.timeout_s = cmd.content.nav_script_time.timeout_s;
nav_scripting.arg1 = cmd.content.nav_script_time.arg1;
nav_scripting.arg2 = cmd.content.nav_script_time.arg2;
} else {
// for safety we set nav_scripting to done to protect against the mission getting stuck
nav_scripting.done = true;
}
}
#endif
/********************************************************************************/
// Condition (May) commands
/********************************************************************************/
@ -1968,4 +2028,17 @@ bool ModeAuto::verify_nav_delay(const AP_Mission::Mission_Command& cmd)
return false;
}
#if AP_SCRIPTING_ENABLED
// check if verify_nav_script_time command has completed
bool ModeAuto::verify_nav_script_time()
{
// if done or timeout then return true
if (nav_scripting.done ||
(AP_HAL::millis() - nav_scripting.start_ms) > (nav_scripting.timeout_s * 1000)) {
return true;
}
return false;
}
#endif
#endif