From 89970e4eaa19e52ef3eae640fb2447b71ffbbcd0 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sun, 13 Mar 2016 10:05:10 +1100
Subject: [PATCH] Plane: support motor_test for quadplanes
---
ArduPlane/GCS_Mavlink.cpp | 8 +++
ArduPlane/motor_test.cpp | 118 ++++++++++++++++++++++++++++++++++++++
ArduPlane/quadplane.cpp | 7 ++-
ArduPlane/quadplane.h | 17 ++++++
4 files changed, 149 insertions(+), 1 deletion(-)
create mode 100644 ArduPlane/motor_test.cpp
diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp
index 0b6a770e07..d8dd883332 100644
--- a/ArduPlane/GCS_Mavlink.cpp
+++ b/ArduPlane/GCS_Mavlink.cpp
@@ -1539,6 +1539,14 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
#endif
+ case MAV_CMD_DO_MOTOR_TEST:
+ // param1 : motor sequence number (a number from 1 to max number of motors on the vehicle)
+ // param2 : throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through. See MOTOR_TEST_THROTTLE_TYPE enum)
+ // param3 : throttle (range depends upon param2)
+ // param4 : timeout (in seconds)
+ result = plane.quadplane.mavlink_motor_test_start(chan, (uint8_t)packet.param1, (uint8_t)packet.param2, (uint16_t)packet.param3, packet.param4);
+ break;
+
case MAV_CMD_DO_VTOL_TRANSITION:
result = plane.quadplane.handle_do_vtol_transition(packet);
break;
diff --git a/ArduPlane/motor_test.cpp b/ArduPlane/motor_test.cpp
new file mode 100644
index 0000000000..8fcce692f2
--- /dev/null
+++ b/ArduPlane/motor_test.cpp
@@ -0,0 +1,118 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#include "Plane.h"
+
+/*
+ mavlink motor test - implements the MAV_CMD_DO_MOTOR_TEST mavlink
+ command so that the quadplane pilot can test an
+ individual motor to ensure proper wiring, rotation.
+ */
+
+// motor test definitions
+#define MOTOR_TEST_PWM_MIN 800 // min pwm value accepted by the test
+#define MOTOR_TEST_PWM_MAX 2200 // max pwm value accepted by the test
+#define MOTOR_TEST_TIMEOUT_MS_MAX 30000 // max timeout is 30 seconds
+
+// motor_test_output - checks for timeout and sends updates to motors objects
+void QuadPlane::motor_test_output()
+{
+ // exit immediately if the motor test is not running
+ if (!motor_test.running) {
+ return;
+ }
+
+ // check for test timeout
+ if ((AP_HAL::millis() - motor_test.start_ms) >= motor_test.timeout_ms) {
+ // stop motor test
+ motor_test_stop();
+ return;
+ }
+
+ int16_t pwm = 0; // pwm that will be output to the motors
+
+ // calculate pwm based on throttle type
+ switch (motor_test.throttle_type) {
+ case MOTOR_TEST_THROTTLE_PERCENT:
+ // sanity check motor_test.throttle value
+ if (motor_test.throttle_value <= 100) {
+ pwm = plane.channel_throttle->radio_min + (plane.channel_throttle->radio_max - plane.channel_throttle->radio_min) * (float)motor_test.throttle_value/100.0f;
+ }
+ break;
+
+ case MOTOR_TEST_THROTTLE_PWM:
+ pwm = motor_test.throttle_value;
+ break;
+
+ case MOTOR_TEST_THROTTLE_PILOT:
+ pwm = plane.channel_throttle->radio_in;
+ break;
+
+ default:
+ motor_test_stop();
+ return;
+ }
+
+ // sanity check throttle values
+ if (pwm >= MOTOR_TEST_PWM_MIN && pwm <= MOTOR_TEST_PWM_MAX ) {
+ // turn on motor to specified pwm vlaue
+ motors->output_test(motor_test.seq, pwm);
+ } else {
+ motor_test_stop();
+ }
+}
+
+// mavlink_motor_test_start - start motor test - spin a single motor at a specified pwm
+// returns MAV_RESULT_ACCEPTED on success, MAV_RESULT_FAILED on failure
+uint8_t QuadPlane::mavlink_motor_test_start(mavlink_channel_t chan, uint8_t motor_seq, uint8_t throttle_type,
+ uint16_t throttle_value, float timeout_sec)
+{
+ if (motors->armed()) {
+ plane.gcs_send_text(MAV_SEVERITY_INFO, "Must be disarmed for motor test");
+ return MAV_RESULT_FAILED;
+ }
+ // if test has not started try to start it
+ if (!motor_test.running) {
+ // start test
+ motor_test.running = true;
+
+ // enable and arm motors
+ set_armed(true);
+
+ // turn on notify leds
+ AP_Notify::flags.esc_calibration = true;
+ }
+
+ // set timeout
+ motor_test.start_ms = AP_HAL::millis();
+ motor_test.timeout_ms = MIN(timeout_sec * 1000, MOTOR_TEST_TIMEOUT_MS_MAX);
+
+ // store required output
+ motor_test.seq = motor_seq;
+ motor_test.throttle_type = throttle_type;
+ motor_test.throttle_value = throttle_value;
+
+ // return success
+ return MAV_RESULT_ACCEPTED;
+}
+
+// motor_test_stop - stops the motor test
+void QuadPlane::motor_test_stop()
+{
+ // exit immediately if the test is not running
+ if (!motor_test.running) {
+ return;
+ }
+
+ // flag test is complete
+ motor_test.running = false;
+
+ // disarm motors
+ set_armed(false);
+
+ // reset timeout
+ motor_test.start_ms = 0;
+ motor_test.timeout_ms = 0;
+
+ // turn off notify leds
+ AP_Notify::flags.esc_calibration = false;
+}
diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp
index 445d7e0757..7414348b45 100644
--- a/ArduPlane/quadplane.cpp
+++ b/ArduPlane/quadplane.cpp
@@ -335,7 +335,7 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = {
// @Param: FRAME_CLASS
// @DisplayName: Frame Class
// @Description: Controls major frame class for multicopter component
- // @Values: 0:Quad, 1:Hexa, 2:Octa
+ // @Values: 0:Quad, 1:Hexa, 2:Octa, 3:OctaQuad
// @User: Standard
AP_GROUPINFO("FRAME_CLASS", 30, QuadPlane, frame_class, 0),
@@ -875,6 +875,11 @@ void QuadPlane::update(void)
return;
}
+ if (motor_test.running) {
+ motor_test_output();
+ return;
+ }
+
if (!in_vtol_mode()) {
update_transition();
} else {
diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h
index 4dcaa80b1d..03e83438db 100644
--- a/ArduPlane/quadplane.h
+++ b/ArduPlane/quadplane.h
@@ -200,5 +200,22 @@ private:
FRAME_CLASS_QUAD=0,
FRAME_CLASS_HEXA=1,
FRAME_CLASS_OCTA=2,
+ FRAME_CLASS_OCTAQUAD=3,
};
+
+ struct {
+ bool running;
+ uint32_t start_ms; // system time the motor test began
+ uint32_t timeout_ms = 0; // test will timeout this many milliseconds after the motor_test_start_ms
+ uint8_t seq = 0; // motor sequence number of motor being tested
+ uint8_t throttle_type = 0; // motor throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through)
+ uint16_t throttle_value = 0; // throttle to be sent to motor, value depends upon it's type
+ } motor_test;
+
+public:
+ void motor_test_output();
+ uint8_t mavlink_motor_test_start(mavlink_channel_t chan, uint8_t motor_seq, uint8_t throttle_type,
+ uint16_t throttle_value, float timeout_sec);
+private:
+ void motor_test_stop();
};