Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: move guided submode enumeration into ModeGuided

This commit is contained in:
Peter Barker 2021-03-30 14:36:25 +11:00 committed by Randy Mackay
parent 3e138aa98a
commit 5df8dd8c50
6 changed files with 56 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/Copter.h
View File

@ -791,7 +791,7 @@ private:
#if FRAME_CONFIG == HELI_FRAME
void Log_Write_Heli(void);
#endif
void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target);
void Log_Write_GuidedTarget(ModeGuided::SubMode submode, const Vector3f& pos_target, const Vector3f& vel_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();

8
ArduCopter/GCS_Mavlink.cpp
View File

@ -126,16 +126,16 @@ void GCS_MAVLINK_Copter::send_position_target_local_ned()
if (!copter.flightmode->in_guided_mode()) {
return;
}
const GuidedMode guided_mode = copter.mode_guided.mode();
const ModeGuided::SubMode guided_mode = copter.mode_guided.submode();
Vector3f target_pos;
Vector3f target_vel;
uint16_t type_mask;
if (guided_mode == Guided_WP) {
if (guided_mode == ModeGuided::SubMode::WP) {
type_mask = 0x0FF8; // ignore everything except position
target_pos = copter.wp_nav->get_wp_destination() * 0.01f; // convert to metres
} else if (guided_mode == Guided_Velocity) {
} else if (guided_mode == ModeGuided::SubMode::Velocity) {
type_mask = 0x0FC7; // ignore everything except velocity
target_vel = copter.flightmode->get_desired_velocity() * 0.01f; // convert to m/s
} else {

6
ArduCopter/Log.cpp
View File

@ -382,12 +382,12 @@ 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
// vel_target is cm/s
void Copter::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target)
void Copter::Log_Write_GuidedTarget(ModeGuided::SubMode target_type, const Vector3f& pos_target, const Vector3f& vel_target)
{
struct log_GuidedTarget pkt = {
LOG_PACKET_HEADER_INIT(LOG_GUIDEDTARGET_MSG),
time_us : AP_HAL::micros64(),
type : target_type,
type : (uint8_t)target_type,
pos_target_x : pos_target.x,
pos_target_y : pos_target.y,
pos_target_z : pos_target.z,
@ -571,7 +571,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(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target) {}
void Copter::Log_Write_GuidedTarget(ModeGuided::SubMode&, const Vector3f& pos_target, const Vector3f& vel_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() {}

9
ArduCopter/defines.h
View File

@ -97,15 +97,6 @@ enum AutoMode {
Auto_NavPayloadPlace,
};
// Guided modes
enum GuidedMode {
Guided_TakeOff,
Guided_WP,
Guided_Velocity,
Guided_PosVel,
Guided_Angle,
};
// Airmode
enum class AirMode {
AIRMODE_NONE,

12
ArduCopter/mode.h
View File

@ -846,7 +846,15 @@ public:
bool do_user_takeoff_start(float takeoff_alt_cm) override;
GuidedMode mode() const { return guided_mode; }
enum class SubMode {
TakeOff,
WP,
Velocity,
PosVel,
Angle,
};
SubMode submode() const { return guided_mode; }
void angle_control_start();
void angle_control_run();
@ -881,7 +889,7 @@ private:
bool use_pilot_yaw(void) const;
// controls which controller is run (pos or vel):
GuidedMode guided_mode = Guided_TakeOff;
SubMode guided_mode = SubMode::TakeOff;
bool send_notification; // used to send one time notification to ground station
};

64
ArduCopter/mode_guided.cpp
View File

@ -55,12 +55,12 @@ void ModeGuided::run()
// call the correct auto controller
switch (guided_mode) {
case Guided_TakeOff:
case SubMode::TakeOff:
// run takeoff controller
takeoff_run();
break;
case Guided_WP:
case SubMode::WP:
// run position controller
pos_control_run();
if (send_notification && wp_nav->reached_wp_destination()) {
@ -69,17 +69,17 @@ void ModeGuided::run()
}
break;
case Guided_Velocity:
case SubMode::Velocity:
// run velocity controller
vel_control_run();
break;
case Guided_PosVel:
case SubMode::PosVel:
// run position-velocity controller
posvel_control_run();
break;
case Guided_Angle:
case SubMode::Angle:
// run angle controller
angle_control_run();
break;
@ -100,7 +100,7 @@ bool ModeGuided::allows_arming(AP_Arming::Method method) const
// do_user_takeoff_start - initialises waypoint controller to implement take-off
bool ModeGuided::do_user_takeoff_start(float takeoff_alt_cm)
{
guided_mode = Guided_TakeOff;
guided_mode = SubMode::TakeOff;
// initialise wpnav destination
Location target_loc = copter.current_loc;
@ -139,7 +139,7 @@ bool ModeGuided::do_user_takeoff_start(float takeoff_alt_cm)
void ModeGuided::pos_control_start()
{
// set to position control mode
guided_mode = Guided_WP;
guided_mode = SubMode::WP;
// initialise waypoint and spline controller
wp_nav->wp_and_spline_init();
@ -159,7 +159,7 @@ void ModeGuided::pos_control_start()
void ModeGuided::vel_control_start()
{
// set guided_mode to velocity controller
guided_mode = Guided_Velocity;
guided_mode = SubMode::Velocity;
// initialise horizontal speed, acceleration
pos_control->set_max_speed_xy(wp_nav->get_default_speed_xy());
@ -177,7 +177,7 @@ void ModeGuided::vel_control_start()
void ModeGuided::posvel_control_start()
{
// set guided_mode to velocity controller
guided_mode = Guided_PosVel;
guided_mode = SubMode::PosVel;
pos_control->init_xy_controller();
@ -202,14 +202,14 @@ void ModeGuided::posvel_control_start()
bool ModeGuided::is_taking_off() const
{
return guided_mode == Guided_TakeOff;
return guided_mode == SubMode::TakeOff;
}
// initialise guided mode's angle controller
void ModeGuided::angle_control_start()
{
// set guided_mode to velocity controller
guided_mode = Guided_Angle;
guided_mode = SubMode::Angle;
// set vertical speed and acceleration
pos_control->set_max_speed_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up());
@ -250,7 +250,7 @@ bool ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, floa
#endif
// ensure we are in position control mode
if (guided_mode != Guided_WP) {
if (guided_mode != SubMode::WP) {
pos_control_start();
}
@ -270,7 +270,7 @@ bool ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, floa
bool ModeGuided::get_wp(Location& destination)
{
if (guided_mode != Guided_WP) {
if (guided_mode != SubMode::WP) {
return false;
}
return wp_nav->get_oa_wp_destination(destination);
@ -292,7 +292,7 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y
#endif
// ensure we are in position control mode
if (guided_mode != Guided_WP) {
if (guided_mode != SubMode::WP) {
pos_control_start();
}
@ -318,7 +318,7 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y
void ModeGuided::set_velocity(const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw, bool log_request)
{
// check we are in velocity control mode
if (guided_mode != Guided_Velocity) {
if (guided_mode != SubMode::Velocity) {
vel_control_start();
}
@ -349,7 +349,7 @@ bool ModeGuided::set_destination_posvel(const Vector3f& destination, const Vecto
#endif
// check we are in velocity control mode
if (guided_mode != Guided_PosVel) {
if (guided_mode != SubMode::PosVel) {
posvel_control_start();
}
@ -371,7 +371,7 @@ bool ModeGuided::set_destination_posvel(const Vector3f& destination, const Vecto
void ModeGuided::set_angle(const Quaternion &q, float climb_rate_cms_or_thrust, bool use_yaw_rate, float yaw_rate_rads, bool use_thrust)
{
// check we are in velocity control mode
if (guided_mode != Guided_Angle) {
if (guided_mode != SubMode::Angle) {
angle_control_start();
}
@ -784,11 +784,11 @@ bool ModeGuided::limit_check()
uint32_t ModeGuided::wp_distance() const
{
switch(mode()) {
case Guided_WP:
switch(guided_mode) {
case SubMode::WP:
return wp_nav->get_wp_distance_to_destination();
break;
case Guided_PosVel:
case SubMode::PosVel:
return pos_control->get_pos_error_xy();
break;
default:
@ -798,25 +798,37 @@ uint32_t ModeGuided::wp_distance() const
int32_t ModeGuided::wp_bearing() const
{
switch(mode()) {
case Guided_WP:
switch(guided_mode) {
case SubMode::WP:
return wp_nav->get_wp_bearing_to_destination();
break;
case Guided_PosVel:
case SubMode::PosVel:
return pos_control->get_bearing_to_target();
break;
default:
case SubMode::TakeOff:
case SubMode::Velocity:
case SubMode::Angle:
// these do not have bearings
return 0;
}
// compiler guarantees we don't get here
return 0.0;
}
float ModeGuided::crosstrack_error() const
{
if (mode() == Guided_WP) {
switch (guided_mode) {
case SubMode::WP:
return wp_nav->crosstrack_error();
} else {
case SubMode::TakeOff:
case SubMode::Velocity:
case SubMode::PosVel:
case SubMode::Angle:
// no track to have a crosstrack to
return 0;
}
// compiler guarantees we don't get here
return 0;
}
#endif