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 5, 2024
1 parent 2e1af82 commit c0bde88
Show file tree
Hide file tree
Showing 5 changed files with 44 additions and 38 deletions.
54 changes: 25 additions & 29 deletions ArduPlane/GCS_Mavlink.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -929,35 +929,27 @@ 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;
}

// compare two floats as near-enough
const float new_target_alt = packet.z * 100;
if (is_equal(plane.guided_state.target_alt,new_target_alt) && plane.guided_state.target_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.
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.last_target_alt = plane.current_loc.alt;
plane.guided_state.last_target_alt_frame = plane.current_loc.get_alt_frame();

plane.guided_state.target_mav_frame = packet.frame;
plane.guided_state.target_alt_frame = new_target_alt_frame;
plane.guided_state.target_alt = new_target_alt;

plane.guided_state.target_alt_time_ms = AP_HAL::millis();

if (is_zero(packet.param3)) {
Expand All @@ -967,10 +959,14 @@ 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) {
// assign an acceleration direction - this only works if the current and target frame are the same
// this should not be happening here - it should be in ModeGuided::update_target_altitude()
if (plane.guided_state.last_target_alt_frame == plane.guided_state.target_alt_frame && plane.guided_state.target_alt < plane.current_loc.alt) {
plane.guided_state.target_alt_accel *= -1.0f;
}
//if ((MAV_FRAME)packet.frame == MAV_FRAME_GLOBAL_TERRAIN_ALT) {
// return MAV_RESULT_ACCEPTED;
// }
return MAV_RESULT_ACCEPTED;
}

Expand Down
13 changes: 8 additions & 5 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 @@ -144,9 +145,10 @@ void Plane::Log_Write_OFG_Guided()
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_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_alt_frame),
};
logger.WriteBlock(&pkt, sizeof(pkt));
}
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
4 changes: 3 additions & 1 deletion ArduPlane/Plane.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -563,9 +563,11 @@ class Plane : public AP_Vehicle {
// 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::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
1 change: 1 addition & 0 deletions ArduPlane/mode.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,6 +59,7 @@ bool Mode::enter()
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_frame = Location::AltFrame::ABSOLUTE;
plane.guided_state.target_alt_time_ms = 0;
plane.guided_state.last_target_alt = 0;
#endif
Expand Down
10 changes: 7 additions & 3 deletions ArduPlane/mode_guided.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -139,13 +139,17 @@ void ModeGuided::update_target_altitude()
// 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,
int32_t temp_alt = plane.guided_state.last_target_alt + delta_amt_i;
// Restore the last altitude that the plane had been flying
if (is_positive(plane.guided_state.target_alt_accel)) {
temp.alt = MIN(plane.guided_state.target_alt, temp.alt);
temp_alt = MIN(plane.guided_state.target_alt, temp_alt);
} else {
temp.alt = MAX(plane.guided_state.target_alt, temp.alt);
temp_alt = MAX(plane.guided_state.target_alt, temp_alt);
}
temp.set_alt_cm(temp_alt, plane.guided_state.target_alt_frame);

plane.guided_state.last_target_alt = temp.alt;
plane.guided_state.last_target_alt_frame = temp.get_alt_frame();
plane.set_target_altitude_location(temp);
} else
#endif // AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
Expand Down

0 comments on commit c0bde88

Please sign in to comment.