mirror of https://github.com/ArduPilot/ardupilot
Rover: move motors_active to be motors.active
This will help when we come to subclass the Motors library.
This commit is contained in:
parent
65b7ca3fbb
commit
cf1f2f9aeb
|
@ -105,7 +105,7 @@ constexpr int8_t Rover::_failsafe_priorities[7];
|
|||
*/
|
||||
void Rover::stats_update(void)
|
||||
{
|
||||
g2.stats.set_flying(motor_active());
|
||||
g2.stats.set_flying(g2.motors.active());
|
||||
g2.stats.update();
|
||||
}
|
||||
#endif
|
||||
|
|
|
@ -673,3 +673,24 @@ float AP_MotorsUGV::get_scaled_throttle(float throttle) const
|
|||
const float throttle_pct = constrain_float(throttle, -100.0f, 100.0f) / 100.0f;
|
||||
return 100.0f * sign * ((_thrust_curve_expo - 1.0f) + safe_sqrt((1.0f - _thrust_curve_expo) * (1.0f - _thrust_curve_expo) + 4.0f * _thrust_curve_expo * fabsf(throttle_pct))) / (2.0f * _thrust_curve_expo);
|
||||
}
|
||||
|
||||
// return true if motors are moving
|
||||
bool AP_MotorsUGV::active() const
|
||||
{
|
||||
// if soft disarmed, motors not active
|
||||
if (!hal.util->get_soft_armed()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// check throttle is active
|
||||
if (!is_zero(get_throttle())) {
|
||||
return true;
|
||||
}
|
||||
|
||||
// skid-steering vehicles active when steering
|
||||
if (have_skid_steering() && !is_zero(get_steering())) {
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
|
|
@ -28,6 +28,9 @@ public:
|
|||
// initialise motors
|
||||
void init();
|
||||
|
||||
// return true if motors are active
|
||||
bool active() const;
|
||||
|
||||
// setup output in case of main CPU failure
|
||||
void setup_safety_output();
|
||||
|
||||
|
|
|
@ -443,7 +443,6 @@ private:
|
|||
void do_aux_function_change_mode(Mode &mode,
|
||||
const aux_switch_pos ch_flag);
|
||||
void read_aux_switch();
|
||||
bool motor_active();
|
||||
|
||||
// crash_check.cpp
|
||||
void crash_check();
|
||||
|
|
|
@ -265,24 +265,3 @@ void Rover::read_aux_switch()
|
|||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// return true if motors are moving
|
||||
bool Rover::motor_active()
|
||||
{
|
||||
// if soft disarmed, motors not active
|
||||
if (!hal.util->get_soft_armed()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// check throttle is active
|
||||
if (!is_zero(g2.motors.get_throttle())) {
|
||||
return true;
|
||||
}
|
||||
|
||||
// skid-steering vehicles active when steering
|
||||
if (g2.motors.have_skid_steering() && !is_zero(g2.motors.get_steering())) {
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
|
|
@ -80,7 +80,7 @@ void Rover::rudder_arm_disarm_check()
|
|||
// not at full right rudder
|
||||
rudder_arm_timer = 0;
|
||||
}
|
||||
} else if (!motor_active()) {
|
||||
} else if (!g2.motors.active()) {
|
||||
// when armed and motor not active (not moving), full left rudder starts disarming counter
|
||||
if (channel_steer->get_control_in() < -4000) {
|
||||
const uint32_t now = millis();
|
||||
|
|
Loading…
Reference in New Issue