From 0f5177b291b7ef4a8d0868e539123f7dd2951722 Mon Sep 17 00:00:00 2001 From: Buzz Date: Sat, 16 May 2020 14:02:01 +1000 Subject: [PATCH] Plane: Support a set of offboard MAVLink guided controls with rates + 3 rounds of fixes --- ArduPlane/ArduPlane.cpp | 4 +- ArduPlane/Attitude.cpp | 30 +++++++ ArduPlane/GCS_Mavlink.cpp | 157 +++++++++++++++++++++++++++++++++++++ ArduPlane/GCS_Mavlink.h | 1 + ArduPlane/Log.cpp | 159 ++++++++++++++++++++++++++++++++++++++ ArduPlane/Parameters.cpp | 6 ++ ArduPlane/Parameters.h | 6 ++ ArduPlane/Plane.h | 25 ++++++ ArduPlane/altitude.cpp | 23 +++++- ArduPlane/config.h | 3 + ArduPlane/defines.h | 12 +++ ArduPlane/mode.cpp | 8 ++ ArduPlane/navigation.cpp | 23 ++++++ 13 files changed, 455 insertions(+), 2 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 9438860abd..18247e0a75 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -221,8 +221,10 @@ void Plane::update_logging2(void) #endif } - if (should_log(MASK_LOG_NTUN)) + if (should_log(MASK_LOG_NTUN)) { Log_Write_Nav_Tuning(); + Log_Write_Guided(); + } if (should_log(MASK_LOG_RC)) Log_Write_RC(); diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 6b3a80a414..09dcb0b11e 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -588,6 +588,36 @@ void Plane::calc_nav_roll() plane.guided_state.last_forced_rpy_ms.x > 0 && millis() - plane.guided_state.last_forced_rpy_ms.x < 3000) { commanded_roll = plane.guided_state.forced_rpy_cd.x; +#if OFFBOARD_GUIDED == ENABLED + // guided_state.target_heading is radians at this point between -pi and pi ( defaults to -4 ) + } else if ((control_mode == &mode_guided) && (guided_state.target_heading_type != GUIDED_HEADING_NONE) ) { + uint32_t tnow = AP_HAL::millis(); + float delta = (tnow - guided_state.target_heading_time_ms) * 1e-3f; + guided_state.target_heading_time_ms = tnow; + + float error = 0.0f; + if (guided_state.target_heading_type == GUIDED_HEADING_HEADING) { + error = wrap_PI(guided_state.target_heading - AP::ahrs().yaw); + } else { + Vector2f groundspeed = AP::ahrs().groundspeed_vector(); + error = wrap_PI(guided_state.target_heading - atan2f(-groundspeed.y, -groundspeed.x) + M_PI); + } + + float bank_limit = degrees(atanf(guided_state.target_heading_accel_limit/GRAVITY_MSS)) * 1e2f; + + g2.guidedHeading.update_error(error); // push error into AC_PID , possible improvement is to use update_all instead.? + g2.guidedHeading.set_dt(delta); + + float i = g2.guidedHeading.get_i(); // get integrator TODO + if (((is_negative(error) && !guided_state.target_heading_limit_low) || (is_positive(error) && !guided_state.target_heading_limit_high))) { + i = g2.guidedHeading.get_i(); + } + + float desired = g2.guidedHeading.get_p() + i + g2.guidedHeading.get_d(); + guided_state.target_heading_limit_low = (desired <= -bank_limit); + guided_state.target_heading_limit_high = (desired >= bank_limit); + commanded_roll = constrain_float(desired, -bank_limit, bank_limit); +#endif // OFFBOARD_GUIDED == ENABLED } nav_roll_cd = constrain_int32(commanded_roll, -roll_limit_cd, roll_limit_cd); diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index d02d2fae51..c6a88a399b 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -794,13 +794,170 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_do_reposition(const mavlink_com return MAV_RESULT_FAILED; } +// these are GUIDED mode commands that are RATE or slew enabled, so you can have more powerful control than default controls. +MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_guided_slew_commands(const mavlink_command_int_t &packet) +{ + switch(packet.command) { + +#if OFFBOARD_GUIDED == ENABLED + case MAV_CMD_GUIDED_CHANGE_SPEED: { + // command is only valid in guided mode + if (plane.control_mode != &plane.mode_guided) { + return MAV_RESULT_FAILED; + } + + // only airspeed commands are supported right now... + if (int(packet.param1) != SPEED_TYPE_AIRSPEED) { // since SPEED_TYPE is int in range 0-1 and packet.param1 is a *float* this works. + return MAV_RESULT_DENIED; + } + + // reject airspeeds that are outside of the tuning envelope + if (packet.param2 > plane.aparm.airspeed_max || packet.param2 < plane.aparm.airspeed_min) { + return MAV_RESULT_DENIED; + } + + // no need to process any new packet/s with the + // same airspeed any further, if we are already doing it. + float new_target_airspeed_cm = packet.param2 * 100; + if ( is_equal(new_target_airspeed_cm,plane.guided_state.target_airspeed_cm)) { + return MAV_RESULT_ACCEPTED; + } + plane.guided_state.target_airspeed_cm = new_target_airspeed_cm; + plane.guided_state.target_airspeed_time_ms = AP_HAL::millis(); + + if (is_zero(packet.param3)) { + // the user wanted /maximum acceleration, pick a large value as close enough + plane.guided_state.target_airspeed_accel = 1000.0f; + } else { + plane.guided_state.target_airspeed_accel = fabsf(packet.param3); + } + + // assign an acceleration direction + if (plane.guided_state.target_airspeed_cm < plane.target_airspeed_cm) { + plane.guided_state.target_airspeed_accel *= -1.0f; + } + return MAV_RESULT_ACCEPTED; + } + + case MAV_CMD_GUIDED_CHANGE_ALTITUDE: { + // command is only valid in guided + if (plane.control_mode != &plane.mode_guided) { + return MAV_RESULT_FAILED; + } + + // disallow default value of -1 and dangerous value of zero + if (is_equal(packet.z, -1.0f) || is_equal(packet.z, 0.0f)){ + return MAV_RESULT_DENIED; + } + + // the requested alt data might be relative or absolute + float new_target_alt = packet.z * 100; + float new_target_alt_rel = packet.z * 100 + plane.home.alt; + + // only global/relative/terrain frames are supported + switch(packet.frame) { + case MAV_FRAME_GLOBAL_RELATIVE_ALT: { + if (is_equal(plane.guided_state.target_alt,new_target_alt_rel) ) { // compare two floats as near-enough + // no need to process any new packet/s with the same ALT any further, if we are already doing it. + return MAV_RESULT_ACCEPTED; + } + plane.guided_state.target_alt = new_target_alt_rel; + break; + } + case MAV_FRAME_GLOBAL: { + if (is_equal(plane.guided_state.target_alt,new_target_alt) ) { // compare two floats as near-enough + // no need to process any new packet/s with the same ALT any further, if we are already doing it. + return MAV_RESULT_ACCEPTED; + } + plane.guided_state.target_alt = new_target_alt; + break; + } + default: + // MAV_RESULT_DENIED means Command is invalid (is supported but has invalid parameters). + return MAV_RESULT_DENIED; + } + + plane.guided_state.target_alt_frame = packet.frame; + plane.guided_state.last_target_alt = plane.current_loc.alt; // FIXME: Reference frame is not corrected for here + plane.guided_state.target_alt_time_ms = AP_HAL::millis(); + + if (is_zero(packet.param3)) { + // the user wanted /maximum acceleration, pick a large value as close enough + plane.guided_state.target_alt_accel = 1000.0; + } else { + plane.guided_state.target_alt_accel = fabsf(packet.param3); + } + + // assign an acceleration direction + if (plane.guided_state.target_alt < plane.current_loc.alt) { + plane.guided_state.target_alt_accel *= -1.0f; + } + return MAV_RESULT_ACCEPTED; + } + + case MAV_CMD_GUIDED_CHANGE_HEADING: { + + // command is only valid in guided mode + if (plane.control_mode != &plane.mode_guided) { + return MAV_RESULT_FAILED; + } + + // don't accept packets outside of [0-360] degree range + if (packet.param2 < 0.0f || packet.param2 >= 360.0f) { + return MAV_RESULT_DENIED; + } + + float new_target_heading = radians(wrap_180(packet.param2)); + + // if packet is requesting us to go to the heading we are already going to, we-re already on it. + if ( (is_equal(new_target_heading,plane.guided_state.target_heading))) { // compare two floats as near-enough + return MAV_RESULT_ACCEPTED; + } + + // course over ground + if ( int(packet.param1) == HEADING_TYPE_COURSE_OVER_GROUND) { // compare as nearest int + plane.guided_state.target_heading_type = GUIDED_HEADING_COG; + plane.prev_WP_loc = plane.current_loc; + // normal vehicle heading + } else if (int(packet.param1) == HEADING_TYPE_HEADING) { // compare as nearest int + plane.guided_state.target_heading_type = GUIDED_HEADING_HEADING; + } else { + // MAV_RESULT_DENIED means Command is invalid (is supported but has invalid parameters). + return MAV_RESULT_DENIED; + } + + plane.g2.guidedHeading.reset_I(); + + plane.guided_state.target_heading = new_target_heading; + plane.guided_state.target_heading_accel_limit = MAX(packet.param3, 0.05f); + plane.guided_state.target_heading_time_ms = AP_HAL::millis(); + return MAV_RESULT_ACCEPTED; + } +#endif // OFFBOARD_GUIDED == ENABLED + + + } + // anything else ... + return MAV_RESULT_UNSUPPORTED; + +} + MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_packet(const mavlink_command_int_t &packet) { + + plane.Log_Write_MavCmdI(packet); + switch(packet.command) { case MAV_CMD_DO_REPOSITION: return handle_command_int_do_reposition(packet); + // special 'slew-enabled' guided commands here... for speed,alt, and direction commands + case MAV_CMD_GUIDED_CHANGE_SPEED: + case MAV_CMD_GUIDED_CHANGE_ALTITUDE: + case MAV_CMD_GUIDED_CHANGE_HEADING: + return handle_command_int_guided_slew_commands(packet); + default: return GCS_MAVLINK::handle_command_int_packet(packet); } diff --git a/ArduPlane/GCS_Mavlink.h b/ArduPlane/GCS_Mavlink.h index 8c37030031..ef8ffb3234 100644 --- a/ArduPlane/GCS_Mavlink.h +++ b/ArduPlane/GCS_Mavlink.h @@ -51,6 +51,7 @@ private: void handle_change_alt_request(AP_Mission::Mission_Command &cmd) override; void handle_rc_channels_override(const mavlink_message_t &msg) override; MAV_RESULT handle_command_int_do_reposition(const mavlink_command_int_t &packet); + MAV_RESULT handle_command_int_guided_slew_commands(const mavlink_command_int_t &packet); bool try_send_message(enum ap_message id) override; diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index 41f09b52ec..f77802800e 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -113,6 +113,37 @@ void Plane::Log_Write_Control_Tuning() logger.WriteBlock(&pkt, sizeof(pkt)); } +struct PACKED log_OFG_Guided { + LOG_PACKET_HEADER; + uint64_t time_us; + float target_airspeed_cm; + float target_airspeed_accel; + float target_alt; + float target_alt_accel; + uint8_t target_alt_frame; + float target_heading; + float target_heading_limit; +}; + +// Write a OFG Guided packet. +void Plane::Log_Write_OFG_Guided() +{ +#if OFFBOARD_GUIDED == ENABLED + struct log_OFG_Guided pkt = { + LOG_PACKET_HEADER_INIT(LOG_OFG_MSG), + time_us : AP_HAL::micros64(), + target_airspeed_cm : (float)guided_state.target_airspeed_cm*(float)0.01, + target_airspeed_accel : guided_state.target_airspeed_accel, + target_alt : guided_state.target_alt, + target_alt_accel : guided_state.target_alt_accel, + target_alt_frame : guided_state.target_alt_frame, + target_heading : guided_state.target_heading, + target_heading_limit : guided_state.target_heading_accel_limit + }; + logger.WriteBlock(&pkt, sizeof(pkt)); +#endif +} + struct PACKED log_Nav_Tuning { LOG_PACKET_HEADER; uint64_t time_us; @@ -216,6 +247,42 @@ void Plane::Log_Write_RC(void) Log_Write_AETR(); } +void Plane::Log_Write_Guided(void) +{ +#if OFFBOARD_GUIDED == ENABLED + if (control_mode != &mode_guided) { + return; + } + + if (guided_state.target_heading_time_ms != 0) { + logger.Write_PID(LOG_PIDG_MSG, g2.guidedHeading.get_pid_info()); + } + + if ( is_positive(guided_state.target_alt) || is_positive(guided_state.target_airspeed_cm) ) { + Log_Write_OFG_Guided(); + } +#endif // OFFBOARD_GUIDED == ENABLED +} + +// incoming-to-vehicle mavlink COMMAND_INT can be logged +struct PACKED log_CMDI { + LOG_PACKET_HEADER; + uint64_t TimeUS; + uint16_t CId; + uint8_t TSys; + uint8_t TCmp; + uint8_t cur; + uint8_t cont; + float Prm1; + float Prm2; + float Prm3; + float Prm4; + int32_t Lat; + int32_t Lng; + float Alt; + uint8_t F; +}; + // type and unit information can be found in // libraries/AP_Logger/Logstructure.h; search for "log_Units" for // units and "Format characters" for field type information @@ -328,6 +395,19 @@ const struct LogStructure Plane::log_structure[] = { { LOG_PIQA_MSG, sizeof(log_PID), "PIQA", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS }, +// @LoggerMessage: PIDG +// @Description: Plane Proportional/Integral/Derivative gain values for Heading when using COMMAND_INT control. +// @Field: TimeUS: Time since system startup +// @Field: Tar: desired value +// @Field: Act: achieved value +// @Field: Err: error between target and achieved +// @Field: P: proportional part of PID +// @Field: I: integral part of PID +// @Field: D: derivative part of PID +// @Field: FF: controller feed-forward portion of response + { LOG_PIDG_MSG, sizeof(log_PID), + "PIDG", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS }, + // @LoggerMessage: AETR // @Description: Normalised pre-mixer control surface outputs // @Field: TimeUS: Time since system startup @@ -338,8 +418,85 @@ const struct LogStructure Plane::log_structure[] = { // @Field: Flap: Pre-mixer value for flaps output (between -4500 to 4500) { LOG_AETR_MSG, sizeof(log_AETR), "AETR", "Qhhhhh", "TimeUS,Ail,Elev,Thr,Rudd,Flap", "s-----", "F-----" }, + +// @LoggerMessage: OFG +// @Description: OFfboard-Guided - an advanced version of GUIDED for companion computers that includes rate/s. +// @Field: TimeUS: Time since system startup +// @Field: Arsp: target airspeed cm +// @Field: ArspA: target airspeed accel +// @Field: Alt: target alt +// @Field: AltA: target alt accel +// @Field: AltF: target alt frame +// @Field: Hdg: target heading +// @Field: HdgA: target heading lim + { LOG_OFG_MSG, sizeof(log_OFG_Guided), + "OFG", "QffffBff", "TimeUS,Arsp,ArspA,Alt,AltA,AltF,Hdg,HdgA", "s-------", "F-------" }, + +// @LoggerMessage: CMDI +// @Description: Generic CommandInt message logger(CMDI) +// @Field: TimeUS: Time since system startup +// @Field: CId: command id +// @Field: TSys: target system +// @Field: TCmp: target component +// @Field: cur: current +// @Field: cont: autocontinue +// @Field: Prm1: parameter 1 +// @Field: Prm2: parameter 2 +// @Field: Prm3: parameter 3 +// @Field: Prm4: parameter 4 +// @Field: Lat: target latitude +// @Field: Lng: target longitude +// @Field: Alt: target altitude +// @Field: F: frame + { LOG_CMDI_MSG, sizeof(log_CMDI), + "CMDI", "QHBBBBffffiifB", "TimeUS,CId,TSys,TCmp,cur,cont,Prm1,Prm2,Prm3,Prm4,Lat,Lng,Alt,F", "s---------DUm-", "F---------GGB-" }, +// these next three are same format as log_CMDI just each a different name for Heading,Speed and Alt COMMAND_INTs + { LOG_CMDS_MSG, sizeof(log_CMDI), + "CMDS", "QHBBBBffffiifB", "TimeUS,CId,TSys,TCmp,cur,cont,Prm1,Prm2,Prm3,Prm4,Lat,Lng,Alt,F", "s---------DUm-", "F---------GGB-" }, + { LOG_CMDA_MSG, sizeof(log_CMDI), + "CMDA", "QHBBBBffffiifB", "TimeUS,CId,TSys,TCmp,cur,cont,Prm1,Prm2,Prm3,Prm4,Lat,Lng,Alt,F", "s---------DUm-", "F---------GGB-" }, + { LOG_CMDH_MSG, sizeof(log_CMDI), + "CMDH", "QHBBBBffffiifB", "TimeUS,CId,TSys,TCmp,cur,cont,Prm1,Prm2,Prm3,Prm4,Lat,Lng,Alt,F", "s---------DUm-", "F---------GGB-" }, + }; + +// Write a COMMAND INT packet. +void Plane::Log_Write_MavCmdI(const mavlink_command_int_t &mav_cmd) +{ + struct log_CMDI pkt = { + LOG_PACKET_HEADER_INIT(LOG_CMDI_MSG), + TimeUS:AP_HAL::micros64(), + CId: mav_cmd.command, + TSys: mav_cmd.target_system, + TCmp: mav_cmd.target_component, + cur: mav_cmd.current, + cont: mav_cmd.autocontinue, + Prm1: mav_cmd.param1, + Prm2: mav_cmd.param2, + Prm3: mav_cmd.param3, + Prm4: mav_cmd.param4, + Lat: mav_cmd.x, + Lng: mav_cmd.y, + Alt: mav_cmd.z, + F: mav_cmd.frame +}; + +// rather than have 4 different functions for these similar outputs, we just create it as a CMDI and rename it here +#if OFFBOARD_GUIDED == ENABLED + if (mav_cmd.command == MAV_CMD_GUIDED_CHANGE_SPEED) { + pkt.msgid = LOG_CMDS_MSG; + } else if (mav_cmd.command == MAV_CMD_GUIDED_CHANGE_ALTITUDE) { + pkt.msgid = LOG_CMDA_MSG; + } else if (mav_cmd.command == MAV_CMD_GUIDED_CHANGE_HEADING) { + pkt.msgid = LOG_CMDH_MSG; + } +#endif + //normally pkt.msgid = LOG_CMDI_MSG + logger.WriteBlock(&pkt, sizeof(pkt)); + +} + void Plane::Log_Write_Vehicle_Startup_Messages() { // only 200(?) bytes are guaranteed by AP_Logger @@ -364,8 +521,10 @@ void Plane::Log_Write_Fast(void) {} void Plane::Log_Write_Performance() {} void Plane::Log_Write_Startup(uint8_t type) {} void Plane::Log_Write_Control_Tuning() {} +void Plane::Log_Write_OFG_Guided() {} void Plane::Log_Write_Nav_Tuning() {} void Plane::Log_Write_Status() {} +void Plane::Log_Write_Guided(void) {} void Plane::Log_Write_RC(void) {} void Plane::Log_Write_Vehicle_Startup_Messages() {} diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 2d08d6c907..be3edac495 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -1287,6 +1287,12 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @User: Standard AP_GROUPINFO("RTL_CLIMB_MIN", 27, ParametersG2, rtl_climb_min, 0), +#if OFFBOARD_GUIDED == ENABLED + // @Group: GUIDED_ + // @Path: ../libraries/AC_PID/AC_PID.cpp + AP_SUBGROUPINFO(guidedHeading, "GUIDED_", 28, ParametersG2, AC_PID), +#endif // OFFBOARD_GUIDED == ENABLED + AP_GROUPEND }; diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 7c7087cb6d..c40307d2ba 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -576,6 +576,12 @@ public: AP_EFI efi; #endif +#if OFFBOARD_GUIDED == ENABLED + // guided yaw heading PID + AC_PID guidedHeading{5000.0, 0.0, 0.0, 0 , 10.0, 5.0, 5.0 , 5.0 , 0.2}; +#endif + + AP_Float fs_ekf_thresh; // min initial climb in RTL diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 3971ab5423..5f28014e88 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -517,6 +517,28 @@ private: // throttle commanded from external controller in percent float forced_throttle; uint32_t last_forced_throttle_ms; + +#if OFFBOARD_GUIDED == ENABLED + // airspeed adjustments + float target_airspeed_cm = -1; // don't default to zero here, as zero is a valid speed. + float target_airspeed_accel; + uint32_t target_airspeed_time_ms; + + // altitude adjustments + float target_alt = -1; // don't default to zero here, as zero is a valid alt. + uint32_t last_target_alt = 0; + float target_alt_accel; + uint32_t target_alt_time_ms = 0; + uint8_t target_alt_frame = 0; + + // heading track + float target_heading = -4; // don't default to zero or -1 here, as both are valid headings in radians + float target_heading_accel_limit; + uint32_t target_heading_time_ms; + guided_heading_type_t target_heading_type; + bool target_heading_limit_low; + bool target_heading_limit_high; +#endif // OFFBOARD_GUIDED == ENABLED } guided_state; #if LANDING_GEAR_ENABLED == ENABLED @@ -773,12 +795,15 @@ private: void Log_Write_Performance(); void Log_Write_Startup(uint8_t type); void Log_Write_Control_Tuning(); + void Log_Write_OFG_Guided(); + void Log_Write_Guided(void); void Log_Write_Nav_Tuning(); void Log_Write_Status(); void Log_Write_RC(void); void Log_Write_Vehicle_Startup_Messages(); void Log_Write_AOA_SSA(); void Log_Write_AETR(); + void Log_Write_MavCmdI(const mavlink_command_int_t &packet); void load_parameters(void) override; void convert_mixers(void); diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index ea3636f671..5cbd812b22 100644 --- a/ArduPlane/altitude.cpp +++ b/ArduPlane/altitude.cpp @@ -31,7 +31,28 @@ void Plane::adjust_altitude_target() control_mode == &mode_cruise) { return; } - if (landing.is_flaring()) { +#if OFFBOARD_GUIDED == ENABLED + if (control_mode == &mode_guided && ((guided_state.target_alt_time_ms != 0) || guided_state.target_alt > -0.001 )) { // target_alt now defaults to -1, and _time_ms defaults to zero. + // offboard altitude demanded + uint32_t now = AP_HAL::millis(); + float delta = 1e-3f * (now - guided_state.target_alt_time_ms); + guided_state.target_alt_time_ms = now; + // determine delta accurately as a float + float delta_amt_f = delta * guided_state.target_alt_accel; + // then scale x100 to match last_target_alt and convert to a signed int32_t as it may be negative + int32_t delta_amt_i = (int32_t)(100.0 * delta_amt_f); + Location temp {}; + temp.alt = guided_state.last_target_alt + delta_amt_i; // ...to avoid floats here, + if (is_positive(guided_state.target_alt_accel)) { + temp.alt = MIN(guided_state.target_alt, temp.alt); + } else { + temp.alt = MAX(guided_state.target_alt, temp.alt); + } + guided_state.last_target_alt = temp.alt; + set_target_altitude_location(temp); + } else +#endif // OFFBOARD_GUIDED == ENABLED + if (landing.is_flaring()) { // during a landing flare, use TECS_LAND_SINK as a target sink // rate, and ignores the target altitude set_target_altitude_location(next_WP_loc); diff --git a/ArduPlane/config.h b/ArduPlane/config.h index cb35d5f96d..f64eeebbe1 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -304,6 +304,9 @@ #ifndef SOARING_ENABLED #define SOARING_ENABLED !HAL_MINIMIZE_FEATURES #endif +#ifndef OFFBOARD_GUIDED + #define OFFBOARD_GUIDED !HAL_MINIMIZE_FEATURES +#endif #ifndef LANDING_GEAR_ENABLED #define LANDING_GEAR_ENABLED !HAL_MINIMIZE_FEATURES diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index 57c093f01a..0c65afc53a 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -90,7 +90,13 @@ enum log_messages { LOG_PIQP_MSG, LOG_PIQY_MSG, LOG_PIQA_MSG, + LOG_PIDG_MSG, LOG_AETR_MSG, + LOG_OFG_MSG, + LOG_CMDI_MSG, + LOG_CMDA_MSG, + LOG_CMDS_MSG, + LOG_CMDH_MSG, }; #define MASK_LOG_ATTITUDE_FAST (1<<0) @@ -153,3 +159,9 @@ enum CrowFlapOptions { PROGRESSIVE_CROW = (1 << 2), }; + +enum guided_heading_type_t { + GUIDED_HEADING_NONE = 0, // no heading track + GUIDED_HEADING_COG, // maintain ground track + GUIDED_HEADING_HEADING, // maintain a heading +}; diff --git a/ArduPlane/mode.cpp b/ArduPlane/mode.cpp index b8c33bd657..e660c7f5c7 100644 --- a/ArduPlane/mode.cpp +++ b/ArduPlane/mode.cpp @@ -32,6 +32,14 @@ bool Mode::enter() plane.guided_state.last_forced_rpy_ms.zero(); plane.guided_state.last_forced_throttle_ms = 0; +#if OFFBOARD_GUIDED == ENABLED + plane.guided_state.target_heading = -4; // radians here are in range -3.14 to 3.14, so a default value needs to be outside that range + plane.guided_state.target_heading_type = GUIDED_HEADING_NONE; + plane.guided_state.target_airspeed_cm = -1; // same as above, although an airspeed of -1 is rare on plane. + plane.guided_state.target_alt = -1; // same as above, although a target alt of -1 is rare on plane. + plane.guided_state.last_target_alt = 0; +#endif + #if CAMERA == ENABLED plane.camera.set_is_auto_mode(this == &plane.mode_auto); #endif diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index 542e35545d..428c5305b3 100644 --- a/ArduPlane/navigation.cpp +++ b/ArduPlane/navigation.cpp @@ -136,7 +136,23 @@ void Plane::calc_airspeed_errors() target_airspeed_cm = ((int32_t)(aparm.airspeed_max - aparm.airspeed_min) * get_throttle_input()) + ((int32_t)aparm.airspeed_min * 100); } +#if OFFBOARD_GUIDED == ENABLED + } else if (control_mode == &mode_guided && !is_zero(guided_state.target_airspeed_cm)) { + // offboard airspeed demanded + uint32_t now = AP_HAL::millis(); + float delta = 1e-3f * (now - guided_state.target_airspeed_time_ms); + guided_state.target_airspeed_time_ms = now; + float delta_amt = 100 * delta * guided_state.target_airspeed_accel; + target_airspeed_cm += delta_amt; + //target_airspeed_cm recalculated then clamped to between MIN airspeed and MAX airspeed by constrain_float + if (is_positive(guided_state.target_airspeed_accel)) { + target_airspeed_cm = constrain_float(MIN(guided_state.target_airspeed_cm, target_airspeed_cm), aparm.airspeed_min *100, aparm.airspeed_max *100); + } else { + target_airspeed_cm = constrain_float(MAX(guided_state.target_airspeed_cm, target_airspeed_cm), aparm.airspeed_min *100, aparm.airspeed_max *100); + } + +#endif // OFFBOARD_GUIDED == ENABLED } else if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) { // Landing airspeed target target_airspeed_cm = landing.get_target_airspeed_cm(); @@ -168,6 +184,13 @@ void Plane::calc_airspeed_errors() } } + // when using the special GUIDED mode features for slew control, don't allow airspeed nudging as it doesn't play nicely. +#if OFFBOARD_GUIDED == ENABLED + if (control_mode == &mode_guided && !is_zero(guided_state.target_airspeed_cm) && (airspeed_nudge_cm != 0)) { + airspeed_nudge_cm = 0; //airspeed_nudge_cm forced to zero + } +#endif + // Bump up the target airspeed based on throttle nudging if (throttle_allows_nudging && airspeed_nudge_cm > 0) { target_airspeed_cm += airspeed_nudge_cm;