diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 12f2704ec7..527f8dd68c 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -932,48 +932,25 @@ 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; } + // 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_alt_frame = packet.frame; - plane.guided_state.last_target_alt = plane.current_loc.alt; // FIXME: Reference frame is not corrected for here + const int32_t new_target_alt_cm = packet.z * 100; + plane.guided_state.target_location.set_alt_cm(new_target_alt_cm, 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; + // the user wanted /maximum altitude change rate, pick a large value as close enough + plane.guided_state.target_alt_rate = 1000.0; } else { - plane.guided_state.target_alt_accel = fabsf(packet.param3); + plane.guided_state.target_alt_rate = 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 b06bb99cb6..a258269779 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -128,10 +128,11 @@ struct PACKED log_OFG_Guided { float target_airspeed_cm; float target_airspeed_accel; float target_alt; - float target_alt_accel; - uint8_t target_alt_frame; + float target_alt_rate; + 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_accel : guided_state.target_alt_accel, - target_alt_frame : guided_state.target_alt_frame, + target_alt : guided_state.target_location.alt * 0.01, + target_alt_rate : guided_state.target_alt_rate, + 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 @@ -490,12 +492,13 @@ const struct LogStructure Plane::log_structure[] = { // @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: AltA: target alt velocity (rate of change) +// @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 030131abc3..70ef5fe7a9 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -577,11 +577,10 @@ private: 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; + Location target_location; + float target_alt_rate; uint32_t target_alt_time_ms = 0; - uint8_t target_alt_frame = 0; + 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 f442bd830c..ade2abb2a3 100644 --- a/ArduPlane/mode.cpp +++ b/ArduPlane/mode.cpp @@ -58,9 +58,8 @@ 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_time_ms = 0; - plane.guided_state.last_target_alt = 0; + plane.guided_state.target_location.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 4fef88ee3c..a97bedcd7d 100644 --- a/ArduPlane/mode_guided.cpp +++ b/ArduPlane/mode_guided.cpp @@ -129,24 +129,29 @@ 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); plane.guided_state.target_alt_time_ms = now; // determine delta accurately as a float - float delta_amt_f = delta * plane.guided_state.target_alt_accel; + float delta_amt_f = delta * plane.guided_state.target_alt_rate; // 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 current altitudes in the target frame + const Location::AltFrame target_frame = plane.guided_state.target_location.get_alt_frame(); + int32_t target_alt_previous_cm; + if (plane.current_loc.initialised() && plane.guided_state.target_location.initialised() && + plane.current_loc.get_alt_cm(target_frame, target_alt_previous_cm)) { + // create a new interim target location that that takes current_location and moves delta_amt_i in the right direction + int32_t temp_alt_cm = constrain_int32(plane.guided_state.target_location.alt, target_alt_previous_cm - delta_amt_i, target_alt_previous_cm + delta_amt_i); + Location temp_location = plane.guided_state.target_location; + temp_location.set_alt_cm(temp_alt_cm, target_frame); + + // 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 {