diff --git a/src/modules/commander/CMakeLists.txt b/src/modules/commander/CMakeLists.txt index 8a9314eb66..94755dce5b 100644 --- a/src/modules/commander/CMakeLists.txt +++ b/src/modules/commander/CMakeLists.txt @@ -51,6 +51,7 @@ px4_add_module( mag_calibration.cpp rc_calibration.cpp state_machine_helper.cpp + worker_thread.cpp DEPENDS circuit_breaker failure_detector diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 44619cb2a9..1282628462 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -48,16 +48,9 @@ #include "Arming/PreFlightCheck/PreFlightCheck.hpp" #include "Arming/ArmAuthorization/ArmAuthorization.h" #include "Arming/HealthFlags/HealthFlags.h" -#include "accelerometer_calibration.h" -#include "airspeed_calibration.h" -#include "calibration_routines.h" #include "commander_helper.h" #include "esc_calibration.h" -#include "gyro_calibration.h" -#include "level_calibration.h" -#include "mag_calibration.h" #include "px4_custom_mode.h" -#include "rc_calibration.h" #include "state_machine_helper.h" /* PX4 headers */ @@ -98,18 +91,12 @@ typedef enum VEHICLE_MODE_FLAG { /* Mavlink log uORB handle */ static orb_advert_t mavlink_log_pub = nullptr; -static volatile bool thread_should_exit = false; /**< daemon exit flag */ static struct vehicle_status_s status = {}; static struct actuator_armed_s armed = {}; static struct vehicle_status_flags_s status_flags = {}; -/** - * Loop that runs at a lower rate and priority for calibration and parameter tasks. - */ -void *commander_low_prio_loop(void *arg); - static void answer_command(const vehicle_command_s &cmd, unsigned result, uORB::Publication &command_ack_pub); @@ -1041,15 +1028,196 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_ break; + case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION: { + + if ((status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) + || status.arming_state == vehicle_status_s::ARMING_STATE_SHUTDOWN || _worker_thread.isBusy()) { + + // reject if armed or shutting down + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED, command_ack_pub); + + } else { + + /* try to go to INIT/PREFLIGHT arming state */ + if (TRANSITION_DENIED == arming_state_transition(&status, safety_s{}, vehicle_status_s::ARMING_STATE_INIT, &armed, + false /* fRunPreArmChecks */, &mavlink_log_pub, &status_flags, + PreFlightCheck::arm_requirements_t{}, // arming requirements not relevant for switching to ARMING_STATE_INIT + 30_s, // time since boot not relevant for switching to ARMING_STATE_INIT + (cmd.from_external ? arm_disarm_reason_t::COMMAND_EXTERNAL : arm_disarm_reason_t::COMMAND_INTERNAL)) + ) { + + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub); + break; + + } + + if ((int)(cmd.param1) == 1) { + /* gyro calibration */ + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); + status_flags.condition_calibration_enabled = true; + _worker_thread.startTask(WorkerThread::Request::GyroCalibration); + + } else if ((int)(cmd.param1) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION || + (int)(cmd.param5) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION || + (int)(cmd.param7) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION) { + /* temperature calibration: handled in events module */ + break; + + } else if ((int)(cmd.param2) == 1) { + /* magnetometer calibration */ + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); + status_flags.condition_calibration_enabled = true; + _worker_thread.startTask(WorkerThread::Request::MagCalibration); + + } else if ((int)(cmd.param3) == 1) { + /* zero-altitude pressure calibration */ + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub); + + } else if ((int)(cmd.param4) == 1) { + /* RC calibration */ + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); + /* disable RC control input completely */ + status_flags.rc_input_blocked = true; + mavlink_log_info(&mavlink_log_pub, "Calibration: Disabling RC input"); + + } else if ((int)(cmd.param4) == 2) { + /* RC trim calibration */ + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); + status_flags.condition_calibration_enabled = true; + _worker_thread.startTask(WorkerThread::Request::RCTrimCalibration); + + } else if ((int)(cmd.param5) == 1) { + /* accelerometer calibration */ + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); + status_flags.condition_calibration_enabled = true; + _worker_thread.startTask(WorkerThread::Request::AccelCalibration); + + } else if ((int)(cmd.param5) == 2) { + // board offset calibration + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); + status_flags.condition_calibration_enabled = true; + _worker_thread.startTask(WorkerThread::Request::LevelCalibration); + + } else if ((int)(cmd.param5) == 4) { + // accelerometer quick calibration + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); + status_flags.condition_calibration_enabled = true; + _worker_thread.startTask(WorkerThread::Request::AccelCalibrationQuick); + + } else if ((int)(cmd.param6) == 1 || (int)(cmd.param6) == 2) { + // TODO: param6 == 1 is deprecated, but we still accept it for a while (feb 2017) + /* airspeed calibration */ + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); + status_flags.condition_calibration_enabled = true; + _worker_thread.startTask(WorkerThread::Request::AirspeedCalibration); + + } else if ((int)(cmd.param7) == 1) { + /* do esc calibration */ + if (check_battery_disconnected(&mavlink_log_pub)) { + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); + status_flags.condition_calibration_enabled = true; + armed.in_esc_calibration_mode = true; + _worker_thread.startTask(WorkerThread::Request::ESCCalibration); + + } else { + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub); + } + + } else if ((int)(cmd.param4) == 0) { + /* RC calibration ended - have we been in one worth confirming? */ + if (status_flags.rc_input_blocked) { + /* enable RC control input */ + status_flags.rc_input_blocked = false; + mavlink_log_info(&mavlink_log_pub, "Calibration: Restoring RC input"); + } + + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); + + } else { + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED, command_ack_pub); + } + } + + break; + } + + case vehicle_command_s::VEHICLE_CMD_FIXED_MAG_CAL_YAW: { + // Magnetometer quick calibration using world magnetic model and known heading + if ((status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) + || (status.arming_state == vehicle_status_s::ARMING_STATE_SHUTDOWN) + || _worker_thread.isBusy()) { + + // reject if armed or shutting down + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED, command_ack_pub); + + } else { + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); + // parameter 1: Yaw in degrees + // parameter 3: Latitude + // parameter 4: Longitude + + // assume vehicle pointing north (0 degrees) if heading isn't specified + const float heading_radians = PX4_ISFINITE(cmd.param1) ? math::radians(roundf(cmd.param1)) : 0.f; + + float latitude = NAN; + float longitude = NAN; + + if (PX4_ISFINITE(cmd.param3) && PX4_ISFINITE(cmd.param4)) { + // invalid if both lat & lon are 0 (current mavlink spec) + if ((fabsf(cmd.param3) > 0) && (fabsf(cmd.param4) > 0)) { + latitude = cmd.param3; + longitude = cmd.param4; + } + } + + status_flags.condition_calibration_enabled = true; + _worker_thread.setMagQuickData(heading_radians, latitude, longitude); + _worker_thread.startTask(WorkerThread::Request::MagCalibrationQuick); + } + + break; + } + + case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE: { + + if ((status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) + || status.arming_state == vehicle_status_s::ARMING_STATE_SHUTDOWN + || _worker_thread.isBusy()) { + + // reject if armed or shutting down + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED, command_ack_pub); + + } else { + + if (((int)(cmd.param1)) == 0) { + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); + _worker_thread.startTask(WorkerThread::Request::ParamLoadDefault); + + } else if (((int)(cmd.param1)) == 1) { + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); + _worker_thread.startTask(WorkerThread::Request::ParamSaveDefault); + + } else if (((int)(cmd.param1)) == 2) { + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); + _worker_thread.startTask(WorkerThread::Request::ParamResetAll); + } + } + + break; + } + + case vehicle_command_s::VEHICLE_CMD_START_RX_PAIR: + /* just ack, implementation handled in the IO driver */ + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); + break; + case vehicle_command_s::VEHICLE_CMD_CUSTOM_0: case vehicle_command_s::VEHICLE_CMD_CUSTOM_1: case vehicle_command_s::VEHICLE_CMD_CUSTOM_2: case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL: case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE: case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT: - case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION: case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: - case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE: case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_UAVCAN: case vehicle_command_s::VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY: case vehicle_command_s::VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY: @@ -1063,7 +1231,6 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_ case vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED: case vehicle_command_s::VEHICLE_CMD_DO_LAND_START: case vehicle_command_s::VEHICLE_CMD_DO_GO_AROUND: - case vehicle_command_s::VEHICLE_CMD_START_RX_PAIR: case vehicle_command_s::VEHICLE_CMD_LOGGING_START: case vehicle_command_s::VEHICLE_CMD_LOGGING_STOP: case vehicle_command_s::VEHICLE_CMD_NAV_DELAY: @@ -1072,7 +1239,6 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_ case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_LOCATION: case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET: case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_NONE: - case vehicle_command_s::VEHICLE_CMD_FIXED_MAG_CAL_YAW: case vehicle_command_s::VEHICLE_CMD_INJECT_FAILURE: /* ignore commands that are handled by other parts of the system */ break; @@ -1290,25 +1456,6 @@ Commander::run() int32_t airmode = 0; int32_t rc_map_arm_switch = 0; - /* initialize low priority thread */ - pthread_attr_t commander_low_prio_attr; - pthread_attr_init(&commander_low_prio_attr); - pthread_attr_setstacksize(&commander_low_prio_attr, PX4_STACK_ADJUSTED(3304)); - -#ifndef __PX4_QURT - // This is not supported by QURT (yet). - struct sched_param param; - pthread_attr_getschedparam(&commander_low_prio_attr, ¶m); - - /* low priority */ - param.sched_priority = SCHED_PRIORITY_DEFAULT - 50; - pthread_attr_setschedparam(&commander_low_prio_attr, ¶m); -#endif - pthread_t commander_low_prio_thread; - pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, nullptr); - pthread_attr_destroy(&commander_low_prio_attr); - - status.system_id = _param_mav_sys_id.get(); arm_auth_init(&mavlink_log_pub, &status.system_id); @@ -2435,6 +2582,23 @@ Commander::run() control_status_leds(&status, &armed, _status_changed, _battery_warning); } + // check if the worker has finished + if (_worker_thread.hasResult()) { + int ret = _worker_thread.getResultAndReset(); + armed.in_esc_calibration_mode = false; + + if (status_flags.condition_calibration_enabled) { // did we do a calibration? + status_flags.condition_calibration_enabled = false; + + if (ret == 0) { + tune_positive(true); + + } else { + tune_negative(true); + } + } + } + _status_changed = false; /* store last position lock state */ @@ -2449,15 +2613,6 @@ Commander::run() px4_usleep(COMMANDER_MONITORING_INTERVAL); } - thread_should_exit = true; - - /* wait for threads to complete */ - int ret = pthread_join(commander_low_prio_thread, nullptr); - - if (ret) { - warn("join failed: %d", ret); - } - rgbled_set_color_and_mode(led_control_s::COLOR_WHITE, led_control_s::MODE_OFF); /* close fds */ @@ -3381,295 +3536,6 @@ void answer_command(const vehicle_command_s &cmd, unsigned result, command_ack_pub.publish(command_ack); } -void *commander_low_prio_loop(void *arg) -{ - /* Set thread name */ - px4_prctl(PR_SET_NAME, "commander_low_prio", px4_getpid()); - - /* Subscribe to command topic */ - int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); - - /* command ack */ - uORB::Publication command_ack_pub{ORB_ID(vehicle_command_ack)}; - - /* wakeup source(s) */ - px4_pollfd_struct_t fds[1]; - - fds[0].fd = cmd_sub; - fds[0].events = POLLIN; - - while (!thread_should_exit) { - /* wait for up to 1000ms for data */ - int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000); - - if (pret < 0) { - /* this is undesirable but not much we can do - might want to flag unhappy status */ - warn("commander: poll error %d, %d", pret, errno); - continue; - - } else if (pret != 0) { - struct vehicle_command_s cmd {}; - - /* if we reach here, we have a valid command */ - orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); - - /* only handle low-priority commands here */ - switch (cmd.command) { - case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION: { - - if ((status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) - || status.arming_state == vehicle_status_s::ARMING_STATE_SHUTDOWN) { - - // reject if armed or shutting down - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED, command_ack_pub); - - } else { - - int calib_ret = PX4_ERROR; - - /* try to go to INIT/PREFLIGHT arming state */ - if (TRANSITION_DENIED == arming_state_transition(&status, safety_s{}, vehicle_status_s::ARMING_STATE_INIT, &armed, - false /* fRunPreArmChecks */, &mavlink_log_pub, &status_flags, - PreFlightCheck::arm_requirements_t{}, // arming requirements not relevant for switching to ARMING_STATE_INIT - 30_s, // time since boot not relevant for switching to ARMING_STATE_INIT - (cmd.from_external ? arm_disarm_reason_t::COMMAND_EXTERNAL : arm_disarm_reason_t::COMMAND_INTERNAL)) - ) { - - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub); - break; - - } else { - status_flags.condition_calibration_enabled = true; - } - - if ((int)(cmd.param1) == 1) { - /* gyro calibration */ - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); - calib_ret = do_gyro_calibration(&mavlink_log_pub); - - } else if ((int)(cmd.param1) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION || - (int)(cmd.param5) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION || - (int)(cmd.param7) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION) { - /* temperature calibration: handled in events module */ - break; - - } else if ((int)(cmd.param2) == 1) { - /* magnetometer calibration */ - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); - calib_ret = do_mag_calibration(&mavlink_log_pub); - - } else if ((int)(cmd.param3) == 1) { - /* zero-altitude pressure calibration */ - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub); - - } else if ((int)(cmd.param4) == 1) { - /* RC calibration */ - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); - /* disable RC control input completely */ - status_flags.rc_input_blocked = true; - calib_ret = OK; - mavlink_log_info(&mavlink_log_pub, "Calibration: Disabling RC input"); - - } else if ((int)(cmd.param4) == 2) { - /* RC trim calibration */ - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); - calib_ret = do_trim_calibration(&mavlink_log_pub); - - } else if ((int)(cmd.param5) == 1) { - /* accelerometer calibration */ - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); - calib_ret = do_accel_calibration(&mavlink_log_pub); - - } else if ((int)(cmd.param5) == 2) { - // board offset calibration - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); - calib_ret = do_level_calibration(&mavlink_log_pub); - - } else if ((int)(cmd.param5) == 4) { - // accelerometer quick calibration - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); - calib_ret = do_accel_calibration_quick(&mavlink_log_pub); - - } else if ((int)(cmd.param6) == 1 || (int)(cmd.param6) == 2) { - // TODO: param6 == 1 is deprecated, but we still accept it for a while (feb 2017) - /* airspeed calibration */ - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); - calib_ret = do_airspeed_calibration(&mavlink_log_pub); - - } else if ((int)(cmd.param7) == 1) { - /* do esc calibration */ - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); - calib_ret = do_esc_calibration(&mavlink_log_pub, &armed); - - } else if ((int)(cmd.param4) == 0) { - /* RC calibration ended - have we been in one worth confirming? */ - if (status_flags.rc_input_blocked) { - /* enable RC control input */ - status_flags.rc_input_blocked = false; - mavlink_log_info(&mavlink_log_pub, "Calibration: Restoring RC input"); - } - - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); - /* this always succeeds */ - calib_ret = OK; - - } else { - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED, command_ack_pub); - } - - status_flags.condition_calibration_enabled = false; - - if (calib_ret == OK) { - tune_positive(true); - - // time since boot not relevant here - if (PreFlightCheck::preflightCheck(&mavlink_log_pub, status, status_flags, false, false, true, 30_s)) { - - status_flags.condition_system_sensors_initialized = true; - } - - arming_state_transition(&status, safety_s{}, vehicle_status_s::ARMING_STATE_STANDBY, &armed, - false /* fRunPreArmChecks */, - &mavlink_log_pub, &status_flags, - PreFlightCheck::arm_requirements_t{}, // arming requirements not relevant for switching to ARMING_STATE_STANDBY - 30_s, // time since boot not relevant for switching to ARMING_STATE_STANDBY - (cmd.from_external ? arm_disarm_reason_t::COMMAND_EXTERNAL : arm_disarm_reason_t::COMMAND_INTERNAL)); - - } else { - tune_negative(true); - } - } - - break; - } - - case vehicle_command_s::VEHICLE_CMD_FIXED_MAG_CAL_YAW: { - // Magnetometer quick calibration using world magnetic model and known heading - if ((status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) - || (status.arming_state == vehicle_status_s::ARMING_STATE_SHUTDOWN) - || status_flags.condition_calibration_enabled) { - - // reject if armed or shutting down - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED, command_ack_pub); - - } else { - // parameter 1: Yaw in degrees - // parameter 3: Latitude - // parameter 4: Longitude - - // assume vehicle pointing north (0 degrees) if heading isn't specified - const float heading_radians = PX4_ISFINITE(cmd.param1) ? math::radians(roundf(cmd.param1)) : 0.f; - - float latitude = NAN; - float longitude = NAN; - - if (PX4_ISFINITE(cmd.param3) && PX4_ISFINITE(cmd.param4)) { - // invalid if both lat & lon are 0 (current mavlink spec) - if ((fabsf(cmd.param3) > 0) && (fabsf(cmd.param4) > 0)) { - latitude = cmd.param3; - longitude = cmd.param4; - } - } - - if (do_mag_calibration_quick(&mavlink_log_pub, heading_radians, latitude, longitude) == PX4_OK) { - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); - tune_positive(true); - - } else { - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED, command_ack_pub); - tune_negative(true); - } - } - - break; - } - - case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE: { - - if ((status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) - || status.arming_state == vehicle_status_s::ARMING_STATE_SHUTDOWN) { - - // reject if armed or shutting down - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED, command_ack_pub); - - } else { - - if (((int)(cmd.param1)) == 0) { - int ret = param_load_default(); - - if (ret == OK) { - mavlink_log_info(&mavlink_log_pub, "Settings loaded"); - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); - - } else { - mavlink_log_critical(&mavlink_log_pub, "Error loading settings"); - - /* convenience as many parts of NuttX use negative errno */ - if (ret < 0) { - ret = -ret; - } - - if (ret < 1000) { - mavlink_log_critical(&mavlink_log_pub, "Error: %s", strerror(ret)); - } - - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED, command_ack_pub); - } - - } else if (((int)(cmd.param1)) == 1) { - - int ret = param_save_default(); - - if (ret == OK) { - /* do not spam MAVLink, but provide the answer / green led mechanism */ - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); - - } else { - mavlink_log_critical(&mavlink_log_pub, "Error saving settings"); - - /* convenience as many parts of NuttX use negative errno */ - if (ret < 0) { - ret = -ret; - } - - if (ret < 1000) { - mavlink_log_critical(&mavlink_log_pub, "Error: %s", strerror(ret)); - } - - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED, command_ack_pub); - } - - } else if (((int)(cmd.param1)) == 2) { - - /* reset parameters and save empty file */ - param_reset_all(); - - /* do not spam MAVLink, but provide the answer / green led mechanism */ - mavlink_log_critical(&mavlink_log_pub, "Onboard parameters reset"); - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); - } - } - - break; - } - - case vehicle_command_s::VEHICLE_CMD_START_RX_PAIR: - /* just ack, implementation handled in the IO driver */ - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); - break; - - default: - /* don't answer on unsupported commands, it will be done in main loop */ - break; - } - } - } - - px4_close(cmd_sub); - - return nullptr; -} - int Commander::task_spawn(int argc, char *argv[]) { _task_id = px4_task_spawn_cmd("commander", diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index a686ce3684..f0c7726b45 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -36,6 +36,7 @@ #include "Arming/PreFlightCheck/PreFlightCheck.hpp" #include "failure_detector/FailureDetector.hpp" #include "state_machine_helper.h" +#include "worker_thread.hpp" #include #include @@ -382,6 +383,8 @@ private: safety_s _safety{}; vtol_vehicle_status_s _vtol_status{}; + WorkerThread _worker_thread; + // Subscriptions uORB::Subscription _actuator_controls_sub{ORB_ID_VEHICLE_ATTITUDE_CONTROLS}; uORB::Subscription _cmd_sub {ORB_ID(vehicle_command)}; diff --git a/src/modules/commander/esc_calibration.cpp b/src/modules/commander/esc_calibration.cpp index 2cf24cd155..d7b181eb03 100644 --- a/src/modules/commander/esc_calibration.cpp +++ b/src/modules/commander/esc_calibration.cpp @@ -54,43 +54,39 @@ using namespace time_literals; -int do_esc_calibration(orb_advert_t *mavlink_log_pub, struct actuator_armed_s *armed) +bool check_battery_disconnected(orb_advert_t *mavlink_log_pub) { - int return_code = PX4_OK; - int fd = -1; - bool batt_connected = true; // for safety reasons assume battery is connected, will be cleared below if not the case - hrt_abstime timeout_start = 0; - - calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "esc"); - uORB::SubscriptionData batt_sub{ORB_ID(battery_status)}; const battery_status_s &battery = batt_sub.get(); - batt_sub.update(); if (battery.timestamp == 0) { calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "battery unavailable"); - return_code = PX4_ERROR; - goto Out; + return false; } // Make sure battery is disconnected // battery is not connected if the connected flag is not set and we have a recent battery measurement - batt_sub.update(); - if (!battery.connected && (hrt_elapsed_time(&battery.timestamp) < 500_ms)) { - batt_connected = false; + return true; } - if (batt_connected) { - calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Disconnect battery and try again"); - return_code = PX4_ERROR; - goto Out; - } + calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Disconnect battery and try again"); + return false; +} - armed->in_esc_calibration_mode = true; +int do_esc_calibration(orb_advert_t *mavlink_log_pub) +{ + int return_code = PX4_OK; + hrt_abstime timeout_start = 0; + calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "esc"); - fd = px4_open(PWM_OUTPUT0_DEVICE_PATH, 0); + uORB::SubscriptionData batt_sub{ORB_ID(battery_status)}; + const battery_status_s &battery = batt_sub.get(); + batt_sub.update(); + bool batt_connected = battery.connected; + + int fd = px4_open(PWM_OUTPUT0_DEVICE_PATH, 0); if (fd < 0) { calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Can't open PWM device"); @@ -152,7 +148,7 @@ int do_esc_calibration(orb_advert_t *mavlink_log_pub, struct actuator_armed_s *a } } - px4_usleep(50_ms); + px4_usleep(50000); } Out: @@ -173,8 +169,6 @@ Out: px4_close(fd); } - armed->in_esc_calibration_mode = false; - if (return_code == PX4_OK) { calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "esc"); } diff --git a/src/modules/commander/esc_calibration.h b/src/modules/commander/esc_calibration.h index 6d8dc071b8..a304376183 100644 --- a/src/modules/commander/esc_calibration.h +++ b/src/modules/commander/esc_calibration.h @@ -44,6 +44,8 @@ #include -int do_esc_calibration(orb_advert_t *mavlink_log_pub, struct actuator_armed_s *armed); +int do_esc_calibration(orb_advert_t *mavlink_log_pub); + +bool check_battery_disconnected(orb_advert_t *mavlink_log_pub); #endif diff --git a/src/modules/commander/worker_thread.cpp b/src/modules/commander/worker_thread.cpp new file mode 100644 index 0000000000..e665891b9c --- /dev/null +++ b/src/modules/commander/worker_thread.cpp @@ -0,0 +1,174 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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. + * + ****************************************************************************/ + +#include "worker_thread.hpp" +#include "accelerometer_calibration.h" +#include "airspeed_calibration.h" +#include "calibration_routines.h" +#include "esc_calibration.h" +#include "gyro_calibration.h" +#include "level_calibration.h" +#include "mag_calibration.h" +#include "rc_calibration.h" + +#include +#include + +WorkerThread::~WorkerThread() +{ + if (_state.load() == (int)State::Running) { + /* wait for thread to complete */ + int ret = pthread_join(_thread_handle, nullptr); + + if (ret) { + PX4_ERR("join failed: %d", ret); + } + } +} + +void WorkerThread::startTask(Request request) +{ + if (isBusy()) { + return; + } + + _request = request; + + /* initialize low priority thread */ + pthread_attr_t low_prio_attr; + pthread_attr_init(&low_prio_attr); + pthread_attr_setstacksize(&low_prio_attr, PX4_STACK_ADJUSTED(3304)); + +#ifndef __PX4_QURT + // This is not supported by QURT (yet). + struct sched_param param; + pthread_attr_getschedparam(&low_prio_attr, ¶m); + + /* low priority */ + param.sched_priority = SCHED_PRIORITY_DEFAULT - 50; + pthread_attr_setschedparam(&low_prio_attr, ¶m); +#endif + int ret = pthread_create(&_thread_handle, &low_prio_attr, &threadEntryTrampoline, this); + pthread_attr_destroy(&low_prio_attr); + + if (ret == 0) { + _state.store((int)State::Running); + + } else { + PX4_ERR("Failed to start thread (%i)", ret); + _state.store((int)State::Finished); + _ret_value = ret; + } +} + +void *WorkerThread::threadEntryTrampoline(void *arg) +{ + WorkerThread *worker_thread = (WorkerThread *)arg; + worker_thread->threadEntry(); + return nullptr; +} + +void WorkerThread::threadEntry() +{ + px4_prctl(PR_SET_NAME, "commander_low_prio", px4_getpid()); + + switch (_request) { + case Request::GyroCalibration: + _ret_value = do_gyro_calibration(&_mavlink_log_pub); + break; + + case Request::MagCalibration: + _ret_value = do_mag_calibration(&_mavlink_log_pub); + break; + + case Request::RCTrimCalibration: + _ret_value = do_trim_calibration(&_mavlink_log_pub); + break; + + case Request::AccelCalibration: + _ret_value = do_accel_calibration(&_mavlink_log_pub); + break; + + case Request::LevelCalibration: + _ret_value = do_level_calibration(&_mavlink_log_pub); + break; + + case Request::AccelCalibrationQuick: + _ret_value = do_accel_calibration_quick(&_mavlink_log_pub); + break; + + case Request::AirspeedCalibration: + _ret_value = do_airspeed_calibration(&_mavlink_log_pub); + break; + + case Request::ESCCalibration: + _ret_value = do_esc_calibration(&_mavlink_log_pub); + break; + + case Request::MagCalibrationQuick: + _ret_value = do_mag_calibration_quick(&_mavlink_log_pub, _heading_radians, _latitude, _longitude); + break; + + case Request::ParamLoadDefault: + _ret_value = param_load_default(); + + if (_ret_value != 0) { + mavlink_log_critical(&_mavlink_log_pub, "Error loading settings"); + } + + break; + + case Request::ParamSaveDefault: + _ret_value = param_save_default(); + + if (_ret_value != 0) { + mavlink_log_critical(&_mavlink_log_pub, "Error saving settings"); + } + + break; + + case Request::ParamResetAll: + param_reset_all(); + _ret_value = 0; + break; + } + + _state.store((int)State::Finished); // set this last to signal the main thread we're done +} + +void WorkerThread::setMagQuickData(float heading_rad, float lat, float lon) +{ + _heading_radians = heading_rad; + _latitude = lat; + _longitude = lon; +} diff --git a/src/modules/commander/worker_thread.hpp b/src/modules/commander/worker_thread.hpp new file mode 100644 index 0000000000..df77a996f9 --- /dev/null +++ b/src/modules/commander/worker_thread.hpp @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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. + * + ****************************************************************************/ + +#pragma once + + +#include +#include +#include +#include + +/** + * @class WorkerThread + * low priority background thread, started on demand, used for: + * - calibration + * - param saving + */ +class WorkerThread +{ +public: + enum class Request { + GyroCalibration, + MagCalibration, + RCTrimCalibration, + AccelCalibration, + LevelCalibration, + AccelCalibrationQuick, + AirspeedCalibration, + ESCCalibration, + MagCalibrationQuick, + + ParamLoadDefault, + ParamSaveDefault, + ParamResetAll, + }; + + WorkerThread() = default; + ~WorkerThread(); + + void setMagQuickData(float heading_rad, float lat, float lon); + + void startTask(Request request); + + bool isBusy() const { return _state.load() != (int)State::Idle; } + bool hasResult() const { return _state.load() == (int)State::Finished; } + int getResultAndReset() { _state.store((int)State::Idle); return _ret_value; } + +private: + enum class State { + Idle, + Running, + Finished + }; + + static void *threadEntryTrampoline(void *arg); + void threadEntry(); + + px4::atomic_int _state{(int)State::Idle}; + pthread_t _thread_handle{}; + int _ret_value{}; + Request _request; + orb_advert_t _mavlink_log_pub{nullptr}; + + // extra arguments + float _heading_radians; + float _latitude; + float _longitude; + +}; +