mirror of https://github.com/ArduPilot/ardupilot
Sub: Update stale references to Copter
This commit is contained in:
parent
65cd28cc6c
commit
1990aa7829
|
@ -316,7 +316,7 @@ void Sub::ten_hz_logging_loop()
|
|||
void Sub::twentyfive_hz_logging()
|
||||
{
|
||||
#if HIL_MODE != HIL_MODE_DISABLED
|
||||
// HIL for a copter needs very fast update of the servo values
|
||||
// HIL needs very fast update of the servo values
|
||||
gcs_send_message(MSG_RADIO_OUT);
|
||||
#endif
|
||||
|
||||
|
|
|
@ -130,7 +130,7 @@ float Sub::get_pilot_desired_climb_rate(float throttle_control)
|
|||
return desired_rate;
|
||||
}
|
||||
|
||||
// get_surface_tracking_climb_rate - hold copter at the desired distance above the ground
|
||||
// get_surface_tracking_climb_rate - hold vehicle 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)
|
||||
{
|
||||
|
|
|
@ -212,7 +212,7 @@ NOINLINE void Sub::send_extended_status1(mavlink_channel_t chan)
|
|||
case AP_Terrain::TerrainStatusDisabled:
|
||||
break;
|
||||
case AP_Terrain::TerrainStatusUnhealthy:
|
||||
// To-Do: restore unhealthy terrain status reporting once terrain is used in copter
|
||||
// To-Do: restore unhealthy terrain status reporting once terrain is used in Sub
|
||||
//control_sensors_present |= MAV_SYS_STATUS_TERRAIN;
|
||||
//control_sensors_enabled |= MAV_SYS_STATUS_TERRAIN;
|
||||
//break;
|
||||
|
|
|
@ -313,42 +313,42 @@ const AP_Param::Info Sub::var_info[] = {
|
|||
// @Param: CH7_OPT
|
||||
// @DisplayName: Channel 7 option
|
||||
// @Description: Select which function if performed when CH7 is above 1800 pwm
|
||||
// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw
|
||||
// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost vehicle Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw
|
||||
// @User: Standard
|
||||
GSCALAR(ch7_option, "CH7_OPT", AUXSW_DO_NOTHING),
|
||||
|
||||
// @Param: CH8_OPT
|
||||
// @DisplayName: Channel 8 option
|
||||
// @Description: Select which function if performed when CH8 is above 1800 pwm
|
||||
// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw
|
||||
// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost vehicle Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw
|
||||
// @User: Standard
|
||||
GSCALAR(ch8_option, "CH8_OPT", AUXSW_DO_NOTHING),
|
||||
|
||||
// @Param: CH9_OPT
|
||||
// @DisplayName: Channel 9 option
|
||||
// @Description: Select which function if performed when CH9 is above 1800 pwm
|
||||
// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw
|
||||
// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost vehicle Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw
|
||||
// @User: Standard
|
||||
GSCALAR(ch9_option, "CH9_OPT", AUXSW_DO_NOTHING),
|
||||
|
||||
// @Param: CH10_OPT
|
||||
// @DisplayName: Channel 10 option
|
||||
// @Description: Select which function if performed when CH10 is above 1800 pwm
|
||||
// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw
|
||||
// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost vehicle Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw
|
||||
// @User: Standard
|
||||
GSCALAR(ch10_option, "CH10_OPT", AUXSW_DO_NOTHING),
|
||||
|
||||
// @Param: CH11_OPT
|
||||
// @DisplayName: Channel 11 option
|
||||
// @Description: Select which function if performed when CH11 is above 1800 pwm
|
||||
// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw
|
||||
// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost vehicle Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw
|
||||
// @User: Standard
|
||||
GSCALAR(ch11_option, "CH11_OPT", AUXSW_DO_NOTHING),
|
||||
|
||||
// @Param: CH12_OPT
|
||||
// @DisplayName: Channel 12 option
|
||||
// @Description: Select which function if performed when CH12 is above 1800 pwm
|
||||
// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw
|
||||
// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost vehicle Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw
|
||||
// @User: Standard
|
||||
GSCALAR(ch12_option, "CH12_OPT", AUXSW_DO_NOTHING),
|
||||
#endif
|
||||
|
|
|
@ -69,10 +69,10 @@
|
|||
#include <AP_Airspeed/AP_Airspeed.h> // needed for AHRS build
|
||||
#include <AP_Vehicle/AP_Vehicle.h> // needed for AHRS build
|
||||
#include <AP_InertialNav/AP_InertialNav.h> // ArduPilot Mega inertial navigation library
|
||||
#include <AC_WPNav/AC_WPNav.h> // ArduCopter waypoint navigation library
|
||||
#include <AC_WPNav/AC_WPNav.h> // Waypoint navigation library
|
||||
#include <AC_WPNav/AC_Circle.h> // circle navigation library
|
||||
#include <AP_Declination/AP_Declination.h> // ArduPilot Mega Declination Helper Library
|
||||
#include <AC_Fence/AC_Fence.h> // ArduCopter Fence library
|
||||
#include <AC_Fence/AC_Fence.h> // Fence library
|
||||
|
||||
#include <AP_Scheduler/AP_Scheduler.h> // main loop scheduler
|
||||
#include <AP_Notify/AP_Notify.h> // Notify library
|
||||
|
@ -106,7 +106,7 @@
|
|||
#endif
|
||||
|
||||
#if AVOIDANCE_ENABLED == ENABLED
|
||||
#include <AC_Avoidance/AC_Avoid.h> // Arducopter stop at fence library
|
||||
#include <AC_Avoidance/AC_Avoid.h> // Stop at fence library
|
||||
#endif
|
||||
|
||||
// Local modules
|
||||
|
|
|
@ -92,7 +92,7 @@ bool Sub::start_command(const AP_Mission::Mission_Command& cmd)
|
|||
break;
|
||||
|
||||
case MAV_CMD_DO_SET_ROI: // 201
|
||||
// point the copter and camera at a region of interest (ROI)
|
||||
// point the vehicle and camera at a region of interest (ROI)
|
||||
do_roi(cmd);
|
||||
break;
|
||||
|
||||
|
@ -782,7 +782,7 @@ void Sub::do_set_home(const AP_Mission::Mission_Command& cmd)
|
|||
|
||||
// do_roi - starts actions required by MAV_CMD_NAV_ROI
|
||||
// this involves either moving the camera to point at the ROI (region of interest)
|
||||
// and possibly rotating the copter to point at the ROI if our mount type does not support a yaw feature
|
||||
// and possibly rotating the vehicle to point at the ROI if our mount type does not support a yaw feature
|
||||
// TO-DO: add support for other features of MAV_CMD_DO_SET_ROI including pointing at a given waypoint
|
||||
void Sub::do_roi(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
|
|
|
@ -35,7 +35,7 @@
|
|||
//////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
#ifndef CONFIG_HAL_BOARD
|
||||
#error CONFIG_HAL_BOARD must be defined to build ArduCopter
|
||||
#error CONFIG_HAL_BOARD must be defined to build ArduSub
|
||||
#endif
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
|
@ -452,7 +452,7 @@
|
|||
#endif
|
||||
|
||||
#ifndef YAW_LOOK_AHEAD_MIN_SPEED
|
||||
# define YAW_LOOK_AHEAD_MIN_SPEED 100 // minimum ground speed in cm/s required before copter is aimed at ground course
|
||||
# define YAW_LOOK_AHEAD_MIN_SPEED 100 // minimum ground speed in cm/s required before vehicle is aimed at ground course
|
||||
#endif
|
||||
|
||||
// Super Simple mode
|
||||
|
|
|
@ -90,7 +90,7 @@ void Sub::get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16
|
|||
// calculate yaw rate request
|
||||
rate_bf_request.z = yaw_in * g.acro_yaw_p;
|
||||
|
||||
// calculate earth frame rate corrections to pull the copter back to level while in ACRO mode
|
||||
// calculate earth frame rate corrections to pull the vehicle back to level while in ACRO mode
|
||||
|
||||
if (g.acro_trainer != ACRO_TRAINER_DISABLED) {
|
||||
// Calculate trainer mode earth frame rate command for roll
|
||||
|
|
|
@ -39,7 +39,7 @@ void Sub::althold_run()
|
|||
|
||||
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)
|
||||
// Sub vehicles 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;
|
||||
|
|
|
@ -124,10 +124,10 @@ void Sub::auto_wp_run()
|
|||
{
|
||||
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
|
||||
if (!motors.armed() || !motors.get_interlock()) {
|
||||
// To-Do: reset waypoint origin to current location because copter is probably on the ground so we don't want it lurching left or right on take-off
|
||||
// To-Do: reset waypoint origin to current location because vehicle is probably on the ground so we don't want it lurching left or right on take-off
|
||||
// (of course it would be better if people just used take-off)
|
||||
// call attitude controller
|
||||
// multicopters do not stabilize roll/pitch/yaw when disarmed
|
||||
// Sub vehicles do not stabilize roll/pitch/yaw when disarmed
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
|
||||
|
@ -211,9 +211,9 @@ void Sub::auto_spline_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()) {
|
||||
// To-Do: reset waypoint origin to current location because copter is probably on the ground so we don't want it lurching left or right on take-off
|
||||
// To-Do: reset waypoint origin to current location because vehicle is probably on the ground so we don't want it lurching left or right on take-off
|
||||
// (of course it would be better if people just used take-off)
|
||||
// multicopters do not stabilize roll/pitch/yaw when disarmed
|
||||
// Sub vehicles do not stabilize roll/pitch/yaw when disarmed
|
||||
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
|
||||
|
@ -283,7 +283,7 @@ void Sub::auto_land_run()
|
|||
// 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
|
||||
// Sub vehicles do not stabilize roll/pitch/yaw when disarmed
|
||||
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
|
||||
// set target to current position
|
||||
|
@ -433,7 +433,7 @@ void Sub::auto_loiter_run()
|
|||
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
|
||||
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 disarmed
|
||||
// Sub vehicles do not stabilize roll/pitch/yaw when disarmed
|
||||
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
|
||||
return;
|
||||
|
@ -655,7 +655,7 @@ float Sub::get_auto_heading(void)
|
|||
case AUTO_YAW_LOOK_AT_NEXT_WP:
|
||||
default:
|
||||
// point towards next waypoint.
|
||||
// we don't use wp_bearing because we don't want the copter to turn too much during flight
|
||||
// we don't use wp_bearing because we don't want the vehicle to turn too much during flight
|
||||
return wp_nav.get_yaw();
|
||||
break;
|
||||
}
|
||||
|
|
|
@ -45,7 +45,7 @@ void Sub::circle_run()
|
|||
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
|
||||
// To-Do: add some initialisation of position controllers
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
// multicopters do not stabilize roll/pitch/yaw when disarmed
|
||||
// Sub vehicles do not stabilize roll/pitch/yaw when disarmed
|
||||
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
|
||||
pos_control.set_alt_target_to_current_alt();
|
||||
|
|
|
@ -282,7 +282,7 @@ void Sub::guided_pos_control_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
|
||||
// Sub vehicles do not stabilize roll/pitch/yaw when disarmed
|
||||
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
|
||||
return;
|
||||
|
@ -326,7 +326,7 @@ void Sub::guided_vel_control_run()
|
|||
// initialise velocity controller
|
||||
pos_control.init_vel_controller_xyz();
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
// multicopters do not stabilize roll/pitch/yaw when disarmed
|
||||
// Sub vehicles do not stabilize roll/pitch/yaw when disarmed
|
||||
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
|
||||
return;
|
||||
|
@ -374,7 +374,7 @@ void Sub::guided_posvel_control_run()
|
|||
pos_control.set_pos_target(inertial_nav.get_position());
|
||||
pos_control.set_desired_velocity(Vector3f(0,0,0));
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
// multicopters do not stabilize roll/pitch/yaw when disarmed
|
||||
// Sub vehicles do not stabilize roll/pitch/yaw when disarmed
|
||||
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
|
||||
return;
|
||||
|
@ -440,7 +440,7 @@ void Sub::guided_angle_control_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
|
||||
// Sub vehicles do not stabilize roll/pitch/yaw when disarmed
|
||||
attitude_control.set_throttle_out_unstabilized(0.0f,true,g.throttle_filt);
|
||||
|
||||
pos_control.relax_alt_hold_controllers(0.0f);
|
||||
|
|
|
@ -1,6 +1,6 @@
|
|||
#include "Sub.h"
|
||||
|
||||
// Code to detect a crash main ArduCopter code
|
||||
// Code to detect a crash
|
||||
#define CRASH_CHECK_TRIGGER_SEC 2 // 2 seconds inverted indicates a crash
|
||||
#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
|
||||
|
|
|
@ -19,7 +19,7 @@ enum autopilot_yaw_mode {
|
|||
AUTO_YAW_LOOK_AT_NEXT_WP = 1, // point towards next waypoint (no pilot input accepted)
|
||||
AUTO_YAW_ROI = 2, // point towards a location held in roi_WP (no pilot input accepted)
|
||||
AUTO_YAW_LOOK_AT_HEADING = 3, // point towards a particular angle (not pilot input accepted)
|
||||
AUTO_YAW_LOOK_AHEAD = 4, // point in the direction the copter is moving
|
||||
AUTO_YAW_LOOK_AHEAD = 4, // point in the direction the vehicle is moving
|
||||
AUTO_YAW_RESETTOARMEDYAW = 5, // point towards heading at time motors were armed
|
||||
AUTO_YAW_CORRECT_XTRACK = 6 // steer the sub in order to correct for crosstrack error during line following
|
||||
};
|
||||
|
@ -75,7 +75,7 @@ enum aux_sw_func {
|
|||
// No landing gear for sub, remove
|
||||
// AUXSW_LANDING_GEAR = 29, // Landing gear controller
|
||||
|
||||
AUXSW_LOST_COPTER_SOUND = 30, // Play lost copter sound
|
||||
AUXSW_LOST_VEHICLE_SOUND = 30, // Play lost vehicle sound
|
||||
AUXSW_MOTOR_ESTOP = 31, // Emergency Stop Switch
|
||||
AUXSW_MOTOR_INTERLOCK = 32, // Motor On/Off switch
|
||||
// AUXSW_BRAKE = 33, // Brake flight mode
|
||||
|
|
|
@ -1,6 +1,6 @@
|
|||
#include "Sub.h"
|
||||
|
||||
// Code to integrate AC_Fence library with main ArduCopter code
|
||||
// Code to integrate AC_Fence library with main ArduSub code
|
||||
|
||||
#if AC_FENCE == ENABLED
|
||||
|
||||
|
|
|
@ -187,7 +187,7 @@ void Sub::lost_vehicle_check()
|
|||
static uint8_t soundalarm_counter;
|
||||
|
||||
// disable if aux switch is setup to vehicle alarm as the two could interfere
|
||||
if (check_if_auxsw_mode_used(AUXSW_LOST_COPTER_SOUND)) {
|
||||
if (check_if_auxsw_mode_used(AUXSW_LOST_VEHICLE_SOUND)) {
|
||||
return;
|
||||
}
|
||||
|
||||
|
@ -196,7 +196,7 @@ void Sub::lost_vehicle_check()
|
|||
if (soundalarm_counter >= LOST_VEHICLE_DELAY) {
|
||||
if (AP_Notify::flags.vehicle_lost == false) {
|
||||
AP_Notify::flags.vehicle_lost = true;
|
||||
gcs_send_text(MAV_SEVERITY_NOTICE,"Locate Copter alarm");
|
||||
gcs_send_text(MAV_SEVERITY_NOTICE,"Locate vehicle alarm");
|
||||
}
|
||||
} else {
|
||||
soundalarm_counter++;
|
||||
|
|
|
@ -126,7 +126,7 @@ void Sub::read_radio()
|
|||
// set_throttle_zero_flag - set throttle_zero flag from debounced throttle control
|
||||
// throttle_zero is used to determine if the pilot intends to shut down the motors
|
||||
// Basically, this signals when we are not flying. We are either on the ground
|
||||
// or the pilot has shut down the copter in the air and it is free-falling
|
||||
// or the pilot has shut down the vehicle in the air and it is free-falling
|
||||
void Sub::set_throttle_zero_flag(int16_t throttle_control)
|
||||
{
|
||||
static uint32_t last_nonzero_throttle_ms = 0;
|
||||
|
|
|
@ -345,7 +345,7 @@ void Sub::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
|
|||
ServoRelayEvents.do_set_relay(0, ch_flag == AUX_SWITCH_HIGH);
|
||||
break;
|
||||
|
||||
case AUXSW_LOST_COPTER_SOUND:
|
||||
case AUXSW_LOST_VEHICLE_SOUND:
|
||||
switch (ch_flag) {
|
||||
case AUX_SWITCH_HIGH:
|
||||
AP_Notify::flags.vehicle_lost = true;
|
||||
|
@ -388,7 +388,7 @@ void Sub::save_trim()
|
|||
}
|
||||
|
||||
// auto_trim - slightly adjusts the ahrs.roll_trim and ahrs.pitch_trim towards the current stick positions
|
||||
// meant to be called continuously while the pilot attempts to keep the copter level
|
||||
// meant to be called continuously while the pilot attempts to keep the vehicle level
|
||||
void Sub::auto_trim()
|
||||
{
|
||||
if (auto_trim_counter > 0) {
|
||||
|
|
Loading…
Reference in New Issue