From b5c58d5c133c3ba2cfc98657040b02b487789ae8 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 25 May 2022 14:20:27 +0900 Subject: [PATCH] Copter: auto supports NAV_ATTITUDE_TIME command --- ArduCopter/mode.h | 13 +++++++++ ArduCopter/mode_auto.cpp | 59 ++++++++++++++++++++++++++++++++++++++++ 2 files changed, 72 insertions(+) diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index a4008e3beb..266f9b6e2c 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -419,6 +419,7 @@ public: LOITER_TO_ALT, NAV_PAYLOAD_PLACE, NAV_SCRIPT_TIME, + NAV_ATTITUDE_TIME, }; // Auto @@ -500,6 +501,7 @@ private: void nav_guided_run(); void loiter_run(); void loiter_to_alt_run(); + void nav_attitude_time_run(); Location loc_from_cmd(const AP_Mission::Mission_Command& cmd, const Location& default_loc) const; @@ -546,6 +548,7 @@ private: #if AP_SCRIPTING_ENABLED void do_nav_script_time(const AP_Mission::Mission_Command& cmd); #endif + void do_nav_attitude_time(const AP_Mission::Mission_Command& cmd); bool verify_takeoff(); bool verify_land(); @@ -567,6 +570,7 @@ private: #if AP_SCRIPTING_ENABLED bool verify_nav_script_time(); #endif + bool verify_nav_attitude_time(const AP_Mission::Mission_Command& cmd); // Loiter control uint16_t loiter_time_max; // How long we should stay in Loiter Mode for mission scripting (time in seconds) @@ -622,6 +626,15 @@ private: float arg2; // 2nd argument provided by mission command } nav_scripting; #endif + + // nav attitude time command variables + struct { + int16_t roll_deg; // target roll angle in degrees. provided by mission command + int8_t pitch_deg; // target pitch angle in degrees. provided by mission command + int16_t yaw_deg; // target yaw angle in degrees. provided by mission command + float climb_rate; // climb rate in m/s. provided by mission command + uint32_t start_ms; // system time that nav attitude time command was recieved (used for timeout) + } nav_attitude_time; }; #if AUTOTUNE_ENABLED == ENABLED diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 5eccb91e93..a500672543 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -152,6 +152,10 @@ void ModeAuto::run() case SubMode::NAV_PAYLOAD_PLACE: payload_place_run(); break; + + case SubMode::NAV_ATTITUDE_TIME: + nav_attitude_time_run(); + break; } // only pretend to be in auto RTL so long as mission still thinks its in a landing sequence or the mission has completed @@ -594,6 +598,10 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd) break; #endif + case MAV_CMD_NAV_ATTITUDE_TIME: + do_nav_attitude_time(cmd); + break; + // // conditional commands // @@ -831,6 +839,10 @@ bool ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd) break; #endif + case MAV_CMD_NAV_ATTITUDE_TIME: + cmd_complete = verify_nav_attitude_time(cmd); + break; + /// /// conditional commands /// @@ -1084,6 +1096,35 @@ void ModeAuto::loiter_to_alt_run() pos_control->update_z_controller(); } +// maintain an attitude for a specified time +void ModeAuto::nav_attitude_time_run() +{ + // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately + if (is_disarmed_or_landed() || !motors->get_interlock()) { + make_safe_ground_handling(); + return; + } + + // constrain climb rate + float target_climb_rate_cms = constrain_float(nav_attitude_time.climb_rate * 100.0, pos_control->get_max_speed_down_cms(), pos_control->get_max_speed_up_cms()); + + // get avoidance adjusted climb rate + target_climb_rate_cms = get_avoidance_adjusted_climbrate(target_climb_rate_cms); + + // limit and scale lean angles + const float angle_limit_cd = MAX(1000.0f, MIN(copter.aparm.angle_max, attitude_control->get_althold_lean_angle_max_cd())); + Vector2f target_rp_cd(nav_attitude_time.roll_deg * 100, nav_attitude_time.pitch_deg * 100); + target_rp_cd.limit_length(angle_limit_cd); + + // send targets to attitude controller + attitude_control->input_euler_angle_roll_pitch_yaw(target_rp_cd.x, target_rp_cd.y, nav_attitude_time.yaw_deg * 100, true); + + // Send the commanded climb rate to the position controller + pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate_cms); + + pos_control->update_z_controller(); +} + // auto_payload_place_run - places an object in auto mode // called by auto_run at 100hz or more void ModeAuto::payload_place_run() @@ -1548,6 +1589,18 @@ void ModeAuto::do_nav_script_time(const AP_Mission::Mission_Command& cmd) } #endif +// start maintaining an attitude for a specified time +void ModeAuto::do_nav_attitude_time(const AP_Mission::Mission_Command& cmd) +{ + _mode = SubMode::NAV_ATTITUDE_TIME; + + // copy command arguments into local structure + nav_attitude_time.roll_deg = cmd.content.nav_attitude_time.roll_deg; + nav_attitude_time.pitch_deg = cmd.content.nav_attitude_time.pitch_deg; + nav_attitude_time.yaw_deg = cmd.content.nav_attitude_time.yaw_deg; + nav_attitude_time.climb_rate = cmd.content.nav_attitude_time.climb_rate; + nav_attitude_time.start_ms = AP_HAL::millis(); +} /********************************************************************************/ // Condition (May) commands @@ -2083,6 +2136,12 @@ bool ModeAuto::verify_nav_script_time() } #endif +// check if nav_attitude_time command has completed +bool ModeAuto::verify_nav_attitude_time(const AP_Mission::Mission_Command& cmd) +{ + return ((AP_HAL::millis() - nav_attitude_time.start_ms) > (cmd.content.nav_attitude_time.time_sec * 1000)); +} + // pause - Prevent aircraft from progressing along the track bool ModeAuto::pause() {