Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Sub: fixes from rebase on ArduPilot master

This commit is contained in:
Jacob Walser 2017-02-03 15:18:27 +11:00 committed by Andrew Tridgell
parent e7a34b0fb3
commit ed87bd9e59
38 changed files with 994 additions and 3470 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

100
ArduSub/ArduSub.cpp
View File

@ -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

122
ArduSub/Attitude.cpp
View File

@ -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()
{

289
ArduSub/GCS_Mavlink.cpp
View File

@ -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

7
ArduSub/Log.cpp
View File

@ -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 {

4
ArduSub/Parameters.cpp
View File

@ -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),

13
ArduSub/Sub.cpp
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
/*
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),

11
ArduSub/Sub.h
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
#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);

47
ArduSub/arming_checks.cpp
View File

@ -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");
}

72
ArduSub/baro_ground_effect.cpp
View File

@ -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

8
ArduSub/compassmot.cpp
View File

@ -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,

59
ArduSub/config.h
View File

@ -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

17
ArduSub/control_acro.cpp
View File

@ -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;

153
ArduSub/control_althold.cpp
View File

@ -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());
}

162
ArduSub/control_auto.cpp
View File

@ -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

62
ArduSub/control_autotune.cpp
View File

@ -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)

4
ArduSub/control_circle.cpp
View File

@ -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) {

121
ArduSub/control_drift.cpp
View File

@ -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);
}

218
ArduSub/control_flip.cpp
View File

@ -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);
}

79
ArduSub/control_guided.cpp
View File

@ -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());

271
ArduSub/control_land.cpp
View File

@ -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);
}

696
ArduSub/control_poshold.cpp
View File

@ -1,83 +1,16 @@
/// -*- 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;
}
@ -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();
// get pilot's desired yaw rate
float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
// 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);
// 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);
// 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) {
// 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
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);
} else { // hold current 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
}
// 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
// final lean angle should be pilot input plus wind compensation
poshold.roll = poshold.pilot_roll + poshold.wind_comp_roll;
break;
// 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
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);
} 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());
}
}
// 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;
}
}
///////////////////
// Update z axis //
// 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;
}
// 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);
// 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;
}
// 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);
}
// final lean angle is braking angle + wind compensation angle
poshold.roll = poshold.brake_roll + poshold.wind_comp_roll;
// 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);
}
}
// check for pilot input
if (!is_zero(target_roll)) {
// init transition to pilot override
poshold_roll_controller_to_pilot_override();
}
break;
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

503
ArduSub/control_rtl.cpp
View File

@ -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;
}

17
ArduSub/control_stabilize.cpp
View File

@ -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());
}

289
ArduSub/control_transect.cpp
View File

@ -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

361
ArduSub/control_velhold.cpp
View File

@ -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

138
ArduSub/crash_check.cpp
View File

@ -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

13
ArduSub/defines.h
View File

@ -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)

23
ArduSub/esc_calibration.cpp
View File

@ -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); }
}

7
ArduSub/flight_mode.cpp
View File

@ -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

5
ArduSub/motor_test.cpp
View File

@ -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:

145
ArduSub/motors.cpp
View File

@ -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;

2
ArduSub/position_vector.cpp
View File

@ -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

129
ArduSub/radio.cpp
View File

@ -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);
}

12
ArduSub/setup.cpp
View File

@ -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)

213
ArduSub/switches.cpp
View File

@ -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

66
ArduSub/system.cpp
View File

@ -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();
}

12
ArduSub/tuning.cpp
View File

@ -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:

10
ArduSub/wscript
View File

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