Skip to content

Commit

Permalink
ArduPlane: use Location::AltFrame for guided_state.target_alt_frame
Browse files Browse the repository at this point in the history
  • Loading branch information
@timtuxworth
timtuxworth committed Sep 9, 2024
1 parent 61de480 commit ec50cb9
Show file tree
Hide file tree
Showing 5 changed files with 52 additions and 52 deletions.
49 changes: 18 additions & 31 deletions ArduPlane/GCS_Mavlink.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -929,48 +929,35 @@ 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;
} 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;
}

Expand Down
17 changes: 10 additions & 7 deletions ArduPlane/Log.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand All @@ -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<uint8_t>(guided_state.target_location.get_alt_frame()),
};
logger.WriteBlock(&pkt, sizeof(pkt));
}
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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
};

Expand Down
10 changes: 7 additions & 3 deletions ArduPlane/Plane.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
7 changes: 5 additions & 2 deletions ArduPlane/mode.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
21 changes: 12 additions & 9 deletions ArduPlane/mode_guided.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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
{
Expand Down

0 comments on commit ec50cb9

Please sign in to comment.