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; }