Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-01 13:38:38 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: autopilot modes to check for motor interlock status

Browse Source
This commit is contained in:
Robert Lefebvre 2015-04-17 13:49:08 -04:00 committed by Randy Mackay
parent a035d5ad1b
commit 8610da7fbc
10 changed files with 37 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduCopter/control_althold.pde
View File

@ -26,8 +26,8 @@ static void althold_run()
float target_climb_rate;
float takeoff_climb_rate = 0.0f;
// if not auto armed set throttle to zero and exit immediately
if(!ap.auto_armed) {
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || !motors.get_interlock()) {
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(g.rc_3.control_in)-throttle_average);
return;

18
ArduCopter/control_auto.pde
View File

@ -109,8 +109,8 @@ static void auto_takeoff_start(float final_alt_above_home)
// called by auto_run at 100hz or more
static void auto_takeoff_run()
{
// if not auto armed set throttle to zero and exit immediately
if(!ap.auto_armed) {
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || !motors.get_interlock()) {
// initialise wpnav targets
wp_nav.shift_wp_origin_to_current_pos();
// reset attitude control targets
@ -156,8 +156,8 @@ static void auto_wp_start(const Vector3f& destination)
// called by auto_run at 100hz or more
static void auto_wp_run()
{
// if not auto armed set throttle to zero and exit immediately
if(!ap.auto_armed) {
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if(!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
// (of course it would be better if people just used take-off)
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
@ -212,8 +212,8 @@ static void auto_spline_start(const Vector3f& destination, bool stopped_at_start
// called by auto_run at 100hz or more
static void auto_spline_run()
{
// if not auto armed set throttle to zero and exit immediately
if(!ap.auto_armed) {
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if(!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
// (of course it would be better if people just used take-off)
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
@ -281,7 +281,7 @@ static void auto_land_run()
int16_t roll_control = 0, pitch_control = 0;
float target_yaw_rate = 0;
// if not auto armed set throttle to zero and exit immediately
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || ap.land_complete) {
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
// set target to current position
@ -447,8 +447,8 @@ bool auto_loiter_start()
// called by auto_run at 100hz or more
void auto_loiter_run()
{
// if not auto armed set throttle to zero and exit immediately
if(!ap.auto_armed || ap.land_complete) {
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
return;
}

4
ArduCopter/control_autotune.pde
View File

@ -260,9 +260,9 @@ static void autotune_run()
float target_yaw_rate;
int16_t target_climb_rate;
// if not auto armed set throttle to zero and exit immediately
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
// this should not actually be possible because of the autotune_init() checks
if (!ap.auto_armed) {
if (!ap.auto_armed || !motors.get_interlock()) {
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(g.rc_3.control_in)-throttle_average);
return;

4
ArduCopter/control_circle.pde
View File

@ -32,8 +32,8 @@ static void circle_run()
float target_yaw_rate = 0;
float target_climb_rate = 0;
// if not auto armed set throttle to zero and exit immediately
if(!ap.auto_armed || ap.land_complete) {
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
// To-Do: add some initialisation of position controllers
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
pos_control.set_alt_target_to_current_alt();

4
ArduCopter/control_drift.pde
View File

@ -46,8 +46,8 @@ static void drift_run()
float target_yaw_rate;
int16_t pilot_throttle_scaled;
// if not armed or landed and throttle at zero, set throttle to zero and exit immediately
if(!motors.armed() || (ap.land_complete && ap.throttle_zero)) {
// if not armed or motor interlock not enabled or landed and throttle at zero, set throttle to zero and exit immediately
if(!motors.armed() || !motors.get_interlock() || (ap.land_complete && ap.throttle_zero)) {
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
return;
}

8
ArduCopter/control_guided.pde
View File

@ -158,8 +158,8 @@ static void guided_set_destination_posvel(const Vector3f& destination, const Vec
// should be called at 100hz or more
static void guided_run()
{
// if not auto armed set throttle to zero and exit immediately
if(!ap.auto_armed) {
// if not auto armed or motors not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || !motors.get_interlock()) {
// To-Do: reset waypoint controller?
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
// To-Do: handle take-offs - these may not only be immediately after auto_armed becomes true
@ -195,8 +195,8 @@ static void guided_run()
// called by guided_run at 100hz or more
static void guided_takeoff_run()
{
// if not auto armed set throttle to zero and exit immediately
if(!ap.auto_armed) {
// if not auto armed or motors interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || !motors.get_interlock()) {
// initialise wpnav targets
wp_nav.shift_wp_origin_to_current_pos();
// reset attitude control targets

8
ArduCopter/control_land.pde
View File

@ -50,8 +50,8 @@ static void land_gps_run()
int16_t roll_control = 0, pitch_control = 0;
float target_yaw_rate = 0;
// if not auto armed or landed set throttle to zero and exit immediately
if(!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(!ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
wp_nav.init_loiter_target();
@ -123,8 +123,8 @@ static void land_nogps_run()
float target_roll = 0.0f, target_pitch = 0.0f;
float target_yaw_rate = 0;
// if not auto armed or landed set throttle to zero and exit immediately
if(!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(!ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
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

4
ArduCopter/control_loiter.pde
View File

@ -33,8 +33,8 @@ static void loiter_run()
float target_climb_rate = 0;
float takeoff_climb_rate = 0.0f;
// if not auto armed set throttle to zero and exit immediately
if(!ap.auto_armed) {
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || !motors.get_interlock()) {
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(g.rc_3.control_in)-throttle_average);

4
ArduCopter/control_poshold.pde
View File

@ -154,8 +154,8 @@ static void poshold_run()
float vel_fw, vel_right; // vehicle's current velocity in body-frame forward and right directions
const Vector3f& vel = inertial_nav.get_velocity();
// if not auto armed set throttle to zero and exit immediately
if(!ap.auto_armed) {
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || !motors.get_interlock()) {
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(g.rc_3.control_in)-throttle_average);

16
ArduCopter/control_rtl.pde
View File

@ -131,8 +131,8 @@ static void rtl_return_start()
// called by rtl_run at 100hz or more
static void rtl_climb_return_run()
{
// if not auto armed set throttle to zero and exit immediately
if(!ap.auto_armed) {
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || !motors.get_interlock()) {
// reset attitude control targets
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
// To-Do: re-initialise wpnav targets
@ -187,8 +187,8 @@ static void rtl_loiterathome_start()
// called by rtl_run at 100hz or more
static void rtl_loiterathome_run()
{
// if not auto armed set throttle to zero and exit immediately
if(!ap.auto_armed) {
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || !motors.get_interlock()) {
// reset attitude control targets
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
// To-Do: re-initialise wpnav targets
@ -257,8 +257,8 @@ static void rtl_descent_run()
int16_t roll_control = 0, pitch_control = 0;
float target_yaw_rate = 0;
// if not auto armed set throttle to zero and exit immediately
if(!ap.auto_armed) {
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || !motors.get_interlock()) {
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
// set target to current position
wp_nav.init_loiter_target();
@ -319,8 +319,8 @@ static void rtl_land_run()
{
int16_t roll_control = 0, pitch_control = 0;
float target_yaw_rate = 0;
// if not auto armed set throttle to zero and exit immediately
if(!ap.auto_armed || ap.land_complete) {
// if not auto armed or landing completed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
// set target to current position
wp_nav.init_loiter_target();