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

Sub: Changes to match recent Copter updates.

Browse Source
This commit is contained in:
Rustom Jehangir 2016-05-22 17:50:44 -07:00 committed by Andrew Tridgell
parent e857f20784
commit 26d0a922c1
29 changed files with 259 additions and 170 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduSub/APM_Config.h
View File

@ -19,7 +19,7 @@
#define AUTOTUNE_ENABLED DISABLED // disable the auto tune functionality to save 7k of flash
//#define AC_FENCE DISABLED // disable fence to save 2k of flash
//#define CAMERA DISABLED // disable camera trigger to save 1k of flash
//#define CONFIG_SONAR DISABLED // disable sonar to save 1k of flash
//#define RANGEFINDER_ENABLED DISABLED // disable rangefinder to save 1k of flash
#define POSHOLD_ENABLED DISABLED // disable PosHold flight mode to save 4.5k of flash
#define AC_RALLY DISABLED // disable rally points library (must also disable terrain which relies on rally)
#define AC_TERRAIN DISABLED // disable terrain library

8
ArduSub/ArduSub.cpp
View File

@ -94,7 +94,8 @@ const AP_Scheduler::Task Sub::scheduler_tasks[] = {
SCHED_TASK(arm_motors_check, 10, 50),
SCHED_TASK(auto_disarm_check, 10, 50),
SCHED_TASK(auto_trim, 10, 75),
SCHED_TASK(update_altitude, 25, 140),
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),
@ -625,15 +626,12 @@ void Sub::read_AHRS(void)
ahrs.update();
}
// read baro and sonar altitude at 10hz
// read baro and rangefinder altitude at 10hz
void Sub::update_altitude()
{
// read in baro altitude
read_barometer();
// read in sonar altitude
sonar_alt = read_sonar();
// write altitude info to dataflash logs
if (should_log(MASK_LOG_CTUN)) {
Log_Write_Control_Tuning();

22
ArduSub/Attitude.cpp
View File

@ -245,6 +245,7 @@ float Sub::get_throttle_pre_takeoff(float input_thr)
// 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)
{
#if RANGEFINDER_ENABLED == ENABLED
static uint32_t last_call_ms = 0;
float distance_error;
float velocity_correction;
@ -253,27 +254,30 @@ float Sub::get_surface_tracking_climb_rate(int16_t target_rate, float current_al
uint32_t now = millis();
// reset target altitude if this controller has just been engaged
if (now - last_call_ms > SONAR_TIMEOUT_MS) {
target_sonar_alt = sonar_alt + current_alt_target - current_alt;
if (now - last_call_ms > RANGEFINDER_TIMEOUT_MS) {
target_rangefinder_alt = rangefinder_state.alt_cm + current_alt_target - current_alt;
}
last_call_ms = now;
// adjust sonar target alt if motors have not hit their limits
// adjust rangefinder target alt if motors have not hit their limits
if ((target_rate<0 && !motors.limit.throttle_lower) || (target_rate>0 && !motors.limit.throttle_upper)) {
target_sonar_alt += target_rate * dt;
target_rangefinder_alt += target_rate * dt;
}
// do not let target altitude get too far from current altitude above ground
// Note: the 750cm limit is perhaps too wide but is consistent with the regular althold limits and helps ensure a smooth transition
target_sonar_alt = constrain_float(target_sonar_alt,sonar_alt-pos_control.get_leash_down_z(),sonar_alt+pos_control.get_leash_up_z());
target_rangefinder_alt = constrain_float(target_rangefinder_alt,rangefinder_state.alt_cm-pos_control.get_leash_down_z(),rangefinder_state.alt_cm+pos_control.get_leash_up_z());
// calc desired velocity correction from target sonar alt vs actual sonar alt (remove the error already passed to Altitude controller to avoid oscillations)
distance_error = (target_sonar_alt - sonar_alt) - (current_alt_target - current_alt);
velocity_correction = distance_error * g.sonar_gain;
// calc desired velocity correction from target rangefinder alt vs actual rangefinder alt (remove the error already passed to Altitude controller to avoid oscillations)
distance_error = (target_rangefinder_alt - rangefinder_state.alt_cm) - (current_alt_target - current_alt);
velocity_correction = distance_error * g.rangefinder_gain;
velocity_correction = constrain_float(velocity_correction, -THR_SURFACE_TRACKING_VELZ_MAX, THR_SURFACE_TRACKING_VELZ_MAX);
// return combined pilot climb rate + rate to correct sonar alt error
// return combined pilot climb rate + rate to correct rangefinder alt error
return (target_rate + velocity_correction);
#else
return (float)target_rate;
#endif
}
// set_accel_throttle_I_from_pilot_throttle - smoothes transition from pilot controlled throttle to autopilot throttle

8
ArduSub/Log.cpp
View File

@ -304,8 +304,8 @@ struct PACKED log_Control_Tuning {
float desired_alt;
float inav_alt;
int32_t baro_alt;
int16_t desired_sonar_alt;
int16_t sonar_alt;
int16_t desired_rangefinder_alt;
int16_t rangefinder_alt;
float terr_alt;
int16_t desired_climb_rate;
int16_t climb_rate;
@ -329,8 +329,8 @@ void Sub::Log_Write_Control_Tuning()
desired_alt : pos_control.get_alt_target() / 100.0f,
inav_alt : inertial_nav.get_altitude() / 100.0f,
baro_alt : baro_alt,
desired_sonar_alt : (int16_t)target_sonar_alt,
sonar_alt : sonar_alt,
desired_rangefinder_alt : (int16_t)target_rangefinder_alt,
rangefinder_alt : rangefinder_state.alt_cm,
terr_alt : terr_alt,
desired_climb_rate : (int16_t)pos_control.get_vel_target_z(),
climb_rate : climb_rate

10
ArduSub/Parameters.cpp
View File

@ -163,7 +163,7 @@ const AP_Param::Info Sub::var_info[] = {
// @Range: 0.01 2.0
// @Increment: 0.01
// @User: Standard
GSCALAR(sonar_gain, "RNGFND_GAIN", SONAR_GAIN_DEFAULT),
GSCALAR(rangefinder_gain, "RNGFND_GAIN", RANGEFINDER_GAIN_DEFAULT),
// @Param: FS_BATT_ENABLE
// @DisplayName: Battery Failsafe Enable
@ -465,8 +465,8 @@ const AP_Param::Info Sub::var_info[] = {
// @Param: ARMING_CHECK
// @DisplayName: Arming check
// @Description: Allows enabling or disabling of pre-arming checks of receiver, accelerometer, barometer, compass and GPS
// @Values: 0:Disabled, 1:Enabled, -3:Skip Baro, -5:Skip Compass, -9:Skip GPS, -17:Skip INS, -33:Skip Params/Sonar, -65:Skip RC, 127:Skip Voltage
// @Bitmask: 0:All,1:Baro,2:Compass,3:GPS,4:INS,5:Parameters+Sonar,6:RC,7:Voltage
// @Values: 0:Disabled, 1:Enabled, -3:Skip Baro, -5:Skip Compass, -9:Skip GPS, -17:Skip INS, -33:Skip Params/Rangefinder, -65:Skip RC, 127:Skip Voltage
// @Bitmask: 0:All,1:Baro,2:Compass,3:GPS,4:INS,5:Parameters+Rangefinder,6:RC,7:Voltage
// @User: Standard
GSCALAR(arming_check, "ARMING_CHECK", ARMING_CHECK_ALL),
@ -974,10 +974,10 @@ const AP_Param::Info Sub::var_info[] = {
// @Path: ../libraries/AP_RSSI/AP_RSSI.cpp
GOBJECT(rssi, "RSSI_", AP_RSSI),
#if CONFIG_SONAR == ENABLED
#if RANGEFINDER_ENABLED == ENABLED
// @Group: RNGFND
// @Path: ../libraries/AP_RangeFinder/RangeFinder.cpp
GOBJECT(sonar, "RNGFND", RangeFinder),
GOBJECT(rangefinder, "RNGFND", RangeFinder),
#endif
#if AP_TERRAIN_AVAILABLE && AC_TERRAIN

10
ArduSub/Parameters.h
View File

@ -99,7 +99,7 @@ public:
k_param_acro_trainer,
k_param_pilot_velocity_z_max,
k_param_circle_rate, // deprecated - remove
k_param_sonar_gain,
k_param_rangefinder_gain,
k_param_ch8_option,
k_param_arming_check,
k_param_sprayer,
@ -122,7 +122,7 @@ public:
k_param_serial1_baud, // deprecated - remove
k_param_serial2_baud, // deprecated - remove
k_param_land_repositioning,
k_param_sonar, // sonar object
k_param_rangefinder, // rangefinder object
k_param_fs_ekf_thresh,
k_param_terrain,
k_param_acro_expo,
@ -232,13 +232,13 @@ public:
k_param_pack_capacity, // deprecated - can be deleted
k_param_compass_enabled,
k_param_compass,
k_param_sonar_enabled_old, // deprecated
k_param_rangefinder_enabled_old, // deprecated
k_param_frame_orientation,
k_param_optflow_enabled, // deprecated
k_param_fs_batt_voltage,
k_param_ch7_option,
k_param_auto_slew_rate, // deprecated - can be deleted
k_param_sonar_type_old, // deprecated
k_param_rangefinder_type_old, // deprecated
k_param_super_simple = 155,
k_param_axis_enabled = 157, // deprecated - remove with next eeprom number change
k_param_copter_leds_mode, // deprecated - remove with next eeprom number change
@ -406,7 +406,7 @@ public:
AP_Int16 rtl_altitude;
AP_Int16 rtl_speed_cms;
AP_Float rtl_cone_slope;
AP_Float sonar_gain;
AP_Float rangefinder_gain;
AP_Int8 failsafe_battery_enabled; // battery failsafe enabled
AP_Float fs_batt_voltage; // battery voltage below which failsafe will be triggered

5
ArduSub/Sub.cpp
View File

@ -23,7 +23,6 @@ const AP_HAL::HAL& hal = AP_HAL::get_HAL();
Sub::Sub(void) :
flight_modes(&g.flight_mode1),
sonar_enabled(true),
mission(ahrs,
FUNCTOR_BIND_MEMBER(&Sub::start_command, bool, const AP_Mission::Mission_Command &),
FUNCTOR_BIND_MEMBER(&Sub::verify_command_callback, bool, const AP_Mission::Mission_Command &),
@ -54,9 +53,7 @@ Sub::Sub(void) :
frsky_telemetry(ahrs, battery),
#endif
climb_rate(0),
sonar_alt(0),
sonar_alt_health(0),
target_sonar_alt(0.0f),
target_rangefinder_alt(0.0f),
baro_alt(0),
baro_climbrate(0.0f),
land_accel_ef_filter(LAND_DETECTOR_ACCEL_LPF_CUTOFF),

42
ArduSub/Sub.h
View File

@ -173,17 +173,21 @@ private:
Compass compass;
AP_InertialSensor ins;
#if CONFIG_SONAR == ENABLED
RangeFinder sonar {serial_manager};
bool sonar_enabled; // enable user switch for sonar
#endif
RangeFinder rangefinder {serial_manager};
struct {
bool enabled:1;
bool alt_healthy:1; // true if we can trust the altitude from the rangefinder
int16_t alt_cm; // tilt compensated altitude (in cm) from rangefinder
uint32_t last_healthy_ms;
LowPassFilterFloat alt_cm_filt; // altitude filter
} rangefinder_state = { false, false, 0, 0 };
AP_RPM rpm_sensor;
// Inertial Navigation EKF
NavEKF EKF{&ahrs, barometer, sonar};
NavEKF2 EKF2{&ahrs, barometer, sonar};
AP_AHRS_NavEKF ahrs{ins, barometer, gps, sonar, EKF, EKF2, AP_AHRS_NavEKF::FLAG_ALWAYS_USE_EKF};
NavEKF EKF{&ahrs, barometer, rangefinder};
NavEKF2 EKF2{&ahrs, barometer, rangefinder};
AP_AHRS_NavEKF ahrs{ins, barometer, gps, rangefinder, EKF, EKF2, AP_AHRS_NavEKF::FLAG_ALWAYS_USE_EKF};
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
SITL::SITL sitl;
@ -395,6 +399,10 @@ private:
uint32_t brake_timeout_start;
uint32_t brake_timeout_ms;
// Delay the next navigation command
int32_t nav_delay_time_max; // used for delaying the navigation commands (eg land,takeoff etc.)
uint32_t nav_delay_time_start;
// Flip
Vector3f flip_orig_attitude; // original Sub attitude before flip
@ -415,10 +423,7 @@ private:
// Altitude
// The cm/s we are moving up or down based on filtered data - Positive = UP
int16_t climb_rate;
// The altitude as reported by Sonar in cm - Values are 20 to 700 generally.
int16_t sonar_alt;
uint8_t sonar_alt_health; // true if we can trust the altitude from the sonar
float target_sonar_alt; // desired altitude in cm above the ground
float target_rangefinder_alt; // desired altitude in cm above the ground
int32_t baro_alt; // barometer altitude in cm above home
float baro_climbrate; // barometer climbrate in cm/s
LowPassFilterVector3f land_accel_ef_filter; // accelerations for land and crash detector tests
@ -766,7 +771,7 @@ private:
void guided_vel_control_start();
void guided_posvel_control_start();
void guided_angle_control_start();
void guided_set_destination(const Vector3f& destination);
bool guided_set_destination(const Vector3f& destination);
bool guided_set_destination(const Location_Class& dest_loc);
void guided_set_velocity(const Vector3f& velocity);
void guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity);
@ -892,7 +897,7 @@ private:
void pre_arm_rc_checks();
bool pre_arm_gps_checks(bool display_failure);
bool pre_arm_ekf_attitude_check();
bool pre_arm_terrain_check();
bool pre_arm_terrain_check(bool display_failure);
bool arm_checks(bool display_failure, bool arming_from_gcs);
void init_disarm_motors();
void motors_output();
@ -918,6 +923,7 @@ private:
float pv_alt_above_home(float alt_above_origin_cm);
float pv_get_bearing_cd(const Vector3f &origin, const Vector3f &destination);
float pv_get_horizontal_distance_cm(const Vector3f &origin, const Vector3f &destination);
float pv_distance_to_home_cm(const Vector3f &destination);
void default_dead_zones();
void init_rc_in();
void init_rc_out();
@ -931,8 +937,9 @@ private:
void radio_passthrough_to_motors();
void init_barometer(bool full_calibration);
void read_barometer(void);
void init_sonar(void);
int16_t read_sonar(void);
void init_rangefinder(void);
void read_rangefinder(void);
bool rangefinder_alt_ok(void);
void init_compass();
void init_optflow();
void update_optical_flow(void);
@ -1004,6 +1011,7 @@ private:
void do_nav_guided_enable(const AP_Mission::Mission_Command& cmd);
void do_guided_limits(const AP_Mission::Mission_Command& cmd);
#endif
void do_nav_delay(const AP_Mission::Mission_Command& cmd);
void do_wait_delay(const AP_Mission::Mission_Command& cmd);
void do_within_distance(const AP_Mission::Mission_Command& cmd);
void do_yaw(const AP_Mission::Mission_Command& cmd);
@ -1027,6 +1035,8 @@ private:
#if NAV_GUIDED == ENABLED
bool verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd);
#endif
bool verify_nav_delay(const AP_Mission::Mission_Command& cmd);
void auto_spline_start(const Location_Class& destination, bool stopped_at_start, AC_WPNav::spline_segment_end_type seg_end_type, const Location_Class& next_destination);
void print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode);
void log_init(void);
@ -1058,7 +1068,7 @@ public:
int8_t test_optflow(uint8_t argc, const Menu::arg *argv);
int8_t test_relay(uint8_t argc, const Menu::arg *argv);
int8_t test_shell(uint8_t argc, const Menu::arg *argv);
int8_t test_sonar(uint8_t argc, const Menu::arg *argv);
int8_t test_rangefinder(uint8_t argc, const Menu::arg *argv);
int8_t reboot_board(uint8_t argc, const Menu::arg *argv);
};

25
ArduSub/arming_checks.cpp
View File

@ -309,9 +309,9 @@ bool Sub::pre_arm_checks(bool display_failure)
return false;
}
#if CONFIG_SONAR == ENABLED && OPTFLOW == ENABLED
#if RANGEFINDER_ENABLED == ENABLED && OPTFLOW == ENABLED
// check range finder if optflow enabled
if (optflow.enabled() && !sonar.pre_arm_check()) {
if (optflow.enabled() && !rangefinder.pre_arm_check()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: check range finder");
}
@ -320,8 +320,7 @@ bool Sub::pre_arm_checks(bool display_failure)
#endif
// check for missing terrain data
if (!pre_arm_terrain_check()) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Waiting for Terrain data");
if (!pre_arm_terrain_check(display_failure)) {
return false;
}
}
@ -475,7 +474,7 @@ bool Sub::pre_arm_ekf_attitude_check()
}
// check we have required terrain data
bool Sub::pre_arm_terrain_check()
bool Sub::pre_arm_terrain_check(bool display_failure)
{
#if AP_TERRAIN_AVAILABLE && AC_TERRAIN
// succeed if not using terrain data
@ -483,10 +482,21 @@ bool Sub::pre_arm_terrain_check()
return true;
}
// check if terrain following is enabled, using a range finder but RTL_ALT is higher than rangefinder's max range
// To-Do: modify RTL return path to fly at or above the RTL_ALT and remove this check
if ((rangefinder.num_sensors() > 0) && (g.rtl_altitude > rangefinder.max_distance_cm())) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: RTL_ALT above rangefinder max range");
return false;
}
// show terrain statistics
uint16_t terr_pending, terr_loaded;
terrain.get_statistics(terr_pending, terr_loaded);
return (terr_pending <= 0);
bool have_all_data = (terr_pending <= 0);
if (!have_all_data && display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Waiting for Terrain data");
}
return have_all_data;
#else
return true;
#endif
@ -637,8 +647,7 @@ bool Sub::arm_checks(bool display_failure, bool arming_from_gcs)
// check for missing terrain data
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_PARAMETERS)) {
if (!pre_arm_terrain_check()) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Waiting for Terrain data");
if (!pre_arm_terrain_check(display_failure)) {
return false;
}
}

31
ArduSub/commands_logic.cpp
View File

@ -53,6 +53,10 @@ bool Sub::start_command(const AP_Mission::Mission_Command& cmd)
break;
#endif
case MAV_CMD_NAV_DELAY: // 94 Delay the next navigation command
do_nav_delay(cmd);
break;
//
// conditional commands
//
@ -212,6 +216,9 @@ bool Sub::verify_command(const AP_Mission::Mission_Command& cmd)
return verify_nav_guided_enable(cmd);
#endif
case MAV_CMD_NAV_DELAY:
return verify_nav_delay(cmd);
///
/// conditional commands
///
@ -515,6 +522,21 @@ void Sub::do_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
}
#endif // NAV_GUIDED
// do_nav_delay - Delay the next navigation command
void Sub::do_nav_delay(const AP_Mission::Mission_Command& cmd)
{
nav_delay_time_start = millis();
if (cmd.content.nav_delay.seconds > 0) {
// relative delay
nav_delay_time_max = cmd.content.nav_delay.seconds * 1000; // convert seconds to milliseconds
} else {
// absolute delay to utc time
nav_delay_time_max = hal.util->get_time_utc(cmd.content.nav_delay.hour_utc, cmd.content.nav_delay.min_utc, cmd.content.nav_delay.sec_utc, 0);
}
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Delaying %u sec",(unsigned int)(nav_delay_time_max/1000));
}
#if PARACHUTE == ENABLED
// do_parachute - configure or release parachute
@ -739,6 +761,15 @@ bool Sub::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
}
#endif // NAV_GUIDED
// verify_nav_delay - check if we have waited long enough
bool Sub::verify_nav_delay(const AP_Mission::Mission_Command& cmd)
{
if (millis() - nav_delay_time_start > (uint32_t)MAX(nav_delay_time_max,0)) {
nav_delay_time_max = 0;
return true;
}
return false;
}
/********************************************************************************/
// Condition (May) commands

37
ArduSub/config.h
View File

@ -47,11 +47,6 @@
#define HIL_MODE HIL_MODE_DISABLED
#endif
#if HIL_MODE != HIL_MODE_DISABLED // we are in HIL mode
#undef CONFIG_SONAR
#define CONFIG_SONAR DISABLED
#endif
#define MAGNETOMETER ENABLED
// run at 400Hz on all systems
@ -90,35 +85,35 @@
#endif
//////////////////////////////////////////////////////////////////////////////
// Sonar
// Rangefinder
//
#ifndef CONFIG_SONAR
# define CONFIG_SONAR ENABLED
#ifndef RANGEFINDER_ENABLED
# define RANGEFINDER_ENABLED ENABLED
#endif
#ifndef SONAR_ALT_HEALTH_MAX
# define SONAR_ALT_HEALTH_MAX 3 // number of good reads that indicates a healthy sonar
#ifndef RANGEFINDER_HEALTH_MAX
# define RANGEFINDER_HEALTH_MAX 3 // number of good reads that indicates a healthy rangefinder
#endif
#ifndef SONAR_RELIABLE_DISTANCE_PCT
# define SONAR_RELIABLE_DISTANCE_PCT 0.60f // we trust the sonar out to 60% of it's maximum range
#endif
#ifndef SONAR_GAIN_DEFAULT
# define SONAR_GAIN_DEFAULT 0.8f // gain for controlling how quickly sonar range adjusts target altitude (lower means slower reaction)
#ifndef RANGEFINDER_GAIN_DEFAULT
# define RANGEFINDER_GAIN_DEFAULT 0.8f // gain for controlling how quickly rangefinder range adjusts target altitude (lower means slower reaction)
#endif
#ifndef THR_SURFACE_TRACKING_VELZ_MAX
# define THR_SURFACE_TRACKING_VELZ_MAX 150 // max vertical speed change while surface tracking with sonar
# define THR_SURFACE_TRACKING_VELZ_MAX 150 // max vertical speed change while surface tracking with rangefinder
#endif
#ifndef SONAR_TIMEOUT_MS
# define SONAR_TIMEOUT_MS 1000 // desired sonar alt will reset to current sonar alt after this many milliseconds without a good sonar alt
#ifndef RANGEFINDER_TIMEOUT_MS
# define RANGEFINDER_TIMEOUT_MS 1000 // desired rangefinder alt will reset to current rangefinder alt after this many milliseconds without a good rangefinder alt
#endif
#ifndef SONAR_TILT_CORRECTION // by disable tilt correction for use of range finder data by EKF
# define SONAR_TILT_CORRECTION DISABLED
#ifndef RANGEFINDER_WPNAV_FILT_HZ
# define RANGEFINDER_WPNAV_FILT_HZ 0.25f // filter frequency for rangefinder altitude provided to waypoint navigation class
#endif
#ifndef RANGEFINDER_TILT_CORRECTION // by disable tilt correction for use of range finder data by EKF
# define RANGEFINDER_TILT_CORRECTION ENABLED
#endif

6
ArduSub/control_althold.cpp
View File

@ -128,9 +128,9 @@ void Sub::althold_run()
// 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 throttle controller
if (sonar_enabled && (sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) {
// if sonar is ok, use surface tracking
// 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);
}

13
ArduSub/control_auto.cpp
View File

@ -25,6 +25,12 @@ bool Sub::auto_init(bool ignore_checks)
if ((position_ok() && mission.num_commands() > 1) || ignore_checks) {
auto_mode = Auto_Loiter;
// reject switching to auto mode if landed with motors armed but first command is not a takeoff (reduce change of flips)
if (motors.armed() && ap.land_complete && !mission.starts_with_takeoff_cmd()) {
gcs_send_text(MAV_SEVERITY_CRITICAL, "Auto: Missing Takeoff Cmd");
return false;
}
// stop ROI from carrying over from previous runs of the mission
// To-Do: reset the yaw as part of auto_wp_start when the previous command was not a wp command to remove the need for this special ROI check
if (auto_yaw_mode == AUTO_YAW_ROI) {
@ -195,6 +201,7 @@ void Sub::auto_wp_start(const Location_Class& dest_loc)
if (!wp_nav.set_wp_destination(dest_loc)) {
// failure to set destination can only be because of missing terrain data
failsafe_terrain_on_event();
return;
}
// initialise yaw
@ -261,9 +268,9 @@ void Sub::auto_spline_start(const Location_Class& destination, bool stopped_at_s
// initialise wpnav
if (!wp_nav.set_spline_destination(destination, stopped_at_start, seg_end_type, next_destination)) {
// failure to set destination (likely because of missing terrain data)
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION);
// To-Do: handle failure
// failure to set destination can only be because of missing terrain data
failsafe_terrain_on_event();
return;
}
// initialise yaw

6
ArduSub/control_circle.cpp
View File

@ -85,9 +85,9 @@ void Sub::circle_run()
attitude_control.input_euler_angle_roll_pitch_yaw(circle_nav.get_roll(), circle_nav.get_pitch(), circle_nav.get_yaw(),true);
}
// run altitude controller
if (sonar_enabled && (sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) {
// if sonar is ok, use surface tracking
// 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

28
ArduSub/control_guided.cpp
View File

@ -166,22 +166,35 @@ void Sub::guided_angle_control_start()
}
// guided_set_destination - sets guided mode's target destination
void Sub::guided_set_destination(const Vector3f& destination)
// Returns true if the fence is enabled and guided waypoint is within the fence
// else return false if the waypoint is outside the fence
bool Sub::guided_set_destination(const Vector3f& destination)
{
// ensure we are in position control mode
if (guided_mode != Guided_WP) {
guided_pos_control_start();
}
#if AC_FENCE == ENABLED
// reject destination if outside the fence
if (!fence.check_destination_within_fence(pv_alt_above_home(destination.z)*0.01f, pv_distance_to_home_cm(destination)*0.01f)) {
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
// failure is propagated to GCS with NAK
return false;
}
#endif
// no need to check return status because terrain data is not used
wp_nav.set_wp_destination(destination, false);
// log target
Log_Write_GuidedTarget(guided_mode, destination, Vector3f());
return true;
}
// sets guided mode's target from a Location object
// returns false if destination could not be set (probably caused by missing terrain data)
// or if the fence is enabled and guided waypoint is outside the fence
bool Sub::guided_set_destination(const Location_Class& dest_loc)
{
// ensure we are in position control mode
@ -189,6 +202,19 @@ bool Sub::guided_set_destination(const Location_Class& dest_loc)
guided_pos_control_start();
}
#if AC_FENCE == ENABLED
// reject destination outside the fence.
// Note: there is a danger that a target specified as a terrain altitude might not be checked if the conversion to alt-above-home fails
int32_t alt_target_cm;
if (dest_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_HOME, alt_target_cm)) {
if (!fence.check_destination_within_fence(alt_target_cm*0.01f, get_distance_cm(ahrs.get_home(), dest_loc)*0.01f)) {
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
// failure is propagated to GCS with NAK
return false;
}
}
#endif
if (!wp_nav.set_wp_destination(dest_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);

10
ArduSub/control_land.cpp
View File

@ -216,10 +216,10 @@ void Sub::land_nogps_run()
// should be called at 100hz or higher
float Sub::get_land_descent_speed()
{
#if CONFIG_SONAR == ENABLED
bool sonar_ok = sonar_enabled && (sonar.status() == RangeFinder::RangeFinder_Good);
#if RANGEFINDER_ENABLED == ENABLED
bool rangefinder_ok = rangefinder_state.enabled && rangefinder_state.alt_healthy;
#else
bool sonar_ok = false;
bool rangefinder_ok = false;
#endif
// get position controller's target altitude above terrain
@ -234,8 +234,8 @@ float Sub::get_land_descent_speed()
Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA);
}
// if we are above 10m and the sonar does not sense anything perform regular alt hold descent
if (target_alt_cm <= LAND_START_ALT && !(sonar_ok && sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) {
// 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);

6
ArduSub/control_loiter.cpp
View File

@ -150,9 +150,9 @@ void Sub::loiter_run()
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
// run altitude controller
if (sonar_enabled && (sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) {
// if sonar is ok, use surface tracking
// 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);
}

6
ArduSub/control_poshold.cpp
View File

@ -519,9 +519,9 @@ void Copter::poshold_run()
// update attitude controller targets
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(poshold.roll, poshold.pitch, target_yaw_rate);
// throttle control
if (sonar_enabled && (sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) {
// if sonar is ok, use surface tracking
// 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

2
ArduSub/control_rtl.cpp
View File

@ -13,7 +13,7 @@
bool Sub::rtl_init(bool ignore_checks)
{
if (position_ok() || ignore_checks) {
rtl_build_path(true);
rtl_build_path(!failsafe.terrain);
rtl_climb_start();
return true;
}else{

6
ArduSub/control_sport.cpp
View File

@ -107,9 +107,9 @@ void Sub::sport_run()
// call attitude controller
attitude_control.input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate);
// call throttle controller
if (sonar_enabled && (sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) {
// if sonar is ok, use surface tracking
// 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);
}

17
ArduSub/defines.h
View File

@ -45,9 +45,9 @@ enum aux_sw_func {
AUXSW_SAVE_TRIM = 5, // save current position as level
AUXSW_SAVE_WP = 7, // save mission waypoint or RTL if in auto mode
AUXSW_CAMERA_TRIGGER = 9, // trigger camera servo or relay
AUXSW_SONAR = 10, // allow enabling or disabling sonar in flight which helps avoid surface tracking when you are far above the ground
AUXSW_FENCE = 11, // allow enabling or disabling fence in flight
AUXSW_RESETTOARMEDYAW = 12, // changes yaw to be same as when quad was armed
AUXSW_RANGEFINDER = 10, // allow enabling or disabling rangefinder in flight which helps avoid surface tracking when you are far above the ground
AUXSW_FENCE = 11, // allow enabling or disabling fence in flight
AUXSW_RESETTOARMEDYAW = 12, // changes yaw to be same as when quad was armed
AUXSW_SUPERSIMPLE_MODE = 13, // change to simple mode in middle, super simple at top
AUXSW_ACRO_TRAINER = 14, // low = disabled, middle = leveled, high = leveled and limited
AUXSW_SPRAYER = 15, // enable/disable the crop sprayer
@ -157,8 +157,7 @@ enum tuning_func {
TUNING_DECLINATION = 38, // compass declination in radians
TUNING_CIRCLE_RATE = 39, // circle turn rate in degrees (hard coded to about 45 degrees in either direction)
TUNING_ACRO_YAW_KP = 40, // acro controller's P term. converts pilot input to a desired roll, pitch or yaw rate
TUNING_SONAR_GAIN = 41, // sonar gain
TUNING_EKF_VERTICAL_POS = 42, // EKF's baro vs accel (higher rely on accels more, baro impact is reduced). Range should be 0.2 ~ 4.0? 2.0 is default
TUNING_RANGEFINDER_GAIN = 41, // rangefinder gain TUNING_EKF_VERTICAL_POS = 42, // EKF's baro vs accel (higher rely on accels more, baro impact is reduced). Range should be 0.2 ~ 4.0? 2.0 is default
TUNING_EKF_HORIZONTAL_POS = 43, // EKF's gps vs accel (higher rely on accels more, gps impact is reduced). Range should be 1.0 ~ 3.0? 1.5 is default
TUNING_EKF_ACCEL_NOISE = 44, // EKF's accel noise (lower means trust accels more, gps & baro less). Range should be 0.02 ~ 0.5 0.5 is default (but very robust at that level)
TUNING_RC_FEEL_RP = 45, // roll-pitch input smoothing
@ -313,17 +312,12 @@ enum ThrowModeState {
#define MASK_LOG_ANY 0xFFFF
// DATA - event logging
#define DATA_MAVLINK_FLOAT 1
#define DATA_MAVLINK_INT32 2
#define DATA_MAVLINK_INT16 3
#define DATA_MAVLINK_INT8 4
#define DATA_AP_STATE 7
#define DATA_SYSTEM_TIME_SET 8
#define DATA_INIT_SIMPLE_BEARING 9
#define DATA_ARMED 10
#define DATA_DISARMED 11
#define DATA_AUTO_ARMED 15
#define DATA_TAKEOFF 16
#define DATA_LAND_COMPLETE_MAYBE 17
#define DATA_LAND_COMPLETE 18
#define DATA_NOT_LANDED 28
@ -344,7 +338,6 @@ enum ThrowModeState {
#define DATA_AUTOTUNE_SAVEDGAINS 37
#define DATA_SAVE_TRIM 38
#define DATA_SAVEWP_ADD_WP 39
#define DATA_SAVEWP_CLEAR_MISSION_RTL 40
#define DATA_FENCE_ENABLE 41
#define DATA_FENCE_DISABLE 42
#define DATA_ACRO_TRAINER_DISABLED 43
@ -352,7 +345,6 @@ enum ThrowModeState {
#define DATA_ACRO_TRAINER_LIMITED 45
#define DATA_EPM_GRAB 46
#define DATA_EPM_RELEASE 47
#define DATA_EPM_NEUTRAL 48 // deprecated
#define DATA_PARACHUTE_DISABLED 49
#define DATA_PARACHUTE_ENABLED 50
#define DATA_PARACHUTE_RELEASED 51
@ -422,6 +414,7 @@ enum ThrowModeState {
#define ERROR_CODE_FAILED_TO_SET_DESTINATION 2
#define ERROR_CODE_RESTARTED_RTL 3
#define ERROR_CODE_FAILED_CIRCLE_INIT 4
#define ERROR_CODE_DEST_OUTSIDE_FENCE 5
// parachute failed to deploy because of low altitude or landed
#define ERROR_CODE_PARACHUTE_TOO_LOW 2

7
ArduSub/position_vector.cpp
View File

@ -66,3 +66,10 @@ float Sub::pv_get_horizontal_distance_cm(const Vector3f &origin, const Vector3f
{
return pythagorous2(destination.x-origin.x,destination.y-origin.y);
}
// returns distance between a destination and home in cm
float Sub::pv_distance_to_home_cm(const Vector3f &destination)
{
Vector3f home = pv_location_to_vector(ahrs.get_home());
return pv_get_horizontal_distance_cm(home, destination);
}

4
ArduSub/precision_landing.cpp
View File

@ -18,8 +18,8 @@ void Copter::update_precland()
float final_alt = current_loc.alt;
// use range finder altitude if it is valid
if (sonar_enabled && (sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) {
final_alt = sonar_alt;
if (rangefinder_alt_ok()) {
final_alt = rangefinder_state.alt_cm;
}
copter.precland.update(final_alt);

72
ArduSub/sensors.cpp
View File

@ -26,49 +26,61 @@ void Sub::read_barometer(void)
motors.set_air_density_ratio(barometer.get_air_density_ratio());
}
#if CONFIG_SONAR == ENABLED
void Sub::init_sonar(void)
void Sub::init_rangefinder(void)
{
sonar.init();
}
#if RANGEFINDER_ENABLED == ENABLED
rangefinder.init();
rangefinder_state.alt_cm_filt.set_cutoff_frequency(RANGEFINDER_WPNAV_FILT_HZ);
rangefinder_state.enabled = (rangefinder.num_sensors() >= 1);
#endif
}
// return sonar altitude in centimeters
int16_t Sub::read_sonar(void)
// return rangefinder altitude in centimeters
void Sub::read_rangefinder(void)
{
#if CONFIG_SONAR == ENABLED
sonar.update();
#if RANGEFINDER_ENABLED == ENABLED
rangefinder.update();
// exit immediately if sonar is disabled
if (sonar.status() != RangeFinder::RangeFinder_Good) {
sonar_alt_health = 0;
return 0;
}
rangefinder_state.alt_healthy = ((rangefinder.status() == RangeFinder::RangeFinder_Good) && (rangefinder.range_valid_count() >= RANGEFINDER_HEALTH_MAX));
int16_t temp_alt = sonar.distance_cm();
int16_t temp_alt = rangefinder.distance_cm();
if (temp_alt >= sonar.min_distance_cm() &&
temp_alt <= sonar.max_distance_cm() * SONAR_RELIABLE_DISTANCE_PCT) {
if ( sonar_alt_health < SONAR_ALT_HEALTH_MAX ) {
sonar_alt_health++;
}
}else{
sonar_alt_health = 0;
}
#if SONAR_TILT_CORRECTION == 1
// correct alt for angle of the sonar
float temp = ahrs.cos_pitch() * ahrs.cos_roll();
temp = MAX(temp, 0.707f);
temp_alt = (float)temp_alt * temp;
#if RANGEFINDER_TILT_CORRECTION == ENABLED
// correct alt for angle of the rangefinder
temp_alt = (float)temp_alt * MAX(0.707f, ahrs.get_rotation_body_to_ned().c.z);
#endif
return temp_alt;
rangefinder_state.alt_cm = temp_alt;
// filter rangefinder for use by AC_WPNav
uint32_t now = AP_HAL::millis();
if (rangefinder_state.alt_healthy) {
if (now - rangefinder_state.last_healthy_ms > RANGEFINDER_TIMEOUT_MS) {
// reset filter if we haven't used it within the last second
rangefinder_state.alt_cm_filt.reset(rangefinder_state.alt_cm);
} else {
rangefinder_state.alt_cm_filt.apply(rangefinder_state.alt_cm, 0.05f);
}
rangefinder_state.last_healthy_ms = now;
}
// send rangefinder altitude and health to waypoint navigation library
wp_nav.set_rangefinder_alt(rangefinder_state.enabled, rangefinder_state.alt_healthy, rangefinder_state.alt_cm_filt.get());
#else
return 0;
rangefinder_state.enabled = false;
rangefinder_state.alt_healthy = false;
rangefinder_state.alt_cm = 0;
#endif
}
// return true if rangefinder_alt can be used
bool Sub::rangefinder_alt_ok()
{
return (rangefinder_state.enabled && rangefinder_state.alt_healthy);
}
/*
update RPM sensors
*/

14
ArduSub/switches.cpp
View File

@ -228,7 +228,7 @@ void Sub::init_aux_switch_function(int8_t ch_option, uint8_t ch_flag)
// init channel options
switch(ch_option) {
case AUXSW_SIMPLE_MODE:
case AUXSW_SONAR:
case AUXSW_RANGEFINDER:
case AUXSW_FENCE:
case AUXSW_RESETTOARMEDYAW:
case AUXSW_SUPERSIMPLE_MODE:
@ -352,14 +352,14 @@ void Sub::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
break;
#endif
case AUXSW_SONAR:
case AUXSW_RANGEFINDER:
// enable or disable the sonar
#if CONFIG_SONAR == ENABLED
if (ch_flag == AUX_SWITCH_HIGH) {
sonar_enabled = true;
#if RANGEFINDER_ENABLED == ENABLED
if ((ch_flag == AUX_SWITCH_HIGH) && (rangefinder.num_sensors() >= 1)) {
rangefinder_state.enabled = true;
}else{
sonar_enabled = false;
}
rangefinder_state.enabled = false;
}
#endif
break;

6
ArduSub/system.cpp
View File

@ -255,9 +255,9 @@ void Sub::init_ardupilot()
//-----------------------------
init_barometer(true);
// initialise sonar
#if CONFIG_SONAR == ENABLED
init_sonar();
// initialise rangefinder
#if RANGEFINDER_ENABLED == ENABLED
init_rangefinder();
#endif
// initialise AP_RPM library

4
ArduSub/terrain.cpp
View File

@ -10,10 +10,10 @@ void Sub::terrain_update()
// tell the rangefinder our height, so it can go into power saving
// mode if available
#if CONFIG_SONAR == ENABLED
#if RANGEFINDER_ENABLED == ENABLED
float height;
if (terrain.height_above_terrain(height, true)) {
sonar.set_estimated_terrain_height(height);
rangefinder.set_estimated_terrain_height(height);
}
#endif
#endif

16
ArduSub/test.cpp
View File

@ -20,7 +20,7 @@ static const struct Menu::command test_menu_commands[] = {
{"shell", MENU_FUNC(test_shell)},
#endif
#if HIL_MODE == HIL_MODE_DISABLED
{"rangefinder", MENU_FUNC(test_sonar)},
{"rangefinder", MENU_FUNC(test_rangefinder)},
#endif
};
@ -243,21 +243,21 @@ int8_t Sub::test_shell(uint8_t argc, const Menu::arg *argv)
/*
* test the rangefinders
*/
int8_t Sub::test_sonar(uint8_t argc, const Menu::arg *argv)
int8_t Sub::test_rangefinder(uint8_t argc, const Menu::arg *argv)
{
#if CONFIG_SONAR == ENABLED
sonar.init();
#if RANGEFINDER_ENABLED == ENABLED
rangefinder.init();
cliSerial->printf("RangeFinder: %d devices detected\n", sonar.num_sensors());
cliSerial->printf("RangeFinder: %d devices detected\n", rangefinder.num_sensors());
print_hit_enter();
while(1) {
delay(100);
sonar.update();
rangefinder.update();
cliSerial->printf("Primary: status %d distance_cm %d \n", (int)sonar.status(), sonar.distance_cm());
cliSerial->printf("Primary: status %d distance_cm %d \n", (int)rangefinder.status(), rangefinder.distance_cm());
cliSerial->printf("All: device_0 type %d status %d distance_cm %d, device_1 type %d status %d distance_cm %d\n",
(int)sonar._type[0], (int)sonar.status(0), sonar.distance_cm(0), (int)sonar._type[1], (int)sonar.status(1), sonar.distance_cm(1));
(int)rangefinder._type[0], (int)rangefinder.status(0), rangefinder.distance_cm(0), (int)rangefinder._type[1], (int)rangefinder.status(1), rangefinder.distance_cm(1));
if(cliSerial->available() > 0) {
return (0);

6
ArduSub/tuning.cpp
View File

@ -116,9 +116,9 @@ void Sub::tuning() {
circle_nav.set_rate((float)g.rc_6.control_in/25.0f-20.0f);
break;
case TUNING_SONAR_GAIN:
// set sonar gain
g.sonar_gain.set(tuning_value);
case TUNING_RANGEFINDER_GAIN:
// set rangefinder gain
g.rangefinder_gain.set(tuning_value);
break;
#if 0