From ed87bd9e59452cf1a74235166f2dfdf84995cd85 Mon Sep 17 00:00:00 2001 From: Jacob Walser Date: Fri, 3 Feb 2017 15:18:27 +1100 Subject: [PATCH] Sub: fixes from rebase on ArduPilot master --- ArduSub/ArduSub.cpp | 100 ++--- ArduSub/Attitude.cpp | 122 +----- ArduSub/GCS_Mavlink.cpp | 289 ++++++-------- ArduSub/Log.cpp | 7 +- ArduSub/Parameters.cpp | 4 +- ArduSub/Sub.cpp | 13 +- ArduSub/Sub.h | 11 +- ArduSub/arming_checks.cpp | 47 +-- ArduSub/baro_ground_effect.cpp | 72 ---- ArduSub/compassmot.cpp | 8 +- ArduSub/config.h | 59 +-- ArduSub/control_acro.cpp | 17 +- ArduSub/control_althold.cpp | 153 +++----- ArduSub/control_auto.cpp | 162 +------- ArduSub/control_autotune.cpp | 64 +-- ArduSub/control_circle.cpp | 4 +- ArduSub/control_drift.cpp | 121 ------ ArduSub/control_flip.cpp | 218 ---------- ArduSub/control_guided.cpp | 79 +--- ArduSub/control_land.cpp | 271 ------------- ArduSub/control_poshold.cpp | 698 +++++---------------------------- ArduSub/control_rtl.cpp | 503 ------------------------ ArduSub/control_stabilize.cpp | 17 +- ArduSub/control_transect.cpp | 289 ++++++++++---- ArduSub/control_velhold.cpp | 361 ++++++++++------- ArduSub/crash_check.cpp | 138 +------ ArduSub/defines.h | 13 +- ArduSub/esc_calibration.cpp | 23 +- ArduSub/flight_mode.cpp | 7 - ArduSub/motor_test.cpp | 5 +- ArduSub/motors.cpp | 145 ++----- ArduSub/position_vector.cpp | 2 +- ArduSub/radio.cpp | 129 ++---- ArduSub/setup.cpp | 12 +- ArduSub/switches.cpp | 213 ++-------- ArduSub/system.cpp | 66 +++- ArduSub/tuning.cpp | 12 +- ArduSub/wscript | 10 +- 38 files changed, 994 insertions(+), 3470 deletions(-) delete mode 100644 ArduSub/baro_ground_effect.cpp delete mode 100644 ArduSub/control_drift.cpp delete mode 100644 ArduSub/control_flip.cpp delete mode 100644 ArduSub/control_land.cpp delete mode 100644 ArduSub/control_rtl.cpp diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp index 8d52e5be6d..be7e491d14 100644 --- a/ArduSub/ArduSub.cpp +++ b/ArduSub/ArduSub.cpp @@ -96,17 +96,10 @@ const AP_Scheduler::Task Sub::scheduler_tasks[] = { SCHED_TASK(read_rangefinder, 20, 100), SCHED_TASK(update_altitude, 10, 100), SCHED_TASK(run_nav_updates, 50, 100), - SCHED_TASK(update_thr_average, 100, 90), SCHED_TASK(three_hz_loop, 3, 75), SCHED_TASK(update_turn_counter, 10, 50), SCHED_TASK(compass_accumulate, 100, 100), SCHED_TASK(barometer_accumulate, 50, 90), -#if PRECISION_LANDING == ENABLED - SCHED_TASK(update_precland, 400, 50), -#endif -#if FRAME_CONFIG == HELI_FRAME - SCHED_TASK(check_dynamic_flight, 50, 75), -#endif SCHED_TASK(update_notify, 50, 90), SCHED_TASK(one_hz_loop, 1, 100), SCHED_TASK(ekf_check, 10, 75), @@ -119,8 +112,7 @@ const AP_Scheduler::Task Sub::scheduler_tasks[] = { SCHED_TASK(camera_tilt_smooth, 50, 50), SCHED_TASK(update_trigger, 50, 75), SCHED_TASK(ten_hz_logging_loop, 10, 350), - SCHED_TASK(fifty_hz_logging_loop, 50, 110), - SCHED_TASK(full_rate_logging_loop,400, 100), + SCHED_TASK(twentyfive_hz_logging, 25, 110), SCHED_TASK(dataflash_periodic, 400, 300), SCHED_TASK(perf_update, 0.1, 75), #if RPM_ENABLED == ENABLED @@ -245,18 +237,10 @@ void Sub::fast_loop() // -------------------- read_AHRS(); - // run low level rate controllers that only require IMU data - attitude_control.rate_controller_run(); - -<<<<<<< 6dafedb2d1ad5061d859a9c319fa4b69b4ac5dd9 - motors.set_forward(channel_forward->control_in); - motors.set_strafe(channel_strafe->control_in); - -======= ->>>>>>> Changed to ArduCopter as the base code. -#if FRAME_CONFIG == HELI_FRAME - update_heli_control_dynamics(); -#endif //HELI_FRAME + if(control_mode != MANUAL) { //don't run rate controller in manual mode + // run low level rate controllers that only require IMU data + attitude_control.rate_controller_run(); + } // send outputs to the motors library motors_output(); @@ -277,8 +261,10 @@ void Sub::fast_loop() // check if we've reached the surface or bottom update_surface_and_bottom_detector(); +#if MOUNT == ENABLED // camera mount's fast update camera_mount.update_fast(); +#endif // log sensor health if (should_log(MASK_LOG_ANY)) { @@ -302,18 +288,6 @@ void Sub::throttle_loop() { // check auto_armed status update_auto_armed(); - -#if FRAME_CONFIG == HELI_FRAME - // update rotor speed - heli_update_rotor_speed_targets(); - - // update trad heli swash plate movement - heli_update_landing_swash(); -#endif - -#if GNDEFFECT_COMPENSATION == ENABLED - update_ground_effect_detector(); -#endif // GNDEFFECT_COMPENSATION == ENABLED } // update_mount - update camera mount position @@ -350,10 +324,10 @@ void Sub::update_batt_compass(void) if(g.compass_enabled) { // update compass with throttle value - used for compassmot - compass.set_throttle(motors.get_throttle()/1000.0f); + compass.set_throttle(motors.get_throttle()); compass.read(); // log compass information - if (should_log(MASK_LOG_COMPASS)) { + if (should_log(MASK_LOG_COMPASS) && !ahrs.have_ekf_logging()) { DataFlash.Log_Write_Compass(compass); } } @@ -389,14 +363,14 @@ void Sub::ten_hz_logging_loop() if (should_log(MASK_LOG_IMU) || should_log(MASK_LOG_IMU_FAST) || should_log(MASK_LOG_IMU_RAW)) { DataFlash.Log_Write_Vibration(ins); } -#if FRAME_CONFIG == HELI_FRAME - Log_Write_Heli(); -#endif + if (should_log(MASK_LOG_CTUN)) { + attitude_control.control_monitor_log(); + } } -// fifty_hz_logging_loop -// should be run at 50hz -void Sub::fifty_hz_logging_loop() +// twentyfive_hz_logging_loop +// should be run at 25hz +void Sub::twentyfive_hz_logging() { #if HIL_MODE != HIL_MODE_DISABLED // HIL for a copter needs very fast update of the servo values @@ -416,27 +390,10 @@ void Sub::fifty_hz_logging_loop() } // log IMU data if we're not already logging at the higher rate - if (should_log(MASK_LOG_IMU) && !(should_log(MASK_LOG_IMU_FAST) || should_log(MASK_LOG_IMU_RAW))) { + if (should_log(MASK_LOG_IMU) && !should_log(MASK_LOG_IMU_RAW)) { DataFlash.Log_Write_IMU(ins); } #endif - -#if PRECISION_LANDING == ENABLED - // log output - Log_Write_Precland(); -#endif -} - -// full_rate_logging_loop -// should be run at the MAIN_LOOP_RATE -void Sub::full_rate_logging_loop() -{ - if (should_log(MASK_LOG_IMU_FAST) && !should_log(MASK_LOG_IMU_RAW)) { - DataFlash.Log_Write_IMU(ins); - } - if (should_log(MASK_LOG_IMU_FAST) || should_log(MASK_LOG_IMU_RAW)) { - DataFlash.Log_Write_IMUDT(ins); - } } void Sub::dataflash_periodic(void) @@ -487,15 +444,8 @@ void Sub::one_hz_loop() update_using_interlock(); -#if FRAME_CONFIG != HELI_FRAME - // check the user hasn't updated the frame orientation - motors.set_frame_orientation(g.frame_orientation); - // set all throttle channel settings - motors.set_throttle_range(g.throttle_min, channel_throttle->radio_min, channel_throttle->radio_max); - // set hover throttle - motors.set_hover_throttle(g.throttle_mid); -#endif + motors.set_throttle_range(channel_throttle->get_radio_min(), channel_throttle->get_radio_max()); } // update assigned functions and enable auxiliary servos @@ -527,8 +477,8 @@ void Sub::update_GPS(void) last_gps_reading[i] = gps.last_message_time_ms(i); // log GPS message - if (should_log(MASK_LOG_GPS)) { - DataFlash.Log_Write_GPS(gps, i, current_loc.alt); + if (should_log(MASK_LOG_GPS) && !ahrs.have_ekf_logging()) { + DataFlash.Log_Write_GPS(gps, i); } gps_updated = true; @@ -583,17 +533,17 @@ void Sub::update_simple_mode(void) if (ap.simple_mode == 1) { // rotate roll, pitch input by -initial simple heading (i.e. north facing) - rollx = channel_roll->control_in*simple_cos_yaw - channel_pitch->control_in*simple_sin_yaw; - pitchx = channel_roll->control_in*simple_sin_yaw + channel_pitch->control_in*simple_cos_yaw; + rollx = channel_roll->get_control_in()*simple_cos_yaw - channel_pitch->get_control_in()*simple_sin_yaw; + pitchx = channel_roll->get_control_in()*simple_sin_yaw + channel_pitch->get_control_in()*simple_cos_yaw; }else{ // rotate roll, pitch input by -super simple heading (reverse of heading to home) - rollx = channel_roll->control_in*super_simple_cos_yaw - channel_pitch->control_in*super_simple_sin_yaw; - pitchx = channel_roll->control_in*super_simple_sin_yaw + channel_pitch->control_in*super_simple_cos_yaw; + rollx = channel_roll->get_control_in()*super_simple_cos_yaw - channel_pitch->get_control_in()*super_simple_sin_yaw; + pitchx = channel_roll->get_control_in()*super_simple_sin_yaw + channel_pitch->get_control_in()*super_simple_cos_yaw; } // rotate roll, pitch input from north facing to vehicle's perspective - channel_roll->control_in = rollx*ahrs.cos_yaw() + pitchx*ahrs.sin_yaw(); - channel_pitch->control_in = -rollx*ahrs.sin_yaw() + pitchx*ahrs.cos_yaw(); + channel_roll->set_control_in(rollx*ahrs.cos_yaw() + pitchx*ahrs.sin_yaw()); + channel_pitch->set_control_in(-rollx*ahrs.sin_yaw() + pitchx*ahrs.cos_yaw()); } // update_super_simple_bearing - adjusts simple bearing based on location diff --git a/ArduSub/Attitude.cpp b/ArduSub/Attitude.cpp index a08406afe4..6b71ddde49 100644 --- a/ArduSub/Attitude.cpp +++ b/ArduSub/Attitude.cpp @@ -2,7 +2,7 @@ #include "Sub.h" -// get_smoothing_gain - returns smoothing gain to be passed into attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth +// get_smoothing_gain - returns smoothing gain to be passed into attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw // result is a number from 2 to 12 with 2 being very sluggish and 12 being very crisp float Sub::get_smoothing_gain() { @@ -25,7 +25,7 @@ void Sub::get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &ro pitch_in *= scaler; // do circular limit - float total_in = pythagorous2((float)pitch_in, (float)roll_in); + float total_in = norm(pitch_in, roll_in); if (total_in > angle_max) { float ratio = angle_max / total_in; roll_in *= ratio; @@ -82,7 +82,7 @@ float Sub::get_roi_yaw() float Sub::get_look_ahead_yaw() { const Vector3f& vel = inertial_nav.get_velocity(); - float speed = pythagorous2(vel.x,vel.y); + float speed = norm(vel.x,vel.y); // Commanded Yaw to automatically look ahead. if (position_ok() && (speed > YAW_LOOK_AHEAD_MIN_SPEED)) { yaw_look_ahead_bearing = degrees(atan2f(vel.y,vel.x))*100.0f; @@ -94,70 +94,6 @@ float Sub::get_look_ahead_yaw() * throttle control ****************************************************************/ -// update_thr_average - update estimated throttle required to hover (if necessary) -// should be called at 100hz -void Sub::update_thr_average() -{ - // ensure throttle_average has been initialised - if( is_zero(throttle_average) ) { - throttle_average = 0.5f; - // update position controller - pos_control.set_throttle_hover(throttle_average); - } - - // if not armed or landed exit - if (!motors.armed() || ap.land_complete) { - return; - } - - // get throttle output - float throttle = motors.get_throttle(); - - // calc average throttle if we are in a level hover - if (throttle > 0.0f && abs(climb_rate) < 60 && labs(ahrs.roll_sensor) < 500 && labs(ahrs.pitch_sensor) < 500) { - throttle_average = throttle_average * 0.99f + throttle * 0.01f; - // update position controller - pos_control.set_throttle_hover(throttle_average); - } -} - -// set_throttle_takeoff - allows parents to tell throttle controller we are taking off so I terms can be cleared -void Sub::set_throttle_takeoff() -{ - // tell position controller to reset alt target and reset I terms - pos_control.init_takeoff(); -} - -// get_pilot_desired_throttle - transform pilot's throttle input to make cruise throttle mid stick -// used only for manual throttle modes -// returns throttle output 0 to 1000 -float Sub::get_pilot_desired_throttle(int16_t throttle_control) -{ - float throttle_out; - - int16_t mid_stick = channel_throttle->get_control_mid(); - - // ensure reasonable throttle values - throttle_control = constrain_int16(throttle_control,0,1000); - // ensure mid throttle is set within a reasonable range - g.throttle_mid = constrain_int16(g.throttle_mid,g.throttle_min+50,700); - float thr_mid = MAX(0,g.throttle_mid-g.throttle_min) / (float)(1000-g.throttle_min); - - // check throttle is above, below or in the deadband - if (throttle_control < mid_stick) { - // below the deadband - throttle_out = ((float)throttle_control)*thr_mid/(float)mid_stick; - }else if(throttle_control > mid_stick) { - // above the deadband - throttle_out = (thr_mid) + ((float)(throttle_control-mid_stick)) * (1.0f - thr_mid) / (float)(1000-mid_stick); - }else{ - // must be in the deadband - throttle_out = thr_mid; - } - - return throttle_out; -} - // get_pilot_desired_climb_rate - transform pilot's throttle input to climb rate in cm/s // without any deadzone at the bottom float Sub::get_pilot_desired_climb_rate(float throttle_control) @@ -196,51 +132,6 @@ float Sub::get_pilot_desired_climb_rate(float throttle_control) return desired_rate; } -// get_non_takeoff_throttle - a throttle somewhere between min and mid throttle which should not lead to a takeoff -float Sub::get_non_takeoff_throttle() -{ - return (((float)g.throttle_mid/1000.0f)/2.0f); -} - -float Sub::get_takeoff_trigger_throttle() -{ - return channel_throttle->get_control_mid() + g.takeoff_trigger_dz; -} - -// get_throttle_pre_takeoff - convert pilot's input throttle to a throttle output (in the range 0 to 1) before take-off -// used only for althold, loiter, hybrid flight modes -float Sub::get_throttle_pre_takeoff(float input_thr) -{ - // exit immediately if input_thr is zero - if (input_thr <= 0.0f) { - return 0.0f; - } - - // ensure reasonable throttle values - input_thr = constrain_float(input_thr,0.0f,1000.0f); - - float in_min = 0.0f; - float in_max = get_takeoff_trigger_throttle(); - - // multicopters will output between spin-when-armed and 1/2 of mid throttle - float out_min = 0.0f; - float out_max = get_non_takeoff_throttle(); - - if ((g.throttle_behavior & THR_BEHAVE_FEEDBACK_FROM_MID_STICK) != 0) { - in_min = channel_throttle->get_control_mid(); - } - - float input_range = in_max-in_min; - float output_range = out_max-out_min; - - // sanity check ranges - if (input_range <= 0.0f || output_range <= 0.0f) { - return 0.0f; - } - - return constrain_float(out_min + (input_thr-in_min)*output_range/input_range, out_min, out_max); -} - // get_surface_tracking_climb_rate - hold copter at the desired distance above the ground // returns climb rate (in cm/s) which should be passed to the position controller float Sub::get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt) @@ -280,13 +171,6 @@ float Sub::get_surface_tracking_climb_rate(int16_t target_rate, float current_al #endif } -// set_accel_throttle_I_from_pilot_throttle - smoothes transition from pilot controlled throttle to autopilot throttle -void Sub::set_accel_throttle_I_from_pilot_throttle(float pilot_throttle) -{ - // shift difference between pilot's throttle and hover throttle into accelerometer I - g.pid_accel_z.set_integrator((pilot_throttle-throttle_average) * 1000.0f); -} - // updates position controller's maximum altitude using fence and EKF limits void Sub::update_poscon_alt_max() { diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index 59dc4c7f4f..f64d575cf6 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -1,6 +1,9 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- #include "Sub.h" +#include "version.h" + +#include "GCS_Mavlink.h" // default sensors are present and healthy: gyro, accelerometer, barometer, rate_control, attitude_stabilization, yaw_position, altitude control, x/y position control, motor_control #define MAVLINK_SENSOR_PRESENT_DEFAULT (MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL | MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE | MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL | MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION | MAV_SYS_STATUS_SENSOR_YAW_POSITION | MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL | MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL | MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS | MAV_SYS_STATUS_AHRS) @@ -33,7 +36,7 @@ NOINLINE void Sub::send_heartbeat(mavlink_channel_t chan) uint32_t custom_mode = control_mode; // set system as critical if any failsafe have triggered - if (failsafe.radio || failsafe.battery || failsafe.gcs || failsafe.ekf || failsafe.terrain) { + if (failsafe.manual_control || failsafe.battery || failsafe.gcs || failsafe.ekf || failsafe.terrain) { system_status = MAV_STATE_CRITICAL; } @@ -78,31 +81,13 @@ NOINLINE void Sub::send_heartbeat(mavlink_channel_t chan) // indicate we have set a custom mode base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; - mavlink_msg_heartbeat_send( - chan, -#if (FRAME_CONFIG == QUAD_FRAME) - MAV_TYPE_QUADROTOR, -#elif (FRAME_CONFIG == TRI_FRAME) - MAV_TYPE_TRICOPTER, -#elif (FRAME_CONFIG == HEXA_FRAME || FRAME_CONFIG == Y6_FRAME) - MAV_TYPE_HEXAROTOR, -#elif (FRAME_CONFIG == OCTA_FRAME || FRAME_CONFIG == OCTA_QUAD_FRAME) - MAV_TYPE_OCTOROTOR, -#elif (FRAME_CONFIG == HELI_FRAME) - MAV_TYPE_HELICOPTER, -#elif (FRAME_CONFIG == SINGLE_FRAME) //because mavlink did not define a singlecopter, we use a rocket - MAV_TYPE_ROCKET, -#elif (FRAME_CONFIG == COAX_FRAME) //because mavlink did not define a singlecopter, we use a rocket - MAV_TYPE_ROCKET, -#elif (FRAME_CONFIG == BLUEROV_FRAME || FRAME_CONFIG == VECTORED_FRAME || FRAME_CONFIG == VECTORED6DOF_FRAME || FRAME_CONFIG == SIMPLEROV_FRAME ) - MAV_TYPE_SUBMARINE, -#else - #error Unrecognised frame type -#endif - MAV_AUTOPILOT_ARDUPILOTMEGA, - base_mode, - custom_mode, - system_status); + uint8_t mav_type; + mav_type = MAV_TYPE_SUBMARINE; + + gcs[chan-MAVLINK_COMM_0].send_heartbeat(mav_type, + base_mode, + custom_mode, + system_status); } NOINLINE void Sub::send_attitude(mavlink_channel_t chan) @@ -147,11 +132,6 @@ NOINLINE void Sub::send_extended_status1(mavlink_channel_t chan) if (optflow.enabled()) { control_sensors_present |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; } -#endif -#if PRECISION_LANDING == ENABLED - if (precland.enabled()) { - control_sensors_present |= MAV_SYS_STATUS_SENSOR_VISION_POSITION; - } #endif if (ap.rc_receiver_present) { control_sensors_present |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER; @@ -206,12 +186,7 @@ NOINLINE void Sub::send_extended_status1(mavlink_channel_t chan) control_sensors_health |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; } #endif -#if PRECISION_LANDING == ENABLED - if (precland.healthy()) { - control_sensors_health |= MAV_SYS_STATUS_SENSOR_VISION_POSITION; - } -#endif - if (ap.rc_receiver_present && !failsafe.radio) { + if (ap.rc_receiver_present) { control_sensors_health |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER; } if (!ins.get_gyro_health_all() || !ins.gyro_calibrated_ok_all()) { @@ -251,14 +226,14 @@ NOINLINE void Sub::send_extended_status1(mavlink_channel_t chan) } #endif -#if CONFIG_SONAR == ENABLED - if (sonar.num_sensors() > 0) { - control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; - control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; - if (sonar.has_data()) { - control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; - } - } +#if RANGEFINDER_ENABLED == ENABLED + if (rangefinder.num_sensors() > 0) { + control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; + control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; + if (rangefinder.has_data()) { + control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; + } + } #endif if (!ap.initialised || ins.calibrating()) { @@ -346,35 +321,19 @@ void NOINLINE Sub::send_servo_out(mavlink_channel_t chan) // normalized values scaled to -10000 to 10000 // This is used for HIL. Do not change without discussing with HIL maintainers -#if FRAME_CONFIG == HELI_FRAME - mavlink_msg_rc_channels_scaled_send( - chan, - millis(), - 0, // port 0 - g.rc_1.servo_out, - g.rc_2.servo_out, - g.rc_3.radio_out, - g.rc_4.servo_out, - 0, - 0, - 0, - 0, - receiver_rssi); -#else mavlink_msg_rc_channels_scaled_send( chan, millis(), 0, // port 0 - g.rc_1.servo_out, - g.rc_2.servo_out, - g.rc_3.radio_out, - g.rc_4.servo_out, + g.rc_1.get_servo_out(), + g.rc_2.get_servo_out(), + g.rc_3.get_servo_out(), + g.rc_4.get_servo_out(), 10000 * g.rc_1.norm_output(), 10000 * g.rc_2.norm_output(), 10000 * g.rc_3.norm_output(), 10000 * g.rc_4.norm_output(), - receiver_rssi); -#endif + 0); #endif // HIL_MODE } @@ -419,17 +378,17 @@ void NOINLINE Sub::send_current_waypoint(mavlink_channel_t chan) mavlink_msg_mission_current_send(chan, mission.get_current_nav_index()); } -#if CONFIG_SONAR == ENABLED +#if RANGEFINDER_ENABLED == ENABLED void NOINLINE Sub::send_rangefinder(mavlink_channel_t chan) { - // exit immediately if sonar is disabled - if (!sonar.has_data()) { + // exit immediately if rangefinder is disabled + if (!rangefinder.has_data()) { return; } mavlink_msg_rangefinder_send( chan, - sonar.distance_cm() * 0.01f, - sonar.voltage_mv() * 0.001f); + rangefinder.distance_cm() * 0.01f, + rangefinder.voltage_mv() * 0.001f); } #endif @@ -521,27 +480,10 @@ void Sub::send_pid_tuning(mavlink_channel_t chan) } } -// are we still delaying telemetry to try to avoid Xbee bricking? -bool Sub::telemetry_delayed(mavlink_channel_t chan) -{ - uint32_t tnow = millis() >> 10; - if (tnow > (uint32_t)g.telem_delay) { - return false; - } - if (chan == MAVLINK_COMM_0 && hal.gpio->usb_connected()) { - // this is USB telemetry, so won't be an Xbee - return false; - } - // we're either on the 2nd UART, or no USB cable is connected - // we need to delay telemetry by the TELEM_DELAY time - return true; -} - - // try to send a message, return false if it won't fit in the serial tx buffer -bool GCS_MAVLINK::try_send_message(enum ap_message id) +bool GCS_MAVLINK_Sub::try_send_message(enum ap_message id) { - if (sub.telemetry_delayed(chan)) { + if (telemetry_delayed(chan)) { return false; } @@ -613,7 +555,7 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id) case MSG_RADIO_IN: CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW); - send_radio_in(sub.receiver_rssi); + send_radio_in(0); break; case MSG_RADIO_OUT: @@ -658,7 +600,7 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id) break; case MSG_RANGEFINDER: -#if CONFIG_SONAR == ENABLED +#if RANGEFINDER_ENABLED == ENABLED CHECK_PAYLOAD_SIZE(RANGEFINDER); sub.send_rangefinder(chan); #endif @@ -779,9 +721,7 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id) break; case MSG_ADSB_VEHICLE: - CHECK_PAYLOAD_SIZE(ADSB_VEHICLE); - sub.adsb.send_adsb_vehicle(chan); - break; + break; // Do nothing for Sub, here to prevent warning } return true; @@ -869,54 +809,11 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = { // @Increment: 1 // @User: Advanced AP_GROUPINFO("PARAMS", 8, GCS_MAVLINK, streamRates[8], 0), - - // @Param: ADSB - // @DisplayName: ADSB stream rate to ground station - // @Description: ADSB stream rate to ground station - // @Units: Hz - // @Range: 0 50 - // @Increment: 1 - // @User: Advanced - AP_GROUPINFO("ADSB", 9, GCS_MAVLINK, streamRates[9], 5), AP_GROUPEND }; - -// see if we should send a stream now. Called at 50Hz -bool GCS_MAVLINK::stream_trigger(enum streams stream_num) -{ - if (stream_num >= NUM_STREAMS) { - return false; - } - float rate = (uint8_t)streamRates[stream_num].get(); - - // send at a much lower rate while handling waypoints and - // parameter sends - if ((stream_num != STREAM_PARAMS) && - (waypoint_receiving || _queued_parameter != NULL)) { - rate *= 0.25f; - } - - if (rate <= 0) { - return false; - } - - if (stream_ticks[stream_num] == 0) { - // we're triggering now, setup the next trigger point - if (rate > 50) { - rate = 50; - } - stream_ticks[stream_num] = (50 / rate) - 1 + stream_slowdown; - return true; - } - - // count down at 50Hz - stream_ticks[stream_num]--; - return false; -} - void -GCS_MAVLINK::data_stream_send(void) +GCS_MAVLINK_Sub::data_stream_send(void) { if (waypoint_receiving) { // don't interfere with mission transfer @@ -1022,19 +919,15 @@ GCS_MAVLINK::data_stream_send(void) } if (sub.gcs_out_of_time) return; - - if (stream_trigger(STREAM_ADSB)) { - send_message(MSG_ADSB_VEHICLE); - } } -bool GCS_MAVLINK::handle_guided_request(AP_Mission::Mission_Command &cmd) +bool GCS_MAVLINK_Sub::handle_guided_request(AP_Mission::Mission_Command &cmd) { return sub.do_guided(cmd); } -void GCS_MAVLINK::handle_change_alt_request(AP_Mission::Mission_Command &cmd) +void GCS_MAVLINK_Sub::handle_change_alt_request(AP_Mission::Mission_Command &cmd) { // add home alt if needed if (cmd.content.location.flags.relative_alt) { @@ -1044,7 +937,7 @@ void GCS_MAVLINK::handle_change_alt_request(AP_Mission::Mission_Command &cmd) // To-Do: update target altitude for loiter or waypoint controller depending upon nav mode } -void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) +void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg) { uint8_t result = MAV_RESULT_FAILED; // assume failure. Each messages id is responsible for return ACK or NAK if required @@ -1062,7 +955,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAVLINK_MSG_ID_SET_MODE: // MAV ID: 11 { #ifdef DISALLOW_GCS_MODE_CHANGE_DURING_RC_FAILSAFE - if (!sub.failsafe.radio) { + if (!sub.failsafe.manual_control) { handle_set_mode(msg, FUNCTOR_BIND(&sub, &Sub::gcs_set_mode, bool, uint8_t)); } else { // don't allow mode changes while in radio failsafe @@ -1184,6 +1077,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) sub.transform_manual_control_to_rc_override(packet.x,packet.y,packet.z,packet.r,packet.buttons); + sub.failsafe.last_manual_control_ms = AP_HAL::millis(); // a RC override message is considered to be a 'heartbeat' from the ground station for failsafe purposes sub.failsafe.last_heartbeat_ms = AP_HAL::millis(); break; @@ -1233,7 +1127,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // y : lon // z : alt // sanity check location - if (labs(packet.x) >= 900000000l || labs(packet.y) >= 1800000000l) { + if (!check_latlng(packet.x, packet.y)) { break; } Location roi_loc; @@ -1325,7 +1219,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) } } else { // sanity check location - if (fabsf(packet.param5) > 90.0f || fabsf(packet.param6) > 180.0f) { + if (!check_latlng(packet.param5, packet.param6)) { break; } Location new_home_loc; @@ -1355,7 +1249,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // param6 : y / lon // param7 : z / alt // sanity check location - if (fabsf(packet.param5) > 90.0f || fabsf(packet.param6) > 180.0f) { + if (!check_latlng(packet.param5, packet.param6)) { break; } Location roi_loc; @@ -1421,9 +1315,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) result = MAV_RESULT_FAILED; } } else if (is_equal(packet.param3,1.0f)) { - // fast barometer calibration - sub.init_barometer(false); - result = MAV_RESULT_ACCEPTED; + if(!sub.ap.depth_sensor_present || sub.motors.armed() || sub.barometer.get_pressure() > 110000) { + result = MAV_RESULT_FAILED; + } else { + sub.init_barometer(false); + result = MAV_RESULT_ACCEPTED; + } } else if (is_equal(packet.param4,1.0f)) { result = MAV_RESULT_UNSUPPORTED; } else if (is_equal(packet.param5,1.0f)) { @@ -1460,17 +1357,21 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; case MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: - if (is_equal(packet.param1,2.0f)) { - // save first compass's offsets - sub.compass.set_and_save_offsets(0, packet.param2, packet.param3, packet.param4); - result = MAV_RESULT_ACCEPTED; + { + uint8_t compassNumber = -1; + if (is_equal(packet.param1, 2.0f)) { + compassNumber = 0; + } else if (is_equal(packet.param1, 5.0f)) { + compassNumber = 1; + } else if (is_equal(packet.param1, 6.0f)) { + compassNumber = 2; + } + if (compassNumber != (uint8_t) -1) { + sub.compass.set_and_save_offsets(compassNumber, packet.param2, packet.param3, packet.param4); + result = MAV_RESULT_ACCEPTED; + } + break; } - if (is_equal(packet.param1,5.0f)) { - // save secondary compass's offsets - sub.compass.set_and_save_offsets(1, packet.param2, packet.param3, packet.param4); - result = MAV_RESULT_ACCEPTED; - } - break; case MAV_CMD_COMPONENT_ARM_DISARM: if (is_equal(packet.param1,1.0f)) { @@ -1735,7 +1636,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) } else if (pos_ignore && !vel_ignore && acc_ignore) { sub.guided_set_velocity(vel_vector); } else if (!pos_ignore && vel_ignore && acc_ignore) { - sub.guided_set_destination(pos_vector); + if (!sub.guided_set_destination(pos_vector)) { + result = MAV_RESULT_FAILED; + } } else { result = MAV_RESULT_FAILED; } @@ -1776,6 +1679,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) Vector3f pos_ned; if(!pos_ignore) { + // sanity check location + if (!check_latlng(packet.lat_int, packet.lon_int)) { + result = MAV_RESULT_FAILED; + break; + } Location loc; loc.lat = packet.lat_int; loc.lng = packet.lon_int; @@ -1804,7 +1712,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) } else if (pos_ignore && !vel_ignore && acc_ignore) { sub.guided_set_velocity(Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f)); } else if (!pos_ignore && vel_ignore && acc_ignore) { - sub.guided_set_destination(pos_ned); + if (!sub.guided_set_destination(pos_ned)) { + result = MAV_RESULT_FAILED; + } } else { result = MAV_RESULT_FAILED; } @@ -1812,12 +1722,31 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } + case MAVLINK_MSG_ID_DISTANCE_SENSOR: + { + result = MAV_RESULT_ACCEPTED; + sub.rangefinder.handle_msg(msg); + break; + } + + case MAVLINK_MSG_ID_GPS_INPUT: + { + result = MAV_RESULT_ACCEPTED; + sub.gps.handle_msg(msg); + break; + } + #if HIL_MODE != HIL_MODE_DISABLED case MAVLINK_MSG_ID_HIL_STATE: // MAV ID: 90 { mavlink_hil_state_t packet; mavlink_msg_hil_state_decode(msg, &packet); + // sanity check location + if (!check_latlng(packet.lat, packet.lon)) { + break; + } + // set gps hil sensor Location loc; loc.lat = packet.lat; @@ -1828,7 +1757,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) gps.setHIL(0, AP_GPS::GPS_OK_FIX_3D, packet.time_usec/1000, - loc, vel, 10, 0, true); + loc, vel, 10, 0); // rad/sec Vector3f gyros; @@ -1886,14 +1815,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) result = MAV_RESULT_ACCEPTED; break; -#if PRECISION_LANDING == ENABLED - case MAVLINK_MSG_ID_LANDING_TARGET: - // configure or release parachute - result = MAV_RESULT_ACCEPTED; - copter.precland.handle_msg(msg); - break; -#endif - #if AC_FENCE == ENABLED // send or receive fence points with GCS case MAVLINK_MSG_ID_FENCE_POINT: // MAV ID: 160 @@ -1949,6 +1870,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } + // sanity check location + if (!check_latlng(packet.lat, packet.lng)) { + break; + } + RallyLocation rally_point; rally_point.lat = packet.lat; rally_point.lng = packet.lng; @@ -2010,8 +1936,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) sub.set_home_to_current_location_and_lock(); } else { // sanity check location - if (labs(packet.latitude) > 90*10e7 || labs(packet.longitude) > 180 * 10e7) { - break; + if (!check_latlng(packet.latitude, packet.longitude)) { + break; } Location new_home_loc; new_home_loc.lat = packet.latitude; @@ -2025,9 +1951,18 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } - case MAVLINK_MSG_ID_ADSB_VEHICLE: + case MAVLINK_MSG_ID_SETUP_SIGNING: + handle_setup_signing(msg); break; + case MAVLINK_MSG_ID_SYS_STATUS: + uint32_t MAV_SENSOR_WATER = 0x20000000; + mavlink_sys_status_t packet; + mavlink_msg_sys_status_decode(msg, &packet); + if((packet.onboard_control_sensors_enabled & MAV_SENSOR_WATER) && !(packet.onboard_control_sensors_health & MAV_SENSOR_WATER)) + sub.leak_detector.set_detect(); + break; + } // end switch } // end handle mavlink diff --git a/ArduSub/Log.cpp b/ArduSub/Log.cpp index 44054ebe33..48df054c5a 100644 --- a/ArduSub/Log.cpp +++ b/ArduSub/Log.cpp @@ -1,6 +1,7 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- #include "Sub.h" +#include "version.h" #if LOGGING_ENABLED == ENABLED @@ -373,7 +374,7 @@ void Sub::Log_Write_Performance() void Sub::Log_Write_Attitude() { Vector3f targets = attitude_control.get_att_target_euler_cd(); - targets.z = wrap_360_cd_float(targets.z); + targets.z = wrap_360_cd(targets.z); DataFlash.Log_Write_Attitude(ahrs, targets); #if OPTFLOW == ENABLED @@ -559,7 +560,9 @@ void Sub::Log_Write_Error(uint8_t sub_system, uint8_t error_code) void Sub::Log_Write_Baro(void) { - DataFlash.Log_Write_Baro(barometer); + if (!ahrs.have_ekf_logging()) { + DataFlash.Log_Write_Baro(barometer); + } } struct PACKED log_ParameterTuning { diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp index 827d2a0d5d..917a84c1f2 100644 --- a/ArduSub/Parameters.cpp +++ b/ArduSub/Parameters.cpp @@ -276,8 +276,8 @@ const AP_Param::Info Sub::var_info[] = { // @Param: LOG_BITMASK // @DisplayName: Log bitmask // @Description: 4 byte bitmap of log types to enable - // @Values: 830:Default,894:Default+RCIN,958:Default+IMU,1854:Default+Motors,-6146:NearlyAll-AC315,45054:NearlyAll,131070:All+DisarmedLogging,131071:All+FastATT,262142:All+MotBatt,393214:All+FastIMU,397310:All+FastIMU+PID,655358:All+FullIMU,0:Disabled - // @Bitmask: 0:ATTITUDE_FAST,1:ATTITUDE_MED,2:GPS,3:PM,4:CTUN,5:NTUN,6:RCIN,7:IMU,8:CMD,9:CURRENT,10:RCOUT,11:OPTFLOW,12:PID,13:COMPASS,14:INAV,15:CAMERA,16:WHEN_DISARMED,17:MOTBATT,18:IMU_FAST,19:IMU_RAW + // @Values: 830:Default,894:Default+RCIN,958:Default+IMU,1854:Default+Motors,-6146:NearlyAll-AC315,45054:NearlyAll,131071:All+FastATT,262142:All+MotBatt,393214:All+FastIMU,397310:All+FastIMU+PID,655358:All+FullIMU,0:Disabled + // @Bitmask: 0:ATTITUDE_FAST,1:ATTITUDE_MED,2:GPS,3:PM,4:CTUN,5:NTUN,6:RCIN,7:IMU,8:CMD,9:CURRENT,10:RCOUT,11:OPTFLOW,12:PID,13:COMPASS,14:INAV,15:CAMERA,17:MOTBATT,18:IMU_FAST,19:IMU_RAW // @User: Standard GSCALAR(log_bitmask, "LOG_BITMASK", DEFAULT_LOG_BITMASK), diff --git a/ArduSub/Sub.cpp b/ArduSub/Sub.cpp index ed78b816b5..943601fe5e 100644 --- a/ArduSub/Sub.cpp +++ b/ArduSub/Sub.cpp @@ -1,6 +1,5 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#include "Sub.h" /* This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -15,13 +14,16 @@ You should have received a copy of the GNU General Public License along with this program. If not, see . */ -/* - constructor for main Sub class - */ +#include "Sub.h" +#include "version.h" const AP_HAL::HAL& hal = AP_HAL::get_HAL(); +/* + constructor for main Sub class + */ Sub::Sub(void) : + DataFlash{FIRMWARE_STRING}, flight_modes(&g.flight_mode1), mission(ahrs, FUNCTOR_BIND_MEMBER(&Sub::start_command, bool, const AP_Mission::Mission_Command &), @@ -89,9 +91,6 @@ Sub::Sub(void) : #endif #if AP_TERRAIN_AVAILABLE && AC_TERRAIN terrain(ahrs, mission, rally), -#endif -#if PRECISION_LANDING == ENABLED - precland(ahrs, inertial_nav, g.pi_precland, MAIN_LOOP_SECONDS), #endif in_mavlink_delay(false), gcs_out_of_time(false), diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 595973fd0d..d11e2895a7 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -1,10 +1,5 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#pragma once - -#define THISFIRMWARE "ArduSub V3.4-dev" -#define FIRMWARE_VERSION 3,4,0,FIRMWARE_VERSION_TYPE_DEV - /* This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -19,6 +14,7 @@ You should have received a copy of the GNU General Public License along with this program. If not, see . */ +#pragma once /* This is the main Sub class */ @@ -168,7 +164,7 @@ private: RC_Channel *channel_lateral; // Dataflash - DataFlash_Class DataFlash{FIRMWARE_STRING}; + DataFlash_Class DataFlash; AP_GPS gps; @@ -492,8 +488,7 @@ private: void update_trigger(); void update_batt_compass(void); void ten_hz_logging_loop(); - void fifty_hz_logging_loop(); - void full_rate_logging_loop(); + void twentyfive_hz_logging(); void three_hz_loop(); void one_hz_loop(); void update_GPS(void); diff --git a/ArduSub/arming_checks.cpp b/ArduSub/arming_checks.cpp index 848c42e273..d73d8d340e 100644 --- a/ArduSub/arming_checks.cpp +++ b/ArduSub/arming_checks.cpp @@ -286,17 +286,6 @@ bool Sub::pre_arm_checks(bool display_failure) return false; } - // failsafe parameter checks - if (g.failsafe_throttle) { - // check throttle min is above throttle failsafe trigger and that the trigger is above ppm encoder's loss-of-signal value of 900 - if (channel_throttle->radio_min <= g.failsafe_throttle_value+10 || g.failsafe_throttle_value < 910) { - if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check FS_THR_VALUE"); - } - return false; - } - } - // lean angle parameter check if (aparm.angle_max < 1000 || aparm.angle_max > 8000) { if (display_failure) { @@ -329,17 +318,6 @@ bool Sub::pre_arm_checks(bool display_failure) } } - // check throttle is above failsafe throttle - // this is near the bottom to allow other failures to be displayed before checking pilot throttle - if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_RC)) { - if (g.failsafe_throttle != FS_THR_DISABLED && channel_throttle->radio_in < g.failsafe_throttle_value) { - if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Throttle below Failsafe"); - } - return false; - } - } - return true; } @@ -358,27 +336,27 @@ void Sub::pre_arm_rc_checks() } // check if radio has been calibrated - if (!channel_throttle->radio_min.configured() && !channel_throttle->radio_max.configured()) { + if (!channel_throttle->min_max_configured()) { return; } // check channels 1 & 2 have min <= 1300 and max >= 1700 - if (channel_roll->radio_min > 1300 || channel_roll->radio_max < 1700 || channel_pitch->radio_min > 1300 || channel_pitch->radio_max < 1700) { + if (channel_roll->get_radio_min() > 1300 || channel_roll->get_radio_max() < 1700 || channel_pitch->get_radio_min() > 1300 || channel_pitch->get_radio_max() < 1700) { return; } // check channels 3 & 4 have min <= 1300 and max >= 1700 - if (channel_throttle->radio_min > 1300 || channel_throttle->radio_max < 1700 || channel_yaw->radio_min > 1300 || channel_yaw->radio_max < 1700) { + if (channel_throttle->get_radio_min() > 1300 || channel_throttle->get_radio_max() < 1700 || channel_yaw->get_radio_min() > 1300 || channel_yaw->get_radio_max() < 1700) { return; } // check channels 1 & 2 have trim >= 1300 and <= 1700 - if (channel_roll->radio_trim < 1300 || channel_roll->radio_trim > 1700 || channel_pitch->radio_trim < 1300 || channel_pitch->radio_trim > 1700) { + if (channel_roll->get_radio_trim() < 1300 || channel_roll->get_radio_trim() > 1700 || channel_pitch->get_radio_trim() < 1300 || channel_pitch->get_radio_trim() > 1700) { return; } // check channel 4 has trim >= 1300 and <= 1700 - if (channel_yaw->radio_trim < 1300 || channel_yaw->radio_trim > 1700) { + if (channel_yaw->get_radio_trim() < 1300 || channel_yaw->get_radio_trim() > 1700) { return; } @@ -652,24 +630,11 @@ bool Sub::arm_checks(bool display_failure, bool arming_from_gcs) // check throttle if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_RC)) { // check throttle is not too low - must be above failsafe throttle - if (g.failsafe_throttle != FS_THR_DISABLED && channel_throttle->radio_in < g.failsafe_throttle_value) { - if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Throttle below Failsafe"); - } - return false; - } // check throttle is not too high - skips checks if arming from GCS in Guided if (!(arming_from_gcs && control_mode == GUIDED)) { - // above top of deadband is too always high - if (channel_throttle->control_in > get_takeoff_trigger_throttle()) { - if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Throttle too high"); - } - return false; - } // in manual modes throttle must be at zero - if ((mode_has_manual_throttle(control_mode) || control_mode == DRIFT) && channel_throttle->control_in > 0) { + if ((mode_has_manual_throttle(control_mode)) && channel_throttle->get_control_in() > 0) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Throttle too high"); } diff --git a/ArduSub/baro_ground_effect.cpp b/ArduSub/baro_ground_effect.cpp deleted file mode 100644 index 5e07ec2824..0000000000 --- a/ArduSub/baro_ground_effect.cpp +++ /dev/null @@ -1,72 +0,0 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#include "Sub.h" -#if GNDEFFECT_COMPENSATION == ENABLED -void Copter::update_ground_effect_detector(void) -{ - if(!motors.armed()) { - // disarmed - disable ground effect and return - gndeffect_state.takeoff_expected = false; - gndeffect_state.touchdown_expected = false; - ahrs.setTakeoffExpected(gndeffect_state.takeoff_expected); - ahrs.setTouchdownExpected(gndeffect_state.touchdown_expected); - return; - } - - // variable initialization - uint32_t tnow_ms = millis(); - float xy_des_speed_cms = 0.0f; - float xy_speed_cms = 0.0f; - float des_climb_rate_cms = pos_control.get_desired_velocity().z; - - if (pos_control.is_active_xy()) { - Vector3f vel_target = pos_control.get_vel_target(); - vel_target.z = 0.0f; - xy_des_speed_cms = vel_target.length(); - } - - if (position_ok() || optflow_position_ok()) { - Vector3f vel = inertial_nav.get_velocity(); - vel.z = 0.0f; - xy_speed_cms = vel.length(); - } - - // takeoff logic - - // if we are armed and haven't yet taken off - if (motors.armed() && ap.land_complete && !gndeffect_state.takeoff_expected) { - gndeffect_state.takeoff_expected = true; - } - - // if we aren't taking off yet, reset the takeoff timer, altitude and complete flag - bool throttle_up = mode_has_manual_throttle(control_mode) && g.rc_3.control_in > 0; - if (!throttle_up && ap.land_complete) { - gndeffect_state.takeoff_time_ms = tnow_ms; - gndeffect_state.takeoff_alt_cm = inertial_nav.get_altitude(); - } - - // if we are in takeoff_expected and we meet the conditions for having taken off - // end the takeoff_expected state - if (gndeffect_state.takeoff_expected && (tnow_ms-gndeffect_state.takeoff_time_ms > 5000 || inertial_nav.get_altitude()-gndeffect_state.takeoff_alt_cm > 50.0f)) { - gndeffect_state.takeoff_expected = false; - } - - // landing logic - Vector3f angle_target_rad = attitude_control.get_att_target_euler_cd() * radians(0.01f); - bool small_angle_request = cosf(angle_target_rad.x)*cosf(angle_target_rad.y) > cosf(radians(7.5f)); - bool xy_speed_low = (position_ok() || optflow_position_ok()) && xy_speed_cms <= 125.0f; - bool xy_speed_demand_low = pos_control.is_active_xy() && xy_des_speed_cms <= 125.0f; - bool slow_horizontal = xy_speed_demand_low || (xy_speed_low && !pos_control.is_active_xy()) || (control_mode == ALT_HOLD && small_angle_request); - - bool descent_demanded = pos_control.is_active_z() && des_climb_rate_cms < 0.0f; - bool slow_descent_demanded = descent_demanded && des_climb_rate_cms >= -100.0f; - bool z_speed_low = abs(inertial_nav.get_velocity_z()) <= 60.0f; - bool slow_descent = (slow_descent_demanded || (z_speed_low && descent_demanded)); - - gndeffect_state.touchdown_expected = slow_horizontal && slow_descent; - - // Prepare the EKF for ground effect if either takeoff or touchdown is expected. - ahrs.setTakeoffExpected(gndeffect_state.takeoff_expected); - ahrs.setTouchdownExpected(gndeffect_state.touchdown_expected); -} -#endif // GNDEFFECT_COMPENSATION == ENABLED diff --git a/ArduSub/compassmot.cpp b/ArduSub/compassmot.cpp index fca31942ba..58b9333ac7 100644 --- a/ArduSub/compassmot.cpp +++ b/ArduSub/compassmot.cpp @@ -63,7 +63,7 @@ uint8_t Sub::mavlink_compassmot(mavlink_channel_t chan) // check throttle is at zero read_radio(); - if (channel_throttle->control_in != 0) { + if (channel_throttle->get_control_in() != 0) { gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL, "Throttle not zero"); ap.compass_mot = false; return 1; @@ -145,7 +145,7 @@ uint8_t Sub::mavlink_compassmot(mavlink_channel_t chan) read_radio(); // pass through throttle to motors - motors.throttle_pass_through(channel_throttle->radio_in); + motors.set_throttle_passthrough_for_esc_calibration(channel_throttle->get_control_in() / 1000.0f); // read some compass values compass.read(); @@ -154,7 +154,7 @@ uint8_t Sub::mavlink_compassmot(mavlink_channel_t chan) read_battery(); // calculate scaling for throttle - throttle_pct = (float)channel_throttle->control_in / 1000.0f; + throttle_pct = (float)channel_throttle->get_control_in() / 1000.0f; throttle_pct = constrain_float(throttle_pct,0.0f,1.0f); // if throttle is near zero, update base x,y,z values @@ -217,7 +217,7 @@ uint8_t Sub::mavlink_compassmot(mavlink_channel_t chan) if (AP_HAL::millis() - last_send_time > 500) { last_send_time = AP_HAL::millis(); mavlink_msg_compassmot_status_send(chan, - channel_throttle->control_in, + channel_throttle->get_control_in(), battery.current_amps(), interference_pct[compass.get_primary()], motor_compensation[compass.get_primary()].x, diff --git a/ArduSub/config.h b/ArduSub/config.h index bb1e11497a..56b874e8f3 100644 --- a/ArduSub/config.h +++ b/ArduSub/config.h @@ -145,6 +145,21 @@ # define RANGEFINDER_TILT_CORRECTION ENABLED #endif +// Avoidance (relies on Proximity and Fence) +#ifndef AVOIDANCE_ENABLED +# define AVOIDANCE_ENABLED DISABLED +#endif + +#if AVOIDANCE_ENABLED == ENABLED // Avoidance Library relies on Proximity and Fence +# define PROXIMITY_ENABLED ENABLED +# define FENCE_ENABLED ENABLED +#endif + +// Proximity sensor +// +#ifndef PROXIMITY_ENABLED + # define PROXIMITY_ENABLED DISABLED +#endif #ifndef MAV_SYSTEM_ID # define MAV_SYSTEM_ID 1 @@ -265,21 +280,9 @@ #endif ////////////////////////////////////////////////////////////////////////////// -// Crop Sprayer -#ifndef SPRAYER - # define SPRAYER DISABLED -#endif - -////////////////////////////////////////////////////////////////////////////// -// Precision Landing with companion computer or IRLock sensor -#ifndef PRECISION_LANDING - # define PRECISION_LANDING ENABLED -#endif - -////////////////////////////////////////////////////////////////////////////// -// EPM cargo gripper -#ifndef EPM_ENABLED - # define EPM_ENABLED ENABLED +// gripper +#ifndef GRIPPER_ENABLED + # define GRIPPER_ENABLED DISABLED #endif ////////////////////////////////////////////////////////////////////////////// @@ -619,17 +622,17 @@ # define CLI_ENABLED ENABLED #endif -//use this to completely disable FRSKY TELEM -#ifndef FRSKY_TELEM_ENABLED - # define FRSKY_TELEM_ENABLED ENABLED -#endif - -/* - build a firmware version string. - GIT_VERSION comes from Makefile builds -*/ -#ifndef GIT_VERSION -#define FIRMWARE_STRING THISFIRMWARE -#else -#define FIRMWARE_STRING THISFIRMWARE " (" GIT_VERSION ")" +#ifndef XTRACK_P +#define XTRACK_P 1.0f +#define XTRACK_I 0.5f +#define XTRACK_D 0.0f +#define XTRACK_IMAX 1000 +#define XTRACK_FILT_HZ 5.0f +#define XTRACK_DT 0.05f +#define HEAD_P 1.0f +#define HEAD_I 0.5f +#define HEAD_D 0.0f +#define HEAD_IMAX 1000 +#define HEAD_FILT_HZ 5.0f +#define HEAD_DT 0.05f #endif diff --git a/ArduSub/control_acro.cpp b/ArduSub/control_acro.cpp index 2fd3d17407..d0ffc86fa1 100644 --- a/ArduSub/control_acro.cpp +++ b/ArduSub/control_acro.cpp @@ -9,10 +9,6 @@ // acro_init - initialise acro controller bool Sub::acro_init(bool ignore_checks) { - // if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high - if (motors.armed() && ap.land_complete && !mode_has_manual_throttle(control_mode) && (get_pilot_desired_throttle(channel_throttle->control_in) > get_non_takeoff_throttle())) { - return false; - } // set target altitude to zero for reporting pos_control.set_alt_target(0); @@ -35,21 +31,18 @@ void Sub::acro_run() motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // convert the input to the desired body frame rate - get_pilot_desired_angle_rates(channel_roll->control_in, channel_pitch->control_in, channel_yaw->control_in, target_roll, target_pitch, target_yaw); - - // get pilot's desired throttle - pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->control_in); + get_pilot_desired_angle_rates(channel_roll->get_control_in(), channel_pitch->get_control_in(), channel_yaw->get_control_in(), target_roll, target_pitch, target_yaw); // run attitude controller attitude_control.input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw); // output pilot's throttle without angle boost - attitude_control.set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt); + attitude_control.set_throttle_out(channel_throttle->norm_input(), false, g.throttle_filt); //control_in is range 0-1000 //radio_in is raw pwm value - motors.set_forward(channel_forward->norm_input_dz()); - motors.set_lateral(channel_lateral->norm_input_dz()); + motors.set_forward(channel_forward->norm_input()); + motors.set_lateral(channel_lateral->norm_input()); } @@ -61,7 +54,7 @@ void Sub::get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16 Vector3f rate_ef_level, rate_bf_level, rate_bf_request; // apply circular limit to pitch and roll inputs - float total_in = pythagorous2((float)pitch_in, (float)roll_in); + float total_in = norm(pitch_in, roll_in); if (total_in > ROLL_PITCH_INPUT_MAX) { float ratio = (float)ROLL_PITCH_INPUT_MAX / total_in; diff --git a/ArduSub/control_althold.cpp b/ArduSub/control_althold.cpp index 60498119c9..9d015e009b 100644 --- a/ArduSub/control_althold.cpp +++ b/ArduSub/control_althold.cpp @@ -23,8 +23,7 @@ bool Sub::althold_init(bool ignore_checks) pos_control.set_alt_target(inertial_nav.get_altitude()); pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z()); - // stop takeoff if running - takeoff_stop(); + last_pilot_heading = ahrs.yaw_sensor; return true; } @@ -33,133 +32,77 @@ bool Sub::althold_init(bool ignore_checks) // should be called at 100hz or more void Sub::althold_run() { - AltHoldModeState althold_state; - float takeoff_climb_rate = 0.0f; + uint32_t tnow = AP_HAL::millis(); // 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); + if(!motors.armed() || !motors.get_interlock()) { + motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); + // Multicopters do not stabilize roll/pitch/yaw when not auto-armed (i.e. on the ground, pilot has never raised throttle) + attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); + pos_control.relax_alt_hold_controllers(motors.get_throttle_hover()); + last_pilot_heading = ahrs.yaw_sensor; + return; + } + + motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + // apply SIMPLE mode transform to pilot inputs update_simple_mode(); // get pilot desired lean angles float target_roll, target_pitch; - get_pilot_desired_lean_angles(channel_roll->control_in, channel_pitch->control_in, target_roll, target_pitch, attitude_control.get_althold_lean_angle_max()); + get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control.get_althold_lean_angle_max()); // get pilot's desired yaw rate - float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); + 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->control_in); + 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); - //bool takeoff_triggered = (ap.land_complete && (channel_throttle->control_in > get_takeoff_trigger_throttle()) && motors.spool_up_complete()); + // call attitude controller + if (!is_zero(target_yaw_rate)) { // call attitude controller with rate yaw determined by pilot input + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); + last_pilot_heading = ahrs.yaw_sensor; + last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading -// // Alt Hold State Machine Determination - if(!ap.auto_armed) { - althold_state = AltHold_Disarmed; -// if (!motors.armed() || !motors.get_interlock()) { - // althold_state = AltHold_MotorStop; - // } else if (!ap.auto_armed){ - // althold_state = AltHold_Disarmed; -// } else if (takeoff_state.running || takeoff_triggered){ -// althold_state = AltHold_Takeoff; -// } else if (ap.land_complete){ -// althold_state = AltHold_Landed; - } else { - althold_state = AltHold_Flying; - } + } else { // hold current heading - // Alt Hold State Machine - switch (althold_state) { + // this check is required to prevent bounce back after very fast yaw maneuvers + // the inertia of the vehicle causes the heading to move slightly past the point when pilot input actually stopped + if(tnow < last_pilot_yaw_input_ms + 250) { // give 250ms to slow down, then set target heading + target_yaw_rate = 0; // Stop rotation on yaw axis - case AltHold_Disarmed: - motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); - // Multicopter do not stabilize roll/pitch/yaw when disarmed - attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); + // call attitude controller with target yaw rate = 0 to decelerate on yaw axis + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); + last_pilot_heading = ahrs.yaw_sensor; // update heading to hold - pos_control.relax_alt_hold_controllers(0); - break; + } else { // call attitude controller holding absolute absolute bearing + attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, last_pilot_heading, true, get_smoothing_gain()); + } + } - case AltHold_MotorStop: - // Multicopter do not stabilize roll/pitch/yaw when motor are stopped - motors.set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN); - attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); - pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average); - break; + // 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); + } - case AltHold_Takeoff: + // call z axis position controller + if(ap.at_bottom) { + pos_control.relax_alt_hold_controllers(motors.get_throttle_hover()); // 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 { + pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); + } - // initiate take-off - if (!takeoff_state.running) { - takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f)); - // indicate we are taking off - set_land_complete(false); - // clear i terms - set_throttle_takeoff(); - } - - // get take-off adjusted pilot and takeoff climb rates - takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate); - - // set motors to full range - motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); - - // call attitude controller - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); - - // call position controller - pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); - pos_control.add_takeoff_climb_rate(takeoff_climb_rate, G_Dt); - pos_control.update_z_controller(); - break; - - case AltHold_Landed: - // Multicopter do not stabilize roll/pitch/yaw when disarmed - attitude_control.set_throttle_out(get_throttle_pre_takeoff(channel_throttle->control_in),false,g.throttle_filt); - // if throttle zero reset attitude and exit immediately - if (ap.throttle_zero) { - motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); - } else { - motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); - } - - pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average); - break; - - case AltHold_Flying: - motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); - // call attitude controller - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(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 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(ap.at_surface) { - if(target_climb_rate < 0.0) { // Dive if the pilot wants to - pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); - } else if(pos_control.get_vel_target_z() > 0.0) { - pos_control.relax_alt_hold_controllers(0.0); // clear velocity and position targets, and integrator - pos_control.set_alt_target(g.surface_depth); // set alt target to the same depth that triggers the surface detector. - } - } else { - pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); - } - - pos_control.update_z_controller(); - break; - } + pos_control.update_z_controller(); //control_in is range 0-1000 //radio_in is raw pwm value - motors.set_forward(channel_forward->norm_input_dz()); - motors.set_lateral(channel_lateral->norm_input_dz()); + motors.set_forward(channel_forward->norm_input()); + motors.set_lateral(channel_lateral->norm_input()); } diff --git a/ArduSub/control_auto.cpp b/ArduSub/control_auto.cpp index df77f4ef1d..0e41ed25d9 100644 --- a/ArduSub/control_auto.cpp +++ b/ArduSub/control_auto.cpp @@ -53,10 +53,6 @@ void Sub::auto_run() // call the correct auto controller switch (auto_mode) { - case Auto_TakeOff: - auto_takeoff_run(); - break; - case Auto_WP: case Auto_CircleMoveToEdge: auto_wp_run(); @@ -90,87 +86,6 @@ void Sub::auto_run() } } -// auto_takeoff_start - initialises waypoint controller to implement take-off -void Sub::auto_takeoff_start(const Location& dest_loc) -{ - auto_mode = Auto_TakeOff; - - // convert location to class - Location_Class dest(dest_loc); - - // set horizontal target - dest.lat = current_loc.lat; - dest.lng = current_loc.lng; - - // get altitude target - int32_t alt_target; - if (!dest.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_HOME, alt_target)) { - // this failure could only happen if take-off alt was specified as an alt-above terrain and we have no terrain data - Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA); - // fall back to altitude above current altitude - alt_target = current_loc.alt + dest.alt; - } - - // sanity check target - if (alt_target < current_loc.alt) { - dest.set_alt_cm(current_loc.alt, Location_Class::ALT_FRAME_ABOVE_HOME); - } - // Note: if taking off from below home this could cause a climb to an unexpectedly high altitude - if (alt_target < 100) { - dest.set_alt_cm(100, Location_Class::ALT_FRAME_ABOVE_HOME); - } - - // set waypoint controller target - if (!wp_nav.set_wp_destination(dest)) { - // failure to set destination can only be because of missing terrain data - failsafe_terrain_on_event(); - return; - } - - // initialise yaw - set_auto_yaw_mode(AUTO_YAW_HOLD); - - // clear i term when we're taking off - set_throttle_takeoff(); -} - -// auto_takeoff_run - takeoff in auto mode -// called by auto_run at 100hz or more -void Sub::auto_takeoff_run() -{ - // 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()) { - // initialise wpnav targets - wp_nav.shift_wp_origin_to_current_pos(); - // multicopters do not stabilize roll/pitch/yaw when disarmed - motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); - // reset attitude control targets - attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); - // clear i term when we're taking off - set_throttle_takeoff(); - return; - } - - // process pilot's yaw input - float target_yaw_rate = 0; - if (!failsafe.radio) { - // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); - } - - // set motors to full range - motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); - - // run waypoint controller - failsafe_terrain_set_status(wp_nav.update_wpnav()); - - // call z-axis position controller (wpnav should have already updated it's alt target) - pos_control.update_z_controller(); - - // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain()); -} - // auto_wp_start - initialises waypoint controller to implement flying to a particular destination void Sub::auto_wp_start(const Vector3f& destination) { @@ -218,16 +133,14 @@ void Sub::auto_wp_run() motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); - // clear i term when we're taking off - set_throttle_takeoff(); return; } // process pilot's yaw input float target_yaw_rate = 0; - if (!failsafe.radio) { + if (!failsafe.manual_control) { // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); + target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); if (!is_zero(target_yaw_rate)) { set_auto_yaw_mode(AUTO_YAW_HOLD); } @@ -306,16 +219,14 @@ void Sub::auto_spline_run() attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); - // clear i term when we're taking off - set_throttle_takeoff(); return; } // process pilot's yaw input float target_yaw_rate = 0; - if (!failsafe.radio) { + if (!failsafe.manual_control) { // get pilot's desired yaw rat - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); + target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); if (!is_zero(target_yaw_rate)) { set_auto_yaw_mode(AUTO_YAW_HOLD); } @@ -359,8 +270,9 @@ void Sub::auto_land_start(const Vector3f& destination) // initialise loiter target destination wp_nav.init_loiter_target(destination); - // initialise altitude target to stopping point - pos_control.set_target_to_stopping_point_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()); // initialise yaw set_auto_yaw_mode(AUTO_YAW_HOLD); @@ -370,11 +282,8 @@ void Sub::auto_land_start(const Vector3f& destination) // called by auto_run at 100hz or more void Sub::auto_land_run() { - int16_t roll_control = 0, pitch_control = 0; - float target_yaw_rate = 0; - - // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately - if (!motors.armed() || !ap.auto_armed || ap.land_complete) { + // if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately + if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) { motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); // multicopters do not stabilize roll/pitch/yaw when disarmed attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); @@ -384,53 +293,12 @@ void Sub::auto_land_run() return; } - // relax loiter targets if we might be landed - if (ap.land_complete_maybe) { - wp_nav.loiter_soften_for_landing(); - } - - // process pilot's input - if (!failsafe.radio) { - if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){ - Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT); - // exit land if throttle is high - if (!set_mode(LOITER, MODE_REASON_THROTTLE_LAND_ESCAPE)) { - set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE); - } - } - - if (g.land_repositioning) { - // apply SIMPLE mode transform to pilot inputs - update_simple_mode(); - - // process pilot's roll and pitch input - roll_control = channel_roll->control_in; - pitch_control = channel_pitch->control_in; - } - - // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); - } - // set motors to full range motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); - // process roll, pitch inputs - wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control); - - // run loiter controller - wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); - - // call z-axis position controller - float cmb_rate = get_land_descent_speed(); - pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt, true); - pos_control.update_z_controller(); - - // record desired climb rate for logging - desired_climb_rate = cmb_rate; - - // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain()); +// land mode replaced by surface mode, does not have this functionality +// land_run_horizontal_control(); +// land_run_vertical_control(); } // auto_circle_movetoedge_start - initialise waypoint controller to move to edge of a circle with it's center at the specified location @@ -476,7 +344,7 @@ void Sub::auto_circle_movetoedge_start(const Location_Class &circle_center, floa // if we are outside the circle, point at the edge, otherwise hold yaw const Vector3f &curr_pos = inertial_nav.get_position(); - float dist_to_center = pythagorous2(circle_center_neu.x - curr_pos.x, circle_center_neu.y - curr_pos.y); + float dist_to_center = norm(circle_center_neu.x - curr_pos.x, circle_center_neu.y - curr_pos.y); if (dist_to_center > circle_nav.get_radius() && dist_to_center > 500) { set_auto_yaw_mode(get_default_auto_yaw_mode(false)); } else { @@ -575,8 +443,8 @@ void Sub::auto_loiter_run() // accept pilot input of yaw float target_yaw_rate = 0; - if(!failsafe.radio) { - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); + if(!failsafe.manual_control) { + target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); } // set motors to full range diff --git a/ArduSub/control_autotune.cpp b/ArduSub/control_autotune.cpp index bb5414638b..c44ff6d121 100644 --- a/ArduSub/control_autotune.cpp +++ b/ArduSub/control_autotune.cpp @@ -217,7 +217,7 @@ void Copter::autotune_stop() autotune_load_orig_gains(); // re-enable angle-to-rate request limits - attitude_control.limit_angle_to_rate_request(true); + attitude_control.use_ff_and_input_shaping(true); // log off event and send message to ground station autotune_update_gcs(AUTOTUNE_MESSAGE_STOPPED); @@ -241,7 +241,7 @@ bool Copter::autotune_start(bool ignore_checks) } // ensure we are flying - if (!motors.armed() || !ap.auto_armed || ap.land_complete) { + if (!motors.armed() || !ap.auto_armed) { return false; } @@ -273,7 +273,7 @@ void Copter::autotune_run() if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) { motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); - pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average); + pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-motors.get_throttle_hover()); return; } @@ -281,40 +281,40 @@ void Copter::autotune_run() update_simple_mode(); // get pilot desired lean angles - get_pilot_desired_lean_angles(channel_roll->control_in, channel_pitch->control_in, target_roll, target_pitch, aparm.angle_max); + get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max); // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); + target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); // get pilot desired climb rate - target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->control_in); + target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in()); // check for pilot requested take-off - this should not actually be possible because of autotune_init() checks - if (ap.land_complete && target_climb_rate > 0) { + if (target_climb_rate > 0) { // indicate we are taking off - set_land_complete(false); +// set_land_complete(false); // clear i term when we're taking off set_throttle_takeoff(); } - // reset target lean angles and heading while landed - if (ap.land_complete) { - if (ap.throttle_zero) { - motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); - } else { - motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); - } - // move throttle to between minimum and non-takeoff-throttle to keep us on the ground - attitude_control.set_throttle_out(get_throttle_pre_takeoff(channel_throttle->control_in),false,g.throttle_filt); - pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average); - }else{ +// // reset target lean angles and heading while landed +// if (ap.land_complete) { +// if (ap.throttle_zero) { +// motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); +// } else { +// motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); +// } +// // move throttle to between minimum and non-takeoff-throttle to keep us on the ground +// attitude_control.set_throttle_out(get_throttle_pre_takeoff(channel_throttle->get_control_in()),false,g.throttle_filt); +// pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-motors.get_throttle_hover()); +// }else{ // check if pilot is overriding the controls if (!is_zero(target_roll) || !is_zero(target_pitch) || !is_zero(target_yaw_rate) || target_climb_rate != 0) { if (!autotune_state.pilot_override) { autotune_state.pilot_override = true; // set gains to their original values autotune_load_orig_gains(); - attitude_control.limit_angle_to_rate_request(true); + attitude_control.use_ff_and_input_shaping(true); } // reset pilot override time autotune_override_time = millis(); @@ -334,7 +334,7 @@ void Copter::autotune_run() // if pilot override call attitude controller if (autotune_state.pilot_override || autotune_state.mode != AUTOTUNE_MODE_TUNING) { - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); }else{ // somehow get attitude requests from autotuning autotune_attitude_control(); @@ -343,7 +343,7 @@ void Copter::autotune_run() // call position controller pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); pos_control.update_z_controller(); - } +// } } // autotune_attitude_controller - sets attitude control targets during tuning @@ -359,10 +359,10 @@ void Copter::autotune_attitude_control() case AUTOTUNE_STEP_WAITING_FOR_LEVEL: // Note: we should be using intra-test gains (which are very close to the original gains but have lower I) // re-enable rate limits - attitude_control.limit_angle_to_rate_request(true); + attitude_control.use_ff_and_input_shaping(true); // hold level attitude - attitude_control.input_euler_angle_roll_pitch_yaw( 0.0f, 0.0f, autotune_desired_yaw, true); + attitude_control.input_euler_angle_roll_pitch_yaw( 0.0f, 0.0f, autotune_desired_yaw, true, get_smoothing_gain()); // hold the copter level for 0.5 seconds before we begin a twitch // reset counter if we are no longer level @@ -434,29 +434,29 @@ void Copter::autotune_attitude_control() // Note: we should be using intra-test gains (which are very close to the original gains but have lower I) // disable rate limits - attitude_control.limit_angle_to_rate_request(false); + attitude_control.use_ff_and_input_shaping(false); if ((autotune_state.tune_type == AUTOTUNE_TYPE_SP_DOWN) || (autotune_state.tune_type == AUTOTUNE_TYPE_SP_UP)) { // Testing increasing stabilize P gain so will set lean angle target switch (autotune_state.axis) { case AUTOTUNE_AXIS_ROLL: // request roll to 20deg - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw( direction_sign * autotune_target_angle + autotune_start_angle, 0.0f, 0.0f); + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw( direction_sign * autotune_target_angle + autotune_start_angle, 0.0f, 0.0f, get_smoothing_gain()); break; case AUTOTUNE_AXIS_PITCH: // request pitch to 20deg - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw( 0.0f, direction_sign * autotune_target_angle + autotune_start_angle, 0.0f); + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw( 0.0f, direction_sign * autotune_target_angle + autotune_start_angle, 0.0f, get_smoothing_gain()); break; case AUTOTUNE_AXIS_YAW: // request pitch to 20deg - attitude_control.input_euler_angle_roll_pitch_yaw( 0.0f, 0.0f, wrap_180_cd_float(direction_sign * autotune_target_angle + autotune_start_angle), false); + attitude_control.input_euler_angle_roll_pitch_yaw( 0.0f, 0.0f, wrap_180_cd(direction_sign * autotune_target_angle + autotune_start_angle), false, get_smoothing_gain()); break; } } else { // Testing rate P and D gains so will set body-frame rate targets. // Rate controller will use existing body-frame rates and convert to motor outputs // for all axes except the one we override here. - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw( 0.0f, 0.0f, 0.0f); + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw( 0.0f, 0.0f, 0.0f, get_smoothing_gain()); switch (autotune_state.axis) { case AUTOTUNE_AXIS_ROLL: // override body-frame roll rate @@ -533,7 +533,7 @@ void Copter::autotune_attitude_control() case AUTOTUNE_STEP_UPDATE_GAINS: // re-enable rate limits - attitude_control.limit_angle_to_rate_request(true); + attitude_control.use_ff_and_input_shaping(true); // log the latest gains if ((autotune_state.tune_type == AUTOTUNE_TYPE_SP_DOWN) || (autotune_state.tune_type == AUTOTUNE_TYPE_SP_UP)) { @@ -732,7 +732,7 @@ void Copter::autotune_attitude_control() autotune_state.positive_direction = !autotune_state.positive_direction; if (autotune_state.axis == AUTOTUNE_AXIS_YAW) { - attitude_control.input_euler_angle_roll_pitch_yaw( 0.0f, 0.0f, ahrs.yaw_sensor, false); + attitude_control.input_euler_angle_roll_pitch_yaw( 0.0f, 0.0f, ahrs.yaw_sensor, false, get_smoothing_gain()); } // set gains to their intra-test values (which are very close to the original gains) @@ -1319,4 +1319,4 @@ void Copter::autotune_twitching_measure_acceleration(float &rate_of_change, floa } } -#endif // AUTOTUNE_ENABLED == ENABLED \ No newline at end of file +#endif // AUTOTUNE_ENABLED == ENABLED diff --git a/ArduSub/control_circle.cpp b/ArduSub/control_circle.cpp index a9cc42a768..e4f6fc0572 100644 --- a/ArduSub/control_circle.cpp +++ b/ArduSub/control_circle.cpp @@ -57,13 +57,13 @@ void Sub::circle_run() // process pilot inputs if (!failsafe.manual_control) { // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); + target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); if (!is_zero(target_yaw_rate)) { circle_pilot_yaw_override = true; } // get pilot desired climb rate - target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->control_in); + target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in()); // // check for pilot requested take-off // if (ap.land_complete && target_climb_rate > 0) { diff --git a/ArduSub/control_drift.cpp b/ArduSub/control_drift.cpp deleted file mode 100644 index 837eb9fd1f..0000000000 --- a/ArduSub/control_drift.cpp +++ /dev/null @@ -1,121 +0,0 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#include "Sub.h" - -/* - * control_drift.pde - init and run calls for drift flight mode - */ - -#ifndef DRIFT_SPEEDGAIN - # define DRIFT_SPEEDGAIN 8.0f -#endif -#ifndef DRIFT_SPEEDLIMIT - # define DRIFT_SPEEDLIMIT 560.0f -#endif - -#ifndef DRIFT_THR_ASSIST_GAIN - # define DRIFT_THR_ASSIST_GAIN 0.0018f // gain controlling amount of throttle assistance -#endif - -#ifndef DRIFT_THR_ASSIST_MAX - # define DRIFT_THR_ASSIST_MAX 0.3f // maximum assistance throttle assist will provide -#endif - -#ifndef DRIFT_THR_MIN - # define DRIFT_THR_MIN 0.213f // throttle assist will be active when pilot's throttle is above this value -#endif -#ifndef DRIFT_THR_MAX - # define DRIFT_THR_MAX 0.787f // throttle assist will be active when pilot's throttle is below this value -#endif - -// drift_init - initialise drift controller -bool Sub::drift_init(bool ignore_checks) -{ - if (position_ok() || ignore_checks) { - return true; - }else{ - return false; - } -} - -// drift_run - runs the drift controller -// should be called at 100hz or more -void Sub::drift_run() -{ - static float breaker = 0.0f; - static float roll_input = 0.0f; - float target_roll, target_pitch; - float target_yaw_rate; - float pilot_throttle_scaled; - - // if landed and throttle at zero, set throttle to zero and exit immediately - if (!motors.armed() || !motors.get_interlock() || (ap.land_complete && ap.throttle_zero)) { - motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); - attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); - return; - } - - // convert pilot input to lean angles - get_pilot_desired_lean_angles(channel_roll->control_in, channel_pitch->control_in, target_roll, target_pitch, aparm.angle_max); - - // get pilot's desired throttle - pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->control_in); - - // Grab inertial velocity - const Vector3f& vel = inertial_nav.get_velocity(); - - // rotate roll, pitch input from north facing to vehicle's perspective - float roll_vel = vel.y * ahrs.cos_yaw() - vel.x * ahrs.sin_yaw(); // body roll vel - float pitch_vel = vel.y * ahrs.sin_yaw() + vel.x * ahrs.cos_yaw(); // body pitch vel - - // gain sceduling for Yaw - float pitch_vel2 = MIN(fabsf(pitch_vel), 2000); - target_yaw_rate = ((float)target_roll/1.0f) * (1.0f - (pitch_vel2 / 5000.0f)) * g.acro_yaw_p; - - roll_vel = constrain_float(roll_vel, -DRIFT_SPEEDLIMIT, DRIFT_SPEEDLIMIT); - pitch_vel = constrain_float(pitch_vel, -DRIFT_SPEEDLIMIT, DRIFT_SPEEDLIMIT); - - roll_input = roll_input * .96f + (float)channel_yaw->control_in * .04f; - - //convert user input into desired roll velocity - float roll_vel_error = roll_vel - (roll_input / DRIFT_SPEEDGAIN); - - // Roll velocity is feed into roll acceleration to minimize slip - target_roll = roll_vel_error * -DRIFT_SPEEDGAIN; - target_roll = constrain_int16(target_roll, -4500, 4500); - - // If we let go of sticks, bring us to a stop - if(is_zero(target_pitch)){ - // .14/ (.03 * 100) = 4.6 seconds till full breaking - breaker += .03f; - breaker = MIN(breaker, DRIFT_SPEEDGAIN); - target_pitch = pitch_vel * breaker; - }else{ - breaker = 0.0f; - } - - // call attitude controller - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); - - // output pilot's throttle with angle boost - attitude_control.set_throttle_out(get_throttle_assist(vel.z, pilot_throttle_scaled), true, g.throttle_filt); -} - -// get_throttle_assist - return throttle output (range 0 ~ 1) based on pilot input and z-axis velocity -float Sub::get_throttle_assist(float velz, float pilot_throttle_scaled) -{ - // throttle assist - adjusts throttle to slow the vehicle's vertical velocity - // Only active when pilot's throttle is between 213 ~ 787 - // Assistance is strongest when throttle is at mid, drops linearly to no assistance at 213 and 787 - float thr_assist = 0.0f; - if (pilot_throttle_scaled > DRIFT_THR_MIN && pilot_throttle_scaled < DRIFT_THR_MAX) { - // calculate throttle assist gain - thr_assist = 1.2f - ((float)fabsf(pilot_throttle_scaled - 0.5f) / 0.24f); - thr_assist = constrain_float(thr_assist, 0.0f, 1.0f) * -DRIFT_THR_ASSIST_GAIN * velz; - - // ensure throttle assist never adjusts the throttle by more than 300 pwm - thr_assist = constrain_float(thr_assist, -DRIFT_THR_ASSIST_MAX, DRIFT_THR_ASSIST_MAX); - } - - return constrain_float(pilot_throttle_scaled + thr_assist, 0.0f, 1.0f); -} diff --git a/ArduSub/control_flip.cpp b/ArduSub/control_flip.cpp deleted file mode 100644 index 044ae6cc4b..0000000000 --- a/ArduSub/control_flip.cpp +++ /dev/null @@ -1,218 +0,0 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#include "Sub.h" - -/* - * control_flip.pde - init and run calls for flip flight mode - * original implementation in 2010 by Jose Julio - * Adapted and updated for AC2 in 2011 by Jason Short - * - * Controls: - * CH7_OPT - CH12_OPT parameter must be set to "Flip" (AUXSW_FLIP) which is "2" - * Pilot switches to Stabilize, Acro or AltHold flight mode and puts ch7/ch8 switch to ON position - * Vehicle will Roll right by default but if roll or pitch stick is held slightly left, forward or back it will flip in that direction - * Vehicle should complete the roll within 2.5sec and will then return to the original flight mode it was in before flip was triggered - * Pilot may manually exit flip by switching off ch7/ch8 or by moving roll stick to >40deg left or right - * - * State machine approach: - * Flip_Start (while copter is leaning <45deg) : roll right at 400deg/sec, increase throttle - * Flip_Roll (while copter is between +45deg ~ -90) : roll right at 400deg/sec, reduce throttle - * Flip_Recover (while copter is between -90deg and original target angle) : use earth frame angle controller to return vehicle to original attitude - */ - -#define FLIP_THR_INC 0.20f // throttle increase during Flip_Start stage (under 45deg lean angle) -#define FLIP_THR_DEC 0.24f // throttle decrease during Flip_Roll stage (between 45deg ~ -90deg roll) -#define FLIP_ROTATION_RATE 40000 // rotation rate request in centi-degrees / sec (i.e. 400 deg/sec) -#define FLIP_TIMEOUT_MS 2500 // timeout after 2.5sec. Vehicle will switch back to original flight mode -#define FLIP_RECOVERY_ANGLE 500 // consider successful recovery when roll is back within 5 degrees of original - -#define FLIP_ROLL_RIGHT 1 // used to set flip_dir -#define FLIP_ROLL_LEFT -1 // used to set flip_dir - -#define FLIP_PITCH_BACK 1 // used to set flip_dir -#define FLIP_PITCH_FORWARD -1 // used to set flip_dir - -FlipState flip_state; // current state of flip -control_mode_t flip_orig_control_mode; // flight mode when flip was initated -uint32_t flip_start_time; // time since flip began -int8_t flip_roll_dir; // roll direction (-1 = roll left, 1 = roll right) -int8_t flip_pitch_dir; // pitch direction (-1 = pitch forward, 1 = pitch back) - -// flip_init - initialise flip controller -bool Sub::flip_init(bool ignore_checks) -{ - // only allow flip from ACRO, Stabilize, AltHold or Drift flight modes - if (control_mode != ACRO && control_mode != STABILIZE && control_mode != ALT_HOLD) { - return false; - } - - // if in acro or stabilize ensure throttle is above zero - if (ap.throttle_zero && (control_mode == ACRO || control_mode == STABILIZE)) { - return false; - } - - // ensure roll input is less than 40deg - if (abs(channel_roll->control_in) >= 4000) { - return false; - } - - // only allow flip when flying - if (!motors.armed() || ap.land_complete) { - return false; - } - - // capture original flight mode so that we can return to it after completion - flip_orig_control_mode = control_mode; - - // initialise state - flip_state = Flip_Start; - flip_start_time = millis(); - - flip_roll_dir = flip_pitch_dir = 0; - - // choose direction based on pilot's roll and pitch sticks - if (channel_pitch->control_in > 300) { - flip_pitch_dir = FLIP_PITCH_BACK; - }else if(channel_pitch->control_in < -300) { - flip_pitch_dir = FLIP_PITCH_FORWARD; - }else if (channel_roll->control_in >= 0) { - flip_roll_dir = FLIP_ROLL_RIGHT; - }else{ - flip_roll_dir = FLIP_ROLL_LEFT; - } - - // log start of flip - Log_Write_Event(DATA_FLIP_START); - - // capture current attitude which will be used during the Flip_Recovery stage - flip_orig_attitude.x = constrain_float(ahrs.roll_sensor, -aparm.angle_max, aparm.angle_max); - flip_orig_attitude.y = constrain_float(ahrs.pitch_sensor, -aparm.angle_max, aparm.angle_max); - flip_orig_attitude.z = ahrs.yaw_sensor; - - return true; -} - -// flip_run - runs the flip controller -// should be called at 100hz or more -void Sub::flip_run() -{ - float throttle_out; - float recovery_angle; - - // if pilot inputs roll > 40deg or timeout occurs abandon flip - if (!motors.armed() || (abs(channel_roll->control_in) >= 4000) || (abs(channel_pitch->control_in) >= 4000) || ((millis() - flip_start_time) > FLIP_TIMEOUT_MS)) { - flip_state = Flip_Abandon; - } - - // get pilot's desired throttle - throttle_out = get_pilot_desired_throttle(channel_throttle->control_in); - - // get corrected angle based on direction and axis of rotation - // we flip the sign of flip_angle to minimize the code repetition - int32_t flip_angle; - - if (flip_roll_dir != 0) { - flip_angle = ahrs.roll_sensor * flip_roll_dir; - } else { - flip_angle = ahrs.pitch_sensor * flip_pitch_dir; - } - - // state machine - switch (flip_state) { - - case Flip_Start: - // under 45 degrees request 400deg/sec roll or pitch - attitude_control.input_rate_bf_roll_pitch_yaw(FLIP_ROTATION_RATE * flip_roll_dir, FLIP_ROTATION_RATE * flip_pitch_dir, 0.0); - - // increase throttle - throttle_out += FLIP_THR_INC; - - // beyond 45deg lean angle move to next stage - if (flip_angle >= 4500) { - if (flip_roll_dir != 0) { - // we are rolling - flip_state = Flip_Roll; - } else { - // we are pitching - flip_state = Flip_Pitch_A; - } - } - break; - - case Flip_Roll: - // between 45deg ~ -90deg request 400deg/sec roll - attitude_control.input_rate_bf_roll_pitch_yaw(FLIP_ROTATION_RATE * flip_roll_dir, 0.0, 0.0); - // decrease throttle - throttle_out = MAX(throttle_out - FLIP_THR_DEC, 0.0f); - - // beyond -90deg move on to recovery - if ((flip_angle < 4500) && (flip_angle > -9000)) { - flip_state = Flip_Recover; - } - break; - - case Flip_Pitch_A: - // between 45deg ~ -90deg request 400deg/sec pitch - attitude_control.input_rate_bf_roll_pitch_yaw(0.0f, FLIP_ROTATION_RATE * flip_pitch_dir, 0.0); - // decrease throttle - throttle_out = MAX(throttle_out - FLIP_THR_DEC, 0.0f); - - // check roll for inversion - if ((labs(ahrs.roll_sensor) > 9000) && (flip_angle > 4500)) { - flip_state = Flip_Pitch_B; - } - break; - - case Flip_Pitch_B: - // between 45deg ~ -90deg request 400deg/sec pitch - attitude_control.input_rate_bf_roll_pitch_yaw(0.0, FLIP_ROTATION_RATE * flip_pitch_dir, 0.0); - // decrease throttle - throttle_out = MAX(throttle_out - FLIP_THR_DEC, 0.0f); - - // check roll for inversion - if ((labs(ahrs.roll_sensor) < 9000) && (flip_angle > -4500)) { - flip_state = Flip_Recover; - } - break; - - case Flip_Recover: - // use originally captured earth-frame angle targets to recover - attitude_control.input_euler_angle_roll_pitch_yaw(flip_orig_attitude.x, flip_orig_attitude.y, flip_orig_attitude.z, false, get_smoothing_gain()); - - // increase throttle to gain any lost altitude - throttle_out += FLIP_THR_INC; - - if (flip_roll_dir != 0) { - // we are rolling - recovery_angle = fabsf(flip_orig_attitude.x - (float)ahrs.roll_sensor); - } else { - // we are pitching - recovery_angle = fabsf(flip_orig_attitude.y - (float)ahrs.pitch_sensor); - } - - // check for successful recovery - if (fabsf(recovery_angle) <= FLIP_RECOVERY_ANGLE) { - // restore original flight mode - if (!set_mode(flip_orig_control_mode, MODE_REASON_FLIP_COMPLETE)) { - // this should never happen but just in case - set_mode(STABILIZE, MODE_REASON_UNKNOWN); - } - // log successful completion - Log_Write_Event(DATA_FLIP_END); - } - break; - - case Flip_Abandon: - // restore original flight mode - if (!set_mode(flip_orig_control_mode, MODE_REASON_FLIP_COMPLETE)) { - // this should never happen but just in case - set_mode(STABILIZE, MODE_REASON_UNKNOWN); - } - // log abandoning flip - Log_Write_Error(ERROR_SUBSYSTEM_FLIP,ERROR_CODE_FLIP_ABANDONED); - break; - } - - // output pilot's throttle without angle boost - attitude_control.set_throttle_out(throttle_out, false, g.throttle_filt); -} diff --git a/ArduSub/control_guided.cpp b/ArduSub/control_guided.cpp index 6a0639c658..e9c8a99091 100644 --- a/ArduSub/control_guided.cpp +++ b/ArduSub/control_guided.cpp @@ -51,35 +51,6 @@ bool Sub::guided_init(bool ignore_checks) // } } - -// guided_takeoff_start - initialises waypoint controller to implement take-off -bool Sub::guided_takeoff_start(float final_alt_above_home) -{ - guided_mode = Guided_TakeOff; - - // initialise wpnav destination - Location_Class target_loc = current_loc; - target_loc.set_alt_cm(final_alt_above_home, Location_Class::ALT_FRAME_ABOVE_HOME); - - if (!wp_nav.set_wp_destination(target_loc)) { - // failure to set destination can only be because of missing terrain data - Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION); - // failure is propagated to GCS with NAK - return false; - } - - // initialise yaw - set_auto_yaw_mode(AUTO_YAW_HOLD); - - // clear i term when we're taking off - set_throttle_takeoff(); - - // get initial alt for WP_TKOFF_NAV_ALT - auto_takeoff_set_start_alt(); - - return true; -} - // initialise guided mode's position controller void Sub::guided_pos_control_start() { @@ -270,7 +241,7 @@ void Sub::guided_set_angle(const Quaternion &q, float climb_rate_cms) q.to_euler(guided_angle_state.roll_cd, guided_angle_state.pitch_cd, guided_angle_state.yaw_cd); guided_angle_state.roll_cd = ToDeg(guided_angle_state.roll_cd) * 100.0f; guided_angle_state.pitch_cd = ToDeg(guided_angle_state.pitch_cd) * 100.0f; - guided_angle_state.yaw_cd = wrap_180_cd_float(ToDeg(guided_angle_state.yaw_cd) * 100.0f); + guided_angle_state.yaw_cd = wrap_180_cd(ToDeg(guided_angle_state.yaw_cd) * 100.0f); guided_angle_state.climb_rate_cms = climb_rate_cms; guided_angle_state.update_time_ms = millis(); @@ -283,11 +254,6 @@ void Sub::guided_run() // call the correct auto controller switch (guided_mode) { - case Guided_TakeOff: - // run takeoff controller - guided_takeoff_run(); - break; - case Guided_WP: // run position controller guided_pos_control_run(); @@ -310,39 +276,6 @@ void Sub::guided_run() } } -// guided_takeoff_run - takeoff in guided mode -// called by guided_run at 100hz or more -void Sub::guided_takeoff_run() -{ - // if not auto armed or motors not enabled set throttle to zero and exit immediately - if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) { - motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); - // multicopters do not stabilize roll/pitch/yaw when disarmed - attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); - - return; - } - - // process pilot's yaw input - float target_yaw_rate = 0; - if (!failsafe.radio) { - // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); - } - - // set motors to full range - motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); - - // run waypoint controller - failsafe_terrain_set_status(wp_nav.update_wpnav()); - - // call z-axis position controller (wpnav should have already updated it's alt target) - pos_control.update_z_controller(); - - // call attitude controller - auto_takeoff_attitude_run(target_yaw_rate); -} - // guided_pos_control_run - runs the guided position controller // called from guided_run void Sub::guided_pos_control_run() @@ -360,7 +293,7 @@ void Sub::guided_pos_control_run() float target_yaw_rate = 0; if (!failsafe.manual_control) { // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); + target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); if (!is_zero(target_yaw_rate)) { set_auto_yaw_mode(AUTO_YAW_HOLD); } @@ -404,7 +337,7 @@ void Sub::guided_vel_control_run() float target_yaw_rate = 0; if (!failsafe.manual_control) { // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); + target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); if (!is_zero(target_yaw_rate)) { set_auto_yaw_mode(AUTO_YAW_HOLD); } @@ -453,7 +386,7 @@ void Sub::guided_posvel_control_run() if (!failsafe.manual_control) { // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); + target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); if (!is_zero(target_yaw_rate)) { set_auto_yaw_mode(AUTO_YAW_HOLD); } @@ -518,7 +451,7 @@ void Sub::guided_angle_control_run() // constrain desired lean angles float roll_in = guided_angle_state.roll_cd; float pitch_in = guided_angle_state.pitch_cd; - float total_in = pythagorous2(roll_in, pitch_in); + float total_in = norm(roll_in, pitch_in); float angle_max = MIN(attitude_control.get_althold_lean_angle_max(), aparm.angle_max); if (total_in > angle_max) { float ratio = angle_max / total_in; @@ -527,7 +460,7 @@ void Sub::guided_angle_control_run() } // wrap yaw request - float yaw_in = wrap_180_cd_float(guided_angle_state.yaw_cd); + float yaw_in = wrap_180_cd(guided_angle_state.yaw_cd); // constrain climb rate float climb_rate_cms = constrain_float(guided_angle_state.climb_rate_cms, -fabsf(wp_nav.get_speed_down()), wp_nav.get_speed_up()); diff --git a/ArduSub/control_land.cpp b/ArduSub/control_land.cpp deleted file mode 100644 index 234e3be541..0000000000 --- a/ArduSub/control_land.cpp +++ /dev/null @@ -1,271 +0,0 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#include "Sub.h" - -static bool land_with_gps; - -static uint32_t land_start_time; -static bool land_pause; - -// land_init - initialise land controller -bool Sub::land_init(bool ignore_checks) -{ - // check if we have GPS and decide which LAND we're going to do - land_with_gps = position_ok(); - if (land_with_gps) { - // set target to stopping point - Vector3f stopping_point; - wp_nav.get_loiter_stopping_point_xy(stopping_point); - wp_nav.init_loiter_target(stopping_point); - } - - // initialize vertical speeds and leash lengths - pos_control.set_speed_z(wp_nav.get_speed_down(), wp_nav.get_speed_up()); - pos_control.set_accel_z(wp_nav.get_accel_z()); - - // initialise altitude target to stopping point - pos_control.set_target_to_stopping_point_z(); - - land_start_time = millis(); - - land_pause = false; - - // reset flag indicating if pilot has applied roll or pitch inputs during landing - ap.land_repo_active = false; - - return true; -} - -// land_run - runs the land controller -// should be called at 100hz or more -void Sub::land_run() -{ - if (land_with_gps) { - land_gps_run(); - }else{ - land_nogps_run(); - } -} - -// land_run - runs the land controller -// horizontal position controlled with loiter controller -// should be called at 100hz or more -void Sub::land_gps_run() -{ - int16_t roll_control = 0, pitch_control = 0; - float target_yaw_rate = 0; - - // if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately - if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) { - motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); - // multicopters do not stabilize roll/pitch/yaw when disarmed - attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); - - wp_nav.init_loiter_target(); - -#if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED - // disarm when the landing detector says we've landed and throttle is at minimum - if (ap.land_complete && (ap.throttle_zero || failsafe.radio)) { - init_disarm_motors(); - } -#else - // disarm when the landing detector says we've landed - if (ap.land_complete) { - init_disarm_motors(); - } -#endif - return; - } - - // relax loiter target if we might be landed - if (ap.land_complete_maybe) { - wp_nav.loiter_soften_for_landing(); - } - - // process pilot inputs - if (!failsafe.radio) { - if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){ - Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT); - // exit land if throttle is high - if (!set_mode(LOITER, MODE_REASON_THROTTLE_LAND_ESCAPE)) { - set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE); - } - } - - if (g.land_repositioning) { - // apply SIMPLE mode transform to pilot inputs - update_simple_mode(); - - // process pilot's roll and pitch input - roll_control = channel_roll->control_in; - pitch_control = channel_pitch->control_in; - - // record if pilot has overriden roll or pitch - if (roll_control != 0 || pitch_control != 0) { - ap.land_repo_active = true; - } - } - - // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); - } - - // set motors to full range - motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); - - // process roll, pitch inputs - wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control); - -#if PRECISION_LANDING == ENABLED - // run precision landing - if (!ap.land_repo_active) { - wp_nav.shift_loiter_target(precland.get_target_shift(wp_nav.get_loiter_target())); - } -#endif - - // run loiter controller - wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); - - // call attitude controller - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain()); - - // pause 4 seconds before beginning land descent - float cmb_rate; - if(land_pause && millis()-land_start_time < 4000) { - cmb_rate = 0; - } else { - land_pause = false; - cmb_rate = get_land_descent_speed(); - } - - // record desired climb rate for logging - desired_climb_rate = cmb_rate; - - // update altitude target and call position controller - pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt, true); - pos_control.update_z_controller(); -} - -// land_nogps_run - runs the land controller -// pilot controls roll and pitch angles -// should be called at 100hz or more -void Sub::land_nogps_run() -{ - float target_roll = 0.0f, target_pitch = 0.0f; - float target_yaw_rate = 0; - - // process pilot inputs - if (!failsafe.radio) { - if (g.land_repositioning) { - // apply SIMPLE mode transform to pilot inputs - update_simple_mode(); - - // get pilot desired lean angles - get_pilot_desired_lean_angles(channel_roll->control_in, channel_pitch->control_in, target_roll, target_pitch, aparm.angle_max); - } - - // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); - } - - // if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately - if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) { - motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); - // multicopters do not stabilize roll/pitch/yaw when disarmed - attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); - -#if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED - // disarm when the landing detector says we've landed and throttle is at minimum - if (ap.land_complete && (ap.throttle_zero || failsafe.radio)) { - init_disarm_motors(); - } -#else - // disarm when the landing detector says we've landed - if (ap.at_surface) { - set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE); - } -#endif - return; - } - - // set motors to full range - motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); - - // call attitude controller - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); - - // pause 4 seconds before beginning land descent - float cmb_rate; - if(land_pause && millis()-land_start_time < LAND_WITH_DELAY_MS) { - cmb_rate = 0; - } else { - land_pause = false; - cmb_rate = get_land_descent_speed(); - } - - // record desired climb rate for logging - desired_climb_rate = cmb_rate; - - // call position controller - pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt, true); - pos_control.update_z_controller(); -} - -// get_land_descent_speed - high level landing logic -// returns climb rate (in cm/s) which should be passed to the position controller -// should be called at 100hz or higher -float Sub::get_land_descent_speed() -{ -#if RANGEFINDER_ENABLED == ENABLED - bool rangefinder_ok = rangefinder_state.enabled && rangefinder_state.alt_healthy; -#else - bool rangefinder_ok = false; -#endif - - // get position controller's target altitude above terrain - Location_Class target_loc = pos_control.get_pos_target(); - int32_t target_alt_cm; - - // get altitude target above home by default - target_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_HOME, target_alt_cm); - - // try to use terrain if enabled - if (terrain_use() && !target_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, target_alt_cm)) { - Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA); - } - - // if we are above 10m and the rangefinder does not sense anything perform regular alt hold descent - if ((target_alt_cm >= LAND_START_ALT) && !rangefinder_ok) { - if (g.land_speed_high > 0) { - // user has asked for a different landing speed than normal descent rate - return -abs(g.land_speed_high); - } - return pos_control.get_speed_up(); - }else{ - return abs(g.land_speed); - } -} - -// land_do_not_use_GPS - forces land-mode to not use the GPS but instead rely on pilot input for roll and pitch -// called during GPS failsafe to ensure that if we were already in LAND mode that we do not use the GPS -// has no effect if we are not already in LAND mode -void Sub::land_do_not_use_GPS() -{ - land_with_gps = false; -} - -// set_mode_land_with_pause - sets mode to LAND and triggers 4 second delay before descent starts -// this is always called from a failsafe so we trigger notification to pilot -void Sub::set_mode_land_with_pause(mode_reason_t reason) -{ - set_mode(LAND, reason); - land_pause = true; - - // alert pilot to mode change - AP_Notify::events.failsafe_mode_change = 1; -} - -// landing_with_GPS - returns true if vehicle is landing using GPS -bool Sub::landing_with_GPS() { - return (control_mode == LAND && land_with_gps); -} diff --git a/ArduSub/control_poshold.cpp b/ArduSub/control_poshold.cpp index ddad37f107..4416370f2a 100644 --- a/ArduSub/control_poshold.cpp +++ b/ArduSub/control_poshold.cpp @@ -1,87 +1,20 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +// ArduSub position hold flight mode +// GPS required +// Jacob Walser August 2016 #include "Sub.h" #if POSHOLD_ENABLED == ENABLED -/* - * control_poshold.pde - init and run calls for PosHold flight mode - * PosHold tries to improve upon regular loiter by mixing the pilot input with the loiter controller - */ - -#define POSHOLD_SPEED_0 10 // speed below which it is always safe to switch to loiter - -// 400hz loop update rate -#define POSHOLD_BRAKE_TIME_ESTIMATE_MAX (600*4) // max number of cycles the brake will be applied before we switch to loiter -#define POSHOLD_BRAKE_TO_LOITER_TIMER (150*4) // Number of cycles to transition from brake mode to loiter mode. Must be lower than POSHOLD_LOITER_STAB_TIMER -#define POSHOLD_WIND_COMP_START_TIMER (150*4) // Number of cycles to start wind compensation update after loiter is engaged -#define POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER (50*4) // Set it from 100 to 200, the number of centiseconds loiter and manual commands are mixed to make a smooth transition. -#define POSHOLD_SMOOTH_RATE_FACTOR 0.0125f // filter applied to pilot's roll/pitch input as it returns to center. A lower number will cause the roll/pitch to return to zero more slowly if the brake_rate is also low. -#define POSHOLD_WIND_COMP_TIMER_10HZ 40 // counter value used to reduce wind compensation to 10hz -#define LOOP_RATE_FACTOR 4 // used to adapt PosHold params to loop_rate -#define TC_WIND_COMP 0.0025f // Time constant for poshold_update_wind_comp_estimate() - -// definitions that are independent of main loop rate -#define POSHOLD_STICK_RELEASE_SMOOTH_ANGLE 1800 // max angle required (in centi-degrees) after which the smooth stick release effect is applied -#define POSHOLD_WIND_COMP_ESTIMATE_SPEED_MAX 10 // wind compensation estimates will only run when velocity is at or below this speed in cm/s - -// mission state enumeration -enum poshold_rp_mode { - POSHOLD_PILOT_OVERRIDE=0, // pilot is controlling this axis (i.e. roll or pitch) - POSHOLD_BRAKE, // this axis is braking towards zero - POSHOLD_BRAKE_READY_TO_LOITER, // this axis has completed braking and is ready to enter loiter mode (both modes must be this value before moving to next stage) - POSHOLD_BRAKE_TO_LOITER, // both vehicle's axis (roll and pitch) are transitioning from braking to loiter mode (braking and loiter controls are mixed) - POSHOLD_LOITER, // both vehicle axis are holding position - POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE // pilot has input controls on this axis and this axis is transitioning to pilot override (other axis will transition to brake if no pilot input) -}; - -static struct { - poshold_rp_mode roll_mode : 3; // roll mode: pilot override, brake or loiter - poshold_rp_mode pitch_mode : 3; // pitch mode: pilot override, brake or loiter - uint8_t braking_time_updated_roll : 1; // true once we have re-estimated the braking time. This is done once as the vehicle begins to flatten out after braking - uint8_t braking_time_updated_pitch : 1; // true once we have re-estimated the braking time. This is done once as the vehicle begins to flatten out after braking - uint8_t loiter_reset_I : 1; // true the very first time PosHold enters loiter, thereafter we trust the i terms loiter has - - // pilot input related variables - float pilot_roll; // pilot requested roll angle (filtered to slow returns to zero) - float pilot_pitch; // pilot requested roll angle (filtered to slow returns to zero) - - // braking related variables - float brake_gain; // gain used during conversion of vehicle's velocity to lean angle during braking (calculated from brake_rate) - int16_t brake_roll; // target roll angle during braking periods - int16_t brake_pitch; // target pitch angle during braking periods - int16_t brake_timeout_roll; // number of cycles allowed for the braking to complete, this timeout will be updated at half-braking - int16_t brake_timeout_pitch; // number of cycles allowed for the braking to complete, this timeout will be updated at half-braking - int16_t brake_angle_max_roll; // maximum lean angle achieved during braking. Used to determine when the vehicle has begun to flatten out so that we can re-estimate the braking time - int16_t brake_angle_max_pitch; // maximum lean angle achieved during braking Used to determine when the vehicle has begun to flatten out so that we can re-estimate the braking time - int16_t brake_to_loiter_timer; // cycles to mix brake and loiter controls in POSHOLD_BRAKE_TO_LOITER - - // loiter related variables - int16_t controller_to_pilot_timer_roll; // cycles to mix controller and pilot controls in POSHOLD_CONTROLLER_TO_PILOT - int16_t controller_to_pilot_timer_pitch; // cycles to mix controller and pilot controls in POSHOLD_CONTROLLER_TO_PILOT - int16_t controller_final_roll; // final roll angle from controller as we exit brake or loiter mode (used for mixing with pilot input) - int16_t controller_final_pitch; // final pitch angle from controller as we exit brake or loiter mode (used for mixing with pilot input) - - // wind compensation related variables - Vector2f wind_comp_ef; // wind compensation in earth frame, filtered lean angles from position controller - int16_t wind_comp_roll; // roll angle to compensate for wind - int16_t wind_comp_pitch; // pitch angle to compensate for wind - uint16_t wind_comp_start_timer; // counter to delay start of wind compensation for a short time after loiter is engaged - int8_t wind_comp_timer; // counter to reduce wind comp roll/pitch lean angle calcs to 10hz - - // final output - int16_t roll; // final roll angle sent to attitude controller - int16_t pitch; // final pitch angle sent to attitude controller -} poshold; - // poshold_init - initialise PosHold controller -bool Copter::poshold_init(bool ignore_checks) +bool Sub::poshold_init(bool ignore_checks) { - // fail to initialise PosHold mode if no GPS lock + // fail to initialise PosHold mode if no GPS lock if (!position_ok() && !ignore_checks) { return false; } - + // 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); @@ -90,575 +23,126 @@ bool Copter::poshold_init(bool ignore_checks) pos_control.set_alt_target(inertial_nav.get_altitude()); pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z()); - // initialise lean angles to current attitude - poshold.pilot_roll = 0; - poshold.pilot_pitch = 0; + // set target to current position + // only init here as we can switch to PosHold in flight with a velocity <> 0 that will be used as _last_vel in PosControl and never updated again as we inhibit Reset_I + wp_nav.init_loiter_target(); - // compute brake_gain - poshold.brake_gain = (15.0f * (float)g.poshold_brake_rate + 95.0f) / 100.0f; - - if (ap.land_complete) { - // if landed begin in loiter mode - poshold.roll_mode = POSHOLD_LOITER; - poshold.pitch_mode = POSHOLD_LOITER; - // set target to current position - // only init here as we can switch to PosHold in flight with a velocity <> 0 that will be used as _last_vel in PosControl and never updated again as we inhibit Reset_I - wp_nav.init_loiter_target(); - }else{ - // if not landed start in pilot override to avoid hard twitch - poshold.roll_mode = POSHOLD_PILOT_OVERRIDE; - poshold.pitch_mode = POSHOLD_PILOT_OVERRIDE; - } - - // loiter's I terms should be reset the first time only - poshold.loiter_reset_I = true; - - // initialise wind_comp each time PosHold is switched on - poshold.wind_comp_ef.zero(); - poshold.wind_comp_roll = 0; - poshold.wind_comp_pitch = 0; - poshold.wind_comp_timer = 0; + last_pilot_heading = ahrs.yaw_sensor; return true; } // poshold_run - runs the PosHold controller // should be called at 100hz or more -void Copter::poshold_run() +void Sub::poshold_run() { - float target_roll, target_pitch; // pilot's roll and pitch angle inputs - float target_yaw_rate = 0; // pilot desired yaw rate in centi-degrees/sec - float target_climb_rate = 0; // pilot desired climb rate in centimeters/sec - float takeoff_climb_rate = 0.0f; // takeoff induced climb rate - float brake_to_loiter_mix; // mix of brake and loiter controls. 0 = fully brake controls, 1 = fully loiter controls - float controller_to_pilot_roll_mix; // mix of controller and pilot controls. 0 = fully last controller controls, 1 = fully pilot controls - float controller_to_pilot_pitch_mix; // mix of controller and pilot controls. 0 = fully last controller controls, 1 = fully pilot controls - float vel_fw, vel_right; // vehicle's current velocity in body-frame forward and right directions - const Vector3f& vel = inertial_nav.get_velocity(); - - // 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); + uint32_t tnow = AP_HAL::millis(); + // convert inertial nav earth-frame velocities to body-frame +// const Vector3f& vel = inertial_nav.get_velocity(); +// 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()) { motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); wp_nav.init_loiter_target(); attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); - pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average); + pos_control.relax_alt_hold_controllers(motors.get_throttle_hover()); return; } - // process pilot inputs - if (!failsafe.radio) { - // apply SIMPLE mode transform to pilot inputs - update_simple_mode(); + // apply SIMPLE mode transform to pilot inputs + update_simple_mode(); - // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); + // set motors to full range + motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); - // get pilot desired climb rate (for alt-hold mode and take-off) - target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->control_in); - target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max, g.pilot_velocity_z_max); + // run loiter controller + wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); - // get takeoff adjusted pilot and takeoff climb rates - takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate); + /////////////////////// + // update xy outputs // + int16_t pilot_lateral = channel_lateral->norm_input(); + int16_t pilot_forward = channel_forward->norm_input(); - // check for take-off - if (ap.land_complete && (takeoff_state.running || channel_throttle->control_in > get_takeoff_trigger_throttle())) { - if (!takeoff_state.running) { - takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f)); - } + float lateral_out = 0; + float forward_out = 0; - // indicate we are taking off - set_land_complete(false); - // clear i term when we're taking off - set_throttle_takeoff(); - } - } + // Allow pilot to reposition the sub + if(pilot_lateral != 0 || pilot_forward != 0) { + lateral_out = pilot_lateral; + forward_out = pilot_forward; + wp_nav.init_loiter_target(); // initialize target to current position after repositioning + } else { + translate_wpnav_rp(lateral_out, forward_out); + } - // relax loiter target if we might be landed - if (ap.land_complete_maybe) { - wp_nav.loiter_soften_for_landing(); - } + motors.set_lateral(lateral_out); + motors.set_forward(forward_out); - // if landed initialise loiter targets, set throttle to zero and exit - if (ap.land_complete) { - // if throttle zero reset attitude and exit immediately - if (ap.throttle_zero) { - motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); - }else{ - motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); - } - wp_nav.init_loiter_target(); - // move throttle to between minimum and non-takeoff-throttle to keep us on the ground - attitude_control.set_throttle_out(get_throttle_pre_takeoff(channel_throttle->control_in),false,g.throttle_filt); - pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average); - return; - }else{ - // convert pilot input to lean angles - get_pilot_desired_lean_angles(channel_roll->control_in, channel_pitch->control_in, target_roll, target_pitch, aparm.angle_max); + ///////////////////// + // Update attitude // - // 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) - vel_fw = vel.x*ahrs.cos_yaw() + vel.y*ahrs.sin_yaw(); - vel_right = -vel.x*ahrs.sin_yaw() + vel.y*ahrs.cos_yaw(); - - // If not in LOITER, retrieve latest wind compensation lean angles related to current yaw - if (poshold.roll_mode != POSHOLD_LOITER || poshold.pitch_mode != POSHOLD_LOITER) - poshold_get_wind_comp_lean_angles(poshold.wind_comp_roll, poshold.wind_comp_pitch); + // get pilot's desired yaw rate + float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); - // Roll state machine - // Each state (aka mode) is responsible for: - // 1. dealing with pilot input - // 2. calculating the final roll output to the attitude controller - // 3. checking if the state (aka mode) should be changed and if 'yes' perform any required initialisation for the new state - switch (poshold.roll_mode) { + // 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); - case POSHOLD_PILOT_OVERRIDE: - // update pilot desired roll angle using latest radio input - // this filters the input so that it returns to zero no faster than the brake-rate - poshold_update_pilot_lean_angle(poshold.pilot_roll, target_roll); + // update attitude controller targets + if (!is_zero(target_yaw_rate)) { // call attitude controller with rate yaw determined by pilot input + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); + last_pilot_heading = ahrs.yaw_sensor; + last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading - // switch to BRAKE mode for next iteration if no pilot input - if (is_zero(target_roll) && (fabsf(poshold.pilot_roll) < 2 * g.poshold_brake_rate)) { - // initialise BRAKE mode - poshold.roll_mode = POSHOLD_BRAKE; // Set brake roll mode - poshold.brake_roll = 0; // initialise braking angle to zero - poshold.brake_angle_max_roll = 0; // reset brake_angle_max so we can detect when vehicle begins to flatten out during braking - poshold.brake_timeout_roll = POSHOLD_BRAKE_TIME_ESTIMATE_MAX; // number of cycles the brake will be applied, updated during braking mode. - poshold.braking_time_updated_roll = false; // flag the braking time can be re-estimated - } + } else { // hold current heading - // final lean angle should be pilot input plus wind compensation - poshold.roll = poshold.pilot_roll + poshold.wind_comp_roll; - break; + // this check is required to prevent bounce back after very fast yaw maneuvers + // the inertia of the vehicle causes the heading to move slightly past the point when pilot input actually stopped + if(tnow < last_pilot_yaw_input_ms + 250) { // give 250ms to slow down, then set target heading + target_yaw_rate = 0; // Stop rotation on yaw axis - case POSHOLD_BRAKE: - case POSHOLD_BRAKE_READY_TO_LOITER: - // calculate brake_roll angle to counter-act velocity - poshold_update_brake_angle_from_velocity(poshold.brake_roll, vel_right); + // call attitude controller with target yaw rate = 0 to decelerate on yaw axis + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); + last_pilot_heading = ahrs.yaw_sensor; // update heading to hold - // update braking time estimate - if (!poshold.braking_time_updated_roll) { - // check if brake angle is increasing - if (abs(poshold.brake_roll) >= poshold.brake_angle_max_roll) { - poshold.brake_angle_max_roll = abs(poshold.brake_roll); - } else { - // braking angle has started decreasing so re-estimate braking time - poshold.brake_timeout_roll = 1+(uint16_t)(LOOP_RATE_FACTOR*15L*(int32_t)(abs(poshold.brake_roll))/(10L*(int32_t)g.poshold_brake_rate)); // the 1.2 (12/10) factor has to be tuned in flight, here it means 120% of the "normal" time. - poshold.braking_time_updated_roll = true; - } - } + } else { // call attitude controller holding absolute absolute bearing + attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, last_pilot_heading, true, get_smoothing_gain()); + } + } - // if velocity is very low reduce braking time to 0.5seconds - if ((fabsf(vel_right) <= POSHOLD_SPEED_0) && (poshold.brake_timeout_roll > 50*LOOP_RATE_FACTOR)) { - poshold.brake_timeout_roll = 50*LOOP_RATE_FACTOR; - } + /////////////////// + // Update z axis // - // reduce braking timer - if (poshold.brake_timeout_roll > 0) { - poshold.brake_timeout_roll--; - } else { - // indicate that we are ready to move to Loiter. - // Loiter will only actually be engaged once both roll_mode and pitch_mode are changed to POSHOLD_BRAKE_READY_TO_LOITER - // logic for engaging loiter is handled below the roll and pitch mode switch statements - poshold.roll_mode = POSHOLD_BRAKE_READY_TO_LOITER; - } + // 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); - // final lean angle is braking angle + wind compensation angle - poshold.roll = poshold.brake_roll + poshold.wind_comp_roll; + // 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); + } - // check for pilot input - if (!is_zero(target_roll)) { - // init transition to pilot override - poshold_roll_controller_to_pilot_override(); - } - break; + // 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); + } + } - case POSHOLD_BRAKE_TO_LOITER: - case POSHOLD_LOITER: - // these modes are combined roll-pitch modes and are handled below - break; - - case POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE: - // update pilot desired roll angle using latest radio input - // this filters the input so that it returns to zero no faster than the brake-rate - poshold_update_pilot_lean_angle(poshold.pilot_roll, target_roll); - - // count-down loiter to pilot timer - if (poshold.controller_to_pilot_timer_roll > 0) { - poshold.controller_to_pilot_timer_roll--; - } else { - // when timer runs out switch to full pilot override for next iteration - poshold.roll_mode = POSHOLD_PILOT_OVERRIDE; - } - - // calculate controller_to_pilot mix ratio - controller_to_pilot_roll_mix = (float)poshold.controller_to_pilot_timer_roll / (float)POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER; - - // mix final loiter lean angle and pilot desired lean angles - poshold.roll = poshold_mix_controls(controller_to_pilot_roll_mix, poshold.controller_final_roll, poshold.pilot_roll + poshold.wind_comp_roll); - break; - } - - // Pitch state machine - // Each state (aka mode) is responsible for: - // 1. dealing with pilot input - // 2. calculating the final pitch output to the attitude contpitcher - // 3. checking if the state (aka mode) should be changed and if 'yes' perform any required initialisation for the new state - switch (poshold.pitch_mode) { - - case POSHOLD_PILOT_OVERRIDE: - // update pilot desired pitch angle using latest radio input - // this filters the input so that it returns to zero no faster than the brake-rate - poshold_update_pilot_lean_angle(poshold.pilot_pitch, target_pitch); - - // switch to BRAKE mode for next iteration if no pilot input - if (is_zero(target_pitch) && (fabsf(poshold.pilot_pitch) < 2 * g.poshold_brake_rate)) { - // initialise BRAKE mode - poshold.pitch_mode = POSHOLD_BRAKE; // set brake pitch mode - poshold.brake_pitch = 0; // initialise braking angle to zero - poshold.brake_angle_max_pitch = 0; // reset brake_angle_max so we can detect when vehicle begins to flatten out during braking - poshold.brake_timeout_pitch = POSHOLD_BRAKE_TIME_ESTIMATE_MAX; // number of cycles the brake will be applied, updated during braking mode. - poshold.braking_time_updated_pitch = false; // flag the braking time can be re-estimated - } - - // final lean angle should be pilot input plus wind compensation - poshold.pitch = poshold.pilot_pitch + poshold.wind_comp_pitch; - break; - - case POSHOLD_BRAKE: - case POSHOLD_BRAKE_READY_TO_LOITER: - // calculate brake_pitch angle to counter-act velocity - poshold_update_brake_angle_from_velocity(poshold.brake_pitch, -vel_fw); - - // update braking time estimate - if (!poshold.braking_time_updated_pitch) { - // check if brake angle is increasing - if (abs(poshold.brake_pitch) >= poshold.brake_angle_max_pitch) { - poshold.brake_angle_max_pitch = abs(poshold.brake_pitch); - } else { - // braking angle has started decreasing so re-estimate braking time - poshold.brake_timeout_pitch = 1+(uint16_t)(LOOP_RATE_FACTOR*15L*(int32_t)(abs(poshold.brake_pitch))/(10L*(int32_t)g.poshold_brake_rate)); // the 1.2 (12/10) factor has to be tuned in flight, here it means 120% of the "normal" time. - poshold.braking_time_updated_pitch = true; - } - } - - // if velocity is very low reduce braking time to 0.5seconds - if ((fabsf(vel_fw) <= POSHOLD_SPEED_0) && (poshold.brake_timeout_pitch > 50*LOOP_RATE_FACTOR)) { - poshold.brake_timeout_pitch = 50*LOOP_RATE_FACTOR; - } - - // reduce braking timer - if (poshold.brake_timeout_pitch > 0) { - poshold.brake_timeout_pitch--; - } else { - // indicate that we are ready to move to Loiter. - // Loiter will only actually be engaged once both pitch_mode and pitch_mode are changed to POSHOLD_BRAKE_READY_TO_LOITER - // logic for engaging loiter is handled below the pitch and pitch mode switch statements - poshold.pitch_mode = POSHOLD_BRAKE_READY_TO_LOITER; - } - - // final lean angle is braking angle + wind compensation angle - poshold.pitch = poshold.brake_pitch + poshold.wind_comp_pitch; - - // check for pilot input - if (!is_zero(target_pitch)) { - // init transition to pilot override - poshold_pitch_controller_to_pilot_override(); - } - break; - - case POSHOLD_BRAKE_TO_LOITER: - case POSHOLD_LOITER: - // these modes are combined pitch-pitch modes and are handled below - break; - - case POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE: - // update pilot desired pitch angle using latest radio input - // this filters the input so that it returns to zero no faster than the brake-rate - poshold_update_pilot_lean_angle(poshold.pilot_pitch, target_pitch); - - // count-down loiter to pilot timer - if (poshold.controller_to_pilot_timer_pitch > 0) { - poshold.controller_to_pilot_timer_pitch--; - } else { - // when timer runs out switch to full pilot override for next iteration - poshold.pitch_mode = POSHOLD_PILOT_OVERRIDE; - } - - // calculate controller_to_pilot mix ratio - controller_to_pilot_pitch_mix = (float)poshold.controller_to_pilot_timer_pitch / (float)POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER; - - // mix final loiter lean angle and pilot desired lean angles - poshold.pitch = poshold_mix_controls(controller_to_pilot_pitch_mix, poshold.controller_final_pitch, poshold.pilot_pitch + poshold.wind_comp_pitch); - break; - } - - // set motors to full range - motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); - - // - // Shared roll & pitch states (POSHOLD_BRAKE_TO_LOITER and POSHOLD_LOITER) - // - - // switch into LOITER mode when both roll and pitch are ready - if (poshold.roll_mode == POSHOLD_BRAKE_READY_TO_LOITER && poshold.pitch_mode == POSHOLD_BRAKE_READY_TO_LOITER) { - poshold.roll_mode = POSHOLD_BRAKE_TO_LOITER; - poshold.pitch_mode = POSHOLD_BRAKE_TO_LOITER; - poshold.brake_to_loiter_timer = POSHOLD_BRAKE_TO_LOITER_TIMER; - // init loiter controller - wp_nav.init_loiter_target(inertial_nav.get_position(), poshold.loiter_reset_I); // (false) to avoid I_term reset. In original code, velocity(0,0,0) was used instead of current velocity: wp_nav.init_loiter_target(inertial_nav.get_position(), Vector3f(0,0,0)); - // at this stage, we are going to run update_loiter that will reset I_term once. From now, we ensure next time that we will enter loiter and update it, I_term won't be reset anymore - poshold.loiter_reset_I = false; - // set delay to start of wind compensation estimate updates - poshold.wind_comp_start_timer = POSHOLD_WIND_COMP_START_TIMER; - } - - // roll-mode is used as the combined roll+pitch mode when in BRAKE_TO_LOITER or LOITER modes - if (poshold.roll_mode == POSHOLD_BRAKE_TO_LOITER || poshold.roll_mode == POSHOLD_LOITER) { - - // force pitch mode to be same as roll_mode just to keep it consistent (it's not actually used in these states) - poshold.pitch_mode = poshold.roll_mode; - - // handle combined roll+pitch mode - switch (poshold.roll_mode) { - case POSHOLD_BRAKE_TO_LOITER: - // reduce brake_to_loiter timer - if (poshold.brake_to_loiter_timer > 0) { - poshold.brake_to_loiter_timer--; - } else { - // progress to full loiter on next iteration - poshold.roll_mode = POSHOLD_LOITER; - poshold.pitch_mode = POSHOLD_LOITER; - } - - // calculate percentage mix of loiter and brake control - brake_to_loiter_mix = (float)poshold.brake_to_loiter_timer / (float)POSHOLD_BRAKE_TO_LOITER_TIMER; - - // calculate brake_roll and pitch angles to counter-act velocity - poshold_update_brake_angle_from_velocity(poshold.brake_roll, vel_right); - poshold_update_brake_angle_from_velocity(poshold.brake_pitch, -vel_fw); - - // run loiter controller - wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); - - // calculate final roll and pitch output by mixing loiter and brake controls - poshold.roll = poshold_mix_controls(brake_to_loiter_mix, poshold.brake_roll + poshold.wind_comp_roll, wp_nav.get_roll()); - poshold.pitch = poshold_mix_controls(brake_to_loiter_mix, poshold.brake_pitch + poshold.wind_comp_pitch, wp_nav.get_pitch()); - - // check for pilot input - if (!is_zero(target_roll) || !is_zero(target_pitch)) { - // if roll input switch to pilot override for roll - if (!is_zero(target_roll)) { - // init transition to pilot override - poshold_roll_controller_to_pilot_override(); - // switch pitch-mode to brake (but ready to go back to loiter anytime) - // no need to reset poshold.brake_pitch here as wind comp has not been updated since last brake_pitch computation - poshold.pitch_mode = POSHOLD_BRAKE_READY_TO_LOITER; - } - // if pitch input switch to pilot override for pitch - if (!is_zero(target_pitch)) { - // init transition to pilot override - poshold_pitch_controller_to_pilot_override(); - if (is_zero(target_roll)) { - // switch roll-mode to brake (but ready to go back to loiter anytime) - // no need to reset poshold.brake_roll here as wind comp has not been updated since last brake_roll computation - poshold.roll_mode = POSHOLD_BRAKE_READY_TO_LOITER; - } - } - } - break; - - case POSHOLD_LOITER: - // run loiter controller - wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); - - // set roll angle based on loiter controller outputs - poshold.roll = wp_nav.get_roll(); - poshold.pitch = wp_nav.get_pitch(); - - // update wind compensation estimate - poshold_update_wind_comp_estimate(); - - // check for pilot input - if (!is_zero(target_roll) || !is_zero(target_pitch)) { - // if roll input switch to pilot override for roll - if (!is_zero(target_roll)) { - // init transition to pilot override - poshold_roll_controller_to_pilot_override(); - // switch pitch-mode to brake (but ready to go back to loiter anytime) - poshold.pitch_mode = POSHOLD_BRAKE_READY_TO_LOITER; - // reset brake_pitch because wind_comp is now different and should give the compensation of the whole previous loiter angle - poshold.brake_pitch = 0; - } - // if pitch input switch to pilot override for pitch - if (!is_zero(target_pitch)) { - // init transition to pilot override - poshold_pitch_controller_to_pilot_override(); - // if roll not overriden switch roll-mode to brake (but be ready to go back to loiter any time) - if (is_zero(target_roll)) { - poshold.roll_mode = POSHOLD_BRAKE_READY_TO_LOITER; - poshold.brake_roll = 0; - } - } - } - break; - - default: - // do nothing for uncombined roll and pitch modes - break; - } - } - - // constrain target pitch/roll angles - poshold.roll = constrain_int16(poshold.roll, -aparm.angle_max, aparm.angle_max); - poshold.pitch = constrain_int16(poshold.pitch, -aparm.angle_max, aparm.angle_max); - - // update attitude controller targets - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(poshold.roll, poshold.pitch, target_yaw_rate); - - // 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); - } - // update altitude target and call position controller - pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); - pos_control.add_takeoff_climb_rate(takeoff_climb_rate, G_Dt); - pos_control.update_z_controller(); - } + pos_control.update_z_controller(); } - -// poshold_update_pilot_lean_angle - update the pilot's filtered lean angle with the latest raw input received -void Copter::poshold_update_pilot_lean_angle(float &lean_angle_filtered, float &lean_angle_raw) -{ - // if raw input is large or reversing the vehicle's lean angle immediately set the fitlered angle to the new raw angle - if ((lean_angle_filtered > 0 && lean_angle_raw < 0) || (lean_angle_filtered < 0 && lean_angle_raw > 0) || (fabsf(lean_angle_raw) > POSHOLD_STICK_RELEASE_SMOOTH_ANGLE)) { - lean_angle_filtered = lean_angle_raw; - } else { - // lean_angle_raw must be pulling lean_angle_filtered towards zero, smooth the decrease - if (lean_angle_filtered > 0) { - // reduce the filtered lean angle at 5% or the brake rate (whichever is faster). - lean_angle_filtered -= MAX((float)lean_angle_filtered * POSHOLD_SMOOTH_RATE_FACTOR, MAX(1, g.poshold_brake_rate/LOOP_RATE_FACTOR)); - // do not let the filtered angle fall below the pilot's input lean angle. - // the above line pulls the filtered angle down and the below line acts as a catch - lean_angle_filtered = MAX(lean_angle_filtered, lean_angle_raw); - }else{ - lean_angle_filtered += MAX(-(float)lean_angle_filtered * POSHOLD_SMOOTH_RATE_FACTOR, MAX(1, g.poshold_brake_rate/LOOP_RATE_FACTOR)); - lean_angle_filtered = MIN(lean_angle_filtered, lean_angle_raw); - } - } -} - -// poshold_mix_controls - mixes two controls based on the mix_ratio -// mix_ratio of 1 = use first_control completely, 0 = use second_control completely, 0.5 = mix evenly -int16_t Copter::poshold_mix_controls(float mix_ratio, int16_t first_control, int16_t second_control) -{ - mix_ratio = constrain_float(mix_ratio, 0.0f, 1.0f); - return (int16_t)((mix_ratio * first_control) + ((1.0f-mix_ratio)*second_control)); -} - -// poshold_update_brake_angle_from_velocity - updates the brake_angle based on the vehicle's velocity and brake_gain -// brake_angle is slewed with the wpnav.poshold_brake_rate and constrained by the wpnav.poshold_braking_angle_max -// velocity is assumed to be in the same direction as lean angle so for pitch you should provide the velocity backwards (i.e. -ve forward velocity) -void Copter::poshold_update_brake_angle_from_velocity(int16_t &brake_angle, float velocity) -{ - float lean_angle; - int16_t brake_rate = g.poshold_brake_rate; - - brake_rate /= 4; - if (brake_rate <= 0) { - brake_rate = 1; - } - - // calculate velocity-only based lean angle - if (velocity >= 0) { - lean_angle = -poshold.brake_gain * velocity * (1.0f+500.0f/(velocity+60.0f)); - } else { - lean_angle = -poshold.brake_gain * velocity * (1.0f+500.0f/(-velocity+60.0f)); - } - - // do not let lean_angle be too far from brake_angle - brake_angle = constrain_int16((int16_t)lean_angle, brake_angle - brake_rate, brake_angle + brake_rate); - - // constrain final brake_angle - brake_angle = constrain_int16(brake_angle, -g.poshold_brake_angle_max, g.poshold_brake_angle_max); -} - -// poshold_update_wind_comp_estimate - updates wind compensation estimate -// should be called at the maximum loop rate when loiter is engaged -void Copter::poshold_update_wind_comp_estimate() -{ - // check wind estimate start has not been delayed - if (poshold.wind_comp_start_timer > 0) { - poshold.wind_comp_start_timer--; - return; - } - - // check horizontal velocity is low - if (inertial_nav.get_velocity_xy() > POSHOLD_WIND_COMP_ESTIMATE_SPEED_MAX) { - return; - } - - // get position controller accel target - // To-Do: clean this up by using accessor in loiter controller (or move entire PosHold controller to a library shared with loiter) - const Vector3f& accel_target = pos_control.get_accel_target(); - - // update wind compensation in earth-frame lean angles - if (is_zero(poshold.wind_comp_ef.x)) { - // if wind compensation has not been initialised set it immediately to the pos controller's desired accel in north direction - poshold.wind_comp_ef.x = accel_target.x; - } else { - // low pass filter the position controller's lean angle output - poshold.wind_comp_ef.x = (1.0f-TC_WIND_COMP)*poshold.wind_comp_ef.x + TC_WIND_COMP*accel_target.x; - } - if (is_zero(poshold.wind_comp_ef.y)) { - // if wind compensation has not been initialised set it immediately to the pos controller's desired accel in north direction - poshold.wind_comp_ef.y = accel_target.y; - } else { - // low pass filter the position controller's lean angle output - poshold.wind_comp_ef.y = (1.0f-TC_WIND_COMP)*poshold.wind_comp_ef.y + TC_WIND_COMP*accel_target.y; - } -} - -// poshold_get_wind_comp_lean_angles - retrieve wind compensation angles in body frame roll and pitch angles -// should be called at the maximum loop rate -void Copter::poshold_get_wind_comp_lean_angles(int16_t &roll_angle, int16_t &pitch_angle) -{ - // reduce rate to 10hz - poshold.wind_comp_timer++; - if (poshold.wind_comp_timer < POSHOLD_WIND_COMP_TIMER_10HZ) { - return; - } - poshold.wind_comp_timer = 0; - - // convert earth frame desired accelerations to body frame roll and pitch lean angles - roll_angle = atanf((-poshold.wind_comp_ef.x*ahrs.sin_yaw() + poshold.wind_comp_ef.y*ahrs.cos_yaw())/981)*(18000/M_PI); - pitch_angle = atanf(-(poshold.wind_comp_ef.x*ahrs.cos_yaw() + poshold.wind_comp_ef.y*ahrs.sin_yaw())/981)*(18000/M_PI); -} - -// poshold_roll_controller_to_pilot_override - initialises transition from a controller submode (brake or loiter) to a pilot override on roll axis -void Copter::poshold_roll_controller_to_pilot_override() -{ - poshold.roll_mode = POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE; - poshold.controller_to_pilot_timer_roll = POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER; - // initialise pilot_roll to 0, wind_comp will be updated to compensate and poshold_update_pilot_lean_angle function shall not smooth this transition at next iteration. so 0 is the right value - poshold.pilot_roll = 0; - // store final controller output for mixing with pilot input - poshold.controller_final_roll = poshold.roll; -} - -// poshold_pitch_controller_to_pilot_override - initialises transition from a controller submode (brake or loiter) to a pilot override on roll axis -void Copter::poshold_pitch_controller_to_pilot_override() -{ - poshold.pitch_mode = POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE; - poshold.controller_to_pilot_timer_pitch = POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER; - // initialise pilot_pitch to 0, wind_comp will be updated to compensate and poshold_update_pilot_lean_angle function shall not smooth this transition at next iteration. so 0 is the right value - poshold.pilot_pitch = 0; - // store final loiter outputs for mixing with pilot input - poshold.controller_final_pitch = poshold.pitch; -} - #endif // POSHOLD_ENABLED == ENABLED diff --git a/ArduSub/control_rtl.cpp b/ArduSub/control_rtl.cpp deleted file mode 100644 index 9e5c943f70..0000000000 --- a/ArduSub/control_rtl.cpp +++ /dev/null @@ -1,503 +0,0 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#include "Sub.h" - -/* - * control_rtl.pde - init and run calls for RTL flight mode - * - * There are two parts to RTL, the high level decision making which controls which state we are in - * and the lower implementation of the waypoint or landing controllers within those states - */ - -// rtl_init - initialise rtl controller -bool Sub::rtl_init(bool ignore_checks) -{ - if (position_ok() || ignore_checks) { - rtl_build_path(!failsafe.terrain); - rtl_climb_start(); - return true; - }else{ - return false; - } -} - -// re-start RTL with terrain following disabled -void Sub::rtl_restart_without_terrain() -{ - // log an error - Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_RESTARTED_RTL); - if (rtl_path.terrain_used) { - rtl_build_path(false); - rtl_climb_start(); - gcs_send_text(MAV_SEVERITY_CRITICAL,"Restarting RTL - Terrain data missing"); - } -} - -// rtl_run - runs the return-to-launch controller -// should be called at 100hz or more -void Sub::rtl_run() -{ - // check if we need to move to next state - if (rtl_state_complete) { - switch (rtl_state) { - case RTL_InitialClimb: - rtl_return_start(); - break; - case RTL_ReturnHome: - rtl_loiterathome_start(); - break; - case RTL_LoiterAtHome: - if (g.rtl_alt_final > 0 && !failsafe.radio) { - rtl_descent_start(); - }else{ - rtl_land_start(); - } - break; - case RTL_FinalDescent: - // do nothing - break; - case RTL_Land: - // do nothing - rtl_land_run will take care of disarming motors - break; - } - } - - // call the correct run function - switch (rtl_state) { - - case RTL_InitialClimb: - rtl_climb_return_run(); - break; - - case RTL_ReturnHome: - rtl_climb_return_run(); - break; - - case RTL_LoiterAtHome: - rtl_loiterathome_run(); - break; - - case RTL_FinalDescent: - rtl_descent_run(); - break; - - case RTL_Land: - rtl_land_run(); - break; - } -} - -// rtl_climb_start - initialise climb to RTL altitude -void Sub::rtl_climb_start() -{ - rtl_state = RTL_InitialClimb; - rtl_state_complete = false; - - // initialise waypoint and spline controller - wp_nav.wp_and_spline_init(); - - // RTL_SPEED == 0 means use WPNAV_SPEED - if (g.rtl_speed_cms != 0) { - wp_nav.set_speed_xy(g.rtl_speed_cms); - } - - // set the destination - if (!wp_nav.set_wp_destination(rtl_path.climb_target)) { - // this should not happen because rtl_build_path will have checked terrain data was available - Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION); - set_mode(LAND, MODE_REASON_TERRAIN_FAILSAFE); - return; - } - wp_nav.set_fast_waypoint(true); - - // hold current yaw during initial climb - set_auto_yaw_mode(AUTO_YAW_HOLD); -} - -// rtl_return_start - initialise return to home -void Sub::rtl_return_start() -{ - rtl_state = RTL_ReturnHome; - rtl_state_complete = false; - - if (!wp_nav.set_wp_destination(rtl_path.return_target)) { - // failure must be caused by missing terrain data, restart RTL - rtl_restart_without_terrain(); - } - - // initialise yaw to point home (maybe) - set_auto_yaw_mode(get_default_auto_yaw_mode(true)); -} - -// rtl_climb_return_run - implements the initial climb, return home and descent portions of RTL which all rely on the wp controller -// called by rtl_run at 100hz or more -void Sub::rtl_climb_return_run() -{ - // 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()) { - motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); - // multicopters do not stabilize roll/pitch/yaw when disarmed - // reset attitude control targets - attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); - - // To-Do: re-initialise wpnav targets - return; - } - - // process pilot's yaw input - float target_yaw_rate = 0; - if (!failsafe.radio) { - // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); - if (!is_zero(target_yaw_rate)) { - set_auto_yaw_mode(AUTO_YAW_HOLD); - } - } - - // set motors to full range - motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); - - // run waypoint controller - failsafe_terrain_set_status(wp_nav.update_wpnav()); - - // call z-axis position controller (wpnav should have already updated it's alt target) - pos_control.update_z_controller(); - - // call attitude controller - if (auto_yaw_mode == AUTO_YAW_HOLD) { - // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain()); - }else{ - // roll, pitch from waypoint controller, yaw heading from auto_heading() - attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true, get_smoothing_gain()); - } - - // check if we've completed this stage of RTL - rtl_state_complete = wp_nav.reached_wp_destination(); -} - -// rtl_loiterathome_start - initialise return to home -void Sub::rtl_loiterathome_start() -{ - rtl_state = RTL_LoiterAtHome; - rtl_state_complete = false; - rtl_loiter_start_time = millis(); - - // yaw back to initial take-off heading yaw unless pilot has already overridden yaw - if(get_default_auto_yaw_mode(true) != AUTO_YAW_HOLD) { - set_auto_yaw_mode(AUTO_YAW_RESETTOARMEDYAW); - } else { - set_auto_yaw_mode(AUTO_YAW_HOLD); - } -} - -// rtl_climb_return_descent_run - implements the initial climb, return home and descent portions of RTL which all rely on the wp controller -// called by rtl_run at 100hz or more -void Sub::rtl_loiterathome_run() -{ - // 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()) { - motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); - // multicopters do not stabilize roll/pitch/yaw when disarmed - // reset attitude control targets - attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); - - // To-Do: re-initialise wpnav targets - return; - } - - // process pilot's yaw input - float target_yaw_rate = 0; - if (!failsafe.radio) { - // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); - if (!is_zero(target_yaw_rate)) { - set_auto_yaw_mode(AUTO_YAW_HOLD); - } - } - - // set motors to full range - motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); - - // run waypoint controller - failsafe_terrain_set_status(wp_nav.update_wpnav()); - - // call z-axis position controller (wpnav should have already updated it's alt target) - pos_control.update_z_controller(); - - // call attitude controller - if (auto_yaw_mode == AUTO_YAW_HOLD) { - // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain()); - }else{ - // roll, pitch from waypoint controller, yaw heading from auto_heading() - attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true, get_smoothing_gain()); - } - - // check if we've completed this stage of RTL - if ((millis() - rtl_loiter_start_time) >= (uint32_t)g.rtl_loiter_time.get()) { - if (auto_yaw_mode == AUTO_YAW_RESETTOARMEDYAW) { - // check if heading is within 2 degrees of heading when vehicle was armed - if (labs(wrap_180_cd(ahrs.yaw_sensor-initial_armed_bearing)) <= 200) { - rtl_state_complete = true; - } - } else { - // we have loitered long enough - rtl_state_complete = true; - } - } -} - -// rtl_descent_start - initialise descent to final alt -void Sub::rtl_descent_start() -{ - rtl_state = RTL_FinalDescent; - rtl_state_complete = false; - - // Set wp navigation target to above home - wp_nav.init_loiter_target(wp_nav.get_wp_destination()); - - // initialise altitude target to stopping point - pos_control.set_target_to_stopping_point_z(); - - // initialise yaw - set_auto_yaw_mode(AUTO_YAW_HOLD); -} - -// rtl_descent_run - implements the final descent to the RTL_ALT -// called by rtl_run at 100hz or more -void Sub::rtl_descent_run() -{ - int16_t roll_control = 0, pitch_control = 0; - float target_yaw_rate = 0; - - // 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()) { - motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); - // multicopters do not stabilize roll/pitch/yaw when disarmed - attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); - - // set target to current position - wp_nav.init_loiter_target(); - return; - } - - // process pilot's input - if (!failsafe.radio) { - if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){ - Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT); - // exit land if throttle is high - if (!set_mode(POSHOLD, MODE_REASON_THROTTLE_LAND_ESCAPE)) { - set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE); - } - } - - if (g.land_repositioning) { - // apply SIMPLE mode transform to pilot inputs - update_simple_mode(); - - // process pilot's roll and pitch input - roll_control = channel_roll->control_in; - pitch_control = channel_pitch->control_in; - } - - // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); - } - - // set motors to full range - motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); - - // process roll, pitch inputs - wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control); - - // run loiter controller - wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); - - // call z-axis position controller - pos_control.set_alt_target_with_slew(rtl_path.descent_target.alt, G_Dt); - pos_control.update_z_controller(); - - // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain()); - - // check if we've reached within 20cm of final altitude - rtl_state_complete = fabsf(rtl_path.descent_target.alt - current_loc.alt) < 20.0f; -} - -// rtl_loiterathome_start - initialise controllers to loiter over home -void Sub::rtl_land_start() -{ - rtl_state = RTL_Land; - rtl_state_complete = false; - - // Set wp navigation target to above home - wp_nav.init_loiter_target(wp_nav.get_wp_destination()); - - // initialise altitude target to stopping point - pos_control.set_target_to_stopping_point_z(); - - // initialise yaw - set_auto_yaw_mode(AUTO_YAW_HOLD); -} - -// rtl_returnhome_run - return home -// called by rtl_run at 100hz or more -void Sub::rtl_land_run() -{ - int16_t roll_control = 0, pitch_control = 0; - float target_yaw_rate = 0; - // if not auto armed or landing completed or motor interlock not enabled set throttle to zero and exit immediately - if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) { - motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); - // multicopters do not stabilize roll/pitch/yaw when disarmed - attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); - - // set target to current position - wp_nav.init_loiter_target(); - -#if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED - // disarm when the landing detector says we've landed and throttle is at minimum -// if (ap.land_complete && (ap.throttle_zero || failsafe.radio)) { -// init_disarm_motors(); -// } -#else - // disarm when the landing detector says we've landed -// if (ap.land_complete) { -// init_disarm_motors(); -// } -#endif - - // check if we've completed this stage of RTL -// rtl_state_complete = ap.land_complete; - return; - } - - // relax loiter target if we might be landed - if (ap.land_complete_maybe) { - wp_nav.loiter_soften_for_landing(); - } - - // process pilot's input - if (!failsafe.radio) { - if (g.land_repositioning) { - // apply SIMPLE mode transform to pilot inputs - update_simple_mode(); - - // process pilot's roll and pitch input - roll_control = channel_roll->control_in; - pitch_control = channel_pitch->control_in; - } - - // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); - } - - // set motors to full range - motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); - - // process pilot's roll and pitch input - wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control); - - // run loiter controller - wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); - - // call z-axis position controller - float cmb_rate = get_land_descent_speed(); - pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt, true); - pos_control.update_z_controller(); - - // record desired climb rate for logging - desired_climb_rate = cmb_rate; - - // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain()); - - // check if we've completed this stage of RTL -// rtl_state_complete = ap.land_complete; -} - -void Sub::rtl_build_path(bool terrain_following_allowed) -{ - // origin point is our stopping point - Vector3f stopping_point; - pos_control.get_stopping_point_xy(stopping_point); - pos_control.get_stopping_point_z(stopping_point); - rtl_path.origin_point = Location_Class(stopping_point); - rtl_path.origin_point.change_alt_frame(Location_Class::ALT_FRAME_ABOVE_HOME); - - // set return target to nearest rally point or home position -#if AC_RALLY == ENABLED - rtl_path.return_target = rally.calc_best_rally_or_home_location(current_loc, ahrs.get_home().alt); -#else - rtl_path.return_target = ahrs.get_home(); -#endif - - // compute return altitude - rtl_compute_return_alt(rtl_path.origin_point, rtl_path.return_target, terrain_following_allowed); - - // climb target is above our origin point at the return altitude - rtl_path.climb_target = Location_Class(rtl_path.origin_point.lat, rtl_path.origin_point.lng, rtl_path.return_target.alt, rtl_path.return_target.get_alt_frame()); - - // descent target is below return target at rtl_alt_final - rtl_path.descent_target = Location_Class(rtl_path.return_target.lat, rtl_path.return_target.lng, g.rtl_alt_final, Location_Class::ALT_FRAME_ABOVE_HOME); - - // set land flag - rtl_path.land = g.rtl_alt_final <= 0; -} - -// return altitude in cm above home at which vehicle should return home -// rtl_origin_point is the stopping point of the vehicle when rtl is initiated -// rtl_return_target is the home or rally point that the vehicle is returning to. It's lat, lng and alt values must already have been filled in before this function is called -// rtl_return_target's altitude is updated to a higher altitude that the vehicle can safely return at (frame may also be set) -void Sub::rtl_compute_return_alt(const Location_Class &rtl_origin_point, Location_Class &rtl_return_target, bool terrain_following_allowed) -{ - float rtl_return_dist_cm = rtl_return_target.get_distance(rtl_origin_point) * 100.0f; - - // curr_alt is current altitude above home or above terrain depending upon use_terrain - int32_t curr_alt = current_loc.alt; - - // decide if we should use terrain altitudes - rtl_path.terrain_used = terrain_use() && terrain_following_allowed; - if (rtl_path.terrain_used) { - // attempt to retrieve terrain alt for current location, stopping point and origin - int32_t origin_terr_alt, return_target_terr_alt; - if (!rtl_origin_point.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, origin_terr_alt) || - !rtl_origin_point.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, return_target_terr_alt) || - !current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, curr_alt)) { - rtl_path.terrain_used = false; - Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA); - } - } - - // maximum of current altitude + climb_min and rtl altitude - float ret = MAX(curr_alt + MAX(0, g.rtl_climb_min), MAX(g.rtl_altitude, RTL_ALT_MIN)); - - // don't allow really shallow slopes - if (g.rtl_cone_slope >= RTL_MIN_CONE_SLOPE) { - ret = MAX(curr_alt, MIN(ret, MAX(rtl_return_dist_cm*g.rtl_cone_slope, curr_alt+RTL_ABS_MIN_CLIMB))); - } - -#if AC_FENCE == ENABLED - // ensure not above fence altitude if alt fence is enabled - // Note: we are assuming the fence alt is the same frame as ret - if ((fence.get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) != 0) { - ret = MIN(ret, fence.get_safe_alt()*100.0f); - } -#endif - - // ensure we do not descend - ret = MAX(ret, curr_alt); - - // convert return-target to alt-above-home or alt-above-terrain - if (!rtl_path.terrain_used || !rtl_return_target.change_alt_frame(Location_Class::ALT_FRAME_ABOVE_TERRAIN)) { - if (!rtl_return_target.change_alt_frame(Location_Class::ALT_FRAME_ABOVE_HOME)) { - // this should never happen but just in case - rtl_return_target.set_alt_cm(0, Location_Class::ALT_FRAME_ABOVE_HOME); - } - } - - // add ret to altitude - rtl_return_target.alt += ret; -} - diff --git a/ArduSub/control_stabilize.cpp b/ArduSub/control_stabilize.cpp index 1da6945bb7..805672a49d 100644 --- a/ArduSub/control_stabilize.cpp +++ b/ArduSub/control_stabilize.cpp @@ -5,10 +5,6 @@ // stabilize_init - initialise stabilize controller bool Sub::stabilize_init(bool ignore_checks) { - // if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high - if (motors.armed() && ap.land_complete && !mode_has_manual_throttle(control_mode) && (get_pilot_desired_throttle(channel_throttle->control_in) > get_non_takeoff_throttle())) { - return false; - } // set target altitude to zero for reporting pos_control.set_alt_target(0); last_pilot_heading = ahrs.yaw_sensor; @@ -39,13 +35,10 @@ void Sub::stabilize_run() // convert pilot input to lean angles // To-Do: convert get_pilot_desired_lean_angles to return angles as floats - get_pilot_desired_lean_angles(channel_roll->control_in, channel_pitch->control_in, target_roll, target_pitch, aparm.angle_max); + get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max); // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); - - // get pilot's desired throttle - pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->control_in); + target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); // call attitude controller // update attitude controller targets @@ -72,10 +65,10 @@ void Sub::stabilize_run() } // output pilot's throttle - attitude_control.set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt); + attitude_control.set_throttle_out(channel_throttle->norm_input(), false, g.throttle_filt); //control_in is range -1000-1000 //radio_in is raw pwm value - motors.set_forward(channel_forward->norm_input_dz()); - motors.set_lateral(channel_lateral->norm_input_dz()); + motors.set_forward(channel_forward->norm_input()); + motors.set_lateral(channel_lateral->norm_input()); } diff --git a/ArduSub/control_transect.cpp b/ArduSub/control_transect.cpp index 49014cbb13..17ebd8f1be 100644 --- a/ArduSub/control_transect.cpp +++ b/ArduSub/control_transect.cpp @@ -1,15 +1,51 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +// ArduSub transect flight mode +// Sub follows a line according to current crosstrack error supplied by XTE NMEA sentence +// Requires GPS providing crosstrack error +// Jacob Walser August 2016 #include "Sub.h" +#include "version.h" +#include "GCS_Mavlink.h" -/* - * control_sport.pde - init and run calls for sport flight mode - */ +#if TRANSECT_ENABLED == ENABLED -// sport_init - initialise sport controller -bool Sub::sport_init(bool ignore_checks) +namespace { + static uint32_t last_transect_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 + + // Heading PID controller update interval + uint32_t last_pid_ms = 0; + uint8_t pid_dt = 1000/20; +} + +// Initialize transect controller +bool Sub::transect_init(bool ignore_checks) { - // initialize vertical speed and acceleration + // fail to initialise transect 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); @@ -17,105 +53,198 @@ bool Sub::sport_init(bool ignore_checks) pos_control.set_alt_target(inertial_nav.get_altitude()); pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z()); + last_pilot_heading = ahrs.yaw_sensor; + des_velf = 0; + des_velr = 0; + des_velx = 0; + des_vely = 0; + return true; } -// sport_run - runs the sport controller // should be called at 100hz or more -void Sub::sport_run() +void Sub::transect_run() { - float target_roll_rate, target_pitch_rate, target_yaw_rate; - float target_climb_rate = 0; - float takeoff_climb_rate = 0.0f; + uint32_t tnow = millis(); - // initialize vertical speed 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); + // convert inertial nav earth-frame velocities to body-frame + const Vector3f& vel = inertial_nav.get_velocity(); + 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 targets + des_velf = 0; + des_velr = 0; + des_velx = 0; + des_vely = 0; + last_pilot_heading = ahrs.yaw_sensor; - // if not armed or throttle at zero, set throttle to zero and exit immediately - if (!motors.armed() || !motors.get_interlock()) { motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); + attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); - pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average); + 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; } - // apply SIMPLE mode transform - update_simple_mode(); + // apply SIMPLE mode transform to pilot inputs + update_simple_mode(); - // get pilot's desired roll and pitch rates + // set motors to full range + motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); - // calculate rate requests - target_roll_rate = channel_roll->control_in * g.acro_rp_p; - target_pitch_rate = channel_pitch->control_in * g.acro_rp_p; + // get pilot's desired yaw rate in centidegrees per second + //float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); + //int16_t xtrack_error = constrain_int16(-gps.crosstrack_error(), -4500, 4500); + int16_t xtrack_error = -channel_lateral->get_control_in() / 10; + double target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); - int32_t roll_angle = wrap_180_cd(ahrs.roll_sensor); - target_roll_rate -= constrain_int32(roll_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_roll; + // 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); - // Calculate trainer mode earth frame rate command for pitch - int32_t pitch_angle = wrap_180_cd(ahrs.pitch_sensor); - target_pitch_rate -= constrain_int32(pitch_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_pitch; + int16_t pilot_lateral = channel_lateral->get_control_in(); + int16_t pilot_forward = channel_forward->get_control_in(); - if (roll_angle > aparm.angle_max){ - target_roll_rate -= g.acro_rp_p*(roll_angle-aparm.angle_max); - }else if (roll_angle < -aparm.angle_max) { - target_roll_rate -= g.acro_rp_p*(roll_angle+aparm.angle_max); - } + float lateral_out = 0; + float forward_out = 0; - if (pitch_angle > aparm.angle_max){ - target_pitch_rate -= g.acro_rp_p*(pitch_angle-aparm.angle_max); - }else if (pitch_angle < -aparm.angle_max) { - target_pitch_rate -= g.acro_rp_p*(pitch_angle+aparm.angle_max); - } + // Pilot adjusts desired velocities to maintain during transect + if(pilot_lateral > 1000 || pilot_lateral < -1000 || pilot_forward > 1000 || pilot_forward < -1000) { - // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); + des_velf += pilot_forward * 0.0001; + des_velr += pilot_lateral * 0.0001; - // get pilot desired climb rate - target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->control_in); - target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max, g.pilot_velocity_z_max); + // 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); + } - // get takeoff adjusted pilot and takeoff climb rates - takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate); + // rotate pilot desired velocities to earth-frame - // check for take-off - if (ap.land_complete && (takeoff_state.running || (channel_throttle->control_in > get_takeoff_trigger_throttle()))) { - if (!takeoff_state.running) { - takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f)); - } + // 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 - // indicate we are taking off - set_land_complete(false); - // clear i term when we're taking off - set_throttle_takeoff(); - } + // 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 - // reset target lean angles and heading while landed - if (ap.land_complete) { - if (ap.throttle_zero) { - motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); - }else{ - motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); - } - // move throttle to between minimum and non-takeoff-throttle to keep us on the ground - attitude_control.set_throttle_out(get_throttle_pre_takeoff(channel_throttle->control_in),false,g.throttle_filt); - pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average); - }else{ - // set motors to full range - motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + //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 - // call attitude controller - attitude_control.input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate); + // set target position and velocity to current position and velocity + pos_control.set_desired_velocity_xy(des_velx, des_vely); - // 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); - } + // run position controller + pos_control.update_xy_controller(AC_PosControl::XY_MODE_POS_AND_VEL_FF, ekfNavVelGainScaler, false); - // call position controller - pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); - pos_control.add_takeoff_climb_rate(takeoff_climb_rate, G_Dt); - pos_control.update_z_controller(); - } + // 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); + + lateral_out = poscontrol_lateral/(float)aparm.angle_max; + forward_out = poscontrol_forward/(float)aparm.angle_max; + + 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); + + int32_t error_heading = 0; + if(!is_zero(target_yaw_rate)) { + // Allow pilot to set approximate heading to maintain during transect + last_pilot_heading = ahrs.yaw_sensor; + last_pilot_yaw_input_ms = tnow; + + } else { + // Set target heading after we have slowed to a stop after pilot input + if(tnow < last_pilot_yaw_input_ms + 250) { + target_yaw_rate = 0; + last_pilot_heading = ahrs.yaw_sensor; + } else { + // We are holding the current heading + error_heading = last_pilot_heading - ahrs.yaw_sensor; + + // Wrap error 0~360 degrees + if(error_heading > 18000) { + error_heading = error_heading - 36000; + } else if(error_heading < -18000) { + error_heading = error_heading + 36000; + } + + // Adjust heading to correct for crosstrack error + target_yaw_rate = g.pid_heading_control.get_pid() + g.pid_crosstrack_control.get_pid(); + } + } + + // Update crosstrack and heading controllers + if(tnow > last_pid_ms + pid_dt) { + last_pid_ms = tnow; + g.pid_heading_control.set_input_filter_all(error_heading); + //g.pid_crosstrack_control.set_input_filter_all(-channel_lateral->get_control_in()); + g.pid_crosstrack_control.set_input_filter_all(xtrack_error); + } + + // 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_transect_message_ms + 200) { + last_transect_message_ms = tnow; + mavlink_msg_command_long_send( + (mavlink_channel_t)0, //channel + 0, //target system + 0, //target component + 47, //command id + 0, //confirmation + des_velf,//1 + des_velr, + vel_fw, + vel_right, + forward_out, + lateral_out, + poscontrol_forward + ); + //gcs_send_text_fmt(MAV_SEVERITY_INFO, "%ld, %ld, %ld, %f, %d", error_heading, ahrs.yaw_sensor, last_pilot_heading, target_yaw_rate, channel_lateral->get_control_in()); + //gcs_send_text_fmt(MAV_SEVERITY_INFO, "%f, %f", g.pid_heading_control.get_pid(), g.pid_crosstrack_control.get_pid()); + //gcs_send_text_fmt(MAV_SEVERITY_INFO, "%f, %ld, %ld, %f, %d", vel_fw, ahrs.yaw_sensor, last_pilot_heading, des_velf, gps.crosstrack_error()); + } } +#endif diff --git a/ArduSub/control_velhold.cpp b/ArduSub/control_velhold.cpp index 5c0b2808b0..2a627ebf92 100644 --- a/ArduSub/control_velhold.cpp +++ b/ArduSub/control_velhold.cpp @@ -1,164 +1,227 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +// 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" -/* - * control_loiter.pde - init and run calls for loiter flight mode - */ +#if POSHOLD_ENABLED == ENABLED -// loiter_init - initialise loiter controller -bool Sub::loiter_init(bool ignore_checks) -{ - if (position_ok() || ignore_checks) { - - // set target to current position - wp_nav.init_loiter_target(); - - // initialize vertical speed 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()); - - return true; - }else{ - return false; - } +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 } -// loiter_run - runs the loiter controller -// should be called at 100hz or more -void Sub::loiter_run() +// Initialize the VelHold controller +bool Sub::velhold_init(bool ignore_checks) { - LoiterModeState loiter_state; - float target_yaw_rate = 0.0f; - float target_climb_rate = 0.0f; - float takeoff_climb_rate = 0.0f; + // fail to initialise VelHold mode if no GPS lock + if (!position_ok() && !ignore_checks) { + return false; + } - // initialize vertical speed and acceleration + 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); - // process pilot inputs unless we are in radio failsafe - if (!failsafe.radio) { - // apply SIMPLE mode transform to pilot inputs - update_simple_mode(); + // 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()); - // process pilot's roll and pitch input - wp_nav.set_pilot_desired_acceleration(channel_roll->control_in, channel_pitch->control_in); + des_velf = 0; + des_velr = 0; + des_velx = 0; + des_vely = 0; - // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); - - // get pilot desired climb rate - target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->control_in); - target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max, g.pilot_velocity_z_max); - } else { - // clear out pilot desired acceleration in case radio failsafe event occurs and we do not switch to RTL for some reason - wp_nav.clear_pilot_desired_acceleration(); - } - - // relax loiter target if we might be landed - if (ap.land_complete_maybe) { - wp_nav.loiter_soften_for_landing(); - } - - // Loiter State Machine Determination - if(!ap.auto_armed) { - loiter_state = Loiter_Disarmed; - } else if (!motors.get_interlock()){ - loiter_state = Loiter_MotorStop; - } else if (takeoff_state.running || (ap.land_complete && (channel_throttle->control_in > get_takeoff_trigger_throttle()))){ - loiter_state = Loiter_Takeoff; - } else if (ap.land_complete){ - loiter_state = Loiter_Landed; - } else { - loiter_state = Loiter_Flying; - } - - // Loiter State Machine - switch (loiter_state) { - - case Loiter_Disarmed: - wp_nav.init_loiter_target(); - motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); - // multicopters do not stabilize roll/pitch/yaw when disarmed - attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); - - pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average); - break; - - case Loiter_MotorStop: - motors.set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN); - wp_nav.init_loiter_target(); - // multicopters do not stabilize roll/pitch/yaw when motors are stopped - attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); - pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average); - break; - - case Loiter_Takeoff: - - if (!takeoff_state.running) { - takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f)); - // indicate we are taking off - set_land_complete(false); - // clear i term when we're taking off - set_throttle_takeoff(); - } - - // get takeoff adjusted pilot and takeoff climb rates - takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate); - - // set motors to full range - motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); - - // run loiter controller - wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); - - // call attitude controller - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate); - - // update altitude target and call position controller - pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); - pos_control.add_takeoff_climb_rate(takeoff_climb_rate, G_Dt); - pos_control.update_z_controller(); - break; - - case Loiter_Landed: - - wp_nav.init_loiter_target(); - // if throttle zero reset attitude and exit immediately - if (ap.throttle_zero) { - motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); - }else{ - motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); - } - // multicopters do not stabilize roll/pitch/yaw when disarmed - attitude_control.set_throttle_out(get_throttle_pre_takeoff(channel_throttle->control_in),false,g.throttle_filt); - pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average); - break; - - case Loiter_Flying: - // set motors to full range - motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); - - // run loiter controller - wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); - - // call attitude controller - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate); - - // 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); - } - - // update altitude target and call position controller - pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); - pos_control.update_z_controller(); - break; - } + 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; + } + + // apply SIMPLE mode transform to pilot inputs + update_simple_mode(); + + // 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/crash_check.cpp b/ArduSub/crash_check.cpp index 1c705a6031..261c8c01f6 100644 --- a/ArduSub/crash_check.cpp +++ b/ArduSub/crash_check.cpp @@ -4,7 +4,7 @@ // Code to detect a crash main ArduCopter code #define CRASH_CHECK_TRIGGER_SEC 2 // 2 seconds inverted indicates a crash -#define CRASH_CHECK_ANGLE_DEVIATION_CD 3000.0f // 30 degrees beyond angle max is signal we are inverted +#define CRASH_CHECK_ANGLE_DEVIATION_DEG 30.0f // 30 degrees beyond angle max is signal we are inverted #define CRASH_CHECK_ACCEL_MAX 3.0f // vehicle must be accelerating less than 3m/s/s to be considered crashed // crash_check - disarms motors if a crash has been detected @@ -33,8 +33,8 @@ void Sub::crash_check() } // check for angle error over 30 degrees - const Vector3f angle_error = attitude_control.get_att_error_rot_vec_cd(); - if (pythagorous2(angle_error.x, angle_error.y) <= CRASH_CHECK_ANGLE_DEVIATION_CD) { + const float angle_error = attitude_control.get_att_error_angle_deg(); + if (angle_error <= CRASH_CHECK_ANGLE_DEVIATION_DEG) { crash_counter = 0; return; } @@ -43,7 +43,7 @@ void Sub::crash_check() crash_counter++; // check if crashing for 2 seconds - if (crash_counter >= (CRASH_CHECK_TRIGGER_SEC * MAIN_LOOP_RATE)) { + if (crash_counter >= (CRASH_CHECK_TRIGGER_SEC * scheduler.get_loop_rate_hz())) { // log an error in the dataflash Log_Write_Error(ERROR_SUBSYSTEM_CRASH_CHECK, ERROR_CODE_CRASH_CHECK_CRASH); // send message to gcs @@ -52,133 +52,3 @@ void Sub::crash_check() init_disarm_motors(); } } - -#if PARACHUTE == ENABLED - -// Code to detect a crash main ArduCopter code -#define PARACHUTE_CHECK_TRIGGER_SEC 1 // 1 second of loss of control triggers the parachute -#define PARACHUTE_CHECK_ANGLE_DEVIATION_CD 3000 // 30 degrees off from target indicates a loss of control - -// parachute_check - disarms motors and triggers the parachute if serious loss of control has been detected -// vehicle is considered to have a "serious loss of control" by the vehicle being more than 30 degrees off from the target roll and pitch angles continuously for 1 second -// called at MAIN_LOOP_RATE -void Sub::parachute_check() -{ - static uint16_t control_loss_count; // number of iterations we have been out of control - static int32_t baro_alt_start; - - // exit immediately if parachute is not enabled - if (!parachute.enabled()) { - return; - } - - // call update to give parachute a chance to move servo or relay back to off position - parachute.update(); - - // return immediately if motors are not armed or pilot's throttle is above zero - if (!motors.armed()) { - control_loss_count = 0; - return; - } - - // return immediately if we are not in an angle stabilize flight mode or we are flipping - if (control_mode == ACRO || control_mode == FLIP) { - control_loss_count = 0; - return; - } - - // ensure we are flying - if (ap.land_complete) { - control_loss_count = 0; - return; - } - - // ensure the first control_loss event is from above the min altitude - if (control_loss_count == 0 && parachute.alt_min() != 0 && (current_loc.alt < (int32_t)parachute.alt_min() * 100)) { - return; - } - - // check for angle error over 30 degrees - const Vector3f angle_error = attitude_control.get_att_error_rot_vec_cd(); - if (pythagorous2(angle_error.x, angle_error.y) <= CRASH_CHECK_ANGLE_DEVIATION_CD) { - control_loss_count = 0; - return; - } - - // increment counter - if (control_loss_count < (PARACHUTE_CHECK_TRIGGER_SEC*MAIN_LOOP_RATE)) { - control_loss_count++; - } - - // record baro alt if we have just started losing control - if (control_loss_count == 1) { - baro_alt_start = baro_alt; - - // exit if baro altitude change indicates we are not falling - } else if (baro_alt >= baro_alt_start) { - control_loss_count = 0; - return; - - // To-Do: add check that the vehicle is actually falling - - // check if loss of control for at least 1 second - } else if (control_loss_count >= (PARACHUTE_CHECK_TRIGGER_SEC*MAIN_LOOP_RATE)) { - // reset control loss counter - control_loss_count = 0; - // log an error in the dataflash - Log_Write_Error(ERROR_SUBSYSTEM_CRASH_CHECK, ERROR_CODE_CRASH_CHECK_LOSS_OF_CONTROL); - // release parachute - parachute_release(); - } -} - -// parachute_release - trigger the release of the parachute, disarm the motors and notify the user -void Sub::parachute_release() -{ - // send message to gcs and dataflash - gcs_send_text(MAV_SEVERITY_INFO,"Parachute: Released"); - Log_Write_Event(DATA_PARACHUTE_RELEASED); - - // disarm motors - init_disarm_motors(); - - // release parachute - parachute.release(); - - // deploy landing gear - landinggear.set_cmd_mode(LandingGear_Deploy); -} - -// parachute_manual_release - trigger the release of the parachute, after performing some checks for pilot error -// checks if the vehicle is landed -void Sub::parachute_manual_release() -{ - // exit immediately if parachute is not enabled - if (!parachute.enabled()) { - return; - } - - // do not release if vehicle is landed - // do not release if we are landed or below the minimum altitude above home - if (ap.land_complete) { - // warn user of reason for failure - gcs_send_text(MAV_SEVERITY_INFO,"Parachute: Landed"); - // log an error in the dataflash - Log_Write_Error(ERROR_SUBSYSTEM_PARACHUTE, ERROR_CODE_PARACHUTE_LANDED); - return; - } - - // do not release if we are landed or below the minimum altitude above home - if ((parachute.alt_min() != 0 && (current_loc.alt < (int32_t)parachute.alt_min() * 100))) { - // warn user of reason for failure - gcs_send_text(MAV_SEVERITY_ALERT,"Parachute: Too low"); - // log an error in the dataflash - Log_Write_Error(ERROR_SUBSYSTEM_PARACHUTE, ERROR_CODE_PARACHUTE_TOO_LOW); - return; - } - - // if we get this far release parachute - parachute_release(); -} - -#endif // PARACHUTE == ENABLED diff --git a/ArduSub/defines.h b/ArduSub/defines.h index 4b41a664b1..bdda1fd05f 100644 --- a/ArduSub/defines.h +++ b/ArduSub/defines.h @@ -190,7 +190,6 @@ enum tuning_func { // Auto modes enum AutoMode { - Auto_TakeOff, Auto_WP, Auto_Land, // Auto_RTL, @@ -204,7 +203,6 @@ enum AutoMode { // Guided modes enum GuidedMode { - Guided_TakeOff, Guided_WP, Guided_Velocity, Guided_PosVel, @@ -222,18 +220,16 @@ enum RTLState { // Alt_Hold states enum AltHoldModeState { - AltHold_Disarmed, - AltHold_MotorStop, - AltHold_Takeoff, + AltHold_MotorStopped, + AltHold_NotAutoArmed, AltHold_Flying, AltHold_Landed }; // Loiter states enum LoiterModeState { - Loiter_Disarmed, - Loiter_MotorStop, - Loiter_Takeoff, + Loiter_MotorStopped, + Loiter_NotAutoArmed, Loiter_Flying, Loiter_Landed }; @@ -284,7 +280,6 @@ enum LoiterModeState { #define MASK_LOG_COMPASS (1<<13) #define MASK_LOG_INAV (1<<14) // deprecated #define MASK_LOG_CAMERA (1<<15) -#define MASK_LOG_WHEN_DISARMED (1UL<<16) #define MASK_LOG_MOTBATT (1UL<<17) #define MASK_LOG_IMU_FAST (1UL<<18) #define MASK_LOG_IMU_RAW (1UL<<19) diff --git a/ArduSub/esc_calibration.cpp b/ArduSub/esc_calibration.cpp index a241e08375..9134bfd0fc 100644 --- a/ArduSub/esc_calibration.cpp +++ b/ArduSub/esc_calibration.cpp @@ -34,7 +34,7 @@ void Sub::esc_calibration_startup_check() switch (g.esc_calibrate) { case ESCCAL_NONE: // check if throttle is high - if (channel_throttle->control_in >= ESC_CALIBRATION_HIGH_THROTTLE) { + if (channel_throttle->get_control_in() >= ESC_CALIBRATION_HIGH_THROTTLE) { // we will enter esc_calibrate mode on next reboot g.esc_calibrate.set_and_save(ESCCAL_PASSTHROUGH_IF_THROTTLE_HIGH); // send message to gcs @@ -42,12 +42,12 @@ void Sub::esc_calibration_startup_check() // turn on esc calibration notification AP_Notify::flags.esc_calibration = true; // block until we restart - while(1) { delay(5); } + while(1) { hal.scheduler->delay(5); } } break; case ESCCAL_PASSTHROUGH_IF_THROTTLE_HIGH: // check if throttle is high - if (channel_throttle->control_in >= ESC_CALIBRATION_HIGH_THROTTLE) { + if (channel_throttle->get_control_in() >= ESC_CALIBRATION_HIGH_THROTTLE) { // pass through pilot throttle to escs esc_calibration_passthrough(); } @@ -94,11 +94,10 @@ void Sub::esc_calibration_passthrough() // read pilot input read_radio(); - delay(10); + hal.scheduler->delay(10); // pass through to motors - motors.throttle_pass_through(channel_throttle->radio_in); - } + motors.set_throttle_passthrough_for_esc_calibration(channel_throttle->get_control_in() / 1000.0f); } } // esc_calibration_auto - calibrate the ESCs automatically using a timer and no pilot input @@ -120,8 +119,8 @@ void Sub::esc_calibration_auto() AP_Notify::flags.esc_calibration = true; // raise throttle to maximum - delay(10); - motors.throttle_pass_through(channel_throttle->radio_max); + hal.scheduler->delay(10); + motors.set_throttle_passthrough_for_esc_calibration(1.0f); // wait for safety switch to be pressed while (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) { @@ -129,18 +128,18 @@ void Sub::esc_calibration_auto() gcs_send_text(MAV_SEVERITY_INFO,"ESC calibration: Push safety switch"); printed_msg = true; } - delay(10); + hal.scheduler->delay(10); } // delay for 5 seconds - delay(5000); + hal.scheduler->delay(5000); // reduce throttle to minimum - motors.throttle_pass_through(channel_throttle->radio_min); + motors.set_throttle_passthrough_for_esc_calibration(0.0f); // clear esc parameter g.esc_calibrate.set_and_save(ESCCAL_NONE); // block until we restart - while(1) { delay(5); } + while(1) { hal.scheduler->delay(5); } } diff --git a/ArduSub/flight_mode.cpp b/ArduSub/flight_mode.cpp index a647abdda4..05e71041bd 100644 --- a/ArduSub/flight_mode.cpp +++ b/ArduSub/flight_mode.cpp @@ -203,13 +203,6 @@ void Sub::exit_mode(control_mode_t old_control_mode, control_mode_t new_control_ camera_mount.set_mode_to_default(); #endif // MOUNT == ENABLED } - - // smooth throttle transition when switching from manual to automatic flight modes - if (mode_has_manual_throttle(old_control_mode) && !mode_has_manual_throttle(new_control_mode) && motors.armed()) { - // this assumes all manual flight modes use get_pilot_desired_throttle to translate pilot input to output throttle - set_accel_throttle_I_from_pilot_throttle(get_pilot_desired_throttle(channel_throttle->control_in)); - } - } // returns true or false whether mode requires GPS diff --git a/ArduSub/motor_test.cpp b/ArduSub/motor_test.cpp index 844a72b130..d23a38bdb2 100644 --- a/ArduSub/motor_test.cpp +++ b/ArduSub/motor_test.cpp @@ -38,9 +38,6 @@ void Sub::motor_test_output() case MOTOR_TEST_THROTTLE_PERCENT: // sanity check motor_test_throttle value - if (motor_test_throttle_value <= 100) { - pwm = channel_throttle->radio_min + (channel_throttle->radio_max - channel_throttle->radio_min) * (float)motor_test_throttle_value/100.0f; - } break; case MOTOR_TEST_THROTTLE_PWM: @@ -48,7 +45,7 @@ void Sub::motor_test_output() break; case MOTOR_TEST_THROTTLE_PILOT: - pwm = channel_throttle->radio_in; + pwm = channel_throttle->get_radio_in(); break; default: diff --git a/ArduSub/motors.cpp b/ArduSub/motors.cpp index 80bdd352cf..eabb5dc982 100644 --- a/ArduSub/motors.cpp +++ b/ArduSub/motors.cpp @@ -7,128 +7,33 @@ #define AUTO_TRIM_DELAY 100 // called at 10hz so 10 seconds #define LOST_VEHICLE_DELAY 10 // called at 10hz so 1 second -static uint32_t auto_disarm_begin; +//static uint32_t auto_disarm_begin; -// arm_motors_check - checks for pilot input to arm or disarm the copter -// called at 10hz -void Sub::arm_motors_check() -{ -<<<<<<< 6dafedb2d1ad5061d859a9c319fa4b69b4ac5dd9 - // Arm motors automatically - if ( !motors.armed() ) { - init_arm_motors(false); - } - - /*static int16_t arming_counter; -======= - static int16_t arming_counter; ->>>>>>> Changed to ArduCopter as the base code. - - // ensure throttle is down - if (channel_throttle->control_in > 0) { - arming_counter = 0; - return; - } - - int16_t tmp = channel_yaw->control_in; - - // full right - if (tmp > 4000) { - - // increase the arming counter to a maximum of 1 beyond the auto trim counter - if( arming_counter <= AUTO_TRIM_DELAY ) { - arming_counter++; - } - - // arm the motors and configure for flight - if (arming_counter == ARM_DELAY && !motors.armed()) { - // reset arming counter if arming fail - if (!init_arm_motors(false)) { - arming_counter = 0; - } - } - - // arm the motors and configure for flight - if (arming_counter == AUTO_TRIM_DELAY && motors.armed() && control_mode == STABILIZE) { - auto_trim_counter = 250; - // ensure auto-disarm doesn't trigger immediately - auto_disarm_begin = millis(); - } - - // full left - }else if (tmp < -4000) { - if (!mode_has_manual_throttle(control_mode) && !ap.land_complete) { - arming_counter = 0; - return; - } - - // increase the counter to a maximum of 1 beyond the disarm delay - if( arming_counter <= DISARM_DELAY ) { - arming_counter++; - } - - // disarm the motors - if (arming_counter == DISARM_DELAY && motors.armed()) { - init_disarm_motors(); - } - - // Yaw is centered so reset arming counter - }else{ - arming_counter = 0; -<<<<<<< 6dafedb2d1ad5061d859a9c319fa4b69b4ac5dd9 - }*/ -======= - } ->>>>>>> Changed to ArduCopter as the base code. -} - -// auto_disarm_check - disarms the copter if it has been sitting on the ground in manual mode with throttle low for at least 15 seconds +// auto_disarm_check +// Automatically disarm the vehicle under some set of conditions +// What those conditions should be TBD void Sub::auto_disarm_check() { -<<<<<<< 6dafedb2d1ad5061d859a9c319fa4b69b4ac5dd9 - /*uint32_t tnow_ms = millis(); -======= - uint32_t tnow_ms = millis(); ->>>>>>> Changed to ArduCopter as the base code. - uint32_t disarm_delay_ms = 1000*constrain_int16(g.disarm_delay, 0, 127); + // Disable for now - // exit immediately if we are already disarmed, or if auto - // disarming is disabled - if (!motors.armed() || disarm_delay_ms == 0) { - auto_disarm_begin = tnow_ms; - return; - } - - // always allow auto disarm if using interlock switch or motors are Emergency Stopped - if ((ap.using_interlock && !motors.get_interlock()) || ap.motor_emergency_stop) { - // use a shorter delay if using throttle interlock switch or Emergency Stop, because it is less - // obvious the copter is armed as the motors will not be spinning - disarm_delay_ms /= 2; - } else { - bool sprung_throttle_stick = (g.throttle_behavior & THR_BEHAVE_FEEDBACK_FROM_MID_STICK) != 0; - bool thr_low; - if (mode_has_manual_throttle(control_mode) || !sprung_throttle_stick) { - thr_low = ap.throttle_zero; - } else { - float deadband_top = g.rc_3.get_control_mid() + g.throttle_deadzone; - thr_low = g.rc_3.control_in <= deadband_top; - } - - if (!thr_low) { - // reset timer - auto_disarm_begin = tnow_ms; - } - } - - // disarm once timer expires - if ((tnow_ms-auto_disarm_begin) >= disarm_delay_ms) { - init_disarm_motors(); - auto_disarm_begin = tnow_ms; -<<<<<<< 6dafedb2d1ad5061d859a9c319fa4b69b4ac5dd9 - }*/ -======= - } ->>>>>>> Changed to ArduCopter as the base code. +// uint32_t tnow_ms = millis(); +// uint32_t disarm_delay_ms = 1000*constrain_int16(g.disarm_delay, 0, 127); +// +// // exit immediately if we are already disarmed, or if auto +// // disarming is disabled +// if (!motors.armed() || disarm_delay_ms == 0) { +// auto_disarm_begin = tnow_ms; +// return; +// } +// +// if(!mode_has_manual_throttle(control_mode) || !ap.throttle_zero) { +// auto_disarm_begin = tnow_ms; +// } +// +// if(tnow > auto_disarm_begin + disarm_delay_ms) { +// init_disarm_motors(); +// auto_disarm_begin = tnow_ms; +// } } // init_arm_motors - performs arming process including initialisation of barometer and gyros @@ -251,7 +156,7 @@ void Sub::init_disarm_motors() mission.reset(); // suspend logging - if (!(g.log_bitmask & MASK_LOG_WHEN_DISARMED)) { + if (!DataFlash.log_while_disarmed()) { DataFlash.EnableWrites(false); } @@ -289,7 +194,7 @@ void Sub::lost_vehicle_check() } // ensure throttle is down, motors not armed, pitch and roll rc at max. Note: rc1=roll rc2=pitch - if (ap.throttle_zero && !motors.armed() && (channel_roll->control_in > 4000) && (channel_pitch->control_in > 4000)) { + if (ap.throttle_zero && !motors.armed() && (channel_roll->get_control_in() > 4000) && (channel_pitch->get_control_in() > 4000)) { if (soundalarm_counter >= LOST_VEHICLE_DELAY) { if (AP_Notify::flags.vehicle_lost == false) { AP_Notify::flags.vehicle_lost = true; diff --git a/ArduSub/position_vector.cpp b/ArduSub/position_vector.cpp index 305c223b4e..ea641d820b 100644 --- a/ArduSub/position_vector.cpp +++ b/ArduSub/position_vector.cpp @@ -44,7 +44,7 @@ float Sub::pv_get_bearing_cd(const Vector3f &origin, const Vector3f &destination // pv_get_horizontal_distance_cm - return distance between two positions in cm float Sub::pv_get_horizontal_distance_cm(const Vector3f &origin, const Vector3f &destination) { - return pythagorous2(destination.x-origin.x,destination.y-origin.y); + return norm(destination.x-origin.x,destination.y-origin.y); } // returns distance between a destination and home in cm diff --git a/ArduSub/radio.cpp b/ArduSub/radio.cpp index f1a8545d43..9bc9fa554f 100644 --- a/ArduSub/radio.cpp +++ b/ArduSub/radio.cpp @@ -18,45 +18,24 @@ void Sub::default_dead_zones() void Sub::init_rc_in() { - channel_roll = RC_Channel::rc_channel(rcmap.roll()-1); - channel_pitch = RC_Channel::rc_channel(rcmap.pitch()-1); - channel_throttle = RC_Channel::rc_channel(rcmap.throttle()-1); - channel_yaw = RC_Channel::rc_channel(rcmap.yaw()-1); -<<<<<<< 6dafedb2d1ad5061d859a9c319fa4b69b4ac5dd9 - channel_forward = RC_Channel::rc_channel(rcmap.forward()-1); - channel_strafe = RC_Channel::rc_channel(rcmap.strafe()-1); -======= ->>>>>>> Changed to ArduCopter as the base code. + channel_pitch = RC_Channels::rc_channel(0); + channel_roll = RC_Channels::rc_channel(1); + channel_throttle = RC_Channels::rc_channel(2); + channel_yaw = RC_Channels::rc_channel(3); + channel_forward = RC_Channels::rc_channel(5); + channel_lateral = RC_Channels::rc_channel(6); // set rc channel ranges channel_roll->set_angle(ROLL_PITCH_INPUT_MAX); channel_pitch->set_angle(ROLL_PITCH_INPUT_MAX); - channel_yaw->set_angle(4500); - channel_throttle->set_range(g.throttle_min, THR_MAX); -<<<<<<< 6dafedb2d1ad5061d859a9c319fa4b69b4ac5dd9 - channel_forward->set_angle(4500); - channel_strafe->set_angle(4500); -======= ->>>>>>> Changed to ArduCopter as the base code. + channel_yaw->set_angle(ROLL_PITCH_INPUT_MAX); + channel_throttle->set_range(1000); + channel_forward->set_angle(ROLL_PITCH_INPUT_MAX); + channel_lateral->set_angle(ROLL_PITCH_INPUT_MAX); - channel_roll->set_type(RC_CHANNEL_TYPE_ANGLE_RAW); - channel_pitch->set_type(RC_CHANNEL_TYPE_ANGLE_RAW); - channel_yaw->set_type(RC_CHANNEL_TYPE_ANGLE_RAW); -<<<<<<< 6dafedb2d1ad5061d859a9c319fa4b69b4ac5dd9 - channel_forward->set_type(RC_CHANNEL_TYPE_ANGLE_RAW); - channel_strafe->set_type(RC_CHANNEL_TYPE_ANGLE_RAW); -======= ->>>>>>> Changed to ArduCopter as the base code. - - for(int i = 0; i < 7; i++) { - RC_Channel *ch = RC_Channel::rc_channel(i); - ch->set_radio_max(1900); - ch->set_radio_min(1100); - ch->set_radio_trim(1500); - ch->save_eeprom(); - } - - RC_Channel::scale_dead_zones(JOYSTICK_INITIAL_GAIN); + // force throttle trim to 1100 + channel_throttle->set_radio_trim(1100); + channel_throttle->save_eeprom(); //set auxiliary servo ranges // g.rc_5.set_range(0,1000); @@ -84,16 +63,17 @@ void Sub::init_rc_in() void Sub::init_rc_out() { motors.set_update_rate(g.rc_speed); - motors.set_frame_orientation(g.frame_orientation); - motors.Init(); // motor initialisation + motors.set_loop_rate(scheduler.get_loop_rate_hz()); + motors.init((AP_Motors::motor_frame_class)g.frame_configuration.get(), (AP_Motors::motor_frame_type)0); for(uint8_t i = 0; i < 5; i++) { hal.scheduler->delay(20); read_radio(); } - // we want the input to be scaled correctly - channel_throttle->set_range_out(0,1000); + // setup correct scaling for ESCs like the UAVCAN PX4ESC which + // take a proportion of speed. + hal.rcout->set_esc_scaling(channel_throttle->get_radio_min(), channel_throttle->get_radio_max()); // check if we should enter esc calibration mode esc_calibration_startup_check(); @@ -105,11 +85,7 @@ void Sub::init_rc_out() } // refresh auxiliary channel to function map - RC_Channel_aux::update_aux_servo_function(); - - // setup correct scaling for ESCs like the UAVCAN PX4ESC which - // take a proportion of speed. - hal.rcout->set_esc_scaling(channel_throttle->radio_min, channel_throttle->radio_max); + SRV_Channels::update_aux_servo_function(); } // enable_motor_output() - enable and output lowest possible value to motors @@ -126,12 +102,10 @@ void Sub::read_radio() uint32_t tnow_ms = millis(); if (hal.rcin->new_input()) { - last_update_ms = tnow_ms; ap.new_radio_frame = true; - RC_Channel::set_pwm_all(); + RC_Channels::set_pwm_all(); - set_throttle_and_failsafe(channel_throttle->radio_in); - set_throttle_zero_flag(channel_throttle->control_in); + set_throttle_zero_flag(channel_throttle->get_control_in()); // flag we must have an rc receiver attached if (!failsafe.rc_override_active) { @@ -139,57 +113,14 @@ void Sub::read_radio() } // update output on any aux channels, for manual passthru - RC_Channel_aux::output_ch_all(); - }else{ - uint32_t elapsed = tnow_ms - last_update_ms; - // turn on throttle failsafe if no update from the RC Radio for 500ms or 2000ms if we are using RC_OVERRIDE - if (((!failsafe.rc_override_active && (elapsed >= FS_RADIO_TIMEOUT_MS)) || (failsafe.rc_override_active && (elapsed >= FS_RADIO_RC_OVERRIDE_TIMEOUT_MS))) && - (g.failsafe_throttle && (ap.rc_receiver_present||motors.armed()) && !failsafe.radio)) { - Log_Write_Error(ERROR_SUBSYSTEM_RADIO, ERROR_CODE_RADIO_LATE_FRAME); - set_failsafe_radio(true); - } - } -} + SRV_Channels::output_ch_all(); -#define FS_COUNTER 3 // radio failsafe kicks in after 3 consecutive throttle values below failsafe_throttle_value -void Sub::set_throttle_and_failsafe(uint16_t throttle_pwm) -{ - // if failsafe not enabled pass through throttle and exit - if(g.failsafe_throttle == FS_THR_DISABLED) { - channel_throttle->set_pwm(throttle_pwm); - return; - } + // pass pilot input through to motors (used to allow wiggling servos while disarmed on heli, single, coax copters) + radio_passthrough_to_motors(); - //check for low throttle value - if (throttle_pwm < (uint16_t)g.failsafe_throttle_value) { - - // if we are already in failsafe or motors not armed pass through throttle and exit - if (failsafe.radio || !(ap.rc_receiver_present || motors.armed())) { - channel_throttle->set_pwm(throttle_pwm); - return; - } - - // check for 3 low throttle values - // Note: we do not pass through the low throttle until 3 low throttle values are recieved - failsafe.radio_counter++; - if( failsafe.radio_counter >= FS_COUNTER ) { - failsafe.radio_counter = FS_COUNTER; // check to ensure we don't overflow the counter - set_failsafe_radio(true); - channel_throttle->set_pwm(throttle_pwm); // pass through failsafe throttle - } - }else{ - // we have a good throttle so reduce failsafe counter - failsafe.radio_counter--; - if( failsafe.radio_counter <= 0 ) { - failsafe.radio_counter = 0; // check to ensure we don't underflow the counter - - // disengage failsafe after three (nearly) consecutive valid throttle values - if (failsafe.radio) { - set_failsafe_radio(false); - } - } - // pass through throttle - channel_throttle->set_pwm(throttle_pwm); + float dt = (tnow_ms - last_update_ms)*1.0e-3f; + rc_throttle_control_in_filter.apply(channel_throttle->get_control_in(), dt); + last_update_ms = tnow_ms; } } @@ -213,3 +144,9 @@ void Sub::set_throttle_zero_flag(int16_t throttle_control) ap.throttle_zero = true; } } + +// pass pilot's inputs to motors library (used to allow wiggling servos while disarmed on heli, single, coax copters) +void Sub::radio_passthrough_to_motors() +{ + motors.set_radio_passthrough(channel_roll->get_control_in()/1000.0f, channel_pitch->get_control_in()/1000.0f, channel_throttle->get_control_in()/1000.0f, channel_yaw->get_control_in()/1000.0f); +} diff --git a/ArduSub/setup.cpp b/ArduSub/setup.cpp index 6ef345f877..32510b4fcc 100644 --- a/ArduSub/setup.cpp +++ b/ArduSub/setup.cpp @@ -383,14 +383,10 @@ void Sub::report_optflow() void Sub::print_radio_values() { - cliSerial->printf("CH1: %d | %d\n", (int)channel_roll->radio_min, (int)channel_roll->radio_max); - cliSerial->printf("CH2: %d | %d\n", (int)channel_pitch->radio_min, (int)channel_pitch->radio_max); - cliSerial->printf("CH3: %d | %d\n", (int)channel_throttle->radio_min, (int)channel_throttle->radio_max); - cliSerial->printf("CH4: %d | %d\n", (int)channel_yaw->radio_min, (int)channel_yaw->radio_max); - cliSerial->printf("CH5: %d | %d\n", (int)g.rc_5.radio_min, (int)g.rc_5.radio_max); - cliSerial->printf("CH6: %d | %d\n", (int)g.rc_6.radio_min, (int)g.rc_6.radio_max); - cliSerial->printf("CH7: %d | %d\n", (int)g.rc_7.radio_min, (int)g.rc_7.radio_max); - cliSerial->printf("CH8: %d | %d\n", (int)g.rc_8.radio_min, (int)g.rc_8.radio_max); + for (uint8_t i=0; i<8; i++) { + RC_Channel *rc = RC_Channels::rc_channel(i); + cliSerial->printf("CH%u: %d | %d\n", (unsigned)i, rc->get_radio_min(), rc->get_radio_max()); + } } void Sub::print_switch(uint8_t p, uint8_t m, bool b) diff --git a/ArduSub/switches.cpp b/ArduSub/switches.cpp index dfbb067fcf..1bb6a8df10 100644 --- a/ArduSub/switches.cpp +++ b/ArduSub/switches.cpp @@ -18,62 +18,6 @@ static union { uint32_t value; } aux_con; -void Sub::read_control_switch() -{ - uint32_t tnow_ms = millis(); - - // calculate position of flight mode switch - int8_t switch_position; - if (g.rc_5.radio_in < 1231) switch_position = 0; - else if (g.rc_5.radio_in < 1361) switch_position = 1; - else if (g.rc_5.radio_in < 1491) switch_position = 2; - else if (g.rc_5.radio_in < 1621) switch_position = 3; - else if (g.rc_5.radio_in < 1750) switch_position = 4; - else switch_position = 5; - - // store time that switch last moved - if(control_switch_state.last_switch_position != switch_position) { - control_switch_state.last_edge_time_ms = tnow_ms; - } - - // debounce switch - bool control_switch_changed = control_switch_state.debounced_switch_position != switch_position; - bool sufficient_time_elapsed = tnow_ms - control_switch_state.last_edge_time_ms > CONTROL_SWITCH_DEBOUNCE_TIME_MS; - bool failsafe_disengaged = !failsafe.radio && failsafe.radio_counter == 0; - - if (control_switch_changed && sufficient_time_elapsed && failsafe_disengaged) { - // set flight mode and simple mode setting - if (set_mode((control_mode_t)flight_modes[switch_position].get(), MODE_REASON_TX_COMMAND)) { - // play a tone - if (control_switch_state.debounced_switch_position != -1) { - // alert user to mode change failure (except if autopilot is just starting up) - if (ap.initialised) { - AP_Notify::events.user_mode_change = 1; - } - } - - if(!check_if_auxsw_mode_used(AUXSW_SIMPLE_MODE) && !check_if_auxsw_mode_used(AUXSW_SUPERSIMPLE_MODE)) { - // if none of the Aux Switches are set to Simple or Super Simple Mode then - // set Simple Mode using stored parameters from EEPROM - if (BIT_IS_SET(g.super_simple, switch_position)) { - set_simple_mode(2); - }else{ - set_simple_mode(BIT_IS_SET(g.simple_modes, switch_position)); - } - } - - } else if (control_switch_state.last_switch_position != -1) { - // alert user to mode change failure - AP_Notify::events.user_mode_change_failed = 1; - } - - // set the debounced switch position - control_switch_state.debounced_switch_position = switch_position; - } - - control_switch_state.last_switch_position = switch_position; -} - // check_if_auxsw_mode_used - Check to see if any of the Aux Switches are set to a given mode. bool Sub::check_if_auxsw_mode_used(uint8_t auxsw_mode_check) { @@ -105,12 +49,6 @@ bool Sub::check_duplicate_auxsw(void) return ret; } -void Sub::reset_control_switch() -{ - control_switch_state.last_switch_position = control_switch_state.debounced_switch_position = -1; - read_control_switch(); -} - // read_3pos_switch uint8_t Sub::read_3pos_switch(int16_t radio_in) { @@ -119,6 +57,16 @@ uint8_t Sub::read_3pos_switch(int16_t radio_in) return AUX_SWITCH_MIDDLE; // switch is in middle position } +// can't take reference to a bitfield member, thus a #define: +#define read_aux_switch(chan, flag, option) \ + do { \ + switch_position = read_3pos_switch(chan); \ + if (flag != switch_position) { \ + flag = switch_position; \ + do_aux_switch_function(option, flag); \ + } \ + } while (false) + // read_aux_switches - checks aux switch positions and invokes configured actions void Sub::read_aux_switches() { @@ -129,85 +77,31 @@ void Sub::read_aux_switches() return; } - // check if ch7 switch has changed position - switch_position = read_3pos_switch(g.rc_7.radio_in); - if (aux_con.CH7_flag != switch_position) { - // set the CH7 flag - aux_con.CH7_flag = switch_position; - - // invoke the appropriate function - do_aux_switch_function(g.ch7_option, aux_con.CH7_flag); - } - - // check if Ch8 switch has changed position - switch_position = read_3pos_switch(g.rc_8.radio_in); - if (aux_con.CH8_flag != switch_position) { - // set the CH8 flag - aux_con.CH8_flag = switch_position; - - // invoke the appropriate function - do_aux_switch_function(g.ch8_option, aux_con.CH8_flag); - } + read_aux_switch(CH_7, aux_con.CH7_flag, g.ch7_option); + read_aux_switch(CH_8, aux_con.CH8_flag, g.ch8_option); + read_aux_switch(CH_9, aux_con.CH9_flag, g.ch9_option); + read_aux_switch(CH_10, aux_con.CH10_flag, g.ch10_option); + read_aux_switch(CH_11, aux_con.CH11_flag, g.ch11_option); #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN - // check if Ch9 switch has changed position - switch_position = read_3pos_switch(g.rc_9.radio_in); - if (aux_con.CH9_flag != switch_position) { - // set the CH9 flag - aux_con.CH9_flag = switch_position; - - // invoke the appropriate function - do_aux_switch_function(g.ch9_option, aux_con.CH9_flag); - } -#endif - - // check if Ch10 switch has changed position - switch_position = read_3pos_switch(g.rc_10.radio_in); - if (aux_con.CH10_flag != switch_position) { - // set the CH10 flag - aux_con.CH10_flag = switch_position; - - // invoke the appropriate function - do_aux_switch_function(g.ch10_option, aux_con.CH10_flag); - } - - // check if Ch11 switch has changed position - switch_position = read_3pos_switch(g.rc_11.radio_in); - if (aux_con.CH11_flag != switch_position) { - // set the CH11 flag - aux_con.CH11_flag = switch_position; - - // invoke the appropriate function - do_aux_switch_function(g.ch11_option, aux_con.CH11_flag); - } - -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN - // check if Ch12 switch has changed position - switch_position = read_3pos_switch(g.rc_12.radio_in); - if (aux_con.CH12_flag != switch_position) { - // set the CH12 flag - aux_con.CH12_flag = switch_position; - - // invoke the appropriate function - do_aux_switch_function(g.ch12_option, aux_con.CH12_flag); - } + read_aux_switch(CH_12, aux_con.CH12_flag, g.ch12_option); #endif } +#undef read_aux_switch + // init_aux_switches - invoke configured actions at start-up for aux function where it is safe to do so void Sub::init_aux_switches() { // set the CH7 ~ CH12 flags - aux_con.CH7_flag = read_3pos_switch(g.rc_7.radio_in); - aux_con.CH8_flag = read_3pos_switch(g.rc_8.radio_in); - aux_con.CH10_flag = read_3pos_switch(g.rc_10.radio_in); - aux_con.CH11_flag = read_3pos_switch(g.rc_11.radio_in); + aux_con.CH7_flag = read_3pos_switch(CH_7); + aux_con.CH8_flag = read_3pos_switch(CH_8); + aux_con.CH10_flag = read_3pos_switch(CH_10); + aux_con.CH11_flag = read_3pos_switch(CH_11); // ch9, ch12 only supported on some boards -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN - aux_con.CH9_flag = read_3pos_switch(g.rc_9.radio_in); - aux_con.CH12_flag = read_3pos_switch(g.rc_12.radio_in); -#endif + aux_con.CH9_flag = read_3pos_switch(CH_9); + aux_con.CH12_flag = read_3pos_switch(CH_12); // initialise functions assigned to switches init_aux_switch_function(g.ch7_option, aux_con.CH7_flag); @@ -216,10 +110,8 @@ void Sub::init_aux_switches() init_aux_switch_function(g.ch11_option, aux_con.CH11_flag); // ch9, ch12 only supported on some boards -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN init_aux_switch_function(g.ch9_option, aux_con.CH9_flag); init_aux_switch_function(g.ch12_option, aux_con.CH12_flag); -#endif } // init_aux_switch_function - initialize aux functions @@ -262,20 +154,8 @@ void Sub::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) set_simple_mode(ch_flag); break; - case AUXSW_RTL: - if (ch_flag == AUX_SWITCH_HIGH) { - // engage RTL (if not possible we remain in current flight mode) - set_mode(RTL, MODE_REASON_TX_COMMAND); - }else{ - // return to flight mode switch's flight mode if we are currently in RTL - if (control_mode == RTL) { - reset_control_switch(); - } - } - break; - case AUXSW_SAVE_TRIM: - if ((ch_flag == AUX_SWITCH_HIGH) && (control_mode <= ACRO) && (channel_throttle->control_in == 0)) { + if ((ch_flag == AUX_SWITCH_HIGH) && (control_mode <= ACRO) && (channel_throttle->get_control_in() == 0)) { save_trim(); } break; @@ -290,7 +170,7 @@ void Sub::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) } // do not allow saving the first waypoint with zero throttle - if((mission.num_commands() == 0) && (channel_throttle->control_in == 0)){ + if((mission.num_commands() == 0) && (channel_throttle->get_control_in() == 0)){ return; } @@ -301,7 +181,7 @@ void Sub::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) cmd.content.location = current_loc; // if throttle is above zero, create waypoint command - if(channel_throttle->control_in > 0) { + if(channel_throttle->get_control_in() > 0) { cmd.id = MAV_CMD_NAV_WAYPOINT; }else{ // with zero throttle, create LAND command @@ -393,7 +273,7 @@ void Sub::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) }else{ // return to flight mode switch's flight mode if we are currently in AUTO if (control_mode == AUTO) { - reset_control_switch(); +// reset_control_switch(); } } break; @@ -406,7 +286,7 @@ void Sub::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) case AUX_SWITCH_MIDDLE: // restore flight mode based on flight mode switch position if (control_mode == AUTOTUNE) { - reset_control_switch(); +// reset_control_switch(); } break; case AUX_SWITCH_HIGH: @@ -484,9 +364,6 @@ void Sub::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) // control signal in tradheli motors.set_interlock(ch_flag == AUX_SWITCH_HIGH || ch_flag == AUX_SWITCH_MIDDLE); - // remember the current value of the motor interlock so that this condition can be restored if we exit the throw mode early - throw_early_exit_interlock = motors.get_interlock(); - // Log new status if (motors.get_interlock()){ Log_Write_Event(DATA_MOTORS_INTERLOCK_ENABLED); @@ -494,30 +371,6 @@ void Sub::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) Log_Write_Event(DATA_MOTORS_INTERLOCK_DISABLED); } break; - - case AUXSW_BRAKE: - // brake flight mode - if (ch_flag == AUX_SWITCH_HIGH) { - set_mode(BRAKE, MODE_REASON_TX_COMMAND); - }else{ - // return to flight mode switch's flight mode if we are currently in BRAKE - if (control_mode == BRAKE) { - reset_control_switch(); - } - } - break; - - case AUXSW_THROW: - // throw flight mode - if (ch_flag == AUX_SWITCH_HIGH) { - set_mode(THROW, MODE_REASON_TX_COMMAND); - } else { - // return to flight mode switch's flight mode if we are currently in throw mode - if (control_mode == THROW) { - reset_control_switch(); - } - } - break; } } @@ -525,8 +378,8 @@ void Sub::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) void Sub::save_trim() { // save roll and pitch trim - float roll_trim = ToRad((float)channel_roll->control_in/100.0f); - float pitch_trim = ToRad((float)channel_pitch->control_in/100.0f); + float roll_trim = ToRad((float)channel_roll->get_control_in()/100.0f); + float pitch_trim = ToRad((float)channel_pitch->get_control_in()/100.0f); ahrs.add_trim(roll_trim, pitch_trim); Log_Write_Event(DATA_SAVE_TRIM); gcs_send_text(MAV_SEVERITY_INFO, "Trim saved"); @@ -543,10 +396,10 @@ void Sub::auto_trim() AP_Notify::flags.save_trim = true; // calculate roll trim adjustment - float roll_trim_adjustment = ToRad((float)channel_roll->control_in / 4000.0f); + float roll_trim_adjustment = ToRad((float)channel_roll->get_control_in() / 4000.0f); // calculate pitch trim adjustment - float pitch_trim_adjustment = ToRad((float)channel_pitch->control_in / 4000.0f); + float pitch_trim_adjustment = ToRad((float)channel_pitch->get_control_in() / 4000.0f); // add trim to ahrs object // save to eeprom on last iteration diff --git a/ArduSub/system.cpp b/ArduSub/system.cpp index 8b58fe0d8a..ac5d8633b2 100644 --- a/ArduSub/system.cpp +++ b/ArduSub/system.cpp @@ -1,6 +1,7 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- #include "Sub.h" +#include "version.h" /***************************************************************************** * The init_ardupilot function processes everything we need for an in - air restart @@ -229,18 +230,51 @@ void Sub::init_ardupilot() ins.set_hil_mode(); #endif - if(barometer.num_instances() > 1) { - //We have an external MS58XX pressure sensor connected - for(int i = 1; i < barometer.num_instances(); i++) { - barometer.set_type(i, BARO_TYPE_WATER); //Altitude (depth) is calculated differently underwater - barometer.set_precision_multiplier(i, 10); //The MS58XX values reported need to be multiplied by 10 to match units everywhere else - } - barometer.set_primary_baro(1); //Set the primary baro to external MS58XX + barometer.calibrate(); + barometer.update(); - } - // read Baro pressure at ground - //----------------------------- - init_barometer(true); + if(barometer.healthy(1)) { // We have an external MS58XX pressure sensor connected + + barometer.set_primary_baro(1); // Set the primary baro to external MS58XX !!Changes and saves parameter value!! + + + ap.depth_sensor_present = true; + for(int i = 1; i < barometer.num_instances(); i++) { + barometer.set_type(i, BARO_TYPE_WATER); // Altitude (depth) is calculated differently underwater + barometer.set_precision_multiplier(i, 40); // The MS58XX values reported need to be multiplied by 10 to match units everywhere else + } + + + EKF2.set_baro_alt_noise(0.1f); // Depth readings are very accurate and up-to-date + EKF3.set_baro_alt_noise(0.1f); + + } else { //We only have onboard baro + + // No external underwater depth sensor detected + barometer.set_primary_baro(0); // Set the primary baro to default board baro !!Changes and saves parameter value!! + + ap.depth_sensor_present = false; + for(int i = 1; i < barometer.num_instances(); i++) { + barometer.set_type(i, BARO_TYPE_AIR); // Default fcu air baro + barometer.set_precision_multiplier(i, 1); // Use default values + } + EKF2.set_baro_alt_noise(10.0f); // Readings won't correspond with rest of INS + EKF3.set_baro_alt_noise(10.0f); + + + } + + leak_detector.init(); + + // read Baro pressure at ground + //----------------------------- + init_barometer(true); + + // cope with MS5607 in place of MS5611 on fake pixhawks + if(barometer.get_pressure(0) < 60000) { + barometer.set_precision_multiplier(0, 2); + init_barometer(true); // recalibrate with correct scalar + } // backwards compatibility if(attitude_control.get_accel_yaw_max() < 110000.0f) { @@ -269,10 +303,6 @@ void Sub::init_ardupilot() startup_INS_ground(); - // set landed flags - set_land_complete(true); - set_land_complete_maybe(true); - // we don't want writes to the serial port to cause us to pause // mid-flight, so set the serial ports non-blocking once we are // ready to fly @@ -396,8 +426,8 @@ void Sub::update_auto_armed() } }else{ // arm checks - // if motors are armed and we are in throw mode, then auto_ermed should be true - if(motors.armed() && (!ap.throttle_zero || control_mode == THROW)) { + // if motors are armed and throttle is above zero auto_armed should be true + if(motors.armed()) { set_auto_armed(true); } } @@ -423,7 +453,7 @@ bool Sub::should_log(uint32_t mask) if (!(mask & g.log_bitmask) || in_mavlink_delay) { return false; } - bool ret = motors.armed() || (g.log_bitmask & MASK_LOG_WHEN_DISARMED) != 0; + bool ret = motors.armed() || DataFlash.log_while_disarmed(); if (ret && !DataFlash.logging_started() && !in_log_download) { start_logging(); } diff --git a/ArduSub/tuning.cpp b/ArduSub/tuning.cpp index 8d39728fbf..61c5bca68e 100644 --- a/ArduSub/tuning.cpp +++ b/ArduSub/tuning.cpp @@ -16,10 +16,10 @@ void Sub::tuning() { return; } - float tuning_value = (float)g.rc_6.control_in / 1000.0f; + float tuning_value = (float)g.rc_6.get_control_in() / 1000.0f; g.rc_6.set_range(g.radio_tuning_low,g.radio_tuning_high); - Log_Write_Parameter_Tuning(g.radio_tuning, tuning_value, g.rc_6.control_in, g.radio_tuning_low, g.radio_tuning_high); + Log_Write_Parameter_Tuning(g.radio_tuning, tuning_value, g.rc_6.get_control_in(), g.radio_tuning_low, g.radio_tuning_high); switch(g.radio_tuning) { @@ -93,7 +93,7 @@ void Sub::tuning() { case TUNING_WP_SPEED: // set waypoint navigation horizontal speed to 0 ~ 1000 cm/s - wp_nav.set_speed_xy(g.rc_6.control_in); + wp_nav.set_speed_xy(g.rc_6.get_control_in()); break; // Acro roll pitch gain @@ -108,12 +108,12 @@ void Sub::tuning() { case TUNING_DECLINATION: // set declination to +-20degrees - compass.set_declination(ToRad((2.0f * g.rc_6.control_in - g.radio_tuning_high)/100.0f), false); // 2nd parameter is false because we do not want to save to eeprom because this would have a performance impact + compass.set_declination(ToRad((2.0f * g.rc_6.get_control_in() - g.radio_tuning_high)/100.0f), false); // 2nd parameter is false because we do not want to save to eeprom because this would have a performance impact break; case TUNING_CIRCLE_RATE: // set circle rate up to approximately 45 deg/sec in either direction - circle_nav.set_rate((float)g.rc_6.control_in/25.0f-20.0f); + circle_nav.set_rate((float)g.rc_6.get_control_in()/25.0f-20.0f); break; case TUNING_RANGEFINDER_GAIN: @@ -154,7 +154,7 @@ void Sub::tuning() { case TUNING_RC_FEEL_RP: // roll-pitch input smoothing - g.rc_feel_rp = g.rc_6.control_in / 10; + g.rc_feel_rp = g.rc_6.get_control_in() / 10; break; case TUNING_RATE_PITCH_KP: diff --git a/ArduSub/wscript b/ArduSub/wscript index cc654dbd7a..c1efc960e3 100644 --- a/ArduSub/wscript +++ b/ArduSub/wscript @@ -5,9 +5,8 @@ def build(bld): vehicle = bld.path.name bld.ap_stlib( name=vehicle + '_libs', - vehicle=vehicle, - libraries=bld.ap_common_vehicle_libraries() + [ - 'AP_ADSB', + ap_vehicle=vehicle, + ap_libraries=bld.ap_common_vehicle_libraries() + [ 'AC_AttitudeControl', 'AC_InputManager', 'AC_Fence', @@ -20,11 +19,7 @@ def build(bld): 'AP_LeakDetector', 'AP_Menu', 'AP_Motors', - 'AP_Mount', - 'AP_Parachute', 'AP_RCMapper', - 'AP_RPM', - 'AP_RSSI', 'AP_Relay', 'AP_ServoRelayEvents', 'AP_Proximity', @@ -32,7 +27,6 @@ def build(bld): 'AP_Beacon', 'AP_TemperatureSensor' ], - use='mavlink', ) frames = (