mirror of https://github.com/ArduPilot/ardupilot
Copter: guided accepts terrain alt position targets
This commit is contained in:
Randy Mackay
parent
f3c25397c7
commit
1ad6b2e6c7
|
|
@ -791,7 +791,7 @@ private:
|
|||
#if FRAME_CONFIG == HELI_FRAME
|
||||
void Log_Write_Heli(void);
|
||||
#endif
|
||||
void Log_Write_GuidedTarget(ModeGuided::SubMode submode, const Vector3f& pos_target, const Vector3f& vel_target, const Vector3f& accel_target);
|
||||
void Log_Write_GuidedTarget(ModeGuided::SubMode submode, const Vector3f& pos_target, bool terrain_alt, const Vector3f& vel_target, const Vector3f& accel_target);
|
||||
void Log_Write_SysID_Setup(uint8_t systemID_axis, float waveform_magnitude, float frequency_start, float frequency_stop, float time_fade_in, float time_const_freq, float time_record, float time_fade_out);
|
||||
void Log_Write_SysID_Data(float waveform_time, float waveform_sample, float waveform_freq, float angle_x, float angle_y, float angle_z, float accel_x, float accel_y, float accel_z);
|
||||
void Log_Write_Vehicle_Startup_Messages();
|
||||
|
|
|
|||
|
|
@ -374,6 +374,7 @@ struct PACKED log_GuidedTarget {
|
|||
float pos_target_x;
|
||||
float pos_target_y;
|
||||
float pos_target_z;
|
||||
uint8_t terrain;
|
||||
float vel_target_x;
|
||||
float vel_target_y;
|
||||
float vel_target_z;
|
||||
|
|
@ -384,8 +385,9 @@ struct PACKED log_GuidedTarget {
|
|||
|
||||
// Write a Guided mode target
|
||||
// pos_target is lat, lon, alt OR offset from ekf origin in cm OR roll, pitch, yaw target in centi-degrees
|
||||
// terrain should be 0 if pos_target.z is alt-above-ekf-origin, 1 if alt-above-terrain
|
||||
// vel_target is cm/s
|
||||
void Copter::Log_Write_GuidedTarget(ModeGuided::SubMode target_type, const Vector3f& pos_target, const Vector3f& vel_target, const Vector3f& accel_target)
|
||||
void Copter::Log_Write_GuidedTarget(ModeGuided::SubMode target_type, const Vector3f& pos_target, bool terrain_alt, const Vector3f& vel_target, const Vector3f& accel_target)
|
||||
{
|
||||
struct log_GuidedTarget pkt = {
|
||||
LOG_PACKET_HEADER_INIT(LOG_GUIDEDTARGET_MSG),
|
||||
|
|
@ -394,6 +396,7 @@ void Copter::Log_Write_GuidedTarget(ModeGuided::SubMode target_type, const Vecto
|
|||
pos_target_x : pos_target.x,
|
||||
pos_target_y : pos_target.y,
|
||||
pos_target_z : pos_target.z,
|
||||
terrain : terrain_alt,
|
||||
vel_target_x : vel_target.x,
|
||||
vel_target_y : vel_target.y,
|
||||
vel_target_z : vel_target.z,
|
||||
|
|
@ -542,6 +545,7 @@ const struct LogStructure Copter::log_structure[] = {
|
|||
// @Field: pX: Target position, X-Axis
|
||||
// @Field: pY: Target position, Y-Axis
|
||||
// @Field: pZ: Target position, Z-Axis
|
||||
// @Field: Terrain: Target position, Z-Axis is alt above terrain
|
||||
// @Field: vX: Target velocity, X-Axis
|
||||
// @Field: vY: Target velocity, Y-Axis
|
||||
// @Field: vZ: Target velocity, Z-Axis
|
||||
|
|
@ -550,7 +554,7 @@ const struct LogStructure Copter::log_structure[] = {
|
|||
// @Field: aZ: Target acceleration, Z-Axis
|
||||
|
||||
{ LOG_GUIDEDTARGET_MSG, sizeof(log_GuidedTarget),
|
||||
"GUID", "QBfffffffff", "TimeUS,Type,pX,pY,pZ,vX,vY,vZ,aX,aY,aZ", "s-mmmnnnooo", "F-BBBBBBBBB" },
|
||||
"GUID", "QBfffbffffff", "TimeUS,Type,pX,pY,pZ,Terrain,vX,vY,vZ,aX,aY,aZ", "s-mmm-nnnooo", "F-BBB-BBBBBB" },
|
||||
};
|
||||
|
||||
void Copter::Log_Write_Vehicle_Startup_Messages()
|
||||
|
|
@ -582,7 +586,7 @@ void Copter::Log_Write_Data(LogDataID id, uint16_t value) {}
|
|||
void Copter::Log_Write_Data(LogDataID id, float value) {}
|
||||
void Copter::Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, float tune_min, float tune_max) {}
|
||||
void Copter::Log_Sensor_Health() {}
|
||||
void Copter::Log_Write_GuidedTarget(ModeGuided::SubMode target_type, const Vector3f& pos_target, const Vector3f& vel_target, const Vector3f& accel_target) {}
|
||||
void Copter::Log_Write_GuidedTarget(ModeGuided::SubMode target_type, const Vector3f& pos_target, bool terrain_alt, const Vector3f& vel_target, const Vector3f& accel_target) {}
|
||||
void Copter::Log_Write_SysID_Setup(uint8_t systemID_axis, float waveform_magnitude, float frequency_start, float frequency_stop, float time_fade_in, float time_const_freq, float time_record, float time_fade_out) {}
|
||||
void Copter::Log_Write_SysID_Data(float waveform_time, float waveform_sample, float waveform_freq, float angle_x, float angle_y, float angle_z, float accel_x, float accel_y, float accel_z) {}
|
||||
void Copter::Log_Write_Vehicle_Startup_Messages() {}
|
||||
|
|
|
|||
|
|
@ -14,6 +14,7 @@
|
|||
#define GUIDED_ATTITUDE_TIMEOUT_MS 1000 // guided mode's attitude controller times out after 1 second with no new updates
|
||||
|
||||
static Vector3p guided_pos_target_cm; // position target (used by posvel controller only)
|
||||
bool guided_pos_terrain_alt; // true if guided_pos_target_cm.z is an alt above terrain
|
||||
static Vector3f guided_vel_target_cms; // velocity target (used by pos_vel_accel controller and vel_accel controller)
|
||||
static Vector3f guided_accel_target_cmss; // acceleration target (used by pos_vel_accel controller vel_accel controller and accel controller)
|
||||
static uint32_t update_time_ms; // system time of last target update to pos_vel_accel, vel_accel or accel controller
|
||||
|
|
@ -270,17 +271,36 @@ bool ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, floa
|
|||
pos_control_start();
|
||||
}
|
||||
|
||||
// initialise terrain following if needed
|
||||
if (terrain_alt) {
|
||||
// get current alt above terrain
|
||||
float origin_terr_offset;
|
||||
if (!wp_nav->get_terrain_offset(origin_terr_offset)) {
|
||||
// if we don't have terrain altitude then stop
|
||||
init(true);
|
||||
return false;
|
||||
}
|
||||
// convert origin to alt-above-terrain if necessary
|
||||
if (!guided_pos_terrain_alt) {
|
||||
// new destination is alt-above-terrain, previous destination was alt-above-ekf-origin
|
||||
pos_control->set_pos_offset_z_cm(origin_terr_offset);
|
||||
}
|
||||
} else {
|
||||
pos_control->set_pos_offset_z_cm(0.0);
|
||||
}
|
||||
|
||||
// set yaw state
|
||||
set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
|
||||
|
||||
// set position target and zero velocity and acceleration
|
||||
guided_pos_target_cm = destination.topostype();
|
||||
guided_pos_terrain_alt = terrain_alt;
|
||||
guided_vel_target_cms.zero();
|
||||
guided_accel_target_cmss.zero();
|
||||
update_time_ms = millis();
|
||||
|
||||
// log target
|
||||
copter.Log_Write_GuidedTarget(guided_mode, guided_pos_target_cm.tofloat(), guided_vel_target_cms, guided_accel_target_cmss);
|
||||
copter.Log_Write_GuidedTarget(guided_mode, guided_pos_target_cm.tofloat(), guided_pos_terrain_alt, guided_vel_target_cms, guided_accel_target_cmss);
|
||||
|
||||
send_notification = true;
|
||||
|
||||
|
|
@ -320,17 +340,36 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y
|
|||
|
||||
// set position target and zero velocity and acceleration
|
||||
Vector3f pos_target_f;
|
||||
if (!dest_loc.get_vector_from_origin_NEU(pos_target_f)) {
|
||||
guided_pos_target_cm = pos_control->get_pos_target_cm();
|
||||
} else {
|
||||
guided_pos_target_cm = pos_target_f.topostype();
|
||||
bool terrain_alt;
|
||||
if (!wp_nav->get_vector_NEU(dest_loc, pos_target_f, terrain_alt)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// initialise terrain following if needed
|
||||
if (terrain_alt) {
|
||||
// get current alt above terrain
|
||||
float origin_terr_offset;
|
||||
if (!wp_nav->get_terrain_offset(origin_terr_offset)) {
|
||||
// if we don't have terrain altitude then stop
|
||||
init(true);
|
||||
return false;
|
||||
}
|
||||
// convert origin to alt-above-terrain if necessary
|
||||
if (!guided_pos_terrain_alt) {
|
||||
// new destination is alt-above-terrain, previous destination was alt-above-ekf-origin
|
||||
pos_control->set_pos_offset_z_cm(origin_terr_offset);
|
||||
}
|
||||
} else {
|
||||
pos_control->set_pos_offset_z_cm(0.0);
|
||||
}
|
||||
|
||||
guided_pos_target_cm = pos_target_f.topostype();
|
||||
guided_pos_terrain_alt = terrain_alt;
|
||||
guided_vel_target_cms.zero();
|
||||
guided_accel_target_cmss.zero();
|
||||
|
||||
// log target
|
||||
copter.Log_Write_GuidedTarget(guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt), guided_vel_target_cms, guided_accel_target_cmss);
|
||||
copter.Log_Write_GuidedTarget(guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt), guided_pos_terrain_alt, guided_vel_target_cms, guided_accel_target_cmss);
|
||||
|
||||
send_notification = true;
|
||||
|
||||
|
|
@ -350,13 +389,14 @@ void ModeGuided::set_accel(const Vector3f& acceleration, bool use_yaw, float yaw
|
|||
|
||||
// set velocity and acceleration targets and zero position
|
||||
guided_pos_target_cm.zero();
|
||||
guided_pos_terrain_alt = false;
|
||||
guided_vel_target_cms.zero();
|
||||
guided_accel_target_cmss = acceleration;
|
||||
update_time_ms = millis();
|
||||
|
||||
// log target
|
||||
if (log_request) {
|
||||
copter.Log_Write_GuidedTarget(guided_mode, guided_pos_target_cm.tofloat(), guided_vel_target_cms, guided_accel_target_cmss);
|
||||
copter.Log_Write_GuidedTarget(guided_mode, guided_pos_target_cm.tofloat(), guided_pos_terrain_alt, guided_vel_target_cms, guided_accel_target_cmss);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -379,13 +419,14 @@ void ModeGuided::set_velaccel(const Vector3f& velocity, const Vector3f& accelera
|
|||
|
||||
// set velocity and acceleration targets and zero position
|
||||
guided_pos_target_cm.zero();
|
||||
guided_pos_terrain_alt = false;
|
||||
guided_vel_target_cms = velocity;
|
||||
guided_accel_target_cmss = acceleration;
|
||||
update_time_ms = millis();
|
||||
|
||||
// log target
|
||||
if (log_request) {
|
||||
copter.Log_Write_GuidedTarget(guided_mode, guided_pos_target_cm.tofloat(), guided_vel_target_cms, guided_accel_target_cmss);
|
||||
copter.Log_Write_GuidedTarget(guided_mode, guided_pos_target_cm.tofloat(), guided_pos_terrain_alt, guided_vel_target_cms, guided_accel_target_cmss);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -419,11 +460,12 @@ bool ModeGuided::set_destination_posvelaccel(const Vector3f& destination, const
|
|||
|
||||
update_time_ms = millis();
|
||||
guided_pos_target_cm = destination.topostype();
|
||||
guided_pos_terrain_alt = false;
|
||||
guided_vel_target_cms = velocity;
|
||||
guided_accel_target_cmss = acceleration;
|
||||
|
||||
// log target
|
||||
copter.Log_Write_GuidedTarget(guided_mode, guided_pos_target_cm.tofloat(), guided_vel_target_cms, guided_accel_target_cmss);
|
||||
copter.Log_Write_GuidedTarget(guided_mode, guided_pos_target_cm.tofloat(), guided_pos_terrain_alt, guided_vel_target_cms, guided_accel_target_cmss);
|
||||
return true;
|
||||
}
|
||||
|
||||
|
|
@ -475,6 +517,7 @@ void ModeGuided::set_angle(const Quaternion &q, float climb_rate_cms_or_thrust,
|
|||
// log target
|
||||
copter.Log_Write_GuidedTarget(guided_mode,
|
||||
Vector3f(guided_angle_state.roll_cd, guided_angle_state.pitch_cd, guided_angle_state.yaw_cd),
|
||||
false,
|
||||
Vector3f(0.0f, 0.0f, climb_rate_cms_or_thrust), Vector3f());
|
||||
}
|
||||
|
||||
|
|
@ -516,13 +559,21 @@ void ModeGuided::pos_control_run()
|
|||
return;
|
||||
}
|
||||
|
||||
// calculate terrain adjustments
|
||||
float terr_offset = 0.0f;
|
||||
if (guided_pos_terrain_alt && !wp_nav->get_terrain_offset(terr_offset)) {
|
||||
// if we don't have terrain altitude then stop
|
||||
init(true);
|
||||
return;
|
||||
}
|
||||
|
||||
// set motors to full range
|
||||
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
|
||||
|
||||
// send position and velocity targets to position controller
|
||||
guided_accel_target_cmss.zero();
|
||||
guided_vel_target_cms.zero();
|
||||
pos_control->input_pos_xyz(guided_pos_target_cm);
|
||||
pos_control->input_pos_xyz(guided_pos_target_cm, terr_offset);
|
||||
|
||||
// run position controllers
|
||||
pos_control->update_xy_controller();
|
||||
|
|
@ -752,6 +803,11 @@ void ModeGuided::posvelaccel_control_run()
|
|||
pos_control->stop_pos_xy_stabilisation();
|
||||
}
|
||||
|
||||
// guided_pos_target z-axis should never be a terrain altitude
|
||||
if (guided_pos_terrain_alt) {
|
||||
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
|
||||
}
|
||||
|
||||
float pz = guided_pos_target_cm.z;
|
||||
pos_control->input_pos_vel_accel_z(pz, guided_vel_target_cms.z, guided_accel_target_cmss.z);
|
||||
guided_pos_target_cm.z = pz;
|
||||
|
|
|
|||
Loading…
Reference in New Issue