Skip to content

Commit

Permalink
Copter: Support NoGPS AltHold like mode
Browse files Browse the repository at this point in the history
  • Loading branch information
@lthall
lthall committed Aug 10, 2023
1 parent 4226371 commit 3c62658
Show file tree
Hide file tree
Showing 4 changed files with 95 additions and 9 deletions.
17 changes: 11 additions & 6 deletions ArduCopter/GCS_Mavlink.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1198,7 +1198,8 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg)
if (packet.coordinate_frame != MAV_FRAME_LOCAL_NED &&
packet.coordinate_frame != MAV_FRAME_LOCAL_OFFSET_NED &&
packet.coordinate_frame != MAV_FRAME_BODY_NED &&
packet.coordinate_frame != MAV_FRAME_BODY_OFFSET_NED) {
packet.coordinate_frame != MAV_FRAME_BODY_OFFSET_NED &&
packet.coordinate_frame != MAV_FRAME_LOCAL_FRD) {
// input is not valid so stop
copter.mode_guided.init(true);
break;
Expand All @@ -1214,6 +1215,8 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg)
// Force inputs are not supported
// Do not accept command if force_set is true and acc_ignore is false
if (force_set && !acc_ignore) {
// input is not valid so stop
copter.mode_guided.init(true);
break;
}

Expand All @@ -1224,13 +1227,15 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg)
pos_vector = Vector3f(packet.x * 100.0f, packet.y * 100.0f, -packet.z * 100.0f);
// rotate to body-frame if necessary
if (packet.coordinate_frame == MAV_FRAME_BODY_NED ||
packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED ||
packet.coordinate_frame == MAV_FRAME_LOCAL_FRD) {
copter.rotate_body_frame_to_NE(pos_vector.x, pos_vector.y);
}
// add body offset if necessary
if (packet.coordinate_frame == MAV_FRAME_LOCAL_OFFSET_NED ||
packet.coordinate_frame == MAV_FRAME_BODY_NED ||
packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED ||
packet.coordinate_frame == MAV_FRAME_LOCAL_FRD) {
pos_vector += copter.inertial_nav.get_position_neu_cm();
}
}
Expand All @@ -1246,7 +1251,7 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg)
}
vel_vector *= 100; // m/s -> cm/s
// rotate to body-frame if necessary
if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED || packet.coordinate_frame == MAV_FRAME_LOCAL_FRD) {
copter.rotate_body_frame_to_NE(vel_vector.x, vel_vector.y);
}
}
Expand All @@ -1257,7 +1262,7 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg)
// convert to cm
accel_vector = Vector3f(packet.afx * 100.0f, packet.afy * 100.0f, -packet.afz * 100.0f);
// rotate to body-frame if necessary
if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED || packet.coordinate_frame == MAV_FRAME_LOCAL_FRD) {
copter.rotate_body_frame_to_NE(accel_vector.x, accel_vector.y);
}
}
Expand All @@ -1268,7 +1273,7 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg)
float yaw_rate_cds = 0.0f;
if (!yaw_ignore) {
yaw_cd = ToDeg(packet.yaw) * 100.0f;
yaw_relative = packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED;
yaw_relative = packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED || packet.coordinate_frame == MAV_FRAME_LOCAL_FRD;
}
if (!yaw_rate_ignore) {
yaw_rate_cds = ToDeg(packet.yaw_rate) * 100.0f;
Expand Down
5 changes: 4 additions & 1 deletion ArduCopter/mode.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1031,6 +1031,7 @@ class ModeGuided : public Mode {

void angle_control_start();
void angle_control_run();
void accel_control_run_nogps();

// return guided mode timeout in milliseconds. Only used for velocity, acceleration, angle control, and angular rate control
uint32_t get_timeout_ms() const;
Expand All @@ -1040,6 +1041,7 @@ class ModeGuided : public Mode {
// pause continue in guided mode
bool pause() override;
bool resume() override;
SubMode guided_mode = SubMode::TakeOff;

// true if weathervaning is allowed in guided
#if WEATHERVANE_ENABLED == ENABLED
Expand Down Expand Up @@ -1087,7 +1089,6 @@ class ModeGuided : public Mode {
void set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle);

// controls which controller is run (pos or vel):
SubMode guided_mode = SubMode::TakeOff;
bool send_notification; // used to send one time notification to ground station
bool takeoff_complete; // true once takeoff has completed (used to trigger retracting of landing gear)

Expand Down Expand Up @@ -1116,6 +1117,8 @@ class ModeGuidedNoGPS : public ModeGuided {
const char *name4() const override { return "GNGP"; }

private:
void accel_control_start();
void accel_control_run();

};

Expand Down
61 changes: 61 additions & 0 deletions ArduCopter/mode_guided.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -546,6 +546,7 @@ void ModeGuided::set_velaccel(const Vector3f& velocity, const Vector3f& accelera
guided_pos_terrain_alt = false;
guided_vel_target_cms = velocity;
guided_accel_target_cmss = acceleration;
guided_angle_state.climb_rate_cms = -velocity.z;
update_time_ms = millis();

// log target
Expand Down Expand Up @@ -976,6 +977,66 @@ void ModeGuided::angle_control_run()
}
}

// velaccel_control_run - runs the guided velocity controller
// called from guided_run
void ModeGuided::accel_control_run_nogps()
{
float climb_rate_cms = 0.0f;
if (!guided_angle_state.use_thrust) {
// constrain climb rate
climb_rate_cms = constrain_float(guided_angle_state.climb_rate_cms, -wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up());

// get avoidance adjusted climb rate
climb_rate_cms = get_avoidance_adjusted_climbrate(climb_rate_cms);
}

// check for timeout - set lean angles and climb rate to zero if no updates received for 3 seconds
uint32_t tnow = millis();
if (tnow - guided_angle_state.update_time_ms > get_timeout_ms()) {
guided_angle_state.attitude_quat.initialise();
guided_angle_state.ang_vel.zero();
climb_rate_cms = 0.0f;
guided_accel_target_cmss.zero();
}

// interpret positive climb rate as triggering take-off
if (motors->armed() && is_positive(climb_rate_cms)) {
copter.set_auto_armed(true);
}

// if not armed set throttle to zero and exit immediately
if (!motors->armed() || !copter.ap.auto_armed || (copter.ap.land_complete && !is_positive(climb_rate_cms))) {
// do not spool down tradheli when on the ground with motor interlock enabled
make_safe_ground_handling(copter.is_tradheli() && motors->get_interlock());
return;
}

// TODO: use get_alt_hold_state
// landed with positive desired climb rate, takeoff
if (copter.ap.land_complete && (guided_angle_state.climb_rate_cms > 0.0f)) {
zero_throttle_and_relax_ac();
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
if (motors->get_spool_state() == AP_Motors::SpoolState::THROTTLE_UNLIMITED) {
set_land_complete(false);
pos_control->init_z_controller();
}
return;
}

// set motors to full range
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);

// call attitude controller
attitude_control->input_quaternion(guided_angle_state.attitude_quat, guided_angle_state.ang_vel);

// call attitude controller with auto yaw
attitude_control->input_thrust_vector_heading(guided_accel_target_cmss, auto_yaw.get_heading());

// call position controller
pos_control->set_pos_target_z_from_climb_rate_cm(climb_rate_cms);
pos_control->update_z_controller();
}

// helper function to set yaw state and targets
void ModeGuided::set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle)
{
Expand Down
21 changes: 19 additions & 2 deletions ArduCopter/mode_guided_nogps.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,8 +18,25 @@ bool ModeGuidedNoGPS::init(bool ignore_checks)
// should be called at 100hz or more
void ModeGuidedNoGPS::run()
{
// run angle controller
ModeGuided::angle_control_run();

// call the correct auto controller

switch (ModeGuided::guided_mode) {

case SubMode::VelAccel:
ModeGuided::accel_control_run_nogps();
break;

case SubMode::Angle:
ModeGuided::angle_control_run();
break;

default:
// this will only run once
ModeGuided::angle_control_start();
ModeGuided::angle_control_run();
break;
}
}

#endif

0 comments on commit 3c62658

Please sign in to comment.