mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-18 22:03:56 -04:00
Plane: support motor_test for quadplanes
This commit is contained in:
parent
d644474817
commit
89970e4eaa
@ -1539,6 +1539,14 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|||||||
break;
|
break;
|
||||||
#endif
|
#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:
|
case MAV_CMD_DO_VTOL_TRANSITION:
|
||||||
result = plane.quadplane.handle_do_vtol_transition(packet);
|
result = plane.quadplane.handle_do_vtol_transition(packet);
|
||||||
break;
|
break;
|
||||||
|
118
ArduPlane/motor_test.cpp
Normal file
118
ArduPlane/motor_test.cpp
Normal file
@ -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;
|
||||||
|
}
|
@ -335,7 +335,7 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = {
|
|||||||
// @Param: FRAME_CLASS
|
// @Param: FRAME_CLASS
|
||||||
// @DisplayName: Frame Class
|
// @DisplayName: Frame Class
|
||||||
// @Description: Controls major frame class for multicopter component
|
// @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
|
// @User: Standard
|
||||||
AP_GROUPINFO("FRAME_CLASS", 30, QuadPlane, frame_class, 0),
|
AP_GROUPINFO("FRAME_CLASS", 30, QuadPlane, frame_class, 0),
|
||||||
|
|
||||||
@ -875,6 +875,11 @@ void QuadPlane::update(void)
|
|||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if (motor_test.running) {
|
||||||
|
motor_test_output();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
if (!in_vtol_mode()) {
|
if (!in_vtol_mode()) {
|
||||||
update_transition();
|
update_transition();
|
||||||
} else {
|
} else {
|
||||||
|
@ -200,5 +200,22 @@ private:
|
|||||||
FRAME_CLASS_QUAD=0,
|
FRAME_CLASS_QUAD=0,
|
||||||
FRAME_CLASS_HEXA=1,
|
FRAME_CLASS_HEXA=1,
|
||||||
FRAME_CLASS_OCTA=2,
|
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();
|
||||||
};
|
};
|
||||||
|
Loading…
Reference in New Issue
Block a user