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 = (