delete systemcmds/motor_test and msg/test_motor.msg

This commit is contained in:
Daniel Agar 2022-08-24 16:57:11 -04:00
parent 72efe84b80
commit bcdd2203d3
50 changed files with 1 additions and 427 deletions

View File

@ -917,7 +917,6 @@ void printTopics() {
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener system_power" || true'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener task_stack_info" || true'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener telemetry_status" || true'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener test_motor" || true'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener trajectory_setpoint" || true'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener tune_control" || true'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_acceleration" || true'

View File

@ -84,7 +84,6 @@ CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_MTD=y
CONFIG_SYSTEMCMDS_NSHTERM=y
CONFIG_SYSTEMCMDS_PARAM=y

View File

@ -38,7 +38,6 @@ CONFIG_SYSTEMCMDS_DMESG=y
CONFIG_SYSTEMCMDS_DUMPFILE=y
CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_PARAM=y
CONFIG_SYSTEMCMDS_PERF=y
CONFIG_SYSTEMCMDS_REBOOT=y

View File

@ -81,7 +81,6 @@ CONFIG_SYSTEMCMDS_GPIO=y
CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_NETMAN=y
CONFIG_SYSTEMCMDS_NSHTERM=y
CONFIG_SYSTEMCMDS_PARAM=y

View File

@ -60,7 +60,6 @@ CONFIG_MODULES_GIMBAL=y
CONFIG_MODULES_VTOL_ATT_CONTROL=y
CONFIG_SYSTEMCMDS_DYN=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_PARAM=y
CONFIG_SYSTEMCMDS_PERF=y
CONFIG_SYSTEMCMDS_SD_BENCH=y

View File

@ -87,7 +87,6 @@ CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_MTD=y
CONFIG_SYSTEMCMDS_NSHTERM=y
CONFIG_SYSTEMCMDS_PARAM=y

View File

@ -88,7 +88,6 @@ CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_MTD=y
CONFIG_SYSTEMCMDS_NSHTERM=y
CONFIG_SYSTEMCMDS_PARAM=y

View File

@ -82,7 +82,6 @@ CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_MTD=y
CONFIG_SYSTEMCMDS_NSHTERM=y
CONFIG_SYSTEMCMDS_PARAM=y

View File

@ -85,7 +85,6 @@ CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_MTD=y
CONFIG_SYSTEMCMDS_NSHTERM=y
CONFIG_SYSTEMCMDS_PARAM=y

View File

@ -62,7 +62,6 @@ CONFIG_MODULES_GIMBAL=y
CONFIG_MODULES_VTOL_ATT_CONTROL=y
CONFIG_SYSTEMCMDS_DYN=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_PARAM=y
CONFIG_SYSTEMCMDS_PERF=y
CONFIG_SYSTEMCMDS_SD_BENCH=y

View File

@ -80,7 +80,6 @@ CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_MTD=y
CONFIG_SYSTEMCMDS_NSHTERM=y
CONFIG_SYSTEMCMDS_PARAM=y

View File

@ -74,7 +74,6 @@ CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_NSHTERM=y
CONFIG_SYSTEMCMDS_PARAM=y
CONFIG_SYSTEMCMDS_PERF=y

View File

@ -90,7 +90,6 @@ CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_MTD=y
CONFIG_SYSTEMCMDS_NSHTERM=y
CONFIG_SYSTEMCMDS_PARAM=y

View File

@ -86,7 +86,6 @@ CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_MTD=y
CONFIG_SYSTEMCMDS_NSHTERM=y
CONFIG_SYSTEMCMDS_PARAM=y

View File

@ -81,7 +81,6 @@ CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_MTD=y
CONFIG_SYSTEMCMDS_NSHTERM=y
CONFIG_SYSTEMCMDS_PARAM=y

View File

@ -82,7 +82,6 @@ CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_MTD=y
CONFIG_SYSTEMCMDS_NSHTERM=y
CONFIG_SYSTEMCMDS_PARAM=y

View File

@ -83,7 +83,6 @@ CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_MTD=y
CONFIG_SYSTEMCMDS_NSHTERM=y
CONFIG_SYSTEMCMDS_PARAM=y

View File

@ -83,7 +83,6 @@ CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_MTD=y
CONFIG_SYSTEMCMDS_NSHTERM=y
CONFIG_SYSTEMCMDS_PARAM=y

View File

@ -82,7 +82,6 @@ CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_MTD=y
CONFIG_SYSTEMCMDS_NSHTERM=y
CONFIG_SYSTEMCMDS_PARAM=y

View File

@ -83,7 +83,6 @@ CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_MTD=y
CONFIG_SYSTEMCMDS_NSHTERM=y
CONFIG_SYSTEMCMDS_PARAM=y

View File

@ -82,7 +82,6 @@ CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_MTD=y
CONFIG_SYSTEMCMDS_NSHTERM=y
CONFIG_SYSTEMCMDS_PARAM=y

View File

@ -84,7 +84,6 @@ CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_MTD=y
CONFIG_SYSTEMCMDS_NSHTERM=y
CONFIG_SYSTEMCMDS_PARAM=y

View File

@ -84,7 +84,6 @@ CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_MTD=y
CONFIG_SYSTEMCMDS_NSHTERM=y
CONFIG_SYSTEMCMDS_PARAM=y

View File

@ -78,7 +78,6 @@ CONFIG_SYSTEMCMDS_DUMPFILE=y
CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_MTD=y
CONFIG_SYSTEMCMDS_NSHTERM=y
CONFIG_SYSTEMCMDS_PARAM=y

View File

@ -83,7 +83,6 @@ CONFIG_SYSTEMCMDS_DUMPFILE=y
CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_MTD=y
CONFIG_SYSTEMCMDS_NSHTERM=y
CONFIG_SYSTEMCMDS_PARAM=y

View File

@ -55,7 +55,6 @@ CONFIG_SYSTEMCMDS_DUMPFILE=y
CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_MTD=y
CONFIG_SYSTEMCMDS_NSHTERM=y
CONFIG_SYSTEMCMDS_PARAM=y

View File

@ -87,7 +87,6 @@ CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_MTD=y
CONFIG_SYSTEMCMDS_NSHTERM=y
CONFIG_SYSTEMCMDS_PARAM=y

View File

@ -88,7 +88,6 @@ CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_MTD=y
CONFIG_SYSTEMCMDS_NSHTERM=y
CONFIG_SYSTEMCMDS_PARAM=y

View File

@ -85,7 +85,6 @@ CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_MTD=y
CONFIG_SYSTEMCMDS_NSHTERM=y
CONFIG_SYSTEMCMDS_PARAM=y

View File

@ -91,7 +91,6 @@ CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_MTD=y
CONFIG_SYSTEMCMDS_NSHTERM=y
CONFIG_SYSTEMCMDS_PARAM=y

View File

@ -93,7 +93,6 @@ CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_MTD=y
CONFIG_SYSTEMCMDS_NETMAN=y
CONFIG_SYSTEMCMDS_NSHTERM=y

View File

@ -72,7 +72,6 @@ CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_MTD=y
CONFIG_SYSTEMCMDS_NSHTERM=y
CONFIG_SYSTEMCMDS_PARAM=y

View File

@ -83,7 +83,6 @@ CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_MTD=y
CONFIG_SYSTEMCMDS_NSHTERM=y
CONFIG_SYSTEMCMDS_PARAM=y

View File

@ -78,7 +78,6 @@ CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_MTD=y
CONFIG_SYSTEMCMDS_NETMAN=y
CONFIG_SYSTEMCMDS_NSHTERM=y

View File

@ -59,7 +59,6 @@ CONFIG_MODULES_GIMBAL=y
CONFIG_MODULES_VTOL_ATT_CONTROL=y
CONFIG_SYSTEMCMDS_DYN=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_PARAM=y
CONFIG_SYSTEMCMDS_PERF=y
CONFIG_SYSTEMCMDS_SD_BENCH=y

View File

@ -48,7 +48,6 @@ CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
CONFIG_SYSTEMCMDS_DYN=y
CONFIG_SYSTEMCMDS_FAILURE=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_PARAM=y
CONFIG_SYSTEMCMDS_PERF=y
CONFIG_SYSTEMCMDS_SD_BENCH=y

View File

@ -37,7 +37,6 @@ CONFIG_SYSTEMCMDS_DMESG=y
CONFIG_SYSTEMCMDS_DUMPFILE=y
CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_NSHTERM=y
CONFIG_SYSTEMCMDS_PARAM=y
CONFIG_SYSTEMCMDS_PERF=y

View File

@ -85,7 +85,6 @@ CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_MTD=y
CONFIG_SYSTEMCMDS_NETMAN=y
CONFIG_SYSTEMCMDS_NSHTERM=y

View File

@ -47,7 +47,6 @@ CONFIG_SYSTEMCMDS_DUMPFILE=y
CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_NSHTERM=y
CONFIG_SYSTEMCMDS_PARAM=y
CONFIG_SYSTEMCMDS_PERF=y

View File

@ -69,7 +69,6 @@ CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_MTD=y
CONFIG_SYSTEMCMDS_NSHTERM=y
CONFIG_SYSTEMCMDS_PARAM=y

View File

@ -165,7 +165,6 @@ set(msg_files
task_stack_info.msg
tecs_status.msg
telemetry_status.msg
test_motor.msg
timesync.msg
timesync_status.msg
trajectory_bezier.msg

View File

@ -1,14 +0,0 @@
uint64 timestamp # time since system start (microseconds)
uint8 NUM_MOTOR_OUTPUTS = 8
uint8 ACTION_STOP = 0 # stop all motors (disable motor test mode)
uint8 ACTION_RUN = 1 # run motor(s) (enable motor test mode)
uint8 action # one of ACTION_* (applies to all motors)
uint32 motor_number # number of motor to spin [0..N-1]
float32 value # output power, range [0..1], -1 to stop individual motor
uint32 timeout_ms # timeout in ms after which to exit test mode (if 0, do not time out)
uint8 driver_instance # select output driver (for boards with multiple outputs, like IO+FMU)
uint8 ORB_QUEUE_LENGTH = 4

View File

@ -401,68 +401,6 @@ void MixingOutput::unregister()
}
}
unsigned MixingOutput::motorTest()
{
test_motor_s test_motor;
bool had_update = false;
while (_motor_test.test_motor_sub.update(&test_motor)) {
if (test_motor.driver_instance != 0 ||
test_motor.timestamp == 0 ||
hrt_elapsed_time(&test_motor.timestamp) > 100_ms) {
continue;
}
bool in_test_mode = test_motor.action == test_motor_s::ACTION_RUN;
if (in_test_mode != _motor_test.in_test_mode) {
// reset all outputs to disarmed on state change
for (int i = 0; i < MAX_ACTUATORS; ++i) {
_current_output_value[i] = _disarmed_value[i];
}
}
if (in_test_mode) {
int idx = test_motor.motor_number;
if (idx < MAX_ACTUATORS) {
if (test_motor.value < 0.f) {
_current_output_value[idx] = _disarmed_value[idx];
} else {
_current_output_value[idx] =
math::constrain<uint16_t>(_min_value[idx] + (uint16_t)((_max_value[idx] - _min_value[idx]) * test_motor.value),
_min_value[idx], _max_value[idx]);
}
}
if (test_motor.timeout_ms > 0) {
_motor_test.timeout = test_motor.timestamp + test_motor.timeout_ms * 1000;
} else {
_motor_test.timeout = 0;
}
}
_motor_test.in_test_mode = in_test_mode;
had_update = true;
}
// check for timeouts
if (_motor_test.timeout != 0 && hrt_absolute_time() > _motor_test.timeout) {
_motor_test.in_test_mode = false;
_motor_test.timeout = 0;
for (int i = 0; i < MAX_ACTUATORS; ++i) {
_current_output_value[i] = _disarmed_value[i];
}
had_update = true;
}
return (_motor_test.in_test_mode || had_update) ? _max_num_outputs : 0;
}
bool MixingOutput::update()
{
// check arming state

View File

@ -58,7 +58,6 @@
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/test_motor.h>
using namespace time_literals;
@ -213,8 +212,6 @@ private:
return (_armed.prearmed && !_armed.armed) || _armed.in_esc_calibration_mode;
}
unsigned motorTest();
void setAndPublishActuatorOutputs(unsigned num_outputs, actuator_outputs_s &actuator_outputs);
void publishMixerStatus(const actuator_outputs_s &actuator_outputs);
void updateLatencyPerfCounter(const actuator_outputs_s &actuator_outputs);
@ -276,13 +273,6 @@ private:
bool _wq_switched{false};
uint8_t _max_num_outputs;
struct MotorTest {
uORB::Subscription test_motor_sub{ORB_ID(test_motor)};
bool in_test_mode{false};
hrt_abstime timeout{0};
};
MotorTest _motor_test;
OutputModuleInterface &_interface;
perf_counter_t _control_latency_perf;

View File

@ -1228,10 +1228,6 @@ Commander::handle_command(const vehicle_command_s &cmd)
break;
case vehicle_command_s::VEHICLE_CMD_DO_MOTOR_TEST:
cmd_result = handle_command_motor_test(cmd);
break;
case vehicle_command_s::VEHICLE_CMD_ACTUATOR_TEST:
cmd_result = handle_command_actuator_test(cmd);
break;
@ -1538,54 +1534,6 @@ Commander::handle_command(const vehicle_command_s &cmd)
return true;
}
unsigned
Commander::handle_command_motor_test(const vehicle_command_s &cmd)
{
if (_arm_state_machine.isArmed() || (_safety.isButtonAvailable() && !_safety.isSafetyOff())) {
return vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED;
}
if (_param_com_mot_test_en.get() != 1) {
return vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED;
}
test_motor_s test_motor{};
test_motor.timestamp = hrt_absolute_time();
test_motor.motor_number = (int)(cmd.param1 + 0.5f) - 1;
int throttle_type = (int)(cmd.param2 + 0.5f);
if (throttle_type != 0) { // 0: MOTOR_TEST_THROTTLE_PERCENT
return vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
}
int motor_count = (int)(cmd.param5 + 0.5);
if (motor_count > 1) {
return vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
}
test_motor.action = test_motor_s::ACTION_RUN;
test_motor.value = math::constrain(cmd.param3 / 100.f, 0.f, 1.f);
if (test_motor.value < FLT_EPSILON) {
// the message spec is not clear on whether 0 means stop, but it should be closer to what a user expects
test_motor.value = -1.f;
}
test_motor.timeout_ms = (int)(cmd.param4 * 1000.f + 0.5f);
// enforce a timeout and a maximum limit
if (test_motor.timeout_ms == 0 || test_motor.timeout_ms > 3000) {
test_motor.timeout_ms = 3000;
}
test_motor.driver_instance = 0; // the mavlink command does not allow to specify the instance, so set to 0 for now
_test_motor_pub.publish(test_motor);
return vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
}
unsigned
Commander::handle_command_actuator_test(const vehicle_command_s &cmd)
{

View File

@ -54,7 +54,6 @@
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/actuator_test.h>
#include <uORB/topics/failure_detector_status.h>
#include <uORB/topics/test_motor.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_status.h>
@ -147,7 +146,6 @@ private:
*/
bool handle_command(const vehicle_command_s &cmd);
unsigned handle_command_motor_test(const vehicle_command_s &cmd);
unsigned handle_command_actuator_test(const vehicle_command_s &cmd);
void executeActionRequest(const action_request_s &action_request);
@ -380,7 +378,6 @@ private:
uORB::Publication<actuator_armed_s> _actuator_armed_pub{ORB_ID(actuator_armed)};
uORB::Publication<commander_state_s> _commander_state_pub{ORB_ID(commander_state)};
uORB::Publication<failure_detector_status_s> _failure_detector_status_pub{ORB_ID(failure_detector_status)};
uORB::Publication<test_motor_s> _test_motor_pub{ORB_ID(test_motor)};
uORB::Publication<actuator_test_s> _actuator_test_pub{ORB_ID(actuator_test)};
uORB::Publication<vehicle_control_mode_s> _vehicle_control_mode_pub{ORB_ID(vehicle_control_mode)};
uORB::Publication<vehicle_status_flags_s> _vehicle_status_flags_pub{ORB_ID(vehicle_status_flags)};

View File

@ -315,7 +315,7 @@ void LoggedTopics::add_debug_topics()
add_topic_multi("satellite_info", 1000, 2);
add_topic("mag_worker_data");
add_topic("sensor_preflight_mag", 500);
add_topic("test_motor", 500);
add_topic("actuator_test", 500);
}
void LoggedTopics::add_estimator_replay_topics()

View File

@ -1,42 +0,0 @@
############################################################################
#
# Copyright (c) 2015 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_module(
MODULE systemcmds__motor_test
MAIN motor_test
STACK_MAIN 4096
COMPILE_FLAGS
SRCS
motor_test.cpp
DEPENDS
)

View File

@ -1,12 +0,0 @@
menuconfig SYSTEMCMDS_MOTOR_TEST
bool "motor_test"
default n
---help---
Enable support for motor_test
menuconfig USER_MOTOR_TEST
bool "motor_test running as userspace module"
default y
depends on BOARD_PROTECTED && SYSTEMCMDS_MOTOR_TEST
---help---
Put motor_test in userspace memory

View File

@ -1,190 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2014 PX4 Development Team. All rights reserved.
* Author: Holger Steinhaus <hsteinhaus@gmx.de>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file motor_test.c
*
* Tool for drive testing
*/
#include <drivers/drv_hrt.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/log.h>
#include <px4_platform_common/module.h>
#include <uORB/Publication.hpp>
#include <uORB/topics/test_motor.h>
extern "C" __EXPORT int motor_test_main(int argc, char *argv[]);
static void motor_test(unsigned channel, float value, uint8_t driver_instance, int timeout_ms);
static void usage(const char *reason);
void motor_test(unsigned channel, float value, uint8_t driver_instance, int timeout_ms)
{
test_motor_s test_motor{};
test_motor.timestamp = hrt_absolute_time();
test_motor.motor_number = channel;
test_motor.value = value;
test_motor.action = value >= 0.f ? test_motor_s::ACTION_RUN : test_motor_s::ACTION_STOP;
test_motor.driver_instance = driver_instance;
test_motor.timeout_ms = timeout_ms;
uORB::Publication<test_motor_s> test_motor_pub{ORB_ID(test_motor)};
test_motor_pub.publish(test_motor);
if (test_motor.action == test_motor_s::ACTION_STOP) {
PX4_INFO("motors stop command sent");
} else {
/* Adjust for 1-based motor indexing */
PX4_INFO("motor %d set to %.2f", channel + 1, (double)value);
}
}
static void usage(const char *reason)
{
if (reason != nullptr) {
PX4_WARN("%s", reason);
}
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
Utility to test motors.
WARNING: remove all props before using this command.
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("motor_test", "command");
PRINT_MODULE_USAGE_COMMAND_DESCR("test", "Set motor(s) to a specific output value");
PRINT_MODULE_USAGE_PARAM_INT('m', -1, 1, 8, "Motor to test (1...8, all if not specified)", true);
PRINT_MODULE_USAGE_PARAM_INT('p', 0, 0, 100, "Power (0...100)", true);
PRINT_MODULE_USAGE_PARAM_INT('t', 0, 0, 100, "Timeout in seconds (default=no timeout)", true);
PRINT_MODULE_USAGE_PARAM_INT('i', 0, 0, 4, "driver instance", true);
PRINT_MODULE_USAGE_COMMAND_DESCR("stop", "Stop all motors");
PRINT_MODULE_USAGE_COMMAND_DESCR("iterate", "Iterate all motors starting and stopping one after the other");
}
int motor_test_main(int argc, char *argv[])
{
int channel = -1; //default to all channels
unsigned long lval;
float value = 0.0f;
uint8_t driver_instance = 0;
int ch;
int timeout_ms = 0;
int myoptind = 1;
const char *myoptarg = nullptr;
while ((ch = px4_getopt(argc, argv, "i:m:p:t:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'i':
driver_instance = (uint8_t)strtol(myoptarg, nullptr, 0);
break;
case 'm':
/* Read in motor number and adjust for 1-based indexing */
channel = (int)strtol(myoptarg, nullptr, 0) - 1;
break;
case 'p':
/* Read in power value */
lval = strtoul(myoptarg, nullptr, 0);
if (lval > 100) {
usage("value invalid");
return 1;
}
value = ((float)lval) / 100.f;
break;
case 't':
timeout_ms = strtol(myoptarg, nullptr, 0) * 1000;
break;
default:
usage(nullptr);
return 1;
}
}
bool run_test = true;
if (myoptind >= 0 && myoptind < argc) {
if (strcmp("stop", argv[myoptind]) == 0) {
channel = 0;
value = -1.f;
} else if (strcmp("iterate", argv[myoptind]) == 0) {
value = 0.15f;
for (int i = 0; i < 8; ++i) {
motor_test(i, value, driver_instance, 0);
px4_usleep(500000);
motor_test(i, -1.f, driver_instance, 0);
px4_usleep(10000);
}
run_test = false;
} else if (strcmp("test", argv[myoptind]) == 0) {
// nothing to do
} else {
usage(nullptr);
return 0;
}
} else {
usage(nullptr);
return 0;
}
if (run_test) {
if (channel < 0) {
for (int i = 0; i < 8; ++i) {
motor_test(i, value, driver_instance, timeout_ms);
px4_usleep(10000);
}
} else {
motor_test(channel, value, driver_instance, timeout_ms);
}
}
return 0;
}