diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index 1b003459d5e..623e14a01d9 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -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: diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 8c624ee2c58..f8353bbeae3 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -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(); diff --git a/ArduSub/control_velhold.cpp b/ArduSub/control_velhold.cpp deleted file mode 100644 index 3832d8394e6..00000000000 --- a/ArduSub/control_velhold.cpp +++ /dev/null @@ -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 diff --git a/ArduSub/defines.h b/ArduSub/defines.h index 96e4b388f10..da0470f2742 100644 --- a/ArduSub/defines.h +++ b/ArduSub/defines.h @@ -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 diff --git a/ArduSub/flight_mode.cpp b/ArduSub/flight_mode.cpp index 1ca6388013c..170b2e79ce2 100644 --- a/ArduSub/flight_mode.cpp +++ b/ArduSub/flight_mode.cpp @@ -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;