mirror of https://github.com/ArduPilot/ardupilot
Plane: Support a set of offboard MAVLink guided controls with rates
+ 3 rounds of fixes
This commit is contained in:
Buzz
Andrew Tridgell
parent
d4df145b4b
commit
0f5177b291
|
|
@ -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();
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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() {}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
};
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
Loading…
Reference in New Issue