aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Sub: Remove VELHOLD mode

Browse Source 
Obsolete since GUIDED implemented
This commit is contained in:
Jacob Walser 2017-03-23 00:21:41 -04:00
parent 5b4ec88f11
commit b0e5a93099
5 changed files with 2 additions and 241 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduSub/GCS_Mavlink.cpp
View File

@ -49,7 +49,6 @@ NOINLINE void Sub::send_heartbeat(mavlink_channel_t chan)
base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED;
switch (control_mode) {
case AUTO:
case VELHOLD:
case GUIDED:
case CIRCLE:
case POSHOLD:
@ -140,7 +139,6 @@ NOINLINE void Sub::send_extended_status1(mavlink_channel_t chan)
case ALT_HOLD:
case AUTO:
case GUIDED:
case VELHOLD:
case CIRCLE:
case SURFACE:
case POSHOLD:

3
ArduSub/Sub.h
View File

@ -610,8 +610,7 @@ private:
void guided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm);
void guided_limit_init_time_and_pos();
bool guided_limit_check();
bool velhold_init(bool ignore_checks);
void velhold_run();
bool poshold_init(bool ignore_checks);
void poshold_run();

223
ArduSub/control_velhold.cpp
View File

@ -1,223 +0,0 @@
// ArduSub velocity hold flight mode
// Pilot adjusts desired forward and lateral body-frame velocities
// Position controller maintains desired velocities
// GPS position required
// Jacob Walser August 2016
#include "Sub.h"
#if POSHOLD_ENABLED == ENABLED
namespace {
static uint32_t last_loiter_message_ms = 0;
float des_velx = 0; // inav earth-frame desired velocity +/- = north/south
float des_vely = 0; // inav earth-frame desired velocity +/- = east/west
float des_velf = 0; // pilot body-frame desired velocity +/- = forward/backward
float des_velr = 0; // pilot body-frame desired velocity +/- = right/left
}
// Initialize the VelHold controller
bool Sub::velhold_init(bool ignore_checks)
{
// fail to initialise VelHold mode if no GPS lock
if (!position_ok() && !ignore_checks) {
return false;
}
pos_control.init_xy_controller();
// set speed and acceleration from wpnav's speed and acceleration
pos_control.set_speed_xy(wp_nav.get_speed_xy());
pos_control.set_accel_xy(wp_nav.get_wp_acceleration());
pos_control.set_jerk_xy_to_default();
const Vector3f& curr_pos = inertial_nav.get_position();
const Vector3f& curr_vel = inertial_nav.get_velocity();
// set target position and velocity to current position and velocity
pos_control.set_xy_target(curr_pos.x, curr_pos.y);
pos_control.set_desired_velocity_xy(curr_vel.x, curr_vel.y);
// initialize vertical speeds and acceleration
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
pos_control.set_accel_z(g.pilot_accel_z);
// initialise position and desired velocity
pos_control.set_alt_target(inertial_nav.get_altitude());
pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
des_velf = 0;
des_velr = 0;
des_velx = 0;
des_vely = 0;
return true;
}
// Run the VelHold controller
// should be called at 100hz or more
void Sub::velhold_run()
{
uint32_t tnow = millis();
const Vector3f& vel = inertial_nav.get_velocity();
// convert inertial nav earth-frame velocities to body-frame
// To-Do: move this to AP_Math (or perhaps we already have a function to do this)
float vel_fw = vel.x*ahrs.cos_yaw() + vel.y*ahrs.sin_yaw();
float vel_right = -vel.x*ahrs.sin_yaw() + vel.y*ahrs.cos_yaw();
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
// reset target velocities
des_velf = 0;
des_velr = 0;
des_velx = 0;
des_vely = 0;
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
// Reset position controller
pos_control.relax_alt_hold_controllers(motors.get_throttle_hover());
pos_control.set_pos_target(inertial_nav.get_position());
pos_control.set_desired_velocity(Vector3f(0,0,0));
return;
}
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// get pilot's desired yaw rate in centidegrees per second
float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
// get pilot desired climb rate
float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());
target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max, g.pilot_velocity_z_max);
int16_t pilot_lateral = channel_lateral->get_control_in();
int16_t pilot_forward = channel_forward->get_control_in();
float lateral_out = 0;
float forward_out = 0;
// Pilot adjusts desired body-frame velocities
if (pilot_lateral > 1000 || pilot_lateral < -1000 || pilot_forward > 1000 || pilot_forward < -1000) {
//Todo find a better way to do this
des_velf += pilot_forward * 0.0001;
des_velr += pilot_lateral * 0.0001;
// desired forward and right speeds in body-frame
des_velf = constrain_float(des_velf, -25.0, 25.0);
des_velr = constrain_float(des_velr, -25.0, 25.0);
}
// rotate pilot desired body-frame velocities to earth-frame
// forward velocity only (maintain zero lateral velocity)
des_vely = des_velf * ahrs.sin_yaw(); // +East / -West
des_velx = des_velf * ahrs.cos_yaw(); // +North / -South
// lateral velocity only (maintain zero forward velocity)
// des_vely = des_velr * ahrs.cos_yaw(); // +East / -West
// des_velx = -des_velr * ahrs.sin_yaw(); // +North / -South
//combined forward/lateral velocities
// des_vely = des_velf * ahrs.sin_yaw() + des_velr * ahrs.cos_yaw(); // +East / -West
// des_velx = des_velf * ahrs.cos_yaw() - des_velr * ahrs.sin_yaw(); // +North / -South
// set target position and velocity to current position and velocity
pos_control.set_desired_velocity_xy(des_velx, des_vely);
// run position controller
pos_control.update_xy_controller(AC_PosControl::XY_MODE_POS_AND_VEL_FF, ekfNavVelGainScaler, false);
// get pos_control forward and lateral outputs from wp_nav pitch and roll (from copter code)
float poscontrol_lateral = pos_control.get_roll(); //
float poscontrol_forward = -pos_control.get_pitch(); // output is reversed
// constrain target forward/lateral values
poscontrol_lateral = constrain_int16(poscontrol_lateral, -aparm.angle_max, aparm.angle_max);
poscontrol_forward = constrain_int16(poscontrol_forward, -aparm.angle_max, aparm.angle_max);
// normalize output values
lateral_out = poscontrol_lateral/(float)aparm.angle_max;
forward_out = poscontrol_forward/(float)aparm.angle_max;
// output
motors.set_lateral(lateral_out);
motors.set_forward(forward_out);
// convert pilot input to lean angles
// To-Do: convert get_pilot_desired_lean_angles to return angles as floats
float target_roll, target_pitch;
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max);
// update attitude controller targets
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
// adjust climb rate using rangefinder
if (rangefinder_alt_ok()) {
// if rangefinder is ok, use surface tracking
target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control.get_alt_target(), G_Dt);
}
// call z axis position controller
if (ap.at_bottom) {
pos_control.relax_alt_hold_controllers(0.0); // clear velocity and position targets, and integrator
pos_control.set_alt_target(inertial_nav.get_altitude() + 10.0f); // set target to 10 cm above bottom
} else {
if (inertial_nav.get_altitude() < g.surface_depth) { // pilot allowed to move up or down freely
pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
} else if (target_climb_rate < 0) { // pilot allowed to move only down freely
if (pos_control.get_vel_target_z() > 0) {
pos_control.relax_alt_hold_controllers(0); // reset target velocity and acceleration
}
pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
} else if (pos_control.get_alt_target() > g.surface_depth) { // hold depth at surface level.
pos_control.set_alt_target(g.surface_depth);
}
}
pos_control.update_z_controller();
if (tnow > last_loiter_message_ms + 200) {
// gcs_send_text_fmt(MAV_SEVERITY_INFO, "desvelf: %f, %f", des_velf, des_velr);
// gcs_send_text_fmt(MAV_SEVERITY_INFO, "desvelx: %f, %f", des_velx, des_vely);
// gcs_send_text_fmt(MAV_SEVERITY_INFO, "vel: %f, %f\n", vel_fw, vel_right);
last_loiter_message_ms = tnow;
mavlink_msg_command_long_send(
(mavlink_channel_t)0, //channel
0, //target system
0, //target component
45, //command
0, //confirmation
des_velf,//1
des_velr,
vel_fw,
vel_right,
forward_out,
lateral_out,
poscontrol_forward
);
// mavlink_msg_command_long_send(
// (mavlink_channel_t)0, //channel
// 0, //target system
// 0, //target component
// 46, //command
// 0, //confirmation
// des_velx,//1
// des_vely,
// vel.x,
// vel.y,
// ahrs.yaw_sensor,
// ahrs.sin_yaw(),
// ahrs.cos_yaw()
// );
}
}
#endif // POSHOLD_ENABLED == ENABLED

1
ArduSub/defines.h
View File

@ -40,7 +40,6 @@ enum control_mode_t {
ALT_HOLD = 2, // manual angle with automatic depth/throttle
AUTO = 3, // not implemented in sub // fully automatic waypoint control using mission commands
GUIDED = 4, // not implemented in sub // fully automatic fly to coordinate or fly at velocity/direction using GCS immediate commands
VELHOLD = 5, // automatic x/y velocity control and automatic depth/throttle
CIRCLE = 7, // not implemented in sub // automatic circular flight with automatic throttle
SURFACE = 9, // automatically return to surface, pilot maintains horizontal control
POSHOLD = 16, // automatic position hold with manual override, with automatic throttle

14
ArduSub/flight_mode.cpp
View File

@ -45,10 +45,6 @@ bool Sub::set_mode(control_mode_t mode, mode_reason_t reason)
success = circle_init(ignore_checks);
break;
case VELHOLD:
success = velhold_init(ignore_checks);
break;
case GUIDED:
success = guided_init(ignore_checks);
break;
@ -132,10 +128,6 @@ void Sub::update_flight_mode()
circle_run();
break;
case VELHOLD:
velhold_run();
break;
case GUIDED:
guided_run();
break;
@ -179,7 +171,6 @@ bool Sub::mode_requires_GPS(control_mode_t mode)
switch (mode) {
case AUTO:
case GUIDED:
case VELHOLD:
case CIRCLE:
case POSHOLD:
return true;
@ -209,7 +200,7 @@ bool Sub::mode_has_manual_throttle(control_mode_t mode)
// arming_from_gcs should be set to true if the arming request comes from the ground station
bool Sub::mode_allows_arming(control_mode_t mode, bool arming_from_gcs)
{
if (mode_has_manual_throttle(mode) || mode == VELHOLD || mode == ALT_HOLD || mode == POSHOLD || (arming_from_gcs && mode == GUIDED)) {
if (mode_has_manual_throttle(mode) || mode == ALT_HOLD || mode == POSHOLD || (arming_from_gcs && mode == GUIDED)) {
return true;
}
return false;
@ -254,9 +245,6 @@ void Sub::print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
case GUIDED:
port->print("GUIDED");
break;
case VELHOLD:
port->print("VELHOLD");
break;
case CIRCLE:
port->print("CIRCLE");
break;