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

Sub: Remove unused motor emergency stop and interlock

Browse Source
This commit is contained in:
Jacob Walser 2017-04-11 12:45:16 -04:00
parent 2fac49a163
commit c16046aadf
13 changed files with 22 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
ArduSub/AP_State.cpp
View File

@ -36,17 +36,3 @@ void Sub::set_auto_armed(bool b)
Log_Write_Event(DATA_AUTO_ARMED);
}
}
void Sub::set_motor_emergency_stop(bool b)
{
if (ap.motor_emergency_stop != b) {
ap.motor_emergency_stop = b;
}
// Log new status
if (ap.motor_emergency_stop) {
Log_Write_Event(DATA_MOTORS_EMERGENCY_STOPPED);
} else {
Log_Write_Event(DATA_MOTORS_EMERGENCY_STOP_CLEARED);
}
}

3
ArduSub/Sub.h
View File

@ -239,8 +239,6 @@ private:
uint8_t system_time_set : 1; // true if the system time has been set from the GPS
uint8_t gps_base_pos_set : 1; // true when the gps base position has been set (used for RTK gps only)
enum HomeState home_state : 2; // home status (unset, set, locked)
uint8_t using_interlock : 1; // aux switch motor interlock function is in use
uint8_t motor_emergency_stop: 1; // motor estop switch, shuts off motors when enabled
uint8_t at_bottom : 1; // true if we are at the bottom
uint8_t at_surface : 1; // true if we are at the surface
uint8_t depth_sensor_present: 1; // true if we have an external baro connected
@ -470,7 +468,6 @@ private:
void set_home_state(enum HomeState new_home_state);
bool home_is_set();
void set_auto_armed(bool b);
void set_motor_emergency_stop(bool b);
float get_smoothing_gain();
void get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &roll_out, float &pitch_out, float angle_max);
float get_pilot_desired_yaw_rate(int16_t stick_angle);

2
ArduSub/control_acro.cpp
View File

@ -21,7 +21,7 @@ void Sub::acro_run()
float target_roll, target_pitch, target_yaw;
// if not armed set throttle to zero and exit immediately
if (!motors.armed() || !motors.get_interlock()) {
if (!motors.armed()) {
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
return;

2
ArduSub/control_althold.cpp
View File

@ -39,7 +39,7 @@ void Sub::althold_run()
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()) {
if (!motors.armed()) {
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// 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);

14
ArduSub/control_auto.cpp
View File

@ -111,8 +111,8 @@ void Sub::auto_wp_start(const Location_Class& dest_loc)
// called by auto_run at 100hz or more
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()) {
// if not auto armed set throttle to zero and exit immediately
if (!motors.armed()) {
// 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
@ -198,8 +198,8 @@ void Sub::auto_spline_start(const Location_Class& destination, bool stopped_at_s
// called by auto_run at 100hz or more
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()) {
// if not auto armed set throttle to zero and exit immediately
if (!motors.armed() || !ap.auto_armed) {
// 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)
// Sub vehicles do not stabilize roll/pitch/yaw when disarmed
@ -376,8 +376,8 @@ bool Sub::auto_loiter_start()
// called by auto_run at 100hz or more
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()) {
// if not auto armed set throttle to zero and exit immediately
if (!motors.armed()) {
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// Sub vehicles do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
@ -661,7 +661,7 @@ void Sub::auto_terrain_recover_run()
static uint32_t rangefinder_recovery_ms = 0;
// if not armed set throttle to zero and exit immediately
if (!motors.armed() || !motors.get_interlock()) {
if (!motors.armed()) {
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
return;

4
ArduSub/control_circle.cpp
View File

@ -39,8 +39,8 @@ void Sub::circle_run()
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 not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
// if not auto armed set throttle to zero and exit immediately
if (!motors.armed() || !ap.auto_armed) {
// To-Do: add some initialisation of position controllers
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// Sub vehicles do not stabilize roll/pitch/yaw when disarmed

8
ArduSub/control_guided.cpp
View File

@ -278,7 +278,7 @@ void Sub::guided_run()
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()) {
if (!motors.armed() || !ap.auto_armed) {
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// Sub vehicles do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
@ -327,7 +327,7 @@ void Sub::guided_pos_control_run()
void Sub::guided_vel_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()) {
if (!motors.armed() || !ap.auto_armed) {
// initialise velocity controller
pos_control.init_vel_controller_xyz();
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
@ -381,7 +381,7 @@ void Sub::guided_vel_control_run()
void Sub::guided_posvel_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()) {
if (!motors.armed() || !ap.auto_armed) {
// set target position and velocity to current position and velocity
pos_control.set_pos_target(inertial_nav.get_position());
pos_control.set_desired_velocity(Vector3f(0,0,0));
@ -457,7 +457,7 @@ void Sub::guided_posvel_control_run()
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()) {
if (!motors.armed() || !ap.auto_armed) {
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// Sub vehicles do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0.0f,true,g.throttle_filt);

2
ArduSub/control_manual.cpp
View File

@ -14,7 +14,7 @@ bool Sub::manual_init(bool ignore_checks)
void Sub::manual_run()
{
// if not armed set throttle to zero and exit immediately
if (!motors.armed() || !motors.get_interlock()) {
if (!motors.armed()) {
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
return;

4
ArduSub/control_poshold.cpp
View File

@ -41,8 +41,8 @@ void Sub::poshold_run()
// 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()) {
// if not auto armed set throttle to zero and exit immediately
if (!motors.armed() || !ap.auto_armed) {
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);

2
ArduSub/control_stabilize.cpp
View File

@ -19,7 +19,7 @@ void Sub::stabilize_run()
float target_yaw_rate;
// if not armed set throttle to zero and exit immediately
if (!motors.armed() || !motors.get_interlock()) {
if (!motors.armed()) {
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
last_pilot_heading = ahrs.yaw_sensor;

2
ArduSub/control_surface.cpp
View File

@ -28,7 +28,7 @@ void Sub::surface_run()
float target_yaw_rate;
// if not armed set throttle to zero and exit immediately
if (!motors.armed() || !motors.get_interlock()) {
if (!motors.armed()) {
motors.output_min();
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);

4
ArduSub/defines.h
View File

@ -162,10 +162,6 @@ enum RTLState {
#define DATA_ACRO_TRAINER_LIMITED 45
#define DATA_GRIPPER_GRAB 46
#define DATA_GRIPPER_RELEASE 47
#define DATA_MOTORS_EMERGENCY_STOPPED 54
#define DATA_MOTORS_EMERGENCY_STOP_CLEARED 55
#define DATA_MOTORS_INTERLOCK_DISABLED 56
#define DATA_MOTORS_INTERLOCK_ENABLED 57
#define DATA_EKF_ALT_RESET 60
#define DATA_SURFACE_CANCELLED_BY_PILOT 61
#define DATA_EKF_YAW_RESET 62

11
ArduSub/motors.cpp
View File

@ -130,16 +130,9 @@ void Sub::motors_output()
// check if we are performing the motor test
if (ap.motor_test) {
return; // Placeholder
} else {
if (!ap.using_interlock) {
// if not using interlock switch, set according to Emergency Stop status
// where Emergency Stop is forced false during arming if Emergency Stop switch
// is not used. Interlock enabled means motors run, so we must
// invert motor_emergency_stop status for motors to run.
motors.set_interlock(!ap.motor_emergency_stop);
}
motors.output();
}
motors.set_interlock(true);
motors.output();
}
// translate wpnav roll/pitch outputs to lateral/forward