bool Sub::handle_do_motor_test(mavlink_command_long_t command) {
last_do_motor_test_ms = AP_HAL::millis();
// If we are not already testing motors, initialize test
if(!ap.motor_test) {
if (!init_motor_test()) {
gcs().send_text(MAV_SEVERITY_WARNING, "motor test initialization failed!");
return false; // init fail
}
}
float motor_number = command.param1;
float throttle_type = command.param2;
float throttle = command.param3;
// float timeout_s = command.param4; // not used
// float motor_count = command.param5; // not used
float test_type = command.param6;
if (!is_equal(test_type, (float)MOTOR_TEST_ORDER_BOARD)) {
gcs().send_text(MAV_SEVERITY_WARNING, "bad test type %0.2f", (double)test_type);
return false; // test type not supported here
}
if (is_equal(throttle_type, (float)MOTOR_TEST_THROTTLE_PILOT)) {
gcs().send_text(MAV_SEVERITY_WARNING, "bad throttle type %0.2f", (double)throttle_type);
return false; // throttle type not supported here
}
if (is_equal(throttle_type, (float)MOTOR_TEST_THROTTLE_PWM)) {
return motors.output_test_num(motor_number, throttle); // true if motor output is set
}
if (is_equal(throttle_type, (float)MOTOR_TEST_THROTTLE_PERCENT)) {
throttle = constrain_float(throttle, 0.0f, 100.0f);
throttle = channel_throttle->get_radio_min() + throttle / 100.0f * (channel_throttle->get_radio_max() - channel_throttle->get_radio_min());
return motors.output_test_num(motor_number, throttle); // true if motor output is set
}
return false;
}
bool CabDiag::tick(Packet* packet)
{
// Initialize State
if (!init)
{
init = true;
press_start_to_exit = true;
reset();
switch (state)
{
case STATE_INTERFACE:
init_interface();
break;
case STATE_OUTPUT:
init_output();
break;
case STATE_INPUT:
init_input();
break;
case STATE_CRT:
init_crt();
break;
case STATE_MOTORT:
init_motor_test();
press_start_to_exit = false; // Not skippable
break;
}
}
if (counter == 60)
ohud.blit_text_new(7, 23, "PRESS START BUTTON TO EXIT", 0x84);
if (press_start_to_exit && counter >= 60 && input.is_pressed(Input::START))
{
done = true;
}
// Tick State
switch (state)
{
case STATE_INTERFACE:
tick_interface(packet);
break;
case STATE_OUTPUT:
tick_output();
break;
case STATE_INPUT:
tick_input(packet);
break;
case STATE_CRT:
break;
case STATE_MOTORT:
press_start_to_exit = outrun.outputs->diag_motor(packet->ai1, packet->mci, 0);
break;
}
osprites.sprite_copy();
osprites.update_sprites();
otiles.write_tilemap_hw();
oroad.tick();
if (press_start_to_exit)
counter++;
return done;
}
