diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 887599d6b985db..054e9ce14b1da8 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -929,37 +929,28 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_guided_slew_commands(const mavl 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; + Location::AltFrame new_target_alt_frame; + if (!mavlink_coordinate_frame_to_location_alt_frame((MAV_FRAME)packet.frame, new_target_alt_frame)) { + return MAV_RESULT_DENIED; + } + + const int32_t new_target_alt = packet.z * 100; + if (is_equal(plane.guided_state.target_location.alt,new_target_alt) && plane.guided_state.target_location.get_alt_frame() == new_target_alt_frame) { + // no need to process any new packet/s with the same ALT any further, if we are already doing it. + // This is quite important as last_target_alt and last_target_alt_frame should only be set if the new one is different + return MAV_RESULT_ACCEPTED; } - 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 + // stash the previous target altitude/frame, used by ModeGuided::update_target_altitude() + plane.guided_state.target_location_previous = plane.current_loc; + + // keep a copy of what came in via MAVLink - this is needed for logging, but not for anything else + plane.guided_state.target_mav_frame = packet.frame; + plane.guided_state.target_location.set_alt_cm(new_target_alt, new_target_alt_frame); + plane.guided_state.target_alt_time_ms = AP_HAL::millis(); + // param3 contains the desired vertical velocity (not acceleration) 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; @@ -967,10 +958,6 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_guided_slew_commands(const mavl 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; } diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index c9fdd0783b767c..6352968ebdd356 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -129,9 +129,10 @@ struct PACKED log_OFG_Guided { float target_airspeed_accel; float target_alt; float target_alt_accel; - uint8_t target_alt_frame; + uint8_t target_mav_frame; // received MavLink frame float target_heading; float target_heading_limit; + uint8_t target_alt_frame; // internal AltFrame }; // Write a OFG Guided packet. @@ -142,11 +143,12 @@ void Plane::Log_Write_OFG_Guided() 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 : guided_state.target_location.alt * 0.01, target_alt_accel : guided_state.target_alt_accel, - target_alt_frame : guided_state.target_alt_frame, + target_mav_frame : guided_state.target_mav_frame, target_heading : guided_state.target_heading, - target_heading_limit : guided_state.target_heading_accel_limit + target_heading_limit : guided_state.target_heading_accel_limit, + target_alt_frame : static_cast(guided_state.target_location.get_alt_frame()), }; logger.WriteBlock(&pkt, sizeof(pkt)); } @@ -274,7 +276,7 @@ void Plane::Log_Write_Guided(void) 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) ) { + if ( guided_state.target_location.alt != -1 || is_positive(guided_state.target_airspeed_cm) ) { Log_Write_OFG_Guided(); } #endif // AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED @@ -491,11 +493,12 @@ const struct LogStructure Plane::log_structure[] = { // @Field: ArspA: target airspeed accel // @Field: Alt: target alt // @Field: AltA: target alt accel -// @Field: AltF: target alt frame +// @Field: AltF: target alt frame (MAVLink) // @Field: Hdg: target heading // @Field: HdgA: target heading lim +// @Field: AltL: target alt frame (Location) { LOG_OFG_MSG, sizeof(log_OFG_Guided), - "OFG", "QffffBff", "TimeUS,Arsp,ArspA,Alt,AltA,AltF,Hdg,HdgA", "s-------", "F-------" , true }, + "OFG", "QffffBffB", "TimeUS,Arsp,ArspA,Alt,AltA,AltF,Hdg,HdgA,AltL", "snnmo-d--", "F--------" , true }, #endif }; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index c86b9b92cbbbe5..fbc38d847c5c35 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -561,11 +561,15 @@ class Plane : public AP_Vehicle { 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; + Location target_location {}; + Location target_location_previous; + //float target_alt = -1; // don't default to zero here, as zero is a valid alt. + //uint32_t last_target_alt = 0; + //Location::AltFrame last_target_alt_frame = Location::AltFrame::ABSOLUTE; float target_alt_accel; uint32_t target_alt_time_ms = 0; - uint8_t target_alt_frame = 0; + //Location::AltFrame target_alt_frame = Location::AltFrame::ABSOLUTE; + uint8_t target_mav_frame = -1; // heading track float target_heading = -4; // don't default to zero or -1 here, as both are valid headings in radians diff --git a/ArduPlane/mode.cpp b/ArduPlane/mode.cpp index a74d7c1a9367a0..9987dd78ca167c 100644 --- a/ArduPlane/mode.cpp +++ b/ArduPlane/mode.cpp @@ -58,9 +58,12 @@ bool Mode::enter() 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.target_alt = -1; // same as above, although a target alt of -1 is rare on plane. + //plane.guided_state.target_alt_frame = Location::AltFrame::ABSOLUTE; plane.guided_state.target_alt_time_ms = 0; - plane.guided_state.last_target_alt = 0; + //plane.guided_state.last_target_alt = -1; + plane.guided_state.target_location.set_alt_cm(-1, Location::AltFrame::ABSOLUTE); + plane.guided_state.target_location_previous.set_alt_cm(-1, Location::AltFrame::ABSOLUTE); #endif #if AP_CAMERA_ENABLED diff --git a/ArduPlane/mode_guided.cpp b/ArduPlane/mode_guided.cpp index 4fef88ee3c73f4..141f69f05f33e4 100644 --- a/ArduPlane/mode_guided.cpp +++ b/ArduPlane/mode_guided.cpp @@ -129,7 +129,8 @@ void ModeGuided::set_radius_and_direction(const float radius, const bool directi void ModeGuided::update_target_altitude() { #if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED - if (((plane.guided_state.target_alt_time_ms != 0) || plane.guided_state.target_alt > -0.001 )) { // target_alt now defaults to -1, and _time_ms defaults to zero. + // target altitude can be negative (e.g. flying below home altitude from the top of a mountain) + if (((plane.guided_state.target_alt_time_ms != 0) || plane.guided_state.target_location.alt != -1 )) { // 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 - plane.guided_state.target_alt_time_ms); @@ -138,15 +139,17 @@ void ModeGuided::update_target_altitude() float delta_amt_f = delta * plane.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 = plane.guided_state.last_target_alt + delta_amt_i; // ...to avoid floats here, - if (is_positive(plane.guided_state.target_alt_accel)) { - temp.alt = MIN(plane.guided_state.target_alt, temp.alt); - } else { - temp.alt = MAX(plane.guided_state.target_alt, temp.alt); + // To calculate the required velocity (up or down), we need to target and previous altitudes in the target frame + int32_t target_alt_previous; + if (plane.guided_state.target_location_previous.get_alt_cm(plane.guided_state.target_location.get_alt_frame(), target_alt_previous)) { + // create a new interim target location that that takes current_location and moves delta_amt_i in the right direction + int32_t temp_alt = constrain_int32(plane.guided_state.target_location.alt, target_alt_previous - delta_amt_i, target_alt_previous + delta_amt_i); + Location temp_location = plane.guided_state.target_location; + temp_location.alt = temp_alt; + + // incrementally step the altitude towards the target + plane.set_target_altitude_location(temp_location); } - plane.guided_state.last_target_alt = temp.alt; - plane.set_target_altitude_location(temp); } else #endif // AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED {