diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index c2dac77664..adcf2a3b37 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -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(); diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp index 8e4354b241..1954a683bf 100644 --- a/ArduCopter/Log.cpp +++ b/ArduCopter/Log.cpp @@ -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() {} diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 879f00f2d8..04b010f262 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -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;