Skip to content
Snippets Groups Projects
Commit d660fb09 authored by Beat Küng's avatar Beat Küng Committed by Lorenz Meier
Browse files

tap_esc: add RPMSTOPPED macro, make sure driver starts with stopped motors

parent d6d3a561
No related branches found
No related tags found
No related merge requests found
......@@ -78,6 +78,7 @@
#define RPMMAX 1900
#define RPMMIN 1200
#define RPMSTOPPED (RPMMIN - 10)
#define MAX_BOOT_TIME_MS (500) // Minimum time to wait after Power on before sending commands
......
......@@ -417,8 +417,8 @@ void TAP_ESC:: send_esc_outputs(const float *pwm, const unsigned num_pwm)
if (rpm[i] > RPMMAX) {
rpm[i] = RPMMAX;
} else if (rpm[i] < RPMMIN) {
rpm[i] = RPMMIN;
} else if (rpm[i] < RPMSTOPPED) {
rpm[i] = RPMSTOPPED;
}
}
......@@ -678,7 +678,7 @@ TAP_ESC::cycle()
* This will be clearly visible on the servo status and will limit the risk of accidentally
* spinning motors. It would be deadly in flight.
*/
_outputs.output[i] = 900;
_outputs.output[i] = RPMSTOPPED;
}
}
......@@ -694,7 +694,7 @@ TAP_ESC::cycle()
if (test_motor_updated) {
struct test_motor_s test_motor;
orb_copy(ORB_ID(test_motor), _test_motor_sub, &test_motor);
_outputs.output[test_motor.motor_number] = 900.f + (1100.f * test_motor.value); //scale to [900, 2000]
_outputs.output[test_motor.motor_number] = RPMSTOPPED + ((RPMMAX - RPMSTOPPED) * test_motor.value);
PX4_INFO("setting motor %i to %.1lf", test_motor.motor_number,
(double)_outputs.output[test_motor.motor_number]);
}
......@@ -702,7 +702,7 @@ TAP_ESC::cycle()
/* set the invalid values to the minimum */
for (unsigned i = 0; i < num_outputs; i++) {
if (!PX4_ISFINITE(_outputs.output[i])) {
_outputs.output[i] = 900;
_outputs.output[i] = RPMSTOPPED;
}
}
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment